text: /*
   * Service Discover Protocol server for QEMU L2CAP devices
   *
   * Copyright (C) 2008 Andrzej Zaborowski  <balrog@zabor.org>
   *
   * This program is free software; you can redistribute it and/or
   * modify it under the terms of the GNU General Public License as
   * published by the Free Software Foundation; either version 2 of
   * the License, or (at your option) any later version.
   *
   * This program is distributed in the hope that it will be useful,
   * but WITHOUT ANY WARRANTY; without even the implied warranty of
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   * GNU General Public License for more details.
   *
   * You should have received a copy of the GNU General Public License along
   * with this program; if not, see <http://www.gnu.org/licenses/>.
   */
  
  #include "qemu-common.h"
  #include "bt.h"
  
  struct bt_l2cap_sdp_state_s {
      struct bt_l2cap_conn_params_s *channel;
  
      struct sdp_service_record_s {
          int match;
  
          int *uuid;
          int uuids;
          struct sdp_service_attribute_s {
              int match;
  
              int attribute_id;
              int len;
              void *pair;
          } *attribute_list;
          int attributes;
      } *service_list;
      int services;
  };
  
  static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left)
  {
      size_t len = *(*element) ++ & SDP_DSIZE_MASK;
  
      if (!*left)
          return -1;
      (*left) --;
  
      if (len < SDP_DSIZE_NEXT1)
          return 1 << len;
      else if (len == SDP_DSIZE_NEXT1) {
          if (*left < 1)
              return -1;
          (*left) --;
  
          return *(*element) ++;
      } else if (len == SDP_DSIZE_NEXT2) {
          if (*left < 2)
              return -1;
          (*left) -= 2;
  
          len = (*(*element) ++) << 8;
          return len | (*(*element) ++);
      } else {
          if (*left < 4)
              return -1;
          (*left) -= 4;
  
          len = (*(*element) ++) << 24;
          len |= (*(*element) ++) << 16;
          len |= (*(*element) ++) << 8;
          return len | (*(*element) ++);
      }
  }
  
  static const uint8_t bt_base_uuid[12] = {
      0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
  };
  
  static int sdp_uuid_match(struct sdp_service_record_s *record,
                  const uint8_t *uuid, ssize_t datalen)
  {
      int *lo, hi, val;
  
      if (datalen == 16 || datalen == 4) {
          if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12))
              return 0;
  
          if (uuid[0] | uuid[1])
              return 0;
          uuid += 2;
      }
  
      val = (uuid[0] << 8) | uuid[1];
      lo = record->uuid;
      hi = record->uuids;
      while (hi >>= 1)
          if (lo[hi] <= val)
              lo += hi;
  
      return *lo == val;
  }
  
  #define CONTINUATION_PARAM_SIZE	(1 + sizeof(int))
  #define MAX_PDU_OUT_SIZE	96	/* Arbitrary */
  #define PDU_HEADER_SIZE		5
  #define MAX_RSP_PARAM_SIZE	(MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \
                  CONTINUATION_PARAM_SIZE)
  
  static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp,
                  const uint8_t **req, ssize_t *len)
  {
      size_t datalen;
      int i;
  
      if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID)
          return 1;
  
      datalen = sdp_datalen(req, len);
      if (datalen != 2 && datalen != 4 && datalen != 16)
          return 1;
  
      for (i = 0; i < sdp->services; i ++)
          if (sdp_uuid_match(&sdp->service_list[i], *req, datalen))
              sdp->service_list[i].match = 1;
  
      (*req) += datalen;
      (*len) -= datalen;
  
      return 0;
  }
  
  static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp,
                  uint8_t *rsp, const uint8_t *req, ssize_t len)
  {
      ssize_t seqlen;
      int i, count, start, end, max;
      int32_t handle;
  
      /* Perform the search */
      for (i = 0; i < sdp->services; i ++)
          sdp->service_list[i].match = 0;
  
      if (len < 1)
          return -SDP_INVALID_SYNTAX;
      if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
          seqlen = sdp_datalen(&req, &len);
          if (seqlen < 3 || len < seqlen)
              return -SDP_INVALID_SYNTAX;
          len -= seqlen;
  
          while (seqlen)
              if (sdp_svc_match(sdp, &req, &seqlen))
                  return -SDP_INVALID_SYNTAX;
      } else if (sdp_svc_match(sdp, &req, &seqlen))
          return -SDP_INVALID_SYNTAX;
  
      if (len < 3)
          return -SDP_INVALID_SYNTAX;
      max = (req[0] << 8) | req[1];
      req += 2;
      len -= 2;
  
      if (*req) {
          if (len <= sizeof(int))
              return -SDP_INVALID_SYNTAX;
          len -= sizeof(int);
          memcpy(&start, req + 1, sizeof(int));
      } else
          start = 0;
  
      if (len > 1)
          return -SDP_INVALID_SYNTAX;
  
      /* Output the results */
      len = 4;
      count = 0;
      end = start;
      for (i = 0; i < sdp->services; i ++)
          if (sdp->service_list[i].match) {
              if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) {
                  handle = i;
                  memcpy(rsp + len, &handle, 4);
                  len += 4;
                  end = count + 1;
              }
  
              count ++;
          }
  
      rsp[0] = count >> 8;
      rsp[1] = count & 0xff;
      rsp[2] = (end - start) >> 8;
      rsp[3] = (end - start) & 0xff;
  
      if (end < count) {
          rsp[len ++] = sizeof(int);
          memcpy(rsp + len, &end, sizeof(int));
          len += 4;
      } else
          rsp[len ++] = 0;
  
      return len;
  }
  
  static int sdp_attr_match(struct sdp_service_record_s *record,
                  const uint8_t **req, ssize_t *len)
  {
      int i, start, end;
  
      if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
          (*req) ++;
          if (*len < 3)
              return 1;
  
          start = (*(*req) ++) << 8;
          start |= *(*req) ++;
          end = start;
          *len -= 3;
      } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
          (*req) ++;
          if (*len < 5)
              return 1;
  
          start = (*(*req) ++) << 8;
          start |= *(*req) ++;
          end = (*(*req) ++) << 8;
          end |= *(*req) ++;
          *len -= 5;
      } else
          return 1;
  
      for (i = 0; i < record->attributes; i ++)
          if (record->attribute_list[i].attribute_id >= start &&
                          record->attribute_list[i].attribute_id <= end)
              record->attribute_list[i].match = 1;
  
      return 0;
  }
  
  static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp,
                  uint8_t *rsp, const uint8_t *req, ssize_t len)
  {
      ssize_t seqlen;
      int i, start, end, max;
      int32_t handle;
      struct sdp_service_record_s *record;
      uint8_t *lst;
  
      /* Perform the search */
      if (len < 7)
          return -SDP_INVALID_SYNTAX;
      memcpy(&handle, req, 4);
      req += 4;
      len -= 4;
  
      if (handle < 0 || handle > sdp->services)
          return -SDP_INVALID_RECORD_HANDLE;
      record = &sdp->service_list[handle];
  
      for (i = 0; i < record->attributes; i ++)
          record->attribute_list[i].match = 0;
  
      max = (req[0] << 8) | req[1];
      req += 2;
      len -= 2;
      if (max < 0x0007)
          return -SDP_INVALID_SYNTAX;
  
      if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
          seqlen = sdp_datalen(&req, &len);
          if (seqlen < 3 || len < seqlen)
              return -SDP_INVALID_SYNTAX;
          len -= seqlen;
  
          while (seqlen)
              if (sdp_attr_match(record, &req, &seqlen))
                  return -SDP_INVALID_SYNTAX;
      } else if (sdp_attr_match(record, &req, &seqlen))
          return -SDP_INVALID_SYNTAX;
  
      if (len < 1)
          return -SDP_INVALID_SYNTAX;
  
      if (*req) {
          if (len <= sizeof(int))
              return -SDP_INVALID_SYNTAX;
          len -= sizeof(int);
          memcpy(&start, req + 1, sizeof(int));
      } else
          start = 0;
  
      if (len > 1)
          return -SDP_INVALID_SYNTAX;
  
      /* Output the results */
      lst = rsp + 2;
      max = MIN(max, MAX_RSP_PARAM_SIZE);
      len = 3 - start;
      end = 0;
      for (i = 0; i < record->attributes; i ++)
          if (record->attribute_list[i].match) {
              if (len >= 0 && len + record->attribute_list[i].len < max) {
                  memcpy(lst + len, record->attribute_list[i].pair,
                                  record->attribute_list[i].len);
                  end = len + record->attribute_list[i].len;
              }
              len += record->attribute_list[i].len;
          }
      if (0 >= start) {
         lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
         lst[1] = (len + start - 3) >> 8;
         lst[2] = (len + start - 3) & 0xff;
      }
  
      rsp[0] = end >> 8;
      rsp[1] = end & 0xff;
  
      if (end < len) {
          len = end + start;
          lst[end ++] = sizeof(int);
          memcpy(lst + end, &len, sizeof(int));
          end += sizeof(int);
      } else
          lst[end ++] = 0;
  
      return end + 2;
  }
  
  static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp,
                  const uint8_t **req, ssize_t *len)
  {
      int i, j, start, end;
      struct sdp_service_record_s *record;
  
      if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
          (*req) ++;
          if (*len < 3)
              return 1;
  
          start = (*(*req) ++) << 8;
          start |= *(*req) ++;
          end = start;
          *len -= 3;
      } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
          (*req) ++;
          if (*len < 5)
              return 1;
  
          start = (*(*req) ++) << 8;
          start |= *(*req) ++;
          end = (*(*req) ++) << 8;
          end |= *(*req) ++;
          *len -= 5;
      } else
          return 1;
  
      for (i = 0; i < sdp->services; i ++)
          if ((record = &sdp->service_list[i])->match)
              for (j = 0; j < record->attributes; j ++)
                  if (record->attribute_list[j].attribute_id >= start &&
                                  record->attribute_list[j].attribute_id <= end)
                      record->attribute_list[j].match = 1;
  
      return 0;
  }
  
  static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp,
                  uint8_t *rsp, const uint8_t *req, ssize_t len)
  {
      ssize_t seqlen;
      int i, j, start, end, max;
      struct sdp_service_record_s *record;
      uint8_t *lst;
  
      /* Perform the search */
      for (i = 0; i < sdp->services; i ++) {
          sdp->service_list[i].match = 0;
              for (j = 0; j < sdp->service_list[i].attributes; j ++)
                  sdp->service_list[i].attribute_list[j].match = 0;
      }
  
      if (len < 1)
          return -SDP_INVALID_SYNTAX;
      if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
          seqlen = sdp_datalen(&req, &len);
          if (seqlen < 3 || len < seqlen)
              return -SDP_INVALID_SYNTAX;
          len -= seqlen;
  
          while (seqlen)
              if (sdp_svc_match(sdp, &req, &seqlen))
                  return -SDP_INVALID_SYNTAX;
      } else if (sdp_svc_match(sdp, &req, &seqlen))
          return -SDP_INVALID_SYNTAX;
  
      if (len < 3)
          return -SDP_INVALID_SYNTAX;
      max = (req[0] << 8) | req[1];
      req += 2;
      len -= 2;
      if (max < 0x0007)
          return -SDP_INVALID_SYNTAX;
  
      if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
          seqlen = sdp_datalen(&req, &len);
          if (seqlen < 3 || len < seqlen)
              return -SDP_INVALID_SYNTAX;
          len -= seqlen;
  
          while (seqlen)
              if (sdp_svc_attr_match(sdp, &req, &seqlen))
                  return -SDP_INVALID_SYNTAX;
      } else if (sdp_svc_attr_match(sdp, &req, &seqlen))
          return -SDP_INVALID_SYNTAX;
  
      if (len < 1)
          return -SDP_INVALID_SYNTAX;
  
      if (*req) {
          if (len <= sizeof(int))
              return -SDP_INVALID_SYNTAX;
          len -= sizeof(int);
          memcpy(&start, req + 1, sizeof(int));
      } else
          start = 0;
  
      if (len > 1)
          return -SDP_INVALID_SYNTAX;
  
      /* Output the results */
      /* This assumes empty attribute lists are never to be returned even
       * for matching Service Records.  In practice this shouldn't happen
       * as the requestor will usually include the always present
       * ServiceRecordHandle AttributeID in AttributeIDList.  */
      lst = rsp + 2;
      max = MIN(max, MAX_RSP_PARAM_SIZE);
      len = 3 - start;
      end = 0;
      for (i = 0; i < sdp->services; i ++)
          if ((record = &sdp->service_list[i])->match) {
              len += 3;
              seqlen = len;
              for (j = 0; j < record->attributes; j ++)
                  if (record->attribute_list[j].match) {
                      if (len >= 0)
                          if (len + record->attribute_list[j].len < max) {
                              memcpy(lst + len, record->attribute_list[j].pair,
                                              record->attribute_list[j].len);
                              end = len + record->attribute_list[j].len;
                          }
                      len += record->attribute_list[j].len;
                  }
              if (seqlen == len)
                  len -= 3;
              else if (seqlen >= 3 && seqlen < max) {
                  lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
                  lst[seqlen - 2] = (len - seqlen) >> 8;
                  lst[seqlen - 1] = (len - seqlen) & 0xff;
              }
          }
      if (len == 3 - start)
          len -= 3;
      else if (0 >= start) {
         lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
         lst[1] = (len + start - 3) >> 8;
         lst[2] = (len + start - 3) & 0xff;
      }
  
      rsp[0] = end >> 8;
      rsp[1] = end & 0xff;
  
      if (end < len) {
          len = end + start;
          lst[end ++] = sizeof(int);
          memcpy(lst + end, &len, sizeof(int));
          end += sizeof(int);
      } else
          lst[end ++] = 0;
  
      return end + 2;
  }
  
  static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len)
  {
      struct bt_l2cap_sdp_state_s *sdp = opaque;
      enum bt_sdp_cmd pdu_id;
      uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out;
      int transaction_id, plen;
      int err = 0;
      int rsp_len = 0;
  
      if (len < 5) {
          fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len);
          return;
      }
  
      pdu_id = *data ++;
      transaction_id = (data[0] << 8) | data[1];
      plen = (data[2] << 8) | data[3];
      data += 4;
      len -= 5;
  
      if (len != plen) {
          fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n",
                          __FUNCTION__, plen, len);
          err = SDP_INVALID_PDU_SIZE;
          goto respond;
      }
  
      switch (pdu_id) {
      case SDP_SVC_SEARCH_REQ:
          rsp_len = sdp_svc_search(sdp, rsp, data, len);
          pdu_id = SDP_SVC_SEARCH_RSP;
          break;
  
      case SDP_SVC_ATTR_REQ:
          rsp_len = sdp_attr_get(sdp, rsp, data, len);
          pdu_id = SDP_SVC_ATTR_RSP;
          break;
  
      case SDP_SVC_SEARCH_ATTR_REQ:
          rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len);
          pdu_id = SDP_SVC_SEARCH_ATTR_RSP;
          break;
  
      case SDP_ERROR_RSP:
      case SDP_SVC_ATTR_RSP:
      case SDP_SVC_SEARCH_RSP:
      case SDP_SVC_SEARCH_ATTR_RSP:
      default:
          fprintf(stderr, "%s: unexpected SDP PDU ID %02x.\n",
                          __FUNCTION__, pdu_id);
          err = SDP_INVALID_SYNTAX;
          break;
      }
  
      if (rsp_len < 0) {
          err = -rsp_len;
          rsp_len = 0;
      }
  
  respond:
      if (err) {
          pdu_id = SDP_ERROR_RSP;
          rsp[rsp_len ++] = err >> 8;
          rsp[rsp_len ++] = err & 0xff;
      }
  
      sdu_out = sdp->channel->sdu_out(sdp->channel, rsp_len + PDU_HEADER_SIZE);
  
      sdu_out[0] = pdu_id;
      sdu_out[1] = transaction_id >> 8;
      sdu_out[2] = transaction_id & 0xff;
      sdu_out[3] = rsp_len >> 8;
      sdu_out[4] = rsp_len & 0xff;
      memcpy(sdu_out + PDU_HEADER_SIZE, rsp, rsp_len);
  
      sdp->channel->sdu_submit(sdp->channel);
  }
  
  static void bt_l2cap_sdp_close_ch(void *opaque)
  {
      struct bt_l2cap_sdp_state_s *sdp = opaque;
      int i;
  
      for (i = 0; i < sdp->services; i ++) {
          qemu_free(sdp->service_list[i].attribute_list->pair);
          qemu_free(sdp->service_list[i].attribute_list);
          qemu_free(sdp->service_list[i].uuid);
      }
      qemu_free(sdp->service_list);
      qemu_free(sdp);
  }
  
  struct sdp_def_service_s {
      uint16_t class_uuid;
      struct sdp_def_attribute_s {
          uint16_t id;
          struct sdp_def_data_element_s {
              uint8_t type;
              union {
                  uint32_t uint;
                  const char *str;
                  struct sdp_def_data_element_s *list;
              } value;
          } data;
      } attributes[];
  };
  
  /* Calculate a safe byte count to allocate that will store the given
   * element, at the same time count elements of a UUID type.  */
  static int sdp_attr_max_size(struct sdp_def_data_element_s *element,
                  int *uuids)
  {
      int type = element->type & ~SDP_DSIZE_MASK;
      int len;
  
      if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_UUID ||
                      type == SDP_DTYPE_BOOL) {
          if (type == SDP_DTYPE_UUID)
              (*uuids) ++;
          return 1 + (1 << (element->type & SDP_DSIZE_MASK));
      }
  
      if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
          if (element->type & SDP_DSIZE_MASK) {
              for (len = 0; element->value.str[len] |
                              element->value.str[len + 1]; len ++);
              return len;
          } else
              return 2 + strlen(element->value.str);
      }
  
      if (type != SDP_DTYPE_SEQ)
          exit(-1);
      len = 2;
      element = element->value.list;
      while (element->type)
          len += sdp_attr_max_size(element ++, uuids);
      if (len > 255)
          exit (-1);
  
      return len;
  }
  
  static int sdp_attr_write(uint8_t *data,
                  struct sdp_def_data_element_s *element, int **uuid)
  {
      int type = element->type & ~SDP_DSIZE_MASK;
      int len = 0;
  
      if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_BOOL) {
          data[len ++] = element->type;
          if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_1)
              data[len ++] = (element->value.uint >>  0) & 0xff;
          else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_2) {
              data[len ++] = (element->value.uint >>  8) & 0xff;
              data[len ++] = (element->value.uint >>  0) & 0xff;
          } else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_4) {
              data[len ++] = (element->value.uint >>  24) & 0xff;
              data[len ++] = (element->value.uint >>  16) & 0xff;
              data[len ++] = (element->value.uint >>  8) & 0xff;
              data[len ++] = (element->value.uint >>  0) & 0xff;
          }
  
          return len;
      }
  
      if (type == SDP_DTYPE_UUID) {
          *(*uuid) ++ = element->value.uint;
  
          data[len ++] = element->type;
          data[len ++] = (element->value.uint >>  24) & 0xff;
          data[len ++] = (element->value.uint >>  16) & 0xff;
          data[len ++] = (element->value.uint >>  8) & 0xff;
          data[len ++] = (element->value.uint >>  0) & 0xff;
          memcpy(data + len, bt_base_uuid, 12);
  
          return len + 12;
      }
  
      data[0] = type | SDP_DSIZE_NEXT1;
      if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
          if (element->type & SDP_DSIZE_MASK)
              for (len = 0; element->value.str[len] |
                              element->value.str[len + 1]; len ++);
          else
              len = strlen(element->value.str);
          memcpy(data + 2, element->value.str, data[1] = len);
  
          return len + 2;
      }
  
      len = 2;
      element = element->value.list;
      while (element->type)
          len += sdp_attr_write(data + len, element ++, uuid);
      data[1] = len - 2;
  
      return len;
  }
  
  static int sdp_attributeid_compare(const struct sdp_service_attribute_s *a,
                  const struct sdp_service_attribute_s *b)
  {
      return (int) b->attribute_id - a->attribute_id;
  }
  
  static int sdp_uuid_compare(const int *a, const int *b)
  {
      return *a - *b;
  }
  
  static void sdp_service_record_build(struct sdp_service_record_s *record,
                  struct sdp_def_service_s *def, int handle)
  {
      int len = 0;
      uint8_t *data;
      int *uuid;
  
      record->uuids = 0;
      while (def->attributes[record->attributes].data.type) {
          len += 3;
          len += sdp_attr_max_size(&def->attributes[record->attributes ++].data,
                          &record->uuids);
      }
      record->uuids = 1 << ffs(record->uuids - 1);
      record->attribute_list =
              qemu_mallocz(record->attributes * sizeof(*record->attribute_list));
      record->uuid =
              qemu_mallocz(record->uuids * sizeof(*record->uuid));
      data = qemu_malloc(len);
  
      record->attributes = 0;
      uuid = record->uuid;
      while (def->attributes[record->attributes].data.type) {
          record->attribute_list[record->attributes].pair = data;
  
          len = 0;
          data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2;
          data[len ++] = def->attributes[record->attributes].id >> 8;
          data[len ++] = def->attributes[record->attributes].id & 0xff;
          len += sdp_attr_write(data + len,
                          &def->attributes[record->attributes].data, &uuid);
  
          /* Special case: assign a ServiceRecordHandle in sequence */
          if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE)
              def->attributes[record->attributes].data.value.uint = handle;
          /* Note: we could also assign a ServiceDescription based on
           * sdp->device.device->lmp_name.  */
  
          record->attribute_list[record->attributes ++].len = len;
          data += len;
      }
  
      /* Sort the attribute list by the AttributeID */
      qsort(record->attribute_list, record->attributes,
                      sizeof(*record->attribute_list),
                      (void *) sdp_attributeid_compare);
      /* Sort the searchable UUIDs list for bisection */
      qsort(record->uuid, record->uuids,
                      sizeof(*record->uuid),
                      (void *) sdp_uuid_compare);
  }
  
  static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp,
                  struct sdp_def_service_s **service)
  {
      sdp->services = 0;
      while (service[sdp->services])
          sdp->services ++;
      sdp->service_list =
              qemu_mallocz(sdp->services * sizeof(*sdp->service_list));
  
      sdp->services = 0;
      while (*service) {
          sdp_service_record_build(&sdp->service_list[sdp->services],
                          *service, sdp->services);
          service ++;
          sdp->services ++;
      }
  }
  
  #define LAST { .type = 0 }
  #define SERVICE(name, attrs)				\
      static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \
          .attributes = { attrs { .data = LAST } },	\
      };
  #define ATTRIBUTE(attrid, val)	{ .id = glue(SDP_ATTR_, attrid), .data = val },
  #define UINT8(val)	{				\
          .type       = SDP_DTYPE_UINT | SDP_DSIZE_1,	\
          .value.uint = val,				\
      },
  #define UINT16(val)	{				\
          .type       = SDP_DTYPE_UINT | SDP_DSIZE_2,	\
          .value.uint = val,				\
      },
  #define UINT32(val)	{				\
          .type       = SDP_DTYPE_UINT | SDP_DSIZE_4,	\
          .value.uint = val,				\
      },
  #define UUID128(val)	{				\
          .type       = SDP_DTYPE_UUID | SDP_DSIZE_16,	\
          .value.uint = val,				\
      },
  #define SDP_TRUE	{				\
          .type       = SDP_DTYPE_BOOL | SDP_DSIZE_1,	\
          .value.uint = 1,				\
      },
  #define SDP_FALSE	{				\
          .type       = SDP_DTYPE_BOOL | SDP_DSIZE_1,	\
          .value.uint = 0,				\
      },
  #define STRING(val)	{				\
          .type       = SDP_DTYPE_STRING,			\
          .value.str  = val,				\
      },
  #define ARRAY(...)	{				\
          .type       = SDP_DTYPE_STRING | SDP_DSIZE_2,	\
          .value.str  = (char []) { __VA_ARGS__, 0, 0 },	\
      },
  #define URL(val)	{				\
          .type       = SDP_DTYPE_URL,			\
          .value.str  = val,				\
      },
  #if 1
  #define LIST(val)	{				\
          .type       = SDP_DTYPE_SEQ,			\
          .value.list = (struct sdp_def_data_element_s []) { val LAST }, \
      },
  #endif
  
  /* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes
   * in resulting SDP data representation size.  */
  
  SERVICE(hid,
      ATTRIBUTE(RECORD_HANDLE,   UINT32(0))	/* Filled in later */
      ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID)))
      ATTRIBUTE(RECORD_STATE,    UINT32(1))
      ATTRIBUTE(PROTO_DESC_LIST, LIST(
          LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL))
          LIST(UUID128(HIDP_UUID))
      ))
      ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
      ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
          UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
      ))
      ATTRIBUTE(PFILE_DESC_LIST, LIST(
          LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100))
      ))
      ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
      ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID"))
      ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse"))
      ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
  
      /* Profile specific */
      ATTRIBUTE(DEVICE_RELEASE_NUMBER,	UINT16(0x0091)) /* Deprecated, remove */
      ATTRIBUTE(PARSER_VERSION,		UINT16(0x0111))
      /* TODO: extract from l2cap_device->device.class[0] */
      ATTRIBUTE(DEVICE_SUBCLASS,		UINT8(0x40))
      ATTRIBUTE(COUNTRY_CODE,		UINT8(0x15))
      ATTRIBUTE(VIRTUAL_CABLE,		SDP_TRUE)
      ATTRIBUTE(RECONNECT_INITIATE,	SDP_FALSE)
      /* TODO: extract from hid->usbdev->report_desc */
      ATTRIBUTE(DESCRIPTOR_LIST,		LIST(
          LIST(UINT8(0x22) ARRAY(
              0x05, 0x01,	/* Usage Page (Generic Desktop) */
              0x09, 0x06,	/* Usage (Keyboard) */
              0xa1, 0x01,	/* Collection (Application) */
              0x75, 0x01,	/*   Report Size (1) */
              0x95, 0x08,	/*   Report Count (8) */
              0x05, 0x07,	/*   Usage Page (Key Codes) */
              0x19, 0xe0,	/*   Usage Minimum (224) */
              0x29, 0xe7,	/*   Usage Maximum (231) */
              0x15, 0x00,	/*   Logical Minimum (0) */
              0x25, 0x01,	/*   Logical Maximum (1) */
              0x81, 0x02,	/*   Input (Data, Variable, Absolute) */
              0x95, 0x01,	/*   Report Count (1) */
              0x75, 0x08,	/*   Report Size (8) */
              0x81, 0x01,	/*   Input (Constant) */
              0x95, 0x05,	/*   Report Count (5) */
              0x75, 0x01,	/*   Report Size (1) */
              0x05, 0x08,	/*   Usage Page (LEDs) */
              0x19, 0x01,	/*   Usage Minimum (1) */
              0x29, 0x05,	/*   Usage Maximum (5) */
              0x91, 0x02,	/*   Output (Data, Variable, Absolute) */
              0x95, 0x01,	/*   Report Count (1) */
              0x75, 0x03,	/*   Report Size (3) */
              0x91, 0x01,	/*   Output (Constant) */
              0x95, 0x06,	/*   Report Count (6) */
              0x75, 0x08,	/*   Report Size (8) */
              0x15, 0x00,	/*   Logical Minimum (0) */
              0x25, 0xff,	/*   Logical Maximum (255) */
              0x05, 0x07,	/*   Usage Page (Key Codes) */
              0x19, 0x00,	/*   Usage Minimum (0) */
              0x29, 0xff,	/*   Usage Maximum (255) */
              0x81, 0x00,	/*   Input (Data, Array) */
              0xc0	/* End Collection */
      ))))
      ATTRIBUTE(LANG_ID_BASE_LIST,	LIST(
          LIST(UINT16(0x0409) UINT16(0x0100))
      ))
      ATTRIBUTE(SDP_DISABLE,		SDP_FALSE)
      ATTRIBUTE(BATTERY_POWER,		SDP_TRUE)
      ATTRIBUTE(REMOTE_WAKEUP,		SDP_TRUE)
      ATTRIBUTE(BOOT_DEVICE,		SDP_TRUE)	/* XXX: untested */
      ATTRIBUTE(SUPERVISION_TIMEOUT,	UINT16(0x0c80))
      ATTRIBUTE(NORMALLY_CONNECTABLE,	SDP_TRUE)
      ATTRIBUTE(PROFILE_VERSION,		UINT16(0x0100))
  )
  
  SERVICE(sdp,
      ATTRIBUTE(RECORD_HANDLE,   UINT32(0))	/* Filled in later */
      ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID)))
      ATTRIBUTE(RECORD_STATE,    UINT32(1))
      ATTRIBUTE(PROTO_DESC_LIST, LIST(
          LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
          LIST(UUID128(SDP_UUID))
      ))
      ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
      ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
          UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
      ))
      ATTRIBUTE(PFILE_DESC_LIST, LIST(
          LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100))
      ))
      ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
      ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
  
      /* Profile specific */
      ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100)))
      ATTRIBUTE(SVCDB_STATE    , UINT32(1))
  )
  
  SERVICE(pnp,
      ATTRIBUTE(RECORD_HANDLE,   UINT32(0))	/* Filled in later */
      ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID)))
      ATTRIBUTE(RECORD_STATE,    UINT32(1))
      ATTRIBUTE(PROTO_DESC_LIST, LIST(
          LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
          LIST(UUID128(SDP_UUID))
      ))
      ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
      ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
          UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
      ))
      ATTRIBUTE(PFILE_DESC_LIST, LIST(
          LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100))
      ))
      ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
      ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
  
      /* Profile specific */
      ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100))
      ATTRIBUTE(VERSION,         UINT16(0x0100))
      ATTRIBUTE(PRIMARY_RECORD,  SDP_TRUE)
  )
  
  static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev,
                  struct bt_l2cap_conn_params_s *params)
  {
      struct bt_l2cap_sdp_state_s *sdp = qemu_mallocz(sizeof(*sdp));
      struct sdp_def_service_s *services[] = {
          &sdp_service_sdp_s,
          &sdp_service_hid_s,
          &sdp_service_pnp_s,
          NULL,
      };
  
      sdp->channel = params;
      sdp->channel->opaque = sdp;
      sdp->channel->close = bt_l2cap_sdp_close_ch;
      sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in;
  
      sdp_service_db_build(sdp, services);
  
      return 0;
  }
  
  void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev)
  {
      bt_l2cap_psm_register(dev, BT_PSM_SDP,
                      MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch);
  }
  