JBBoardに接続したモーター2つをRCBControllerでコントロールするテストです。
Dependencies: FatFileSystem TB6612FNG2 mbed
Fork of JBB_BTLE_Test by
Diff: BTstack/att.c
- Revision:
- 1:6078e430af82
- Child:
- 6:cf06ba884429
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BTstack/att.c Thu Feb 07 14:01:01 2013 +0000 @@ -0,0 +1,913 @@ +/* + * Copyright (C) 2011-2012 by Matthias Ringwald + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS + * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS + * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * Please inquire about commercial licensing options at btstack@ringwald.ch + * + */ + +#include <stdio.h> +#include <string.h> + +#include "att.h" + +// from src/utils. +#define READ_BT_16( buffer, pos) ( ((uint16_t) buffer[pos]) | (((uint16_t)buffer[pos+1]) << 8)) + +// Buetooth Base UUID 00000000-0000-1000-8000-00805F9B34FB in little endian +static const uint8_t bluetooth_base_uuid[] = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + +static void bt_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ + buffer[pos++] = value; + buffer[pos++] = value >> 8; +} + +static void hexdump2(void const *data, int size){ + int i; + for (i=0; i<size;i++){ + printf("%02X ", ((uint8_t *)data)[i]); + } + printf("\n"); +} + +static void printUUID128(const uint8_t * uuid){ + int i; + for (i=15; i >= 0 ; i--){ + printf("%02X", uuid[i]); + switch (i){ + case 4: + case 6: + case 8: + case 10: + printf("-"); + break; + default: + break; + } + } +} + +static int is_Bluetooth_Base_UUID(uint8_t const *uuid){ + if (memcmp(&uuid[0], &bluetooth_base_uuid[0], 12)) return 0; + if (memcmp(&uuid[14], &bluetooth_base_uuid[14], 2)) return 0; + return 1; + +} + +// ATT Database +static uint8_t const * att_db = NULL; +static att_read_callback_t att_read_callback = NULL; +static att_write_callback_t att_write_callback = NULL; + +// new java-style iterator +typedef struct att_iterator { + // private + uint8_t const * att_ptr; + // public + uint16_t size; + uint16_t flags; + uint16_t handle; + uint8_t const * uuid; + uint16_t value_len; + uint8_t const * value; +} att_iterator_t; + +void att_iterator_init(att_iterator_t *it){ + it->att_ptr = att_db; +} + +int att_iterator_has_next(att_iterator_t *it){ + return it->att_ptr != NULL; +} + +void att_iterator_fetch_next(att_iterator_t *it){ + it->size = READ_BT_16(it->att_ptr, 0); + if (it->size == 0){ + it->flags = 0; + it->handle = 0; + it->uuid = NULL; + it->value_len = 0; + it->value = NULL; + it->att_ptr = NULL; + return; + } + it->flags = READ_BT_16(it->att_ptr, 2); + it->handle = READ_BT_16(it->att_ptr, 4); + it->uuid = &it->att_ptr[6]; + // handle 128 bit UUIDs + if (it->flags & ATT_PROPERTY_UUID128){ + it->value_len = it->size - 22; + it->value = &it->att_ptr[22]; + } else { + it->value_len = it->size - 8; + it->value = &it->att_ptr[8]; + } + // advance AFTER setting values + it->att_ptr += it->size; +} + +int att_iterator_match_uuid16(att_iterator_t *it, uint16_t uuid){ + if (it->handle == 0) return 0; + if (it->flags & ATT_PROPERTY_UUID128){ + if (!is_Bluetooth_Base_UUID(it->uuid)) return 0; + return READ_BT_16(it->uuid, 12) == uuid; + } + return READ_BT_16(it->uuid, 0) == uuid; +} + +int att_iterator_match_uuid(att_iterator_t *it, uint8_t *uuid, uint16_t uuid_len){ + if (it->handle == 0) return 0; + // input: UUID16 + if (uuid_len == 2) { + return att_iterator_match_uuid16(it, READ_BT_16(uuid, 0)); + } + // input and db: UUID128 + if (it->flags & ATT_PROPERTY_UUID128){ + return memcmp(it->uuid, uuid, 16) == 0; + } + // input: UUID128, db: UUID16 + if (!is_Bluetooth_Base_UUID(uuid)) return 0; + return READ_BT_16(uuid, 12) == READ_BT_16(it->uuid, 0); +} + + +int att_find_handle(att_iterator_t *it, uint16_t handle){ + att_iterator_init(it); + while (att_iterator_has_next(it)){ + att_iterator_fetch_next(it); + if (it->handle != handle) continue; + return 1; + } + return 0; +} + +static void att_update_value_len(att_iterator_t *it){ + if ((it->flags & ATT_PROPERTY_DYNAMIC) == 0 || !att_read_callback) return; + it->value_len = (*att_read_callback)(it->handle, 0, NULL, 0); + return; +} + +static int att_copy_value(att_iterator_t *it, uint16_t offset, uint8_t * buffer, uint16_t buffer_size){ + + // DYNAMIC + if ((it->flags & ATT_PROPERTY_DYNAMIC) && att_read_callback) { + return (*att_read_callback)(it->handle, offset, buffer, buffer_size); + } + + // STATIC + uint16_t bytes_to_copy = it->value_len; + if (bytes_to_copy > buffer_size){ + bytes_to_copy = buffer_size; + } + memcpy(buffer, it->value, bytes_to_copy); + return bytes_to_copy; +} + +void att_set_db(uint8_t const * db){ + att_db = db; +} + +void att_set_read_callback(att_read_callback_t callback){ + att_read_callback = callback; +} + +void att_set_write_callback(att_write_callback_t callback){ + att_write_callback = callback; +} + +void att_dump_attributes(void){ + att_iterator_t it; + att_iterator_init(&it); + while (att_iterator_has_next(&it)){ + att_iterator_fetch_next(&it); + if (it.handle == 0) { + printf("Handle: END\n"); + return; + } + printf("Handle: 0x%04x, flags: 0x%04x, uuid: ", it.handle, it.flags); + if (it.flags & ATT_PROPERTY_UUID128){ + printUUID128(it.uuid); + } else { + printf("%04x", READ_BT_16(it.uuid, 0)); + } + printf(", value_len: %u, value: ", it.value_len); + hexdump2(it.value, it.value_len); + } +} + +static uint16_t setup_error(uint8_t * response_buffer, uint16_t request, uint16_t handle, uint8_t error_code){ + response_buffer[0] = ATT_ERROR_RESPONSE; + response_buffer[1] = request; + bt_store_16(response_buffer, 2, handle); + response_buffer[4] = error_code; + return 5; +} + +static uint16_t setup_error_atribute_not_found(uint8_t * response_buffer, uint16_t request, uint16_t start_handle){ + return setup_error(response_buffer, request, start_handle, ATT_ERROR_ATTRIBUTE_NOT_FOUND); +} + +static uint16_t setup_error_invalid_handle(uint8_t * response_buffer, uint16_t request, uint16_t handle){ + return setup_error(response_buffer, request, handle, ATT_ERROR_ATTRIBUTE_INVALID); +} + +static uint16_t setup_error_invalid_offset(uint8_t * response_buffer, uint16_t request, uint16_t handle){ + return setup_error(response_buffer, request, handle, ATT_ERROR_INVALID_OFFSET); +} + +// +// MARK: ATT_EXCHANGE_MTU_REQUEST +// +static uint16_t handle_exchange_mtu_request(att_connection_t * att_connection, uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer){ + + uint16_t client_rx_mtu = READ_BT_16(request_buffer, 1); + if (client_rx_mtu < att_connection->mtu){ + att_connection->mtu = client_rx_mtu; + } + + response_buffer[0] = ATT_EXCHANGE_MTU_RESPONSE; + bt_store_16(response_buffer, 1, att_connection->mtu); + return 3; +} + + +// +// MARK: ATT_FIND_INFORMATION_REQUEST +// +// TODO: handle other types then GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID +// +static uint16_t handle_find_information_request2(uint8_t * response_buffer, uint16_t response_buffer_size, + uint16_t start_handle, uint16_t end_handle){ + + printf("ATT_FIND_INFORMATION_REQUEST: from %04X to %04X\n", start_handle, end_handle); + + uint16_t offset = 1; + uint16_t pair_len = 0; + + att_iterator_t it; + att_iterator_init(&it); + while (att_iterator_has_next(&it)){ + att_iterator_fetch_next(&it); + if (!it.handle) break; + if (it.handle > end_handle) break; + if (it.handle < start_handle) continue; + + att_update_value_len(&it); + + // printf("Handle 0x%04x\n", it.handle); + + // check if value has same len as last one + uint16_t this_pair_len = 2 + it.value_len; + if (offset > 1){ + if (pair_len != this_pair_len) { + break; + } + } + + // first + if (offset == 1) { + pair_len = this_pair_len; + if (it.value_len == 2) { + response_buffer[offset] = 0x01; // format + } else { + response_buffer[offset] = 0x02; + } + offset++; + } + + // space? + if (offset + pair_len > response_buffer_size) { + if (offset > 2) break; + it.value_len = response_buffer_size - 4; + } + + // store + bt_store_16(response_buffer, offset, it.handle); + offset += 2; + uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len); + offset += bytes_copied; + } + + if (offset == 1){ + return setup_error_atribute_not_found(response_buffer, ATT_FIND_INFORMATION_REQUEST, start_handle); + } + + response_buffer[0] = ATT_FIND_INFORMATION_REPLY; + return offset; +} + +static uint16_t handle_find_information_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + return handle_find_information_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1), READ_BT_16(request_buffer, 3)); +} + +// +// MARK: ATT_FIND_BY_TYPE_VALUE +// +// "Only attributes with attribute handles between and including the Starting Handle parameter +// and the Ending Handle parameter that match the requested attri- bute type and the attribute +// value that have sufficient permissions to allow reading will be returned" -> (1) +// +// TODO: handle other types then GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID +// +// NOTE: doesn't handle DYNAMIC values +// NOTE: only supports 16 bit UUIDs +// +static uint16_t handle_find_by_type_value_request2(uint8_t * response_buffer, uint16_t response_buffer_size, + uint16_t start_handle, uint16_t end_handle, + uint16_t attribute_type, uint16_t attribute_len, uint8_t* attribute_value){ + + printf("ATT_FIND_BY_TYPE_VALUE_REQUEST: from %04X to %04X, type %04X, value: ", start_handle, end_handle, attribute_type); + hexdump2(attribute_value, attribute_len); + + uint16_t offset = 1; + uint16_t in_group = 0; + uint16_t prev_handle = 0; + + att_iterator_t it; + att_iterator_init(&it); + while (att_iterator_has_next(&it)){ + att_iterator_fetch_next(&it); + + if (it.handle && it.handle < start_handle) continue; + if (it.handle > end_handle) break; // (1) + + // close current tag, if within a group and a new service definition starts or we reach end of att db + if (in_group && + (it.handle == 0 || att_iterator_match_uuid16(&it, GATT_PRIMARY_SERVICE_UUID) || att_iterator_match_uuid16(&it, GATT_SECONDARY_SERVICE_UUID))){ + + printf("End of group, handle 0x%04x\n", prev_handle); + bt_store_16(response_buffer, offset, prev_handle); + offset += 2; + in_group = 0; + + // check if space for another handle pair available + if (offset + 4 > response_buffer_size){ + break; + } + } + + // keep track of previous handle + prev_handle = it.handle; + + // does current attribute match + if (it.handle && att_iterator_match_uuid16(&it, attribute_type) && attribute_len == it.value_len && memcmp(attribute_value, it.value, it.value_len) == 0){ + printf("Begin of group, handle 0x%04x\n", it.handle); + bt_store_16(response_buffer, offset, it.handle); + offset += 2; + in_group = 1; + } + } + + if (offset == 1){ + return setup_error_atribute_not_found(response_buffer, ATT_FIND_BY_TYPE_VALUE_REQUEST, start_handle); + } + + response_buffer[0] = ATT_FIND_BY_TYPE_VALUE_RESPONSE; + return offset; +} + +static uint16_t handle_find_by_type_value_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + int attribute_len = request_len - 7; + return handle_find_by_type_value_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1), + READ_BT_16(request_buffer, 3), READ_BT_16(request_buffer, 5), attribute_len, &request_buffer[7]); +} + +// +// MARK: ATT_READ_BY_TYPE_REQUEST +// +static uint16_t handle_read_by_type_request2(uint8_t * response_buffer, uint16_t response_buffer_size, + uint16_t start_handle, uint16_t end_handle, + uint16_t attribute_type_len, uint8_t * attribute_type){ + + printf("ATT_READ_BY_TYPE_REQUEST: from %04X to %04X, type: ", start_handle, end_handle); + hexdump2(attribute_type, attribute_type_len); + + uint16_t offset = 1; + uint16_t pair_len = 0; + + att_iterator_t it; + att_iterator_init(&it); + while (att_iterator_has_next(&it)){ + att_iterator_fetch_next(&it); + + if (!it.handle) break; + if (it.handle < start_handle) continue; + if (it.handle > end_handle) break; // (1) + + // does current attribute match + if (!att_iterator_match_uuid(&it, attribute_type, attribute_type_len)) continue; + + att_update_value_len(&it); + + // check if value has same len as last one + uint16_t this_pair_len = 2 + it.value_len; + if (offset > 1){ + if (pair_len != this_pair_len) { + break; + } + } + + // first + if (offset == 1) { + pair_len = this_pair_len; + response_buffer[offset] = pair_len; + offset++; + } + + // space? + if (offset + pair_len > response_buffer_size) { + if (offset > 2) break; + it.value_len = response_buffer_size - 4; + } + + // store + bt_store_16(response_buffer, offset, it.handle); + offset += 2; + uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len); + offset += bytes_copied; + } + + if (offset == 1){ + return setup_error_atribute_not_found(response_buffer, ATT_READ_BY_TYPE_REQUEST, start_handle); + } + + response_buffer[0] = ATT_READ_BY_TYPE_RESPONSE; + return offset; +} + +static uint16_t handle_read_by_type_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + int attribute_type_len; + if (request_len <= 7){ + attribute_type_len = 2; + } else { + attribute_type_len = 16; + } + return handle_read_by_type_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1), READ_BT_16(request_buffer, 3), attribute_type_len, &request_buffer[5]); +} + +// +// MARK: ATT_READ_BY_TYPE_REQUEST +// +static uint16_t handle_read_request2(uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t handle){ + + printf("ATT_READ_REQUEST: handle %04x\n", handle); + + att_iterator_t it; + int ok = att_find_handle(&it, handle); + if (!ok){ + return setup_error_atribute_not_found(response_buffer, ATT_READ_REQUEST, handle); + } + + att_update_value_len(&it); + + uint16_t offset = 1; + // limit data + if (offset + it.value_len > response_buffer_size) { + it.value_len = response_buffer_size - 1; + } + + // store + uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len); + offset += bytes_copied; + + response_buffer[0] = ATT_READ_RESPONSE; + return offset; +} + +static uint16_t handle_read_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + return handle_read_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1)); +} + +// +// MARK: ATT_READ_BLOB_REQUEST 0x0c +// +static uint16_t handle_read_blob_request2(uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t handle, uint16_t value_offset){ + printf("ATT_READ_BLOB_REQUEST: handle %04x, offset %u\n", handle, value_offset); + + att_iterator_t it; + int ok = att_find_handle(&it, handle); + if (!ok){ + return setup_error_atribute_not_found(response_buffer, ATT_READ_BLOB_REQUEST, handle); + } + + att_update_value_len(&it); + + if (value_offset >= it.value_len){ + return setup_error_invalid_offset(response_buffer, ATT_READ_BLOB_REQUEST, handle); + } + + // limit data + uint16_t offset = 1; + if (offset + it.value_len - value_offset > response_buffer_size) { + it.value_len = response_buffer_size - 1 + value_offset; + } + + // store + uint16_t bytes_copied = att_copy_value(&it, value_offset, response_buffer + offset, it.value_len - value_offset); + offset += bytes_copied; + + response_buffer[0] = ATT_READ_BLOB_RESPONSE; + return offset; +} + +uint16_t handle_read_blob_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + return handle_read_blob_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1), READ_BT_16(request_buffer, 3)); +} + +// +// MARK: ATT_READ_MULTIPLE_REQUEST 0x0e +// +static uint16_t handle_read_multiple_request2(uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t num_handles, uint16_t * handles){ + printf("ATT_READ_MULTIPLE_REQUEST: num handles %u\n", num_handles); + + uint16_t offset = 1; + + int i; + for (i=0;i<num_handles;i++){ + uint16_t handle = handles[i]; + + if (handle == 0){ + return setup_error_invalid_handle(response_buffer, ATT_READ_MULTIPLE_REQUEST, handle); + } + + att_iterator_t it; + + int ok = att_find_handle(&it, handle); + if (!ok){ + return setup_error_invalid_handle(response_buffer, ATT_READ_MULTIPLE_REQUEST, handle); + } + + att_update_value_len(&it); + + // limit data + if (offset + it.value_len > response_buffer_size) { + it.value_len = response_buffer_size - 1; + } + + // store + uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len); + offset += bytes_copied; + } + + response_buffer[0] = ATT_READ_MULTIPLE_RESPONSE; + return offset; +} +uint16_t handle_read_multiple_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + int num_handles = (request_len - 1) >> 1; + return handle_read_multiple_request2(response_buffer, response_buffer_size, num_handles, (uint16_t*) &request_buffer[1]); +} + +// +// MARK: ATT_READ_BY_GROUP_TYPE_REQUEST 0x10 +// +// TODO: handle other types then GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID +// +// NOTE: doesn't handle DYNAMIC values +// +static uint16_t handle_read_by_group_type_request2(uint8_t * response_buffer, uint16_t response_buffer_size, + uint16_t start_handle, uint16_t end_handle, + uint16_t attribute_type_len, uint8_t * attribute_type){ + + printf("ATT_READ_BY_GROUP_TYPE_REQUEST: from %04X to %04X, buffer size %u, type: ", start_handle, end_handle, response_buffer_size); + hexdump2(attribute_type, attribute_type_len); + + uint16_t offset = 1; + uint16_t pair_len = 0; + uint16_t in_group = 0; + uint16_t group_start_handle = 0; + uint8_t const * group_start_value = NULL; + uint16_t prev_handle = 0; + + att_iterator_t it; + att_iterator_init(&it); + while (att_iterator_has_next(&it)){ + att_iterator_fetch_next(&it); + + if (it.handle && it.handle < start_handle) continue; + if (it.handle > end_handle) break; // (1) + + // close current tag, if within a group and a new service definition starts or we reach end of att db + if (in_group && + (it.handle == 0 || att_iterator_match_uuid16(&it, GATT_PRIMARY_SERVICE_UUID) || att_iterator_match_uuid16(&it, GATT_SECONDARY_SERVICE_UUID))){ + // TODO: check if handle is included in start/end range + // printf("End of group, handle 0x%04x, val_len: %u\n", prev_handle, pair_len - 4); + + bt_store_16(response_buffer, offset, group_start_handle); + offset += 2; + bt_store_16(response_buffer, offset, prev_handle); + offset += 2; + memcpy(response_buffer + offset, group_start_value, pair_len - 4); + offset += pair_len - 4; + in_group = 0; + + // check if space for another handle pair available + if (offset + pair_len > response_buffer_size){ + break; + } + } + + // keep track of previous handle + prev_handle = it.handle; + + // does current attribute match + // printf("compare: %04x == %04x\n", *(uint16_t*) context->attribute_type, *(uint16_t*) uuid); + if (it.handle && att_iterator_match_uuid(&it, attribute_type, attribute_type_len)) { + + // check if value has same len as last one + uint16_t this_pair_len = 4 + it.value_len; + if (offset > 1){ + if (this_pair_len != pair_len) { + break; + } + } + + // printf("Begin of group, handle 0x%04x\n", it.handle); + + // first + if (offset == 1) { + pair_len = this_pair_len; + response_buffer[offset] = this_pair_len; + offset++; + } + + group_start_handle = it.handle; + group_start_value = it.value; + in_group = 1; + } + } + + if (offset == 1){ + return setup_error_atribute_not_found(response_buffer, ATT_READ_BY_GROUP_TYPE_REQUEST, start_handle); + } + + response_buffer[0] = ATT_READ_BY_GROUP_TYPE_RESPONSE; + return offset; +} +uint16_t handle_read_by_group_type_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + int attribute_type_len; + if (request_len <= 7){ + attribute_type_len = 2; + } else { + attribute_type_len = 16; + } + return handle_read_by_group_type_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1), READ_BT_16(request_buffer, 3), attribute_type_len, &request_buffer[5]); +} + +// +// MARK: ATT_WRITE_REQUEST 0x12 +static uint16_t handle_write_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + uint16_t handle = READ_BT_16(request_buffer, 1); + if (!att_write_callback) { + // TODO: Use "Write Not Permitted" + return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle); + } + att_iterator_t it; + int ok = att_find_handle(&it, handle); + if (!ok) { + return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle); + } + if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) { + // TODO: Use "Write Not Permitted" + return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle); + } + (*att_write_callback)(handle, ATT_TRANSACTION_MODE_NONE, 0, request_buffer + 3, request_len - 3, NULL); + response_buffer[0] = ATT_WRITE_RESPONSE; + return 1; +} + +// +// MARK: ATT_PREPARE_WRITE_REQUEST 0x16 +static uint16_t handle_prepare_write_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + uint16_t handle = READ_BT_16(request_buffer, 1); + if (!att_write_callback) { + // TODO: Use "Write Not Permitted" + return setup_error_atribute_not_found(response_buffer, ATT_PREPARE_WRITE_REQUEST, handle); + } + att_iterator_t it; + int ok = att_find_handle(&it, handle); + if (!ok) { + return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle); + } + if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) { + // TODO: Use "Write Not Permitted" + return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle); + } + (*att_write_callback)(handle, ATT_TRANSACTION_MODE_ACTIVE, 0, request_buffer + 3, request_len - 3, NULL); + + // response: echo request + memcpy(response_buffer, request_buffer, request_len); + response_buffer[0] = ATT_PREPARE_WRITE_RESPONSE; + return request_len; +} + +// MARK: ATT_EXECUTE_WRITE_REQUEST 0x18 +static uint16_t handle_execute_write_request(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + if (!att_write_callback) { + // TODO: Use "Write Not Permitted" + return setup_error_atribute_not_found(response_buffer, ATT_EXECUTE_WRITE_REQUEST, 0); + } + if (request_buffer[1]) { + (*att_write_callback)(0, ATT_TRANSACTION_MODE_EXECUTE, 0, request_buffer + 3, request_len - 3, NULL); + } else { + (*att_write_callback)(0, ATT_TRANSACTION_MODE_CANCEL, 0, request_buffer + 3, request_len - 3, NULL); + } + response_buffer[0] = ATT_EXECUTE_WRITE_RESPONSE; + return 1; +} + +// MARK: ATT_WRITE_COMMAND 0x52 +static void handle_write_command(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + if (!att_write_callback) return; + uint16_t handle = READ_BT_16(request_buffer, 1); + att_iterator_t it; + int ok = att_find_handle(&it, handle); + if (!ok) return; + if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) return; + (*att_write_callback)(handle, ATT_TRANSACTION_MODE_NONE, 0, request_buffer + 3, request_len - 3, NULL); +} + +// MARK: ATT_SIGNED_WRITE_COMAND 0xD2 +static void handle_signed_write_command(uint8_t * request_buffer, uint16_t request_len, + uint8_t * response_buffer, uint16_t response_buffer_size){ + + if (request_len < 15) return; + if (!att_write_callback) return; + uint16_t handle = READ_BT_16(request_buffer, 1); + att_iterator_t it; + int ok = att_find_handle(&it, handle); + if (!ok) return; + if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) return; + (*att_write_callback)(handle, ATT_TRANSACTION_MODE_NONE, 0, request_buffer + 3, request_len - 3 - 12, (signature_t *) request_buffer + request_len - 12); +} + +// MARK: helper for ATT_HANDLE_VALUE_NOTIFICATION and ATT_HANDLE_VALUE_INDICATION +static uint16_t prepare_handle_value(att_connection_t * att_connection, + uint16_t handle, + uint8_t *value, + uint16_t value_len, + uint8_t * response_buffer){ + bt_store_16(response_buffer, 1, handle); + if (value_len > att_connection->mtu - 3){ + value_len = att_connection->mtu - 3; + } + memcpy(&response_buffer[3], value, value_len); + return value_len + 3; +} + +// MARK: ATT_HANDLE_VALUE_NOTIFICATION 0x1b +uint16_t att_prepare_handle_value_notification(att_connection_t * att_connection, + uint16_t handle, + uint8_t *value, + uint16_t value_len, + uint8_t * response_buffer){ + + response_buffer[0] = ATT_HANDLE_VALUE_NOTIFICATION; + return prepare_handle_value(att_connection, handle, value, value_len, response_buffer); +} + +// MARK: ATT_HANDLE_VALUE_INDICATION 0x1d +uint16_t att_prepare_handle_value_indication(att_connection_t * att_connection, + uint16_t handle, + uint8_t *value, + uint16_t value_len, + uint8_t * response_buffer){ + + response_buffer[0] = ATT_HANDLE_VALUE_INDICATION; + return prepare_handle_value(att_connection, handle, value, value_len, response_buffer); +} + +// MARK: Dispatcher +uint16_t att_handle_request(att_connection_t * att_connection, + uint8_t * request_buffer, + uint16_t request_len, + uint8_t * response_buffer){ + uint16_t response_len = 0; + uint16_t response_buffer_size = att_connection->mtu; + + switch (request_buffer[0]){ + case ATT_EXCHANGE_MTU_REQUEST: + response_len = handle_exchange_mtu_request(att_connection, request_buffer, request_len, response_buffer); + break; + case ATT_FIND_INFORMATION_REQUEST: + response_len = handle_find_information_request(request_buffer, request_len,response_buffer, response_buffer_size); + break; + case ATT_FIND_BY_TYPE_VALUE_REQUEST: + response_len = handle_find_by_type_value_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_READ_BY_TYPE_REQUEST: + response_len = handle_read_by_type_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_READ_REQUEST: + response_len = handle_read_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_READ_BLOB_REQUEST: + response_len = handle_read_blob_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_READ_MULTIPLE_REQUEST: + response_len = handle_read_multiple_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_READ_BY_GROUP_TYPE_REQUEST: + response_len = handle_read_by_group_type_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_WRITE_REQUEST: + response_len = handle_write_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_PREPARE_WRITE_REQUEST: + response_len = handle_prepare_write_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_EXECUTE_WRITE_REQUEST: + response_len = handle_execute_write_request(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_WRITE_COMMAND: + handle_write_command(request_buffer, request_len, response_buffer, response_buffer_size); + break; + case ATT_SIGNED_WRITE_COMAND: + handle_signed_write_command(request_buffer, request_len, response_buffer, response_buffer_size); + break; + default: + printf("Unhandled ATT Command: %02X, DATA: ", request_buffer[0]); + hexdump2(&request_buffer[9], request_len-9); + break; + } + return response_len; +} + +#if 0 + +// test profile +#include "profile.h" + +int main(){ + int acl_buffer_size; + uint8_t acl_buffer[27]; + att_set_db(profile_data); + att_dump_attributes(); + + uint8_t uuid_1[] = { 0x00, 0x18}; + acl_buffer_size = handle_find_by_type_value_request2(acl_buffer, 19, 0, 0xffff, 0x2800, 2, (uint8_t *) &uuid_1); + hexdump2(acl_buffer, acl_buffer_size); + + uint8_t uuid_3[] = { 0x00, 0x2a}; + acl_buffer_size = handle_read_by_type_request2(acl_buffer, 19, 0, 0xffff, 2, (uint8_t *) &uuid_3); + hexdump2(acl_buffer, acl_buffer_size); + + acl_buffer_size = handle_find_by_type_value_request2(acl_buffer, 19, 0, 0xffff, 0x2800, 2, (uint8_t *) &uuid_1); + hexdump2(acl_buffer, acl_buffer_size); + + uint8_t uuid_4[] = { 0x00, 0x28}; + acl_buffer_size = handle_read_by_group_type_request2(acl_buffer, 20, 0, 0xffff, 2, (uint8_t *) &uuid_4); + hexdump2(acl_buffer, acl_buffer_size); + + acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 0, 0xffff); + hexdump2(acl_buffer, acl_buffer_size); + acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 3, 0xffff); + hexdump2(acl_buffer, acl_buffer_size); + acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 5, 0xffff); + hexdump2(acl_buffer, acl_buffer_size); + + acl_buffer_size = handle_read_request2(acl_buffer, 19, 0x0003); + hexdump2(acl_buffer, acl_buffer_size); + + return 0; +} +#endif