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