David 'Digit' Turner | 5d8f37a | 2009-09-14 14:32:27 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Service Discover Protocol server for QEMU L2CAP devices |
| 3 | * |
| 4 | * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.org> |
| 5 | * |
| 6 | * This program is free software; you can redistribute it and/or |
| 7 | * modify it under the terms of the GNU General Public License as |
| 8 | * published by the Free Software Foundation; either version 2 of |
| 9 | * the License, or (at your option) any later version. |
| 10 | * |
| 11 | * This program is distributed in the hope that it will be useful, |
| 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 14 | * GNU General Public License for more details. |
| 15 | * |
| 16 | * You should have received a copy of the GNU General Public License along |
David Turner | a253513 | 2010-09-10 10:15:07 +0200 | [diff] [blame] | 17 | * with this program; if not, see <http://www.gnu.org/licenses/>. |
David 'Digit' Turner | 5d8f37a | 2009-09-14 14:32:27 -0700 | [diff] [blame] | 18 | */ |
| 19 | |
| 20 | #include "qemu-common.h" |
| 21 | #include "bt.h" |
| 22 | |
| 23 | struct bt_l2cap_sdp_state_s { |
| 24 | struct bt_l2cap_conn_params_s *channel; |
| 25 | |
| 26 | struct sdp_service_record_s { |
| 27 | int match; |
| 28 | |
| 29 | int *uuid; |
| 30 | int uuids; |
| 31 | struct sdp_service_attribute_s { |
| 32 | int match; |
| 33 | |
| 34 | int attribute_id; |
| 35 | int len; |
| 36 | void *pair; |
| 37 | } *attribute_list; |
| 38 | int attributes; |
| 39 | } *service_list; |
| 40 | int services; |
| 41 | }; |
| 42 | |
| 43 | static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left) |
| 44 | { |
| 45 | size_t len = *(*element) ++ & SDP_DSIZE_MASK; |
| 46 | |
| 47 | if (!*left) |
| 48 | return -1; |
| 49 | (*left) --; |
| 50 | |
| 51 | if (len < SDP_DSIZE_NEXT1) |
| 52 | return 1 << len; |
| 53 | else if (len == SDP_DSIZE_NEXT1) { |
| 54 | if (*left < 1) |
| 55 | return -1; |
| 56 | (*left) --; |
| 57 | |
| 58 | return *(*element) ++; |
| 59 | } else if (len == SDP_DSIZE_NEXT2) { |
| 60 | if (*left < 2) |
| 61 | return -1; |
| 62 | (*left) -= 2; |
| 63 | |
| 64 | len = (*(*element) ++) << 8; |
| 65 | return len | (*(*element) ++); |
| 66 | } else { |
| 67 | if (*left < 4) |
| 68 | return -1; |
| 69 | (*left) -= 4; |
| 70 | |
| 71 | len = (*(*element) ++) << 24; |
| 72 | len |= (*(*element) ++) << 16; |
| 73 | len |= (*(*element) ++) << 8; |
| 74 | return len | (*(*element) ++); |
| 75 | } |
| 76 | } |
| 77 | |
| 78 | static const uint8_t bt_base_uuid[12] = { |
| 79 | 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb, |
| 80 | }; |
| 81 | |
| 82 | static int sdp_uuid_match(struct sdp_service_record_s *record, |
| 83 | const uint8_t *uuid, ssize_t datalen) |
| 84 | { |
| 85 | int *lo, hi, val; |
| 86 | |
| 87 | if (datalen == 16 || datalen == 4) { |
| 88 | if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12)) |
| 89 | return 0; |
| 90 | |
| 91 | if (uuid[0] | uuid[1]) |
| 92 | return 0; |
| 93 | uuid += 2; |
| 94 | } |
| 95 | |
| 96 | val = (uuid[0] << 8) | uuid[1]; |
| 97 | lo = record->uuid; |
| 98 | hi = record->uuids; |
| 99 | while (hi >>= 1) |
| 100 | if (lo[hi] <= val) |
| 101 | lo += hi; |
| 102 | |
| 103 | return *lo == val; |
| 104 | } |
| 105 | |
| 106 | #define CONTINUATION_PARAM_SIZE (1 + sizeof(int)) |
| 107 | #define MAX_PDU_OUT_SIZE 96 /* Arbitrary */ |
| 108 | #define PDU_HEADER_SIZE 5 |
| 109 | #define MAX_RSP_PARAM_SIZE (MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \ |
| 110 | CONTINUATION_PARAM_SIZE) |
| 111 | |
| 112 | static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp, |
| 113 | const uint8_t **req, ssize_t *len) |
| 114 | { |
| 115 | size_t datalen; |
| 116 | int i; |
| 117 | |
| 118 | if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID) |
| 119 | return 1; |
| 120 | |
| 121 | datalen = sdp_datalen(req, len); |
| 122 | if (datalen != 2 && datalen != 4 && datalen != 16) |
| 123 | return 1; |
| 124 | |
| 125 | for (i = 0; i < sdp->services; i ++) |
| 126 | if (sdp_uuid_match(&sdp->service_list[i], *req, datalen)) |
| 127 | sdp->service_list[i].match = 1; |
| 128 | |
| 129 | (*req) += datalen; |
| 130 | (*len) -= datalen; |
| 131 | |
| 132 | return 0; |
| 133 | } |
| 134 | |
| 135 | static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp, |
| 136 | uint8_t *rsp, const uint8_t *req, ssize_t len) |
| 137 | { |
| 138 | ssize_t seqlen; |
| 139 | int i, count, start, end, max; |
| 140 | int32_t handle; |
| 141 | |
| 142 | /* Perform the search */ |
| 143 | for (i = 0; i < sdp->services; i ++) |
| 144 | sdp->service_list[i].match = 0; |
| 145 | |
| 146 | if (len < 1) |
| 147 | return -SDP_INVALID_SYNTAX; |
| 148 | if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) { |
| 149 | seqlen = sdp_datalen(&req, &len); |
| 150 | if (seqlen < 3 || len < seqlen) |
| 151 | return -SDP_INVALID_SYNTAX; |
| 152 | len -= seqlen; |
| 153 | |
| 154 | while (seqlen) |
| 155 | if (sdp_svc_match(sdp, &req, &seqlen)) |
| 156 | return -SDP_INVALID_SYNTAX; |
| 157 | } else if (sdp_svc_match(sdp, &req, &seqlen)) |
| 158 | return -SDP_INVALID_SYNTAX; |
| 159 | |
| 160 | if (len < 3) |
| 161 | return -SDP_INVALID_SYNTAX; |
David Turner | a253513 | 2010-09-10 10:15:07 +0200 | [diff] [blame] | 162 | max = (req[0] << 8) | req[1]; |
David 'Digit' Turner | 5d8f37a | 2009-09-14 14:32:27 -0700 | [diff] [blame] | 163 | req += 2; |
| 164 | len -= 2; |
| 165 | |
| 166 | if (*req) { |
| 167 | if (len <= sizeof(int)) |
| 168 | return -SDP_INVALID_SYNTAX; |
| 169 | len -= sizeof(int); |
| 170 | memcpy(&start, req + 1, sizeof(int)); |
| 171 | } else |
| 172 | start = 0; |
| 173 | |
David Turner | a253513 | 2010-09-10 10:15:07 +0200 | [diff] [blame] | 174 | if (len > 1) |
David 'Digit' Turner | 5d8f37a | 2009-09-14 14:32:27 -0700 | [diff] [blame] | 175 | return -SDP_INVALID_SYNTAX; |
| 176 | |
| 177 | /* Output the results */ |
| 178 | len = 4; |
| 179 | count = 0; |
| 180 | end = start; |
| 181 | for (i = 0; i < sdp->services; i ++) |
| 182 | if (sdp->service_list[i].match) { |
| 183 | if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) { |
| 184 | handle = i; |
| 185 | memcpy(rsp + len, &handle, 4); |
| 186 | len += 4; |
| 187 | end = count + 1; |
| 188 | } |
| 189 | |
| 190 | count ++; |
| 191 | } |
| 192 | |
| 193 | rsp[0] = count >> 8; |
| 194 | rsp[1] = count & 0xff; |
| 195 | rsp[2] = (end - start) >> 8; |
| 196 | rsp[3] = (end - start) & 0xff; |
| 197 | |
| 198 | if (end < count) { |
| 199 | rsp[len ++] = sizeof(int); |
| 200 | memcpy(rsp + len, &end, sizeof(int)); |
| 201 | len += 4; |
| 202 | } else |
| 203 | rsp[len ++] = 0; |
| 204 | |
| 205 | return len; |
| 206 | } |
| 207 | |
| 208 | static int sdp_attr_match(struct sdp_service_record_s *record, |
| 209 | const uint8_t **req, ssize_t *len) |
| 210 | { |
| 211 | int i, start, end; |
| 212 | |
| 213 | if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) { |
| 214 | (*req) ++; |
| 215 | if (*len < 3) |
| 216 | return 1; |
| 217 | |
| 218 | start = (*(*req) ++) << 8; |
| 219 | start |= *(*req) ++; |
| 220 | end = start; |
| 221 | *len -= 3; |
| 222 | } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) { |
| 223 | (*req) ++; |
| 224 | if (*len < 5) |
| 225 | return 1; |
| 226 | |
| 227 | start = (*(*req) ++) << 8; |
| 228 | start |= *(*req) ++; |
| 229 | end = (*(*req) ++) << 8; |
| 230 | end |= *(*req) ++; |
| 231 | *len -= 5; |
| 232 | } else |
| 233 | return 1; |
| 234 | |
| 235 | for (i = 0; i < record->attributes; i ++) |
| 236 | if (record->attribute_list[i].attribute_id >= start && |
| 237 | record->attribute_list[i].attribute_id <= end) |
| 238 | record->attribute_list[i].match = 1; |
| 239 | |
| 240 | return 0; |
| 241 | } |
| 242 | |
| 243 | static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp, |
| 244 | uint8_t *rsp, const uint8_t *req, ssize_t len) |
| 245 | { |
| 246 | ssize_t seqlen; |
| 247 | int i, start, end, max; |
| 248 | int32_t handle; |
| 249 | struct sdp_service_record_s *record; |
| 250 | uint8_t *lst; |
| 251 | |
| 252 | /* Perform the search */ |
| 253 | if (len < 7) |
| 254 | return -SDP_INVALID_SYNTAX; |
| 255 | memcpy(&handle, req, 4); |
| 256 | req += 4; |
| 257 | len -= 4; |
| 258 | |
| 259 | if (handle < 0 || handle > sdp->services) |
| 260 | return -SDP_INVALID_RECORD_HANDLE; |
| 261 | record = &sdp->service_list[handle]; |
| 262 | |
| 263 | for (i = 0; i < record->attributes; i ++) |
| 264 | record->attribute_list[i].match = 0; |
| 265 | |
| 266 | max = (req[0] << 8) | req[1]; |
| 267 | req += 2; |
| 268 | len -= 2; |
| 269 | if (max < 0x0007) |
| 270 | return -SDP_INVALID_SYNTAX; |
| 271 | |
| 272 | if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) { |
| 273 | seqlen = sdp_datalen(&req, &len); |
| 274 | if (seqlen < 3 || len < seqlen) |
| 275 | return -SDP_INVALID_SYNTAX; |
| 276 | len -= seqlen; |
| 277 | |
| 278 | while (seqlen) |
| 279 | if (sdp_attr_match(record, &req, &seqlen)) |
| 280 | return -SDP_INVALID_SYNTAX; |
| 281 | } else if (sdp_attr_match(record, &req, &seqlen)) |
| 282 | return -SDP_INVALID_SYNTAX; |
| 283 | |
| 284 | if (len < 1) |
| 285 | return -SDP_INVALID_SYNTAX; |
| 286 | |
| 287 | if (*req) { |
| 288 | if (len <= sizeof(int)) |
| 289 | return -SDP_INVALID_SYNTAX; |
| 290 | len -= sizeof(int); |
| 291 | memcpy(&start, req + 1, sizeof(int)); |
| 292 | } else |
| 293 | start = 0; |
| 294 | |
| 295 | if (len > 1) |
| 296 | return -SDP_INVALID_SYNTAX; |
| 297 | |
| 298 | /* Output the results */ |
| 299 | lst = rsp + 2; |
| 300 | max = MIN(max, MAX_RSP_PARAM_SIZE); |
| 301 | len = 3 - start; |
| 302 | end = 0; |
| 303 | for (i = 0; i < record->attributes; i ++) |
| 304 | if (record->attribute_list[i].match) { |
| 305 | if (len >= 0 && len + record->attribute_list[i].len < max) { |
| 306 | memcpy(lst + len, record->attribute_list[i].pair, |
| 307 | record->attribute_list[i].len); |
| 308 | end = len + record->attribute_list[i].len; |
| 309 | } |
| 310 | len += record->attribute_list[i].len; |
| 311 | } |
| 312 | if (0 >= start) { |
| 313 | lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2; |
| 314 | lst[1] = (len + start - 3) >> 8; |
| 315 | lst[2] = (len + start - 3) & 0xff; |
| 316 | } |
| 317 | |
| 318 | rsp[0] = end >> 8; |
| 319 | rsp[1] = end & 0xff; |
| 320 | |
| 321 | if (end < len) { |
| 322 | len = end + start; |
| 323 | lst[end ++] = sizeof(int); |
| 324 | memcpy(lst + end, &len, sizeof(int)); |
| 325 | end += sizeof(int); |
| 326 | } else |
| 327 | lst[end ++] = 0; |
| 328 | |
| 329 | return end + 2; |
| 330 | } |
| 331 | |
| 332 | static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp, |
| 333 | const uint8_t **req, ssize_t *len) |
| 334 | { |
| 335 | int i, j, start, end; |
| 336 | struct sdp_service_record_s *record; |
| 337 | |
| 338 | if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) { |
| 339 | (*req) ++; |
| 340 | if (*len < 3) |
| 341 | return 1; |
| 342 | |
| 343 | start = (*(*req) ++) << 8; |
| 344 | start |= *(*req) ++; |
| 345 | end = start; |
| 346 | *len -= 3; |
| 347 | } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) { |
| 348 | (*req) ++; |
| 349 | if (*len < 5) |
| 350 | return 1; |
| 351 | |
| 352 | start = (*(*req) ++) << 8; |
| 353 | start |= *(*req) ++; |
| 354 | end = (*(*req) ++) << 8; |
| 355 | end |= *(*req) ++; |
| 356 | *len -= 5; |
| 357 | } else |
| 358 | return 1; |
| 359 | |
| 360 | for (i = 0; i < sdp->services; i ++) |
| 361 | if ((record = &sdp->service_list[i])->match) |
| 362 | for (j = 0; j < record->attributes; j ++) |
| 363 | if (record->attribute_list[j].attribute_id >= start && |
| 364 | record->attribute_list[j].attribute_id <= end) |
| 365 | record->attribute_list[j].match = 1; |
| 366 | |
| 367 | return 0; |
| 368 | } |
| 369 | |
| 370 | static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp, |
| 371 | uint8_t *rsp, const uint8_t *req, ssize_t len) |
| 372 | { |
| 373 | ssize_t seqlen; |
| 374 | int i, j, start, end, max; |
| 375 | struct sdp_service_record_s *record; |
| 376 | uint8_t *lst; |
| 377 | |
| 378 | /* Perform the search */ |
| 379 | for (i = 0; i < sdp->services; i ++) { |
| 380 | sdp->service_list[i].match = 0; |
| 381 | for (j = 0; j < sdp->service_list[i].attributes; j ++) |
| 382 | sdp->service_list[i].attribute_list[j].match = 0; |
| 383 | } |
| 384 | |
| 385 | if (len < 1) |
| 386 | return -SDP_INVALID_SYNTAX; |
| 387 | if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) { |
| 388 | seqlen = sdp_datalen(&req, &len); |
| 389 | if (seqlen < 3 || len < seqlen) |
| 390 | return -SDP_INVALID_SYNTAX; |
| 391 | len -= seqlen; |
| 392 | |
| 393 | while (seqlen) |
| 394 | if (sdp_svc_match(sdp, &req, &seqlen)) |
| 395 | return -SDP_INVALID_SYNTAX; |
| 396 | } else if (sdp_svc_match(sdp, &req, &seqlen)) |
| 397 | return -SDP_INVALID_SYNTAX; |
| 398 | |
| 399 | if (len < 3) |
| 400 | return -SDP_INVALID_SYNTAX; |
| 401 | max = (req[0] << 8) | req[1]; |
| 402 | req += 2; |
| 403 | len -= 2; |
| 404 | if (max < 0x0007) |
| 405 | return -SDP_INVALID_SYNTAX; |
| 406 | |
| 407 | if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) { |
| 408 | seqlen = sdp_datalen(&req, &len); |
| 409 | if (seqlen < 3 || len < seqlen) |
| 410 | return -SDP_INVALID_SYNTAX; |
| 411 | len -= seqlen; |
| 412 | |
| 413 | while (seqlen) |
| 414 | if (sdp_svc_attr_match(sdp, &req, &seqlen)) |
| 415 | return -SDP_INVALID_SYNTAX; |
| 416 | } else if (sdp_svc_attr_match(sdp, &req, &seqlen)) |
| 417 | return -SDP_INVALID_SYNTAX; |
| 418 | |
| 419 | if (len < 1) |
| 420 | return -SDP_INVALID_SYNTAX; |
| 421 | |
| 422 | if (*req) { |
| 423 | if (len <= sizeof(int)) |
| 424 | return -SDP_INVALID_SYNTAX; |
| 425 | len -= sizeof(int); |
| 426 | memcpy(&start, req + 1, sizeof(int)); |
| 427 | } else |
| 428 | start = 0; |
| 429 | |
| 430 | if (len > 1) |
| 431 | return -SDP_INVALID_SYNTAX; |
| 432 | |
| 433 | /* Output the results */ |
| 434 | /* This assumes empty attribute lists are never to be returned even |
| 435 | * for matching Service Records. In practice this shouldn't happen |
| 436 | * as the requestor will usually include the always present |
| 437 | * ServiceRecordHandle AttributeID in AttributeIDList. */ |
| 438 | lst = rsp + 2; |
| 439 | max = MIN(max, MAX_RSP_PARAM_SIZE); |
| 440 | len = 3 - start; |
| 441 | end = 0; |
| 442 | for (i = 0; i < sdp->services; i ++) |
| 443 | if ((record = &sdp->service_list[i])->match) { |
| 444 | len += 3; |
| 445 | seqlen = len; |
| 446 | for (j = 0; j < record->attributes; j ++) |
| 447 | if (record->attribute_list[j].match) { |
| 448 | if (len >= 0) |
| 449 | if (len + record->attribute_list[j].len < max) { |
| 450 | memcpy(lst + len, record->attribute_list[j].pair, |
| 451 | record->attribute_list[j].len); |
| 452 | end = len + record->attribute_list[j].len; |
| 453 | } |
| 454 | len += record->attribute_list[j].len; |
| 455 | } |
| 456 | if (seqlen == len) |
| 457 | len -= 3; |
| 458 | else if (seqlen >= 3 && seqlen < max) { |
| 459 | lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2; |
| 460 | lst[seqlen - 2] = (len - seqlen) >> 8; |
| 461 | lst[seqlen - 1] = (len - seqlen) & 0xff; |
| 462 | } |
| 463 | } |
| 464 | if (len == 3 - start) |
| 465 | len -= 3; |
| 466 | else if (0 >= start) { |
| 467 | lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2; |
| 468 | lst[1] = (len + start - 3) >> 8; |
| 469 | lst[2] = (len + start - 3) & 0xff; |
| 470 | } |
| 471 | |
| 472 | rsp[0] = end >> 8; |
| 473 | rsp[1] = end & 0xff; |
| 474 | |
| 475 | if (end < len) { |
| 476 | len = end + start; |
| 477 | lst[end ++] = sizeof(int); |
| 478 | memcpy(lst + end, &len, sizeof(int)); |
| 479 | end += sizeof(int); |
| 480 | } else |
| 481 | lst[end ++] = 0; |
| 482 | |
| 483 | return end + 2; |
| 484 | } |
| 485 | |
| 486 | static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len) |
| 487 | { |
| 488 | struct bt_l2cap_sdp_state_s *sdp = opaque; |
| 489 | enum bt_sdp_cmd pdu_id; |
| 490 | uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out; |
| 491 | int transaction_id, plen; |
| 492 | int err = 0; |
| 493 | int rsp_len = 0; |
| 494 | |
| 495 | if (len < 5) { |
| 496 | fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len); |
| 497 | return; |
| 498 | } |
| 499 | |
| 500 | pdu_id = *data ++; |
| 501 | transaction_id = (data[0] << 8) | data[1]; |
| 502 | plen = (data[2] << 8) | data[3]; |
| 503 | data += 4; |
| 504 | len -= 5; |
| 505 | |
| 506 | if (len != plen) { |
| 507 | fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n", |
| 508 | __FUNCTION__, plen, len); |
| 509 | err = SDP_INVALID_PDU_SIZE; |
| 510 | goto respond; |
| 511 | } |
| 512 | |
| 513 | switch (pdu_id) { |
| 514 | case SDP_SVC_SEARCH_REQ: |
| 515 | rsp_len = sdp_svc_search(sdp, rsp, data, len); |
| 516 | pdu_id = SDP_SVC_SEARCH_RSP; |
| 517 | break; |
| 518 | |
| 519 | case SDP_SVC_ATTR_REQ: |
| 520 | rsp_len = sdp_attr_get(sdp, rsp, data, len); |
| 521 | pdu_id = SDP_SVC_ATTR_RSP; |
| 522 | break; |
| 523 | |
| 524 | case SDP_SVC_SEARCH_ATTR_REQ: |
| 525 | rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len); |
| 526 | pdu_id = SDP_SVC_SEARCH_ATTR_RSP; |
| 527 | break; |
| 528 | |
| 529 | case SDP_ERROR_RSP: |
| 530 | case SDP_SVC_ATTR_RSP: |
| 531 | case SDP_SVC_SEARCH_RSP: |
| 532 | case SDP_SVC_SEARCH_ATTR_RSP: |
| 533 | default: |
| 534 | fprintf(stderr, "%s: unexpected SDP PDU ID %02x.\n", |
| 535 | __FUNCTION__, pdu_id); |
| 536 | err = SDP_INVALID_SYNTAX; |
| 537 | break; |
| 538 | } |
| 539 | |
| 540 | if (rsp_len < 0) { |
| 541 | err = -rsp_len; |
| 542 | rsp_len = 0; |
| 543 | } |
| 544 | |
| 545 | respond: |
| 546 | if (err) { |
| 547 | pdu_id = SDP_ERROR_RSP; |
| 548 | rsp[rsp_len ++] = err >> 8; |
| 549 | rsp[rsp_len ++] = err & 0xff; |
| 550 | } |
| 551 | |
| 552 | sdu_out = sdp->channel->sdu_out(sdp->channel, rsp_len + PDU_HEADER_SIZE); |
| 553 | |
| 554 | sdu_out[0] = pdu_id; |
| 555 | sdu_out[1] = transaction_id >> 8; |
| 556 | sdu_out[2] = transaction_id & 0xff; |
| 557 | sdu_out[3] = rsp_len >> 8; |
| 558 | sdu_out[4] = rsp_len & 0xff; |
| 559 | memcpy(sdu_out + PDU_HEADER_SIZE, rsp, rsp_len); |
| 560 | |
| 561 | sdp->channel->sdu_submit(sdp->channel); |
| 562 | } |
| 563 | |
| 564 | static void bt_l2cap_sdp_close_ch(void *opaque) |
| 565 | { |
| 566 | struct bt_l2cap_sdp_state_s *sdp = opaque; |
| 567 | int i; |
| 568 | |
| 569 | for (i = 0; i < sdp->services; i ++) { |
| 570 | qemu_free(sdp->service_list[i].attribute_list->pair); |
| 571 | qemu_free(sdp->service_list[i].attribute_list); |
| 572 | qemu_free(sdp->service_list[i].uuid); |
| 573 | } |
| 574 | qemu_free(sdp->service_list); |
| 575 | qemu_free(sdp); |
| 576 | } |
| 577 | |
| 578 | struct sdp_def_service_s { |
| 579 | uint16_t class_uuid; |
| 580 | struct sdp_def_attribute_s { |
| 581 | uint16_t id; |
| 582 | struct sdp_def_data_element_s { |
| 583 | uint8_t type; |
| 584 | union { |
| 585 | uint32_t uint; |
| 586 | const char *str; |
| 587 | struct sdp_def_data_element_s *list; |
| 588 | } value; |
| 589 | } data; |
| 590 | } attributes[]; |
| 591 | }; |
| 592 | |
| 593 | /* Calculate a safe byte count to allocate that will store the given |
| 594 | * element, at the same time count elements of a UUID type. */ |
| 595 | static int sdp_attr_max_size(struct sdp_def_data_element_s *element, |
| 596 | int *uuids) |
| 597 | { |
| 598 | int type = element->type & ~SDP_DSIZE_MASK; |
| 599 | int len; |
| 600 | |
| 601 | if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_UUID || |
| 602 | type == SDP_DTYPE_BOOL) { |
| 603 | if (type == SDP_DTYPE_UUID) |
| 604 | (*uuids) ++; |
| 605 | return 1 + (1 << (element->type & SDP_DSIZE_MASK)); |
| 606 | } |
| 607 | |
| 608 | if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) { |
| 609 | if (element->type & SDP_DSIZE_MASK) { |
| 610 | for (len = 0; element->value.str[len] | |
| 611 | element->value.str[len + 1]; len ++); |
| 612 | return len; |
| 613 | } else |
| 614 | return 2 + strlen(element->value.str); |
| 615 | } |
| 616 | |
| 617 | if (type != SDP_DTYPE_SEQ) |
| 618 | exit(-1); |
| 619 | len = 2; |
| 620 | element = element->value.list; |
| 621 | while (element->type) |
| 622 | len += sdp_attr_max_size(element ++, uuids); |
| 623 | if (len > 255) |
| 624 | exit (-1); |
| 625 | |
| 626 | return len; |
| 627 | } |
| 628 | |
| 629 | static int sdp_attr_write(uint8_t *data, |
| 630 | struct sdp_def_data_element_s *element, int **uuid) |
| 631 | { |
| 632 | int type = element->type & ~SDP_DSIZE_MASK; |
| 633 | int len = 0; |
| 634 | |
| 635 | if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_BOOL) { |
| 636 | data[len ++] = element->type; |
| 637 | if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_1) |
| 638 | data[len ++] = (element->value.uint >> 0) & 0xff; |
| 639 | else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_2) { |
| 640 | data[len ++] = (element->value.uint >> 8) & 0xff; |
| 641 | data[len ++] = (element->value.uint >> 0) & 0xff; |
| 642 | } else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_4) { |
| 643 | data[len ++] = (element->value.uint >> 24) & 0xff; |
| 644 | data[len ++] = (element->value.uint >> 16) & 0xff; |
| 645 | data[len ++] = (element->value.uint >> 8) & 0xff; |
| 646 | data[len ++] = (element->value.uint >> 0) & 0xff; |
| 647 | } |
| 648 | |
| 649 | return len; |
| 650 | } |
| 651 | |
| 652 | if (type == SDP_DTYPE_UUID) { |
| 653 | *(*uuid) ++ = element->value.uint; |
| 654 | |
| 655 | data[len ++] = element->type; |
| 656 | data[len ++] = (element->value.uint >> 24) & 0xff; |
| 657 | data[len ++] = (element->value.uint >> 16) & 0xff; |
| 658 | data[len ++] = (element->value.uint >> 8) & 0xff; |
| 659 | data[len ++] = (element->value.uint >> 0) & 0xff; |
| 660 | memcpy(data + len, bt_base_uuid, 12); |
| 661 | |
| 662 | return len + 12; |
| 663 | } |
| 664 | |
| 665 | data[0] = type | SDP_DSIZE_NEXT1; |
| 666 | if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) { |
| 667 | if (element->type & SDP_DSIZE_MASK) |
| 668 | for (len = 0; element->value.str[len] | |
| 669 | element->value.str[len + 1]; len ++); |
| 670 | else |
| 671 | len = strlen(element->value.str); |
| 672 | memcpy(data + 2, element->value.str, data[1] = len); |
| 673 | |
| 674 | return len + 2; |
| 675 | } |
| 676 | |
| 677 | len = 2; |
| 678 | element = element->value.list; |
| 679 | while (element->type) |
| 680 | len += sdp_attr_write(data + len, element ++, uuid); |
| 681 | data[1] = len - 2; |
| 682 | |
| 683 | return len; |
| 684 | } |
| 685 | |
| 686 | static int sdp_attributeid_compare(const struct sdp_service_attribute_s *a, |
| 687 | const struct sdp_service_attribute_s *b) |
| 688 | { |
| 689 | return (int) b->attribute_id - a->attribute_id; |
| 690 | } |
| 691 | |
| 692 | static int sdp_uuid_compare(const int *a, const int *b) |
| 693 | { |
| 694 | return *a - *b; |
| 695 | } |
| 696 | |
| 697 | static void sdp_service_record_build(struct sdp_service_record_s *record, |
| 698 | struct sdp_def_service_s *def, int handle) |
| 699 | { |
| 700 | int len = 0; |
| 701 | uint8_t *data; |
| 702 | int *uuid; |
| 703 | |
| 704 | record->uuids = 0; |
| 705 | while (def->attributes[record->attributes].data.type) { |
| 706 | len += 3; |
| 707 | len += sdp_attr_max_size(&def->attributes[record->attributes ++].data, |
| 708 | &record->uuids); |
| 709 | } |
| 710 | record->uuids = 1 << ffs(record->uuids - 1); |
| 711 | record->attribute_list = |
| 712 | qemu_mallocz(record->attributes * sizeof(*record->attribute_list)); |
| 713 | record->uuid = |
| 714 | qemu_mallocz(record->uuids * sizeof(*record->uuid)); |
| 715 | data = qemu_malloc(len); |
| 716 | |
| 717 | record->attributes = 0; |
| 718 | uuid = record->uuid; |
| 719 | while (def->attributes[record->attributes].data.type) { |
| 720 | record->attribute_list[record->attributes].pair = data; |
| 721 | |
| 722 | len = 0; |
| 723 | data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2; |
| 724 | data[len ++] = def->attributes[record->attributes].id >> 8; |
| 725 | data[len ++] = def->attributes[record->attributes].id & 0xff; |
| 726 | len += sdp_attr_write(data + len, |
| 727 | &def->attributes[record->attributes].data, &uuid); |
| 728 | |
| 729 | /* Special case: assign a ServiceRecordHandle in sequence */ |
| 730 | if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE) |
| 731 | def->attributes[record->attributes].data.value.uint = handle; |
| 732 | /* Note: we could also assign a ServiceDescription based on |
| 733 | * sdp->device.device->lmp_name. */ |
| 734 | |
| 735 | record->attribute_list[record->attributes ++].len = len; |
| 736 | data += len; |
| 737 | } |
| 738 | |
| 739 | /* Sort the attribute list by the AttributeID */ |
| 740 | qsort(record->attribute_list, record->attributes, |
| 741 | sizeof(*record->attribute_list), |
| 742 | (void *) sdp_attributeid_compare); |
| 743 | /* Sort the searchable UUIDs list for bisection */ |
| 744 | qsort(record->uuid, record->uuids, |
| 745 | sizeof(*record->uuid), |
| 746 | (void *) sdp_uuid_compare); |
| 747 | } |
| 748 | |
| 749 | static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp, |
| 750 | struct sdp_def_service_s **service) |
| 751 | { |
| 752 | sdp->services = 0; |
| 753 | while (service[sdp->services]) |
| 754 | sdp->services ++; |
| 755 | sdp->service_list = |
| 756 | qemu_mallocz(sdp->services * sizeof(*sdp->service_list)); |
| 757 | |
| 758 | sdp->services = 0; |
| 759 | while (*service) { |
| 760 | sdp_service_record_build(&sdp->service_list[sdp->services], |
| 761 | *service, sdp->services); |
| 762 | service ++; |
| 763 | sdp->services ++; |
| 764 | } |
| 765 | } |
| 766 | |
| 767 | #define LAST { .type = 0 } |
| 768 | #define SERVICE(name, attrs) \ |
| 769 | static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \ |
| 770 | .attributes = { attrs { .data = LAST } }, \ |
| 771 | }; |
| 772 | #define ATTRIBUTE(attrid, val) { .id = glue(SDP_ATTR_, attrid), .data = val }, |
| 773 | #define UINT8(val) { \ |
| 774 | .type = SDP_DTYPE_UINT | SDP_DSIZE_1, \ |
| 775 | .value.uint = val, \ |
| 776 | }, |
| 777 | #define UINT16(val) { \ |
| 778 | .type = SDP_DTYPE_UINT | SDP_DSIZE_2, \ |
| 779 | .value.uint = val, \ |
| 780 | }, |
| 781 | #define UINT32(val) { \ |
| 782 | .type = SDP_DTYPE_UINT | SDP_DSIZE_4, \ |
| 783 | .value.uint = val, \ |
| 784 | }, |
| 785 | #define UUID128(val) { \ |
| 786 | .type = SDP_DTYPE_UUID | SDP_DSIZE_16, \ |
| 787 | .value.uint = val, \ |
| 788 | }, |
| 789 | #define TRUE { \ |
| 790 | .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \ |
| 791 | .value.uint = 1, \ |
| 792 | }, |
| 793 | #define FALSE { \ |
| 794 | .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \ |
| 795 | .value.uint = 0, \ |
| 796 | }, |
| 797 | #define STRING(val) { \ |
| 798 | .type = SDP_DTYPE_STRING, \ |
| 799 | .value.str = val, \ |
| 800 | }, |
| 801 | #define ARRAY(...) { \ |
| 802 | .type = SDP_DTYPE_STRING | SDP_DSIZE_2, \ |
| 803 | .value.str = (char []) { __VA_ARGS__, 0, 0 }, \ |
| 804 | }, |
| 805 | #define URL(val) { \ |
| 806 | .type = SDP_DTYPE_URL, \ |
| 807 | .value.str = val, \ |
| 808 | }, |
| 809 | #if 1 |
| 810 | #define LIST(val) { \ |
| 811 | .type = SDP_DTYPE_SEQ, \ |
| 812 | .value.list = (struct sdp_def_data_element_s []) { val LAST }, \ |
| 813 | }, |
| 814 | #endif |
| 815 | |
| 816 | /* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes |
| 817 | * in resulting SDP data representation size. */ |
| 818 | |
| 819 | SERVICE(hid, |
| 820 | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
| 821 | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID))) |
| 822 | ATTRIBUTE(RECORD_STATE, UINT32(1)) |
| 823 | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
| 824 | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL)) |
| 825 | LIST(UUID128(HIDP_UUID)) |
| 826 | )) |
| 827 | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) |
| 828 | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
| 829 | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
| 830 | )) |
| 831 | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
| 832 | LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100)) |
| 833 | )) |
| 834 | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html")) |
| 835 | ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID")) |
| 836 | ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse")) |
| 837 | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION)) |
| 838 | |
| 839 | /* Profile specific */ |
| 840 | ATTRIBUTE(DEVICE_RELEASE_NUMBER, UINT16(0x0091)) /* Deprecated, remove */ |
| 841 | ATTRIBUTE(PARSER_VERSION, UINT16(0x0111)) |
| 842 | /* TODO: extract from l2cap_device->device.class[0] */ |
| 843 | ATTRIBUTE(DEVICE_SUBCLASS, UINT8(0x40)) |
| 844 | ATTRIBUTE(COUNTRY_CODE, UINT8(0x15)) |
| 845 | ATTRIBUTE(VIRTUAL_CABLE, TRUE) |
| 846 | ATTRIBUTE(RECONNECT_INITIATE, FALSE) |
| 847 | /* TODO: extract from hid->usbdev->report_desc */ |
| 848 | ATTRIBUTE(DESCRIPTOR_LIST, LIST( |
| 849 | LIST(UINT8(0x22) ARRAY( |
| 850 | 0x05, 0x01, /* Usage Page (Generic Desktop) */ |
| 851 | 0x09, 0x06, /* Usage (Keyboard) */ |
| 852 | 0xa1, 0x01, /* Collection (Application) */ |
| 853 | 0x75, 0x01, /* Report Size (1) */ |
| 854 | 0x95, 0x08, /* Report Count (8) */ |
| 855 | 0x05, 0x07, /* Usage Page (Key Codes) */ |
| 856 | 0x19, 0xe0, /* Usage Minimum (224) */ |
| 857 | 0x29, 0xe7, /* Usage Maximum (231) */ |
| 858 | 0x15, 0x00, /* Logical Minimum (0) */ |
| 859 | 0x25, 0x01, /* Logical Maximum (1) */ |
| 860 | 0x81, 0x02, /* Input (Data, Variable, Absolute) */ |
| 861 | 0x95, 0x01, /* Report Count (1) */ |
| 862 | 0x75, 0x08, /* Report Size (8) */ |
| 863 | 0x81, 0x01, /* Input (Constant) */ |
| 864 | 0x95, 0x05, /* Report Count (5) */ |
| 865 | 0x75, 0x01, /* Report Size (1) */ |
| 866 | 0x05, 0x08, /* Usage Page (LEDs) */ |
| 867 | 0x19, 0x01, /* Usage Minimum (1) */ |
| 868 | 0x29, 0x05, /* Usage Maximum (5) */ |
| 869 | 0x91, 0x02, /* Output (Data, Variable, Absolute) */ |
| 870 | 0x95, 0x01, /* Report Count (1) */ |
| 871 | 0x75, 0x03, /* Report Size (3) */ |
| 872 | 0x91, 0x01, /* Output (Constant) */ |
| 873 | 0x95, 0x06, /* Report Count (6) */ |
| 874 | 0x75, 0x08, /* Report Size (8) */ |
| 875 | 0x15, 0x00, /* Logical Minimum (0) */ |
| 876 | 0x25, 0xff, /* Logical Maximum (255) */ |
| 877 | 0x05, 0x07, /* Usage Page (Key Codes) */ |
| 878 | 0x19, 0x00, /* Usage Minimum (0) */ |
| 879 | 0x29, 0xff, /* Usage Maximum (255) */ |
| 880 | 0x81, 0x00, /* Input (Data, Array) */ |
| 881 | 0xc0 /* End Collection */ |
| 882 | )))) |
| 883 | ATTRIBUTE(LANG_ID_BASE_LIST, LIST( |
| 884 | LIST(UINT16(0x0409) UINT16(0x0100)) |
| 885 | )) |
| 886 | ATTRIBUTE(SDP_DISABLE, FALSE) |
| 887 | ATTRIBUTE(BATTERY_POWER, TRUE) |
| 888 | ATTRIBUTE(REMOTE_WAKEUP, TRUE) |
| 889 | ATTRIBUTE(BOOT_DEVICE, TRUE) /* XXX: untested */ |
| 890 | ATTRIBUTE(SUPERVISION_TIMEOUT, UINT16(0x0c80)) |
| 891 | ATTRIBUTE(NORMALLY_CONNECTABLE, TRUE) |
| 892 | ATTRIBUTE(PROFILE_VERSION, UINT16(0x0100)) |
| 893 | ) |
| 894 | |
| 895 | SERVICE(sdp, |
| 896 | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
| 897 | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID))) |
| 898 | ATTRIBUTE(RECORD_STATE, UINT32(1)) |
| 899 | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
| 900 | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP)) |
| 901 | LIST(UUID128(SDP_UUID)) |
| 902 | )) |
| 903 | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) |
| 904 | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
| 905 | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
| 906 | )) |
| 907 | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
| 908 | LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100)) |
| 909 | )) |
| 910 | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html")) |
| 911 | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION)) |
| 912 | |
| 913 | /* Profile specific */ |
| 914 | ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100))) |
| 915 | ATTRIBUTE(SVCDB_STATE , UINT32(1)) |
| 916 | ) |
| 917 | |
| 918 | SERVICE(pnp, |
| 919 | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
| 920 | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID))) |
| 921 | ATTRIBUTE(RECORD_STATE, UINT32(1)) |
| 922 | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
| 923 | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP)) |
| 924 | LIST(UUID128(SDP_UUID)) |
| 925 | )) |
| 926 | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) |
| 927 | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
| 928 | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
| 929 | )) |
| 930 | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
| 931 | LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100)) |
| 932 | )) |
| 933 | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html")) |
| 934 | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION)) |
| 935 | |
| 936 | /* Profile specific */ |
| 937 | ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100)) |
| 938 | ATTRIBUTE(VERSION, UINT16(0x0100)) |
| 939 | ATTRIBUTE(PRIMARY_RECORD, TRUE) |
| 940 | ) |
| 941 | |
| 942 | static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev, |
| 943 | struct bt_l2cap_conn_params_s *params) |
| 944 | { |
| 945 | struct bt_l2cap_sdp_state_s *sdp = qemu_mallocz(sizeof(*sdp)); |
| 946 | struct sdp_def_service_s *services[] = { |
| 947 | &sdp_service_sdp_s, |
| 948 | &sdp_service_hid_s, |
| 949 | &sdp_service_pnp_s, |
| 950 | NULL, |
| 951 | }; |
| 952 | |
| 953 | sdp->channel = params; |
| 954 | sdp->channel->opaque = sdp; |
| 955 | sdp->channel->close = bt_l2cap_sdp_close_ch; |
| 956 | sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in; |
| 957 | |
| 958 | sdp_service_db_build(sdp, services); |
| 959 | |
| 960 | return 0; |
| 961 | } |
| 962 | |
| 963 | void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev) |
| 964 | { |
| 965 | bt_l2cap_psm_register(dev, BT_PSM_SDP, |
| 966 | MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch); |
| 967 | } |