1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2010 Nokia Corporation 6 * Copyright (C) 2010 Marcel Holtmann <marcel (at) holtmann.org> 7 * 8 * 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License as published by 11 * the Free Software Foundation; either version 2 of the License, or 12 * (at your option) any later version. 13 * 14 * This program is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 * GNU General Public License for more details. 18 * 19 * You should have received a copy of the GNU General Public License 20 * along with this program; if not, write to the Free Software 21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 * 23 */ 24 25 #ifdef HAVE_CONFIG_H 26 #include <config.h> 27 #endif 28 29 #include <errno.h> 30 #include <stdint.h> 31 #include <string.h> 32 #include <unistd.h> 33 #include <glib.h> 34 35 #include <bluetooth/bluetooth.h> 36 #include <bluetooth/uuid.h> 37 #include <bluetooth/sdp.h> 38 #include <bluetooth/sdp_lib.h> 39 40 #include "log.h" 41 #include "glib-helper.h" 42 #include "btio.h" 43 #include "sdpd.h" 44 #include "hcid.h" 45 #include "att.h" 46 #include "gattrib.h" 47 48 #include "attrib-server.h" 49 50 static GSList *database = NULL; 51 52 struct gatt_channel { 53 bdaddr_t src; 54 bdaddr_t dst; 55 GSList *configs; 56 GSList *notify; 57 GSList *indicate; 58 GAttrib *attrib; 59 guint mtu; 60 gboolean le; 61 guint id; 62 gboolean encrypted; 63 }; 64 65 struct group_elem { 66 uint16_t handle; 67 uint16_t end; 68 uint8_t *data; 69 uint16_t len; 70 }; 71 72 static GIOChannel *l2cap_io = NULL; 73 static GIOChannel *le_io = NULL; 74 static GSList *clients = NULL; 75 static uint32_t gatt_sdp_handle = 0; 76 static uint32_t gap_sdp_handle = 0; 77 78 /* GAP attribute handles */ 79 static uint16_t name_handle = 0x0000; 80 static uint16_t appearance_handle = 0x0000; 81 82 static bt_uuid_t prim_uuid = { 83 .type = BT_UUID16, 84 .value.u16 = GATT_PRIM_SVC_UUID 85 }; 86 static bt_uuid_t snd_uuid = { 87 .type = BT_UUID16, 88 .value.u16 = GATT_SND_SVC_UUID 89 }; 90 91 static sdp_record_t *server_record_new(uuid_t *uuid, uint16_t start, uint16_t end) 92 { 93 sdp_list_t *svclass_id, *apseq, *proto[2], *root, *aproto; 94 uuid_t root_uuid, proto_uuid, l2cap; 95 sdp_record_t *record; 96 sdp_data_t *psm, *sh, *eh; 97 uint16_t lp = ATT_PSM; 98 99 if (uuid == NULL) 100 return NULL; 101 102 if (start > end) 103 return NULL; 104 105 record = sdp_record_alloc(); 106 if (record == NULL) 107 return NULL; 108 109 sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP); 110 root = sdp_list_append(NULL, &root_uuid); 111 sdp_set_browse_groups(record, root); 112 sdp_list_free(root, NULL); 113 114 svclass_id = sdp_list_append(NULL, uuid); 115 sdp_set_service_classes(record, svclass_id); 116 sdp_list_free(svclass_id, NULL); 117 118 sdp_uuid16_create(&l2cap, L2CAP_UUID); 119 proto[0] = sdp_list_append(NULL, &l2cap); 120 psm = sdp_data_alloc(SDP_UINT16, &lp); 121 proto[0] = sdp_list_append(proto[0], psm); 122 apseq = sdp_list_append(NULL, proto[0]); 123 124 sdp_uuid16_create(&proto_uuid, ATT_UUID); 125 proto[1] = sdp_list_append(NULL, &proto_uuid); 126 sh = sdp_data_alloc(SDP_UINT16, &start); 127 proto[1] = sdp_list_append(proto[1], sh); 128 eh = sdp_data_alloc(SDP_UINT16, &end); 129 proto[1] = sdp_list_append(proto[1], eh); 130 apseq = sdp_list_append(apseq, proto[1]); 131 132 aproto = sdp_list_append(NULL, apseq); 133 sdp_set_access_protos(record, aproto); 134 135 sdp_data_free(psm); 136 sdp_data_free(sh); 137 sdp_data_free(eh); 138 sdp_list_free(proto[0], NULL); 139 sdp_list_free(proto[1], NULL); 140 sdp_list_free(apseq, NULL); 141 sdp_list_free(aproto, NULL); 142 143 return record; 144 } 145 146 static int handle_cmp(gconstpointer a, gconstpointer b) 147 { 148 const struct attribute *attrib = a; 149 uint16_t handle = GPOINTER_TO_UINT(b); 150 151 return attrib->handle - handle; 152 } 153 154 static int attribute_cmp(gconstpointer a1, gconstpointer a2) 155 { 156 const struct attribute *attrib1 = a1; 157 const struct attribute *attrib2 = a2; 158 159 return attrib1->handle - attrib2->handle; 160 } 161 162 static uint8_t att_check_reqs(struct gatt_channel *channel, uint8_t opcode, 163 int reqs) 164 { 165 /* FIXME: currently, it is assumed an encrypted link is enough for 166 * authentication. This will allow to enable the SMP negotiation once 167 * it is on upstream kernel. High security level should be mapped 168 * to authentication and medium to encryption permission. */ 169 if (!channel->encrypted) 170 channel->encrypted = g_attrib_is_encrypted(channel->attrib); 171 if (reqs == ATT_AUTHENTICATION && !channel->encrypted) 172 return ATT_ECODE_AUTHENTICATION; 173 else if (reqs == ATT_AUTHORIZATION) 174 return ATT_ECODE_AUTHORIZATION; 175 176 switch (opcode) { 177 case ATT_OP_READ_BY_GROUP_REQ: 178 case ATT_OP_READ_BY_TYPE_REQ: 179 case ATT_OP_READ_REQ: 180 case ATT_OP_READ_BLOB_REQ: 181 case ATT_OP_READ_MULTI_REQ: 182 if (reqs == ATT_NOT_PERMITTED) 183 return ATT_ECODE_READ_NOT_PERM; 184 break; 185 case ATT_OP_PREP_WRITE_REQ: 186 case ATT_OP_WRITE_REQ: 187 case ATT_OP_WRITE_CMD: 188 if (reqs == ATT_NOT_PERMITTED) 189 return ATT_ECODE_WRITE_NOT_PERM; 190 break; 191 } 192 193 return 0; 194 } 195 196 static uint8_t client_set_notifications(struct attribute *attr, 197 gpointer user_data) 198 { 199 struct gatt_channel *channel = user_data; 200 struct attribute *last_chr_val = NULL; 201 uint16_t cfg_val; 202 uint8_t props; 203 bt_uuid_t uuid; 204 GSList *l; 205 206 cfg_val = att_get_u16(attr->data); 207 208 bt_uuid16_create(&uuid, GATT_CHARAC_UUID); 209 for (l = database, props = 0; l != NULL; l = l->next) { 210 struct attribute *a = l->data; 211 static uint16_t handle = 0; 212 213 if (a->handle >= attr->handle) 214 break; 215 216 if (bt_uuid_cmp(&a->uuid, &uuid) == 0) { 217 props = att_get_u8(&a->data[0]); 218 handle = att_get_u16(&a->data[1]); 219 continue; 220 } 221 222 if (handle && a->handle == handle) 223 last_chr_val = a; 224 } 225 226 if (last_chr_val == NULL) 227 return 0; 228 229 if ((cfg_val & 0x0001) && !(props & ATT_CHAR_PROPER_NOTIFY)) 230 return ATT_ECODE_WRITE_NOT_PERM; 231 232 if ((cfg_val & 0x0002) && !(props & ATT_CHAR_PROPER_INDICATE)) 233 return ATT_ECODE_WRITE_NOT_PERM; 234 235 if (cfg_val & 0x0001) 236 channel->notify = g_slist_append(channel->notify, last_chr_val); 237 else 238 channel->notify = g_slist_remove(channel->notify, last_chr_val); 239 240 if (cfg_val & 0x0002) 241 channel->indicate = g_slist_append(channel->indicate, 242 last_chr_val); 243 else 244 channel->indicate = g_slist_remove(channel->indicate, 245 last_chr_val); 246 247 return 0; 248 } 249 250 static struct attribute *client_cfg_attribute(struct gatt_channel *channel, 251 struct attribute *orig_attr, 252 const uint8_t *value, int vlen) 253 { 254 guint handle = orig_attr->handle; 255 bt_uuid_t uuid; 256 GSList *l; 257 258 bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID); 259 if (bt_uuid_cmp(&orig_attr->uuid, &uuid) != 0) 260 return NULL; 261 262 /* Value is unchanged, not need to create a private copy yet */ 263 if (vlen == orig_attr->len && memcmp(orig_attr->data, value, vlen) == 0) 264 return orig_attr; 265 266 l = g_slist_find_custom(channel->configs, GUINT_TO_POINTER(handle), 267 handle_cmp); 268 if (!l) { 269 struct attribute *a; 270 271 /* Create a private copy of the Client Characteristic 272 * Configuration attribute */ 273 a = g_malloc0(sizeof(*a) + vlen); 274 memcpy(a, orig_attr, sizeof(*a)); 275 memcpy(a->data, value, vlen); 276 a->write_cb = client_set_notifications; 277 a->cb_user_data = channel; 278 279 channel->configs = g_slist_insert_sorted(channel->configs, a, 280 attribute_cmp); 281 282 return a; 283 } 284 285 return l->data; 286 } 287 288 static uint16_t read_by_group(struct gatt_channel *channel, uint16_t start, 289 uint16_t end, bt_uuid_t *uuid, 290 uint8_t *pdu, int len) 291 { 292 struct att_data_list *adl; 293 struct attribute *a; 294 struct group_elem *cur, *old = NULL; 295 GSList *l, *groups; 296 uint16_t length, last_handle, last_size = 0; 297 uint8_t status; 298 int i; 299 300 if (start > end || start == 0x0000) 301 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start, 302 ATT_ECODE_INVALID_HANDLE, pdu, len); 303 304 /* 305 * Only <<Primary Service>> and <<Secondary Service>> grouping 306 * types may be used in the Read By Group Type Request. 307 */ 308 309 if (bt_uuid_cmp(uuid, &prim_uuid) != 0 && 310 bt_uuid_cmp(uuid, &snd_uuid) != 0) 311 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, 0x0000, 312 ATT_ECODE_UNSUPP_GRP_TYPE, pdu, len); 313 314 last_handle = end; 315 for (l = database, groups = NULL, cur = NULL; l; l = l->next) { 316 struct attribute *client_attr; 317 318 a = l->data; 319 320 if (a->handle < start) 321 continue; 322 323 if (a->handle >= end) 324 break; 325 326 /* The old group ends when a new one starts */ 327 if (old && (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 || 328 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)) { 329 old->end = last_handle; 330 old = NULL; 331 } 332 333 if (bt_uuid_cmp(&a->uuid, uuid) != 0) { 334 /* Still inside a service, update its last handle */ 335 if (old) 336 last_handle = a->handle; 337 continue; 338 } 339 340 if (last_size && (last_size != a->len)) 341 break; 342 343 status = att_check_reqs(channel, ATT_OP_READ_BY_GROUP_REQ, 344 a->read_reqs); 345 346 client_attr = client_cfg_attribute(channel, a, a->data, a->len); 347 if (client_attr) 348 a = client_attr; 349 350 if (status == 0x00 && a->read_cb) 351 status = a->read_cb(a, a->cb_user_data); 352 353 if (status) { 354 g_slist_foreach(groups, (GFunc) g_free, NULL); 355 g_slist_free(groups); 356 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, 357 a->handle, status, pdu, len); 358 } 359 360 cur = g_new0(struct group_elem, 1); 361 cur->handle = a->handle; 362 cur->data = a->data; 363 cur->len = a->len; 364 365 /* Attribute Grouping Type found */ 366 groups = g_slist_append(groups, cur); 367 368 last_size = a->len; 369 old = cur; 370 last_handle = cur->handle; 371 } 372 373 if (groups == NULL) 374 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start, 375 ATT_ECODE_ATTR_NOT_FOUND, pdu, len); 376 377 if (l == NULL) 378 cur->end = a->handle; 379 else 380 cur->end = last_handle; 381 382 length = g_slist_length(groups); 383 384 adl = att_data_list_alloc(length, last_size + 4); 385 386 for (i = 0, l = groups; l; l = l->next, i++) { 387 uint8_t *value; 388 389 cur = l->data; 390 391 value = (void *) adl->data[i]; 392 393 att_put_u16(cur->handle, value); 394 att_put_u16(cur->end, &value[2]); 395 /* Attribute Value */ 396 memcpy(&value[4], cur->data, cur->len); 397 } 398 399 length = enc_read_by_grp_resp(adl, pdu, len); 400 401 att_data_list_free(adl); 402 g_slist_foreach(groups, (GFunc) g_free, NULL); 403 g_slist_free(groups); 404 405 return length; 406 } 407 408 static uint16_t read_by_type(struct gatt_channel *channel, uint16_t start, 409 uint16_t end, bt_uuid_t *uuid, 410 uint8_t *pdu, int len) 411 { 412 struct att_data_list *adl; 413 GSList *l, *types; 414 struct attribute *a; 415 uint16_t num, length; 416 uint8_t status; 417 int i; 418 419 if (start > end || start == 0x0000) 420 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start, 421 ATT_ECODE_INVALID_HANDLE, pdu, len); 422 423 for (l = database, length = 0, types = NULL; l; l = l->next) { 424 struct attribute *client_attr; 425 426 a = l->data; 427 428 if (a->handle < start) 429 continue; 430 431 if (a->handle > end) 432 break; 433 434 if (bt_uuid_cmp(&a->uuid, uuid) != 0) 435 continue; 436 437 status = att_check_reqs(channel, ATT_OP_READ_BY_TYPE_REQ, 438 a->read_reqs); 439 440 client_attr = client_cfg_attribute(channel, a, a->data, a->len); 441 if (client_attr) 442 a = client_attr; 443 444 if (status == 0x00 && a->read_cb) 445 status = a->read_cb(a, a->cb_user_data); 446 447 if (status) { 448 g_slist_free(types); 449 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, 450 a->handle, status, pdu, len); 451 } 452 453 /* All elements must have the same length */ 454 if (length == 0) 455 length = a->len; 456 else if (a->len != length) 457 break; 458 459 types = g_slist_append(types, a); 460 } 461 462 if (types == NULL) 463 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start, 464 ATT_ECODE_ATTR_NOT_FOUND, pdu, len); 465 466 num = g_slist_length(types); 467 468 /* Handle length plus attribute value length */ 469 length += 2; 470 471 adl = att_data_list_alloc(num, length); 472 473 for (i = 0, l = types; l; i++, l = l->next) { 474 uint8_t *value; 475 476 a = l->data; 477 478 value = (void *) adl->data[i]; 479 480 att_put_u16(a->handle, value); 481 482 /* Attribute Value */ 483 memcpy(&value[2], a->data, a->len); 484 } 485 486 length = enc_read_by_type_resp(adl, pdu, len); 487 488 att_data_list_free(adl); 489 g_slist_free(types); 490 491 return length; 492 } 493 494 static int find_info(uint16_t start, uint16_t end, uint8_t *pdu, int len) 495 { 496 struct attribute *a; 497 struct att_data_list *adl; 498 GSList *l, *info; 499 uint8_t format, last_type = BT_UUID_UNSPEC; 500 uint16_t length, num; 501 int i; 502 503 if (start > end || start == 0x0000) 504 return enc_error_resp(ATT_OP_FIND_INFO_REQ, start, 505 ATT_ECODE_INVALID_HANDLE, pdu, len); 506 507 for (l = database, info = NULL, num = 0; l; l = l->next) { 508 a = l->data; 509 510 if (a->handle < start) 511 continue; 512 513 if (a->handle > end) 514 break; 515 516 if (last_type == BT_UUID_UNSPEC) 517 last_type = a->uuid.type; 518 519 if (a->uuid.type != last_type) 520 break; 521 522 info = g_slist_append(info, a); 523 num++; 524 525 last_type = a->uuid.type; 526 } 527 528 if (info == NULL) 529 return enc_error_resp(ATT_OP_FIND_INFO_REQ, start, 530 ATT_ECODE_ATTR_NOT_FOUND, pdu, len); 531 532 if (last_type == BT_UUID16) { 533 length = 2; 534 format = 0x01; 535 } else if (last_type == BT_UUID128) { 536 length = 16; 537 format = 0x02; 538 } else { 539 g_slist_free(info); 540 return 0; 541 } 542 543 adl = att_data_list_alloc(num, length + 2); 544 545 for (i = 0, l = info; l; i++, l = l->next) { 546 uint8_t *value; 547 548 a = l->data; 549 550 value = (void *) adl->data[i]; 551 552 att_put_u16(a->handle, value); 553 554 /* Attribute Value */ 555 att_put_uuid(a->uuid, &value[2]); 556 } 557 558 length = enc_find_info_resp(format, adl, pdu, len); 559 560 att_data_list_free(adl); 561 g_slist_free(info); 562 563 return length; 564 } 565 566 static int find_by_type(uint16_t start, uint16_t end, bt_uuid_t *uuid, 567 const uint8_t *value, int vlen, uint8_t *opdu, int mtu) 568 { 569 struct attribute *a; 570 struct att_range *range; 571 GSList *l, *matches; 572 int len; 573 574 if (start > end || start == 0x0000) 575 return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start, 576 ATT_ECODE_INVALID_HANDLE, opdu, mtu); 577 578 /* Searching first requested handle number */ 579 for (l = database, matches = NULL, range = NULL; l; l = l->next) { 580 a = l->data; 581 582 if (a->handle < start) 583 continue; 584 585 if (a->handle > end) 586 break; 587 588 /* Primary service? Attribute value matches? */ 589 if ((bt_uuid_cmp(&a->uuid, uuid) == 0) && (a->len == vlen) && 590 (memcmp(a->data, value, vlen) == 0)) { 591 592 range = g_new0(struct att_range, 1); 593 range->start = a->handle; 594 /* It is allowed to have end group handle the same as 595 * start handle, for groups with only one attribute. */ 596 range->end = a->handle; 597 598 matches = g_slist_append(matches, range); 599 } else if (range) { 600 /* Update the last found handle or reset the pointer 601 * to track that a new group started: Primary or 602 * Secondary service. */ 603 if (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 || 604 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0) 605 range = NULL; 606 else 607 range->end = a->handle; 608 } 609 } 610 611 if (matches == NULL) 612 return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start, 613 ATT_ECODE_ATTR_NOT_FOUND, opdu, mtu); 614 615 len = enc_find_by_type_resp(matches, opdu, mtu); 616 617 g_slist_foreach(matches, (GFunc) g_free, NULL); 618 g_slist_free(matches); 619 620 return len; 621 } 622 623 static struct attribute *find_primary_range(uint16_t start, uint16_t *end) 624 { 625 struct attribute *attrib; 626 guint h = start; 627 GSList *l; 628 629 if (end == NULL) 630 return NULL; 631 632 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp); 633 if (!l) 634 return NULL; 635 636 attrib = l->data; 637 638 if (bt_uuid_cmp(&attrib->uuid, &prim_uuid) != 0) 639 return NULL; 640 641 *end = start; 642 643 for (l = l->next; l; l = l->next) { 644 struct attribute *a = l->data; 645 646 if (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 || 647 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0) 648 break; 649 650 *end = a->handle; 651 } 652 653 return attrib; 654 } 655 656 static uint16_t read_value(struct gatt_channel *channel, uint16_t handle, 657 uint8_t *pdu, int len) 658 { 659 struct attribute *a, *client_attr; 660 uint8_t status; 661 GSList *l; 662 guint h = handle; 663 664 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp); 665 if (!l) 666 return enc_error_resp(ATT_OP_READ_REQ, handle, 667 ATT_ECODE_INVALID_HANDLE, pdu, len); 668 669 a = l->data; 670 671 status = att_check_reqs(channel, ATT_OP_READ_REQ, a->read_reqs); 672 673 client_attr = client_cfg_attribute(channel, a, a->data, a->len); 674 if (client_attr) 675 a = client_attr; 676 677 if (status == 0x00 && a->read_cb) 678 status = a->read_cb(a, a->cb_user_data); 679 680 if (status) 681 return enc_error_resp(ATT_OP_READ_REQ, handle, status, pdu, 682 len); 683 684 return enc_read_resp(a->data, a->len, pdu, len); 685 } 686 687 static uint16_t read_blob(struct gatt_channel *channel, uint16_t handle, 688 uint16_t offset, uint8_t *pdu, int len) 689 { 690 struct attribute *a, *client_attr; 691 uint8_t status; 692 GSList *l; 693 guint h = handle; 694 695 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp); 696 if (!l) 697 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle, 698 ATT_ECODE_INVALID_HANDLE, pdu, len); 699 700 a = l->data; 701 702 if (a->len <= offset) 703 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle, 704 ATT_ECODE_INVALID_OFFSET, pdu, len); 705 706 status = att_check_reqs(channel, ATT_OP_READ_BLOB_REQ, a->read_reqs); 707 708 client_attr = client_cfg_attribute(channel, a, a->data, a->len); 709 if (client_attr) 710 a = client_attr; 711 712 if (status == 0x00 && a->read_cb) 713 status = a->read_cb(a, a->cb_user_data); 714 715 if (status) 716 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle, status, 717 pdu, len); 718 719 return enc_read_blob_resp(a->data, a->len, offset, pdu, len); 720 } 721 722 static uint16_t write_value(struct gatt_channel *channel, uint16_t handle, 723 const uint8_t *value, int vlen, 724 uint8_t *pdu, int len) 725 { 726 struct attribute *a, *client_attr; 727 uint8_t status; 728 GSList *l; 729 guint h = handle; 730 731 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp); 732 if (!l) 733 return enc_error_resp(ATT_OP_WRITE_REQ, handle, 734 ATT_ECODE_INVALID_HANDLE, pdu, len); 735 736 a = l->data; 737 738 status = att_check_reqs(channel, ATT_OP_WRITE_REQ, a->write_reqs); 739 if (status) 740 return enc_error_resp(ATT_OP_WRITE_REQ, handle, status, pdu, 741 len); 742 743 client_attr = client_cfg_attribute(channel, a, value, vlen); 744 if (client_attr) 745 a = client_attr; 746 else 747 attrib_db_update(a->handle, NULL, value, vlen, &a); 748 749 if (a->write_cb) { 750 status = a->write_cb(a, a->cb_user_data); 751 if (status) 752 return enc_error_resp(ATT_OP_WRITE_REQ, handle, status, 753 pdu, len); 754 } 755 756 DBG("Notifications: %d, indications: %d", 757 g_slist_length(channel->notify), 758 g_slist_length(channel->indicate)); 759 760 return enc_write_resp(pdu, len); 761 } 762 763 static uint16_t mtu_exchange(struct gatt_channel *channel, uint16_t mtu, 764 uint8_t *pdu, int len) 765 { 766 guint old_mtu = channel->mtu; 767 768 if (mtu < ATT_DEFAULT_LE_MTU) 769 channel->mtu = ATT_DEFAULT_LE_MTU; 770 else 771 channel->mtu = MIN(mtu, channel->mtu); 772 773 bt_io_set(le_io, BT_IO_L2CAP, NULL, 774 BT_IO_OPT_OMTU, channel->mtu, 775 BT_IO_OPT_INVALID); 776 777 return enc_mtu_resp(old_mtu, pdu, len); 778 } 779 780 static void channel_disconnect(void *user_data) 781 { 782 struct gatt_channel *channel = user_data; 783 784 g_attrib_unref(channel->attrib); 785 clients = g_slist_remove(clients, channel); 786 787 g_slist_free(channel->notify); 788 g_slist_free(channel->indicate); 789 g_slist_foreach(channel->configs, (GFunc) g_free, NULL); 790 g_slist_free(channel->configs); 791 792 g_free(channel); 793 } 794 795 static void channel_handler(const uint8_t *ipdu, uint16_t len, 796 gpointer user_data) 797 { 798 struct gatt_channel *channel = user_data; 799 uint8_t opdu[ATT_MAX_MTU], value[ATT_MAX_MTU]; 800 uint16_t length, start, end, mtu, offset; 801 bt_uuid_t uuid; 802 uint8_t status = 0; 803 int vlen; 804 805 DBG("op 0x%02x", ipdu[0]); 806 807 switch (ipdu[0]) { 808 case ATT_OP_READ_BY_GROUP_REQ: 809 length = dec_read_by_grp_req(ipdu, len, &start, &end, &uuid); 810 if (length == 0) { 811 status = ATT_ECODE_INVALID_PDU; 812 goto done; 813 } 814 815 length = read_by_group(channel, start, end, &uuid, opdu, 816 channel->mtu); 817 break; 818 case ATT_OP_READ_BY_TYPE_REQ: 819 length = dec_read_by_type_req(ipdu, len, &start, &end, &uuid); 820 if (length == 0) { 821 status = ATT_ECODE_INVALID_PDU; 822 goto done; 823 } 824 825 length = read_by_type(channel, start, end, &uuid, opdu, 826 channel->mtu); 827 break; 828 case ATT_OP_READ_REQ: 829 length = dec_read_req(ipdu, len, &start); 830 if (length == 0) { 831 status = ATT_ECODE_INVALID_PDU; 832 goto done; 833 } 834 835 length = read_value(channel, start, opdu, channel->mtu); 836 break; 837 case ATT_OP_READ_BLOB_REQ: 838 length = dec_read_blob_req(ipdu, len, &start, &offset); 839 if (length == 0) { 840 status = ATT_ECODE_INVALID_PDU; 841 goto done; 842 } 843 844 length = read_blob(channel, start, offset, opdu, channel->mtu); 845 break; 846 case ATT_OP_MTU_REQ: 847 if (!channel->le) { 848 status = ATT_ECODE_REQ_NOT_SUPP; 849 goto done; 850 } 851 852 length = dec_mtu_req(ipdu, len, &mtu); 853 if (length == 0) { 854 status = ATT_ECODE_INVALID_PDU; 855 goto done; 856 } 857 858 length = mtu_exchange(channel, mtu, opdu, channel->mtu); 859 break; 860 case ATT_OP_FIND_INFO_REQ: 861 length = dec_find_info_req(ipdu, len, &start, &end); 862 if (length == 0) { 863 status = ATT_ECODE_INVALID_PDU; 864 goto done; 865 } 866 867 length = find_info(start, end, opdu, channel->mtu); 868 break; 869 case ATT_OP_WRITE_REQ: 870 length = dec_write_req(ipdu, len, &start, value, &vlen); 871 if (length == 0) { 872 status = ATT_ECODE_INVALID_PDU; 873 goto done; 874 } 875 876 length = write_value(channel, start, value, vlen, opdu, 877 channel->mtu); 878 break; 879 case ATT_OP_WRITE_CMD: 880 length = dec_write_cmd(ipdu, len, &start, value, &vlen); 881 if (length > 0) 882 write_value(channel, start, value, vlen, opdu, 883 channel->mtu); 884 return; 885 case ATT_OP_FIND_BY_TYPE_REQ: 886 length = dec_find_by_type_req(ipdu, len, &start, &end, 887 &uuid, value, &vlen); 888 if (length == 0) { 889 status = ATT_ECODE_INVALID_PDU; 890 goto done; 891 } 892 893 length = find_by_type(start, end, &uuid, value, vlen, 894 opdu, channel->mtu); 895 break; 896 case ATT_OP_HANDLE_CNF: 897 return; 898 case ATT_OP_READ_MULTI_REQ: 899 case ATT_OP_PREP_WRITE_REQ: 900 case ATT_OP_EXEC_WRITE_REQ: 901 default: 902 DBG("Unsupported request 0x%02x", ipdu[0]); 903 status = ATT_ECODE_REQ_NOT_SUPP; 904 goto done; 905 } 906 907 if (length == 0) 908 status = ATT_ECODE_IO; 909 910 done: 911 if (status) 912 length = enc_error_resp(ipdu[0], 0x0000, status, opdu, 913 channel->mtu); 914 915 g_attrib_send(channel->attrib, 0, opdu[0], opdu, length, 916 NULL, NULL, NULL); 917 } 918 919 static void connect_event(GIOChannel *io, GError *err, void *user_data) 920 { 921 struct gatt_channel *channel; 922 uint16_t cid; 923 GError *gerr = NULL; 924 925 if (err) { 926 error("%s", err->message); 927 return; 928 } 929 930 channel = g_new0(struct gatt_channel, 1); 931 932 bt_io_get(io, BT_IO_L2CAP, &gerr, 933 BT_IO_OPT_SOURCE_BDADDR, &channel->src, 934 BT_IO_OPT_DEST_BDADDR, &channel->dst, 935 BT_IO_OPT_CID, &cid, 936 BT_IO_OPT_OMTU, &channel->mtu, 937 BT_IO_OPT_INVALID); 938 if (gerr) { 939 error("bt_io_get: %s", gerr->message); 940 g_error_free(gerr); 941 g_free(channel); 942 g_io_channel_shutdown(io, TRUE, NULL); 943 return; 944 } 945 946 if (channel->mtu > ATT_MAX_MTU) 947 channel->mtu = ATT_MAX_MTU; 948 949 if (cid != ATT_CID) 950 channel->le = FALSE; 951 else 952 channel->le = TRUE; 953 954 channel->attrib = g_attrib_new(io); 955 g_io_channel_unref(io); 956 957 channel->id = g_attrib_register(channel->attrib, GATTRIB_ALL_EVENTS, 958 channel_handler, channel, NULL); 959 960 g_attrib_set_disconnect_function(channel->attrib, channel_disconnect, 961 channel); 962 963 clients = g_slist_append(clients, channel); 964 } 965 966 static void confirm_event(GIOChannel *io, void *user_data) 967 { 968 GError *gerr = NULL; 969 970 if (bt_io_accept(io, connect_event, user_data, NULL, &gerr) == FALSE) { 971 error("bt_io_accept: %s", gerr->message); 972 g_error_free(gerr); 973 g_io_channel_unref(io); 974 } 975 976 return; 977 } 978 979 static void attrib_notify_clients(struct attribute *attr) 980 { 981 guint handle = attr->handle; 982 GSList *l; 983 984 for (l = clients; l; l = l->next) { 985 struct gatt_channel *channel = l->data; 986 987 /* Notification */ 988 if (g_slist_find_custom(channel->notify, 989 GUINT_TO_POINTER(handle), handle_cmp)) { 990 uint8_t pdu[ATT_MAX_MTU]; 991 uint16_t len; 992 993 len = enc_notification(attr, pdu, channel->mtu); 994 if (len == 0) 995 continue; 996 997 g_attrib_send(channel->attrib, 0, pdu[0], pdu, len, 998 NULL, NULL, NULL); 999 } 1000 1001 /* Indication */ 1002 if (g_slist_find_custom(channel->indicate, 1003 GUINT_TO_POINTER(handle), handle_cmp)) { 1004 uint8_t pdu[ATT_MAX_MTU]; 1005 uint16_t len; 1006 1007 len = enc_indication(attr, pdu, channel->mtu); 1008 if (len == 0) 1009 return; 1010 1011 g_attrib_send(channel->attrib, 0, pdu[0], pdu, len, 1012 NULL, NULL, NULL); 1013 } 1014 } 1015 } 1016 1017 static gboolean register_core_services(void) 1018 { 1019 uint8_t atval[256]; 1020 bt_uuid_t uuid; 1021 uint16_t appearance = 0x0000; 1022 1023 /* GAP service: primary service definition */ 1024 bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID); 1025 att_put_u16(GENERIC_ACCESS_PROFILE_ID, &atval[0]); 1026 attrib_db_add(0x0001, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 2); 1027 1028 /* GAP service: device name characteristic */ 1029 name_handle = 0x0006; 1030 bt_uuid16_create(&uuid, GATT_CHARAC_UUID); 1031 atval[0] = ATT_CHAR_PROPER_READ; 1032 att_put_u16(name_handle, &atval[1]); 1033 att_put_u16(GATT_CHARAC_DEVICE_NAME, &atval[3]); 1034 attrib_db_add(0x0004, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 5); 1035 1036 /* GAP service: device name attribute */ 1037 bt_uuid16_create(&uuid, GATT_CHARAC_DEVICE_NAME); 1038 attrib_db_add(name_handle, &uuid, ATT_NONE, ATT_NOT_PERMITTED, 1039 NULL, 0); 1040 1041 /* GAP service: device appearance characteristic */ 1042 appearance_handle = 0x0008; 1043 bt_uuid16_create(&uuid, GATT_CHARAC_UUID); 1044 atval[0] = ATT_CHAR_PROPER_READ; 1045 att_put_u16(appearance_handle, &atval[1]); 1046 att_put_u16(GATT_CHARAC_APPEARANCE, &atval[3]); 1047 attrib_db_add(0x0007, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 5); 1048 1049 /* GAP service: device appearance attribute */ 1050 bt_uuid16_create(&uuid, GATT_CHARAC_APPEARANCE); 1051 att_put_u16(appearance, &atval[0]); 1052 attrib_db_add(appearance_handle, &uuid, ATT_NONE, ATT_NOT_PERMITTED, 1053 atval, 2); 1054 gap_sdp_handle = attrib_create_sdp(0x0001, "Generic Access Profile"); 1055 if (gap_sdp_handle == 0) { 1056 error("Failed to register GAP service record"); 1057 goto failed; 1058 } 1059 1060 /* GATT service: primary service definition */ 1061 bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID); 1062 att_put_u16(GENERIC_ATTRIB_PROFILE_ID, &atval[0]); 1063 attrib_db_add(0x0010, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 2); 1064 1065 gatt_sdp_handle = attrib_create_sdp(0x0010, 1066 "Generic Attribute Profile"); 1067 if (gatt_sdp_handle == 0) { 1068 error("Failed to register GATT service record"); 1069 goto failed; 1070 } 1071 1072 return TRUE; 1073 1074 failed: 1075 if (gap_sdp_handle) 1076 remove_record_from_server(gap_sdp_handle); 1077 1078 return FALSE; 1079 } 1080 1081 int attrib_server_init(void) 1082 { 1083 GError *gerr = NULL; 1084 1085 /* BR/EDR socket */ 1086 l2cap_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event, 1087 NULL, NULL, &gerr, 1088 BT_IO_OPT_SOURCE_BDADDR, BDADDR_ANY, 1089 BT_IO_OPT_PSM, ATT_PSM, 1090 BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW, 1091 BT_IO_OPT_INVALID); 1092 if (l2cap_io == NULL) { 1093 error("%s", gerr->message); 1094 g_error_free(gerr); 1095 return -1; 1096 } 1097 1098 if (!register_core_services()) 1099 goto failed; 1100 1101 if (!main_opts.le) 1102 return 0; 1103 1104 /* LE socket */ 1105 le_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event, 1106 &le_io, NULL, &gerr, 1107 BT_IO_OPT_SOURCE_BDADDR, BDADDR_ANY, 1108 BT_IO_OPT_CID, ATT_CID, 1109 BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW, 1110 BT_IO_OPT_INVALID); 1111 if (le_io == NULL) { 1112 error("%s", gerr->message); 1113 g_error_free(gerr); 1114 /* Doesn't have LE support, continue */ 1115 } 1116 1117 return 0; 1118 1119 failed: 1120 g_io_channel_unref(l2cap_io); 1121 l2cap_io = NULL; 1122 1123 if (le_io) { 1124 g_io_channel_unref(le_io); 1125 le_io = NULL; 1126 } 1127 1128 return -1; 1129 } 1130 1131 void attrib_server_exit(void) 1132 { 1133 GSList *l; 1134 1135 g_slist_foreach(database, (GFunc) g_free, NULL); 1136 g_slist_free(database); 1137 1138 if (l2cap_io) { 1139 g_io_channel_unref(l2cap_io); 1140 g_io_channel_shutdown(l2cap_io, FALSE, NULL); 1141 } 1142 1143 if (le_io) { 1144 g_io_channel_unref(le_io); 1145 g_io_channel_shutdown(le_io, FALSE, NULL); 1146 } 1147 1148 for (l = clients; l; l = l->next) { 1149 struct gatt_channel *channel = l->data; 1150 1151 g_slist_free(channel->notify); 1152 g_slist_free(channel->indicate); 1153 g_slist_foreach(channel->configs, (GFunc) g_free, NULL); 1154 g_slist_free(channel->configs); 1155 1156 g_attrib_unref(channel->attrib); 1157 g_free(channel); 1158 } 1159 1160 g_slist_free(clients); 1161 1162 if (gatt_sdp_handle) 1163 remove_record_from_server(gatt_sdp_handle); 1164 1165 if (gap_sdp_handle) 1166 remove_record_from_server(gap_sdp_handle); 1167 } 1168 1169 uint32_t attrib_create_sdp(uint16_t handle, const char *name) 1170 { 1171 sdp_record_t *record; 1172 struct attribute *a; 1173 uint16_t end = 0; 1174 uuid_t svc, gap_uuid; 1175 1176 a = find_primary_range(handle, &end); 1177 1178 if (a == NULL) 1179 return 0; 1180 1181 if (a->len == 2) 1182 sdp_uuid16_create(&svc, att_get_u16(a->data)); 1183 else if (a->len == 16) 1184 sdp_uuid128_create(&svc, a->data); 1185 else 1186 return 0; 1187 1188 record = server_record_new(&svc, handle, end); 1189 if (record == NULL) 1190 return 0; 1191 1192 if (name) 1193 sdp_set_info_attr(record, name, "BlueZ", NULL); 1194 1195 sdp_uuid16_create(&gap_uuid, GENERIC_ACCESS_PROFILE_ID); 1196 if (sdp_uuid_cmp(&svc, &gap_uuid) == 0) { 1197 sdp_set_url_attr(record, "http://www.bluez.org/", 1198 "http://www.bluez.org/", 1199 "http://www.bluez.org/"); 1200 } 1201 1202 if (add_record_to_server(BDADDR_ANY, record) < 0) 1203 sdp_record_free(record); 1204 else 1205 return record->handle; 1206 1207 return 0; 1208 } 1209 1210 void attrib_free_sdp(uint32_t sdp_handle) 1211 { 1212 remove_record_from_server(sdp_handle); 1213 } 1214 1215 uint16_t attrib_db_find_avail(uint16_t nitems) 1216 { 1217 uint16_t handle; 1218 GSList *l; 1219 1220 g_assert(nitems > 0); 1221 1222 for (l = database, handle = 0; l; l = l->next) { 1223 struct attribute *a = l->data; 1224 1225 if (handle && (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 || 1226 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0) && 1227 a->handle - handle >= nitems) 1228 /* Note: the range above excludes the current handle */ 1229 return handle; 1230 1231 if (a->handle == 0xffff) 1232 return 0; 1233 1234 handle = a->handle + 1; 1235 } 1236 1237 if (0xffff - handle + 1 >= nitems) 1238 return handle; 1239 1240 return 0; 1241 } 1242 1243 struct attribute *attrib_db_add(uint16_t handle, bt_uuid_t *uuid, int read_reqs, 1244 int write_reqs, const uint8_t *value, int len) 1245 { 1246 struct attribute *a; 1247 guint h = handle; 1248 1249 DBG("handle=0x%04x", handle); 1250 1251 if (g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp)) 1252 return NULL; 1253 1254 a = g_malloc0(sizeof(struct attribute) + len); 1255 a->handle = handle; 1256 memcpy(&a->uuid, uuid, sizeof(bt_uuid_t)); 1257 a->read_reqs = read_reqs; 1258 a->write_reqs = write_reqs; 1259 a->len = len; 1260 memcpy(a->data, value, len); 1261 1262 database = g_slist_insert_sorted(database, a, attribute_cmp); 1263 1264 return a; 1265 } 1266 1267 int attrib_db_update(uint16_t handle, bt_uuid_t *uuid, const uint8_t *value, 1268 int len, struct attribute **attr) 1269 { 1270 struct attribute *a; 1271 GSList *l; 1272 guint h = handle; 1273 1274 DBG("handle=0x%04x", handle); 1275 1276 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp); 1277 if (!l) 1278 return -ENOENT; 1279 1280 a = g_try_realloc(l->data, sizeof(struct attribute) + len); 1281 if (a == NULL) 1282 return -ENOMEM; 1283 1284 l->data = a; 1285 if (uuid != NULL) 1286 memcpy(&a->uuid, uuid, sizeof(bt_uuid_t)); 1287 a->len = len; 1288 memcpy(a->data, value, len); 1289 1290 attrib_notify_clients(a); 1291 1292 if (attr) 1293 *attr = a; 1294 1295 return 0; 1296 } 1297 1298 int attrib_db_del(uint16_t handle) 1299 { 1300 struct attribute *a; 1301 GSList *l; 1302 guint h = handle; 1303 1304 DBG("handle=0x%04x", handle); 1305 1306 l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp); 1307 if (!l) 1308 return -ENOENT; 1309 1310 a = l->data; 1311 database = g_slist_remove(database, a); 1312 g_free(a); 1313 1314 return 0; 1315 } 1316 1317 int attrib_gap_set(uint16_t uuid, const uint8_t *value, int len) 1318 { 1319 uint16_t handle; 1320 1321 /* FIXME: Missing Privacy and Reconnection Address */ 1322 1323 switch (uuid) { 1324 case GATT_CHARAC_DEVICE_NAME: 1325 handle = name_handle; 1326 break; 1327 case GATT_CHARAC_APPEARANCE: 1328 handle = appearance_handle; 1329 break; 1330 default: 1331 return -ENOSYS; 1332 } 1333 1334 return attrib_db_update(handle, NULL, value, len, NULL); 1335 } 1336
