1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 39 #include <stdio.h> 40 #include <string.h> 41 42 #include "bluetooth.h" 43 #include "ble/att_db.h" 44 #include "ble/core.h" 45 #include "btstack_debug.h" 46 #include "btstack_util.h" 47 48 // Buetooth Base UUID 00000000-0000-1000-8000-00805F9B34FB in little endian 49 static const uint8_t bluetooth_base_uuid[] = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 50 51 52 static int is_Bluetooth_Base_UUID(uint8_t const *uuid){ 53 if (memcmp(&uuid[0], &bluetooth_base_uuid[0], 12)) return 0; 54 if (memcmp(&uuid[14], &bluetooth_base_uuid[14], 2)) return 0; 55 return 1; 56 57 } 58 59 static uint16_t uuid16_from_uuid(uint16_t uuid_len, uint8_t * uuid){ 60 if (uuid_len == 2) return little_endian_read_16(uuid, 0); 61 if (!is_Bluetooth_Base_UUID(uuid)) return 0; 62 return little_endian_read_16(uuid, 12); 63 } 64 65 // ATT Database 66 static uint8_t const * att_db = NULL; 67 static att_read_callback_t att_read_callback = NULL; 68 static att_write_callback_t att_write_callback = NULL; 69 static uint8_t att_prepare_write_error_code = 0; 70 static uint16_t att_prepare_write_error_handle = 0x0000; 71 72 // new java-style iterator 73 typedef struct att_iterator { 74 // private 75 uint8_t const * att_ptr; 76 // public 77 uint16_t size; 78 uint16_t flags; 79 uint16_t handle; 80 uint8_t const * uuid; 81 uint16_t value_len; 82 uint8_t const * value; 83 } att_iterator_t; 84 85 static void att_iterator_init(att_iterator_t *it){ 86 it->att_ptr = att_db; 87 } 88 89 static int att_iterator_has_next(att_iterator_t *it){ 90 return it->att_ptr != NULL; 91 } 92 93 static void att_iterator_fetch_next(att_iterator_t *it){ 94 it->size = little_endian_read_16(it->att_ptr, 0); 95 if (it->size == 0){ 96 it->flags = 0; 97 it->handle = 0; 98 it->uuid = NULL; 99 it->value_len = 0; 100 it->value = NULL; 101 it->att_ptr = NULL; 102 return; 103 } 104 it->flags = little_endian_read_16(it->att_ptr, 2); 105 it->handle = little_endian_read_16(it->att_ptr, 4); 106 it->uuid = &it->att_ptr[6]; 107 // handle 128 bit UUIDs 108 if (it->flags & ATT_PROPERTY_UUID128){ 109 it->value_len = it->size - 22; 110 it->value = &it->att_ptr[22]; 111 } else { 112 it->value_len = it->size - 8; 113 it->value = &it->att_ptr[8]; 114 } 115 // advance AFTER setting values 116 it->att_ptr += it->size; 117 } 118 119 static int att_iterator_match_uuid16(att_iterator_t *it, uint16_t uuid){ 120 if (it->handle == 0) return 0; 121 if (it->flags & ATT_PROPERTY_UUID128){ 122 if (!is_Bluetooth_Base_UUID(it->uuid)) return 0; 123 return little_endian_read_16(it->uuid, 12) == uuid; 124 } 125 return little_endian_read_16(it->uuid, 0) == uuid; 126 } 127 128 static int att_iterator_match_uuid(att_iterator_t *it, uint8_t *uuid, uint16_t uuid_len){ 129 if (it->handle == 0) return 0; 130 // input: UUID16 131 if (uuid_len == 2) { 132 return att_iterator_match_uuid16(it, little_endian_read_16(uuid, 0)); 133 } 134 // input and db: UUID128 135 if (it->flags & ATT_PROPERTY_UUID128){ 136 return memcmp(it->uuid, uuid, 16) == 0; 137 } 138 // input: UUID128, db: UUID16 139 if (!is_Bluetooth_Base_UUID(uuid)) return 0; 140 return little_endian_read_16(uuid, 12) == little_endian_read_16(it->uuid, 0); 141 } 142 143 144 static int att_find_handle(att_iterator_t *it, uint16_t handle){ 145 if (handle == 0) return 0; 146 att_iterator_init(it); 147 while (att_iterator_has_next(it)){ 148 att_iterator_fetch_next(it); 149 if (it->handle != handle) continue; 150 return 1; 151 } 152 return 0; 153 } 154 155 // experimental client API 156 uint16_t att_uuid_for_handle(uint16_t attribute_handle){ 157 att_iterator_t it; 158 int ok = att_find_handle(&it, attribute_handle); 159 if (!ok) return 0; 160 if (it.flags & ATT_PROPERTY_UUID128) return 0; 161 return little_endian_read_16(it.uuid, 0); 162 } 163 // end of client API 164 165 static void att_update_value_len(att_iterator_t *it, hci_con_handle_t con_handle){ 166 if ((it->flags & ATT_PROPERTY_DYNAMIC) == 0 || !att_read_callback) return; 167 it->value_len = (*att_read_callback)(con_handle, it->handle, 0, NULL, 0); 168 return; 169 } 170 171 // copy attribute value from offset into buffer with given size 172 static int att_copy_value(att_iterator_t *it, uint16_t offset, uint8_t * buffer, uint16_t buffer_size, hci_con_handle_t con_handle){ 173 174 // DYNAMIC 175 if ((it->flags & ATT_PROPERTY_DYNAMIC) && att_read_callback) { 176 return (*att_read_callback)(con_handle, it->handle, offset, buffer, buffer_size); 177 } 178 179 // STATIC 180 uint16_t bytes_to_copy = it->value_len - offset; 181 if (bytes_to_copy > buffer_size){ 182 bytes_to_copy = buffer_size; 183 } 184 memcpy(buffer, it->value, bytes_to_copy); 185 return bytes_to_copy; 186 } 187 188 void att_set_db(uint8_t const * db){ 189 att_db = db; 190 } 191 192 void att_set_read_callback(att_read_callback_t callback){ 193 att_read_callback = callback; 194 } 195 196 void att_set_write_callback(att_write_callback_t callback){ 197 att_write_callback = callback; 198 } 199 200 void att_dump_attributes(void){ 201 att_iterator_t it; 202 att_iterator_init(&it); 203 uint8_t uuid128[16]; 204 while (att_iterator_has_next(&it)){ 205 att_iterator_fetch_next(&it); 206 if (it.handle == 0) { 207 log_info("Handle: END"); 208 return; 209 } 210 log_info("Handle: 0x%04x, flags: 0x%04x, uuid: ", it.handle, it.flags); 211 if (it.flags & ATT_PROPERTY_UUID128){ 212 reverse_128(it.uuid, uuid128); 213 log_info("%s", uuid128_to_str(uuid128)); 214 } else { 215 log_info("%04x", little_endian_read_16(it.uuid, 0)); 216 } 217 log_info(", value_len: %u, value: ", it.value_len); 218 log_info_hexdump(it.value, it.value_len); 219 } 220 } 221 222 static void att_prepare_write_reset(void){ 223 att_prepare_write_error_code = 0; 224 att_prepare_write_error_handle = 0x0000; 225 } 226 227 static void att_prepare_write_update_errors(uint8_t error_code, uint16_t handle){ 228 // first ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH has highest priority 229 if (error_code == ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH && error_code != att_prepare_write_error_code){ 230 att_prepare_write_error_code = error_code; 231 att_prepare_write_error_handle = handle; 232 return; 233 } 234 // first ATT_ERROR_INVALID_OFFSET is next 235 if (error_code == ATT_ERROR_INVALID_OFFSET && att_prepare_write_error_code == 0){ 236 att_prepare_write_error_code = error_code; 237 att_prepare_write_error_handle = handle; 238 return; 239 } 240 } 241 242 static uint16_t setup_error(uint8_t * response_buffer, uint16_t request, uint16_t handle, uint8_t error_code){ 243 response_buffer[0] = ATT_ERROR_RESPONSE; 244 response_buffer[1] = request; 245 little_endian_store_16(response_buffer, 2, handle); 246 response_buffer[4] = error_code; 247 return 5; 248 } 249 250 static inline uint16_t setup_error_read_not_permitted(uint8_t * response_buffer, uint16_t request, uint16_t start_handle){ 251 return setup_error(response_buffer, request, start_handle, ATT_ERROR_READ_NOT_PERMITTED); 252 } 253 254 static inline uint16_t setup_error_write_not_permitted(uint8_t * response_buffer, uint16_t request, uint16_t start_handle){ 255 return setup_error(response_buffer, request, start_handle, ATT_ERROR_WRITE_NOT_PERMITTED); 256 } 257 258 static inline uint16_t setup_error_atribute_not_found(uint8_t * response_buffer, uint16_t request, uint16_t start_handle){ 259 return setup_error(response_buffer, request, start_handle, ATT_ERROR_ATTRIBUTE_NOT_FOUND); 260 } 261 262 static inline uint16_t setup_error_invalid_handle(uint8_t * response_buffer, uint16_t request, uint16_t handle){ 263 return setup_error(response_buffer, request, handle, ATT_ERROR_INVALID_HANDLE); 264 } 265 266 static inline uint16_t setup_error_invalid_offset(uint8_t * response_buffer, uint16_t request, uint16_t handle){ 267 return setup_error(response_buffer, request, handle, ATT_ERROR_INVALID_OFFSET); 268 } 269 270 static uint8_t att_validate_security(att_connection_t * att_connection, att_iterator_t * it){ 271 int required_encryption_size = it->flags >> 12; 272 if (required_encryption_size) required_encryption_size++; // store -1 to fit into 4 bit 273 log_info("att_validate_security. flags 0x%04x - req enc size %u, authorized %u, authenticated %u, encryption_key_size %u", 274 it->flags, required_encryption_size, att_connection->authorized, att_connection->authenticated, att_connection->encryption_key_size); 275 if ((it->flags & ATT_PROPERTY_AUTHENTICATION_REQUIRED) && att_connection->authenticated == 0) { 276 return ATT_ERROR_INSUFFICIENT_AUTHENTICATION; 277 } 278 if ((it->flags & ATT_PROPERTY_AUTHORIZATION_REQUIRED) && att_connection->authorized == 0) { 279 return ATT_ERROR_INSUFFICIENT_AUTHORIZATION; 280 } 281 if (required_encryption_size > 0 && att_connection->encryption_key_size == 0){ 282 return ATT_ERROR_INSUFFICIENT_ENCRYPTION; 283 } 284 if (required_encryption_size > att_connection->encryption_key_size){ 285 return ATT_ERROR_INSUFFICIENT_ENCRYPTION_KEY_SIZE; 286 } 287 return 0; 288 } 289 290 // 291 // MARK: ATT_EXCHANGE_MTU_REQUEST 292 // 293 static uint16_t handle_exchange_mtu_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 294 uint8_t * response_buffer){ 295 296 uint16_t client_rx_mtu = little_endian_read_16(request_buffer, 1); 297 298 // find min(local max mtu, remote mtu) and use as mtu for this connection 299 if (client_rx_mtu < att_connection->max_mtu){ 300 att_connection->mtu = client_rx_mtu; 301 } else { 302 att_connection->mtu = att_connection->max_mtu; 303 } 304 305 response_buffer[0] = ATT_EXCHANGE_MTU_RESPONSE; 306 little_endian_store_16(response_buffer, 1, att_connection->mtu); 307 return 3; 308 } 309 310 311 // 312 // MARK: ATT_FIND_INFORMATION_REQUEST 313 // 314 // TODO: handle other types then GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID 315 // 316 static uint16_t handle_find_information_request2(att_connection_t * att_connection, uint8_t * response_buffer, uint16_t response_buffer_size, 317 uint16_t start_handle, uint16_t end_handle){ 318 319 log_info("ATT_FIND_INFORMATION_REQUEST: from %04X to %04X", start_handle, end_handle); 320 uint8_t request_type = ATT_FIND_INFORMATION_REQUEST; 321 322 if (start_handle > end_handle || start_handle == 0){ 323 return setup_error_invalid_handle(response_buffer, request_type, start_handle); 324 } 325 326 uint16_t offset = 1; 327 uint16_t uuid_len = 0; 328 329 att_iterator_t it; 330 att_iterator_init(&it); 331 while (att_iterator_has_next(&it)){ 332 att_iterator_fetch_next(&it); 333 if (!it.handle) break; 334 if (it.handle > end_handle) break; 335 if (it.handle < start_handle) continue; 336 337 // log_info("Handle 0x%04x", it.handle); 338 339 uint16_t this_uuid_len = (it.flags & ATT_PROPERTY_UUID128) ? 16 : 2; 340 341 // check if value has same len as last one if not first result 342 if (offset > 1){ 343 if (this_uuid_len != uuid_len) { 344 break; 345 } 346 } 347 348 // first 349 if (offset == 1) { 350 uuid_len = this_uuid_len; 351 // set format field 352 response_buffer[offset] = (it.flags & ATT_PROPERTY_UUID128) ? 0x02 : 0x01; 353 offset++; 354 } 355 356 // space? 357 if (offset + 2 + uuid_len > response_buffer_size) break; 358 359 // store 360 little_endian_store_16(response_buffer, offset, it.handle); 361 offset += 2; 362 363 memcpy(response_buffer + offset, it.uuid, uuid_len); 364 offset += uuid_len; 365 } 366 367 if (offset == 1){ 368 return setup_error_atribute_not_found(response_buffer, request_type, start_handle); 369 } 370 371 response_buffer[0] = ATT_FIND_INFORMATION_REPLY; 372 return offset; 373 } 374 375 static uint16_t handle_find_information_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 376 uint8_t * response_buffer, uint16_t response_buffer_size){ 377 return handle_find_information_request2(att_connection, response_buffer, response_buffer_size, little_endian_read_16(request_buffer, 1), little_endian_read_16(request_buffer, 3)); 378 } 379 380 // 381 // MARK: ATT_FIND_BY_TYPE_VALUE 382 // 383 // "Only attributes with attribute handles between and including the Starting Handle parameter 384 // and the Ending Handle parameter that match the requested attri- bute type and the attribute 385 // value that have sufficient permissions to allow reading will be returned" -> (1) 386 // 387 // TODO: handle other types then GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID 388 // 389 // NOTE: doesn't handle DYNAMIC values 390 // NOTE: only supports 16 bit UUIDs 391 // 392 static uint16_t handle_find_by_type_value_request2(att_connection_t * att_connection, uint8_t * response_buffer, uint16_t response_buffer_size, 393 uint16_t start_handle, uint16_t end_handle, 394 uint16_t attribute_type, uint16_t attribute_len, uint8_t* attribute_value){ 395 396 log_info("ATT_FIND_BY_TYPE_VALUE_REQUEST: from %04X to %04X, type %04X, value: ", start_handle, end_handle, attribute_type); 397 log_info_hexdump(attribute_value, attribute_len); 398 uint8_t request_type = ATT_FIND_BY_TYPE_VALUE_REQUEST; 399 400 if (start_handle > end_handle || start_handle == 0){ 401 return setup_error_invalid_handle(response_buffer, request_type, start_handle); 402 } 403 404 uint16_t offset = 1; 405 uint16_t in_group = 0; 406 uint16_t prev_handle = 0; 407 408 att_iterator_t it; 409 att_iterator_init(&it); 410 while (att_iterator_has_next(&it)){ 411 att_iterator_fetch_next(&it); 412 413 if (it.handle && it.handle < start_handle) continue; 414 if (it.handle > end_handle) break; // (1) 415 416 // close current tag, if within a group and a new service definition starts or we reach end of att db 417 if (in_group && 418 (it.handle == 0 || att_iterator_match_uuid16(&it, GATT_PRIMARY_SERVICE_UUID) || att_iterator_match_uuid16(&it, GATT_SECONDARY_SERVICE_UUID))){ 419 420 log_info("End of group, handle 0x%04x", prev_handle); 421 little_endian_store_16(response_buffer, offset, prev_handle); 422 offset += 2; 423 in_group = 0; 424 425 // check if space for another handle pair available 426 if (offset + 4 > response_buffer_size){ 427 break; 428 } 429 } 430 431 // keep track of previous handle 432 prev_handle = it.handle; 433 434 // does current attribute match 435 if (it.handle && att_iterator_match_uuid16(&it, attribute_type) && attribute_len == it.value_len && memcmp(attribute_value, it.value, it.value_len) == 0){ 436 log_info("Begin of group, handle 0x%04x", it.handle); 437 little_endian_store_16(response_buffer, offset, it.handle); 438 offset += 2; 439 in_group = 1; 440 } 441 } 442 443 if (offset == 1){ 444 return setup_error_atribute_not_found(response_buffer, request_type, start_handle); 445 } 446 447 response_buffer[0] = ATT_FIND_BY_TYPE_VALUE_RESPONSE; 448 return offset; 449 } 450 451 static uint16_t handle_find_by_type_value_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 452 uint8_t * response_buffer, uint16_t response_buffer_size){ 453 int attribute_len = request_len - 7; 454 return handle_find_by_type_value_request2(att_connection, response_buffer, response_buffer_size, little_endian_read_16(request_buffer, 1), 455 little_endian_read_16(request_buffer, 3), little_endian_read_16(request_buffer, 5), attribute_len, &request_buffer[7]); 456 } 457 458 // 459 // MARK: ATT_READ_BY_TYPE_REQUEST 460 // 461 static uint16_t handle_read_by_type_request2(att_connection_t * att_connection, uint8_t * response_buffer, uint16_t response_buffer_size, 462 uint16_t start_handle, uint16_t end_handle, 463 uint16_t attribute_type_len, uint8_t * attribute_type){ 464 465 log_info("ATT_READ_BY_TYPE_REQUEST: from %04X to %04X, type: ", start_handle, end_handle); 466 log_info_hexdump(attribute_type, attribute_type_len); 467 uint8_t request_type = ATT_READ_BY_TYPE_REQUEST; 468 469 if (start_handle > end_handle || start_handle == 0){ 470 return setup_error_invalid_handle(response_buffer, request_type, start_handle); 471 } 472 473 uint16_t offset = 1; 474 uint16_t pair_len = 0; 475 476 att_iterator_t it; 477 att_iterator_init(&it); 478 uint8_t error_code = 0; 479 uint16_t first_matching_but_unreadable_handle = 0; 480 481 while (att_iterator_has_next(&it)){ 482 att_iterator_fetch_next(&it); 483 484 if (!it.handle) break; 485 if (it.handle < start_handle) continue; 486 if (it.handle > end_handle) break; // (1) 487 488 // does current attribute match 489 if (!att_iterator_match_uuid(&it, attribute_type, attribute_type_len)) continue; 490 491 // skip handles that cannot be read but rembember that there has been at least one 492 if ((it.flags & ATT_PROPERTY_READ) == 0) { 493 if (first_matching_but_unreadable_handle == 0) { 494 first_matching_but_unreadable_handle = it.handle; 495 } 496 continue; 497 } 498 499 // check security requirements 500 error_code = att_validate_security(att_connection, &it); 501 if (error_code) break; 502 503 att_update_value_len(&it, att_connection->con_handle); 504 505 // check if value has same len as last one 506 uint16_t this_pair_len = 2 + it.value_len; 507 if (offset > 1){ 508 if (pair_len != this_pair_len) { 509 break; 510 } 511 } 512 513 // first 514 if (offset == 1) { 515 pair_len = this_pair_len; 516 response_buffer[offset] = pair_len; 517 offset++; 518 } 519 520 // space? 521 if (offset + pair_len > response_buffer_size) { 522 if (offset > 2) break; 523 it.value_len = response_buffer_size - 4; 524 response_buffer[1] = 2 + it.value_len; 525 } 526 527 // store 528 little_endian_store_16(response_buffer, offset, it.handle); 529 offset += 2; 530 uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len, att_connection->con_handle); 531 offset += bytes_copied; 532 } 533 534 // at least one attribute could be read 535 if (offset > 1){ 536 response_buffer[0] = ATT_READ_BY_TYPE_RESPONSE; 537 return offset; 538 } 539 540 // first attribute had an error 541 if (error_code){ 542 return setup_error(response_buffer, request_type, start_handle, error_code); 543 } 544 545 // no other errors, but all found attributes had been non-readable 546 if (first_matching_but_unreadable_handle){ 547 return setup_error_read_not_permitted(response_buffer, request_type, first_matching_but_unreadable_handle); 548 } 549 550 // attribute not found 551 return setup_error_atribute_not_found(response_buffer, request_type, start_handle); 552 } 553 554 static uint16_t handle_read_by_type_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 555 uint8_t * response_buffer, uint16_t response_buffer_size){ 556 int attribute_type_len; 557 if (request_len <= 7){ 558 attribute_type_len = 2; 559 } else { 560 attribute_type_len = 16; 561 } 562 return handle_read_by_type_request2(att_connection, response_buffer, response_buffer_size, little_endian_read_16(request_buffer, 1), little_endian_read_16(request_buffer, 3), attribute_type_len, &request_buffer[5]); 563 } 564 565 // 566 // MARK: ATT_READ_BY_TYPE_REQUEST 567 // 568 static uint16_t handle_read_request2(att_connection_t * att_connection, uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t handle){ 569 570 log_info("ATT_READ_REQUEST: handle %04x", handle); 571 uint8_t request_type = ATT_READ_REQUEST; 572 573 att_iterator_t it; 574 int ok = att_find_handle(&it, handle); 575 if (!ok){ 576 return setup_error_invalid_handle(response_buffer, request_type, handle); 577 } 578 579 // check if handle can be read 580 if ((it.flags & ATT_PROPERTY_READ) == 0) { 581 return setup_error_read_not_permitted(response_buffer, request_type, handle); 582 } 583 584 // check security requirements 585 uint8_t error_code = att_validate_security(att_connection, &it); 586 if (error_code) { 587 return setup_error(response_buffer, request_type, handle, error_code); 588 } 589 590 att_update_value_len(&it, att_connection->con_handle); 591 592 uint16_t offset = 1; 593 // limit data 594 if (offset + it.value_len > response_buffer_size) { 595 it.value_len = response_buffer_size - 1; 596 } 597 598 // store 599 uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len, att_connection->con_handle); 600 offset += bytes_copied; 601 602 response_buffer[0] = ATT_READ_RESPONSE; 603 return offset; 604 } 605 606 static uint16_t handle_read_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 607 uint8_t * response_buffer, uint16_t response_buffer_size){ 608 return handle_read_request2(att_connection, response_buffer, response_buffer_size, little_endian_read_16(request_buffer, 1)); 609 } 610 611 // 612 // MARK: ATT_READ_BLOB_REQUEST 0x0c 613 // 614 static uint16_t handle_read_blob_request2(att_connection_t * att_connection, uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t handle, uint16_t value_offset){ 615 log_info("ATT_READ_BLOB_REQUEST: handle %04x, offset %u", handle, value_offset); 616 uint8_t request_type = ATT_READ_BLOB_REQUEST; 617 618 att_iterator_t it; 619 int ok = att_find_handle(&it, handle); 620 if (!ok){ 621 return setup_error_invalid_handle(response_buffer, request_type, handle); 622 } 623 624 // check if handle can be read 625 if ((it.flags & ATT_PROPERTY_READ) == 0) { 626 return setup_error_read_not_permitted(response_buffer, request_type, handle); 627 } 628 629 // check security requirements 630 uint8_t error_code = att_validate_security(att_connection, &it); 631 if (error_code) { 632 return setup_error(response_buffer, request_type, handle, error_code); 633 } 634 635 att_update_value_len(&it, att_connection->con_handle); 636 637 if (value_offset > it.value_len){ 638 return setup_error_invalid_offset(response_buffer, request_type, handle); 639 } 640 641 // limit data 642 uint16_t offset = 1; 643 if (offset + it.value_len - value_offset > response_buffer_size) { 644 it.value_len = response_buffer_size - 1 + value_offset; 645 } 646 647 // store 648 uint16_t bytes_copied = att_copy_value(&it, value_offset, response_buffer + offset, it.value_len - value_offset, att_connection->con_handle); 649 offset += bytes_copied; 650 651 response_buffer[0] = ATT_READ_BLOB_RESPONSE; 652 return offset; 653 } 654 655 static uint16_t handle_read_blob_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 656 uint8_t * response_buffer, uint16_t response_buffer_size){ 657 return handle_read_blob_request2(att_connection, response_buffer, response_buffer_size, little_endian_read_16(request_buffer, 1), little_endian_read_16(request_buffer, 3)); 658 } 659 660 // 661 // MARK: ATT_READ_MULTIPLE_REQUEST 0x0e 662 // 663 static uint16_t handle_read_multiple_request2(att_connection_t * att_connection, uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t num_handles, uint8_t * handles){ 664 log_info("ATT_READ_MULTIPLE_REQUEST: num handles %u", num_handles); 665 uint8_t request_type = ATT_READ_MULTIPLE_REQUEST; 666 667 // TODO: figure out which error to respond with 668 // if (num_handles < 2){ 669 // return setup_error(response_buffer, ATT_READ_MULTIPLE_REQUEST, handle, ???); 670 // } 671 672 uint16_t offset = 1; 673 674 int i; 675 uint8_t error_code = 0; 676 uint16_t handle = 0; 677 for (i=0;i<num_handles;i++){ 678 handle = little_endian_read_16(handles, i << 1); 679 680 if (handle == 0){ 681 return setup_error_invalid_handle(response_buffer, request_type, handle); 682 } 683 684 att_iterator_t it; 685 686 int ok = att_find_handle(&it, handle); 687 if (!ok){ 688 return setup_error_invalid_handle(response_buffer, request_type, handle); 689 } 690 691 // check if handle can be read 692 if ((it.flags & ATT_PROPERTY_READ) == 0) { 693 error_code = ATT_ERROR_READ_NOT_PERMITTED; 694 break; 695 } 696 697 // check security requirements 698 error_code = att_validate_security(att_connection, &it); 699 if (error_code) break; 700 701 att_update_value_len(&it, att_connection->con_handle); 702 703 // limit data 704 if (offset + it.value_len > response_buffer_size) { 705 it.value_len = response_buffer_size - 1; 706 } 707 708 // store 709 uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len, att_connection->con_handle); 710 offset += bytes_copied; 711 } 712 713 if (error_code){ 714 return setup_error(response_buffer, request_type, handle, error_code); 715 } 716 717 response_buffer[0] = ATT_READ_MULTIPLE_RESPONSE; 718 return offset; 719 } 720 static uint16_t handle_read_multiple_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 721 uint8_t * response_buffer, uint16_t response_buffer_size){ 722 int num_handles = (request_len - 1) >> 1; 723 return handle_read_multiple_request2(att_connection, response_buffer, response_buffer_size, num_handles, &request_buffer[1]); 724 } 725 726 // 727 // MARK: ATT_READ_BY_GROUP_TYPE_REQUEST 0x10 728 // 729 // Only handles GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID 730 // Core v4.0, vol 3, part g, 2.5.3 731 // "The «Primary Service» and «Secondary Service» grouping types may be used in the Read By Group Type Request. 732 // The «Characteristic» grouping type shall not be used in the ATT Read By Group Type Request." 733 // 734 // NOTE: doesn't handle DYNAMIC values 735 // 736 // NOTE: we don't check for security as PRIMARY and SECONDAY SERVICE definition shouldn't be protected 737 // Core 4.0, vol 3, part g, 8.1 738 // "The list of services and characteristics that a device supports is not considered private or 739 // confidential information, and therefore the Service and Characteristic Discovery procedures 740 // shall always be permitted. " 741 // 742 static uint16_t handle_read_by_group_type_request2(att_connection_t * att_connection, uint8_t * response_buffer, uint16_t response_buffer_size, 743 uint16_t start_handle, uint16_t end_handle, 744 uint16_t attribute_type_len, uint8_t * attribute_type){ 745 746 log_info("ATT_READ_BY_GROUP_TYPE_REQUEST: from %04X to %04X, buffer size %u, type: ", start_handle, end_handle, response_buffer_size); 747 log_info_hexdump(attribute_type, attribute_type_len); 748 uint8_t request_type = ATT_READ_BY_GROUP_TYPE_REQUEST; 749 750 if (start_handle > end_handle || start_handle == 0){ 751 return setup_error_invalid_handle(response_buffer, request_type, start_handle); 752 } 753 754 // assert UUID is primary or secondary service uuid 755 uint16_t uuid16 = uuid16_from_uuid(attribute_type_len, attribute_type); 756 if (uuid16 != GATT_PRIMARY_SERVICE_UUID && uuid16 != GATT_SECONDARY_SERVICE_UUID){ 757 return setup_error(response_buffer, request_type, start_handle, ATT_ERROR_UNSUPPORTED_GROUP_TYPE); 758 } 759 760 uint16_t offset = 1; 761 uint16_t pair_len = 0; 762 uint16_t in_group = 0; 763 uint16_t group_start_handle = 0; 764 uint8_t const * group_start_value = NULL; 765 uint16_t prev_handle = 0; 766 767 att_iterator_t it; 768 att_iterator_init(&it); 769 while (att_iterator_has_next(&it)){ 770 att_iterator_fetch_next(&it); 771 772 if (it.handle && it.handle < start_handle) continue; 773 if (it.handle > end_handle) break; // (1) 774 775 // log_info("Handle 0x%04x", it.handle); 776 777 // close current tag, if within a group and a new service definition starts or we reach end of att db 778 if (in_group && 779 (it.handle == 0 || att_iterator_match_uuid16(&it, GATT_PRIMARY_SERVICE_UUID) || att_iterator_match_uuid16(&it, GATT_SECONDARY_SERVICE_UUID))){ 780 // log_info("End of group, handle 0x%04x, val_len: %u", prev_handle, pair_len - 4); 781 782 little_endian_store_16(response_buffer, offset, group_start_handle); 783 offset += 2; 784 little_endian_store_16(response_buffer, offset, prev_handle); 785 offset += 2; 786 memcpy(response_buffer + offset, group_start_value, pair_len - 4); 787 offset += pair_len - 4; 788 in_group = 0; 789 790 // check if space for another handle pair available 791 if (offset + pair_len > response_buffer_size){ 792 break; 793 } 794 } 795 796 // keep track of previous handle 797 prev_handle = it.handle; 798 799 // does current attribute match 800 // log_info("compare: %04x == %04x", *(uint16_t*) context->attribute_type, *(uint16_t*) uuid); 801 if (it.handle && att_iterator_match_uuid(&it, attribute_type, attribute_type_len)) { 802 803 // check if value has same len as last one 804 uint16_t this_pair_len = 4 + it.value_len; 805 if (offset > 1){ 806 if (this_pair_len != pair_len) { 807 break; 808 } 809 } 810 811 // log_info("Begin of group, handle 0x%04x", it.handle); 812 813 // first 814 if (offset == 1) { 815 pair_len = this_pair_len; 816 response_buffer[offset] = this_pair_len; 817 offset++; 818 } 819 820 group_start_handle = it.handle; 821 group_start_value = it.value; 822 in_group = 1; 823 } 824 } 825 826 if (offset == 1){ 827 return setup_error_atribute_not_found(response_buffer, request_type, start_handle); 828 } 829 830 response_buffer[0] = ATT_READ_BY_GROUP_TYPE_RESPONSE; 831 return offset; 832 } 833 static uint16_t handle_read_by_group_type_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 834 uint8_t * response_buffer, uint16_t response_buffer_size){ 835 int attribute_type_len; 836 if (request_len <= 7){ 837 attribute_type_len = 2; 838 } else { 839 attribute_type_len = 16; 840 } 841 return handle_read_by_group_type_request2(att_connection, response_buffer, response_buffer_size, little_endian_read_16(request_buffer, 1), little_endian_read_16(request_buffer, 3), attribute_type_len, &request_buffer[5]); 842 } 843 844 // 845 // MARK: ATT_WRITE_REQUEST 0x12 846 static uint16_t handle_write_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 847 uint8_t * response_buffer, uint16_t response_buffer_size){ 848 849 uint8_t request_type = ATT_WRITE_REQUEST; 850 851 uint16_t handle = little_endian_read_16(request_buffer, 1); 852 att_iterator_t it; 853 int ok = att_find_handle(&it, handle); 854 if (!ok) { 855 return setup_error_invalid_handle(response_buffer, request_type, handle); 856 } 857 if (!att_write_callback) { 858 return setup_error_write_not_permitted(response_buffer, request_type, handle); 859 } 860 if ((it.flags & ATT_PROPERTY_WRITE) == 0) { 861 return setup_error_write_not_permitted(response_buffer, request_type, handle); 862 } 863 if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) { 864 return setup_error_write_not_permitted(response_buffer, request_type, handle); 865 } 866 // check security requirements 867 uint8_t error_code = att_validate_security(att_connection, &it); 868 if (error_code) { 869 return setup_error(response_buffer, request_type, handle, error_code); 870 } 871 error_code = (*att_write_callback)(att_connection->con_handle, handle, ATT_TRANSACTION_MODE_NONE, 0, request_buffer + 3, request_len - 3); 872 if (error_code) { 873 return setup_error(response_buffer, request_type, handle, error_code); 874 } 875 response_buffer[0] = ATT_WRITE_RESPONSE; 876 return 1; 877 } 878 879 // 880 // MARK: ATT_PREPARE_WRITE_REQUEST 0x16 881 static uint16_t handle_prepare_write_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 882 uint8_t * response_buffer, uint16_t response_buffer_size){ 883 884 uint8_t request_type = ATT_PREPARE_WRITE_REQUEST; 885 886 uint16_t handle = little_endian_read_16(request_buffer, 1); 887 uint16_t offset = little_endian_read_16(request_buffer, 3); 888 if (!att_write_callback) { 889 return setup_error_write_not_permitted(response_buffer, request_type, handle); 890 } 891 att_iterator_t it; 892 int ok = att_find_handle(&it, handle); 893 if (!ok) { 894 return setup_error_invalid_handle(response_buffer, request_type, handle); 895 } 896 if ((it.flags & ATT_PROPERTY_WRITE) == 0) { 897 return setup_error_write_not_permitted(response_buffer, request_type, handle); 898 } 899 if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) { 900 return setup_error_write_not_permitted(response_buffer, request_type, handle); 901 } 902 // check security requirements 903 uint8_t error_code = att_validate_security(att_connection, &it); 904 if (error_code) { 905 return setup_error(response_buffer, request_type, handle, error_code); 906 } 907 908 error_code = (*att_write_callback)(att_connection->con_handle, handle, ATT_TRANSACTION_MODE_ACTIVE, offset, request_buffer + 5, request_len - 5); 909 switch (error_code){ 910 case 0: 911 break; 912 case ATT_ERROR_INVALID_OFFSET: 913 case ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH: 914 // postpone to execute write request 915 att_prepare_write_update_errors(error_code, handle); 916 break; 917 default: 918 return setup_error(response_buffer, request_type, handle, error_code); 919 } 920 921 // response: echo request 922 memcpy(response_buffer, request_buffer, request_len); 923 response_buffer[0] = ATT_PREPARE_WRITE_RESPONSE; 924 return request_len; 925 } 926 927 /* 928 * @brief transcation queue of prepared writes, e.g., after disconnect 929 */ 930 void att_clear_transaction_queue(att_connection_t * att_connection){ 931 if (!att_write_callback) return; 932 (*att_write_callback)(att_connection->con_handle, 0, ATT_TRANSACTION_MODE_CANCEL, 0, NULL, 0); 933 } 934 935 // MARK: ATT_EXECUTE_WRITE_REQUEST 0x18 936 // NOTE: security has been verified by handle_prepare_write_request 937 static uint16_t handle_execute_write_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 938 uint8_t * response_buffer, uint16_t response_buffer_size){ 939 940 uint8_t request_type = ATT_EXECUTE_WRITE_REQUEST; 941 942 if (!att_write_callback) { 943 return setup_error_write_not_permitted(response_buffer, request_type, 0); 944 } 945 if (request_buffer[1]) { 946 // deliver queued errors 947 if (att_prepare_write_error_code){ 948 att_clear_transaction_queue(att_connection); 949 uint8_t error_code = att_prepare_write_error_code; 950 uint16_t handle = att_prepare_write_error_handle; 951 att_prepare_write_reset(); 952 return setup_error(response_buffer, request_type, handle, error_code); 953 } 954 (*att_write_callback)(att_connection->con_handle, 0, ATT_TRANSACTION_MODE_EXECUTE, 0, NULL, 0); 955 } else { 956 att_clear_transaction_queue(att_connection); 957 } 958 response_buffer[0] = ATT_EXECUTE_WRITE_RESPONSE; 959 return 1; 960 } 961 962 // MARK: ATT_WRITE_COMMAND 0x52 963 // Core 4.0, vol 3, part F, 3.4.5.3 964 // "No Error Response or Write Response shall be sent in response to this command" 965 static void handle_write_command(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, 966 uint8_t * response_buffer, uint16_t response_buffer_size){ 967 968 if (!att_write_callback) return; 969 uint16_t handle = little_endian_read_16(request_buffer, 1); 970 att_iterator_t it; 971 int ok = att_find_handle(&it, handle); 972 if (!ok) return; 973 if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) return; 974 if ((it.flags & ATT_PROPERTY_WRITE_WITHOUT_RESPONSE) == 0) return; 975 if (att_validate_security(att_connection, &it)) return; 976 (*att_write_callback)(att_connection->con_handle, handle, ATT_TRANSACTION_MODE_NONE, 0, request_buffer + 3, request_len - 3); 977 } 978 979 // MARK: helper for ATT_HANDLE_VALUE_NOTIFICATION and ATT_HANDLE_VALUE_INDICATION 980 static uint16_t prepare_handle_value(att_connection_t * att_connection, 981 uint16_t handle, 982 uint8_t *value, 983 uint16_t value_len, 984 uint8_t * response_buffer){ 985 little_endian_store_16(response_buffer, 1, handle); 986 if (value_len > att_connection->mtu - 3){ 987 value_len = att_connection->mtu - 3; 988 } 989 memcpy(&response_buffer[3], value, value_len); 990 return value_len + 3; 991 } 992 993 // MARK: ATT_HANDLE_VALUE_NOTIFICATION 0x1b 994 uint16_t att_prepare_handle_value_notification(att_connection_t * att_connection, 995 uint16_t handle, 996 uint8_t *value, 997 uint16_t value_len, 998 uint8_t * response_buffer){ 999 1000 response_buffer[0] = ATT_HANDLE_VALUE_NOTIFICATION; 1001 return prepare_handle_value(att_connection, handle, value, value_len, response_buffer); 1002 } 1003 1004 // MARK: ATT_HANDLE_VALUE_INDICATION 0x1d 1005 uint16_t att_prepare_handle_value_indication(att_connection_t * att_connection, 1006 uint16_t handle, 1007 uint8_t *value, 1008 uint16_t value_len, 1009 uint8_t * response_buffer){ 1010 1011 response_buffer[0] = ATT_HANDLE_VALUE_INDICATION; 1012 return prepare_handle_value(att_connection, handle, value, value_len, response_buffer); 1013 } 1014 1015 // MARK: Dispatcher 1016 uint16_t att_handle_request(att_connection_t * att_connection, 1017 uint8_t * request_buffer, 1018 uint16_t request_len, 1019 uint8_t * response_buffer){ 1020 uint16_t response_len = 0; 1021 uint16_t response_buffer_size = att_connection->mtu; 1022 1023 switch (request_buffer[0]){ 1024 case ATT_EXCHANGE_MTU_REQUEST: 1025 response_len = handle_exchange_mtu_request(att_connection, request_buffer, request_len, response_buffer); 1026 break; 1027 case ATT_FIND_INFORMATION_REQUEST: 1028 response_len = handle_find_information_request(att_connection, request_buffer, request_len,response_buffer, response_buffer_size); 1029 break; 1030 case ATT_FIND_BY_TYPE_VALUE_REQUEST: 1031 response_len = handle_find_by_type_value_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1032 break; 1033 case ATT_READ_BY_TYPE_REQUEST: 1034 response_len = handle_read_by_type_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1035 break; 1036 case ATT_READ_REQUEST: 1037 response_len = handle_read_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1038 break; 1039 case ATT_READ_BLOB_REQUEST: 1040 response_len = handle_read_blob_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1041 break; 1042 case ATT_READ_MULTIPLE_REQUEST: 1043 response_len = handle_read_multiple_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1044 break; 1045 case ATT_READ_BY_GROUP_TYPE_REQUEST: 1046 response_len = handle_read_by_group_type_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1047 break; 1048 case ATT_WRITE_REQUEST: 1049 response_len = handle_write_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1050 break; 1051 case ATT_PREPARE_WRITE_REQUEST: 1052 response_len = handle_prepare_write_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1053 break; 1054 case ATT_EXECUTE_WRITE_REQUEST: 1055 response_len = handle_execute_write_request(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1056 break; 1057 case ATT_WRITE_COMMAND: 1058 handle_write_command(att_connection, request_buffer, request_len, response_buffer, response_buffer_size); 1059 break; 1060 case ATT_SIGNED_WRITE_COMMAND: 1061 log_info("handle_signed_write_command preprocessed by att_server.c"); 1062 break; 1063 default: 1064 log_info("Unhandled ATT Command: %02X, DATA: ", request_buffer[0]); 1065 log_info_hexdump(&request_buffer[9], request_len-9); 1066 break; 1067 } 1068 return response_len; 1069 } 1070 1071 #if 0 1072 1073 // test profile 1074 #include "profile.h" 1075 1076 int main(void){ 1077 int acl_buffer_size; 1078 uint8_t acl_buffer[27]; 1079 att_set_db(profile_data); 1080 att_dump_attributes(); 1081 1082 uint8_t uuid_1[] = { 0x00, 0x18}; 1083 acl_buffer_size = handle_find_by_type_value_request2(acl_buffer, 19, 0, 0xffff, 0x2800, 2, (uint8_t *) &uuid_1); 1084 log_info_hexdump(acl_buffer, acl_buffer_size); 1085 1086 uint8_t uuid_3[] = { 0x00, 0x2a}; 1087 acl_buffer_size = handle_read_by_type_request2(acl_buffer, 19, 0, 0xffff, 2, (uint8_t *) &uuid_3); 1088 log_info_hexdump(acl_buffer, acl_buffer_size); 1089 1090 acl_buffer_size = handle_find_by_type_value_request2(acl_buffer, 19, 0, 0xffff, 0x2800, 2, (uint8_t *) &uuid_1); 1091 log_info_hexdump(acl_buffer, acl_buffer_size); 1092 1093 uint8_t uuid_4[] = { 0x00, 0x28}; 1094 acl_buffer_size = handle_read_by_group_type_request2(acl_buffer, 20, 0, 0xffff, 2, (uint8_t *) &uuid_4); 1095 log_info_hexdump(acl_buffer, acl_buffer_size); 1096 1097 acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 0, 0xffff); 1098 log_info_hexdump(acl_buffer, acl_buffer_size); 1099 acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 3, 0xffff); 1100 log_info_hexdump(acl_buffer, acl_buffer_size); 1101 acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 5, 0xffff); 1102 log_info_hexdump(acl_buffer, acl_buffer_size); 1103 1104 acl_buffer_size = handle_read_request2(acl_buffer, 19, 0x0003); 1105 log_info_hexdump(acl_buffer, acl_buffer_size); 1106 1107 return 0; 1108 } 1109 #endif 1110