| /* |
| * 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/osdep.h" |
| #include "qemu-common.h" |
| #include "qemu/host-utils.h" |
| #include "hw/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) |
| { |
| uint32_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, &len)) { |
| 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, &len)) { |
| 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, &len)) { |
| 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, &len)) { |
| 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 ++) { |
| g_free(sdp->service_list[i].attribute_list[0].pair); |
| g_free(sdp->service_list[i].attribute_list); |
| g_free(sdp->service_list[i].uuid); |
| } |
| g_free(sdp->service_list); |
| g_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); |
| } |
| |
| assert(len > 0); |
| record->uuids = pow2ceil(record->uuids); |
| record->attribute_list = |
| g_malloc0(record->attributes * sizeof(*record->attribute_list)); |
| record->uuid = |
| g_malloc0(record->uuids * sizeof(*record->uuid)); |
| data = g_malloc(len); |
| |
| record->attributes = 0; |
| uuid = record->uuid; |
| while (def->attributes[record->attributes].data.type) { |
| int attribute_id = def->attributes[record->attributes].id; |
| record->attribute_list[record->attributes].pair = data; |
| record->attribute_list[record->attributes].attribute_id = attribute_id; |
| |
| len = 0; |
| data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2; |
| data[len ++] = attribute_id >> 8; |
| data[len ++] = attribute_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. The first must be |
| * SDP_ATTR_RECORD_HANDLE so that bt_l2cap_sdp_close_ch can free |
| * the buffer. |
| */ |
| qsort(record->attribute_list, record->attributes, |
| sizeof(*record->attribute_list), |
| (void *) sdp_attributeid_compare); |
| assert(record->attribute_list[0].pair == data); |
| |
| /* 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 = |
| g_malloc0(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")) |
| |
| /* 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")) |
| |
| /* 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")) |
| |
| /* 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 = g_malloc0(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); |
| } |
