att.c

00001 /*
00002  * Copyright (C) 2011-2012 by Matthias Ringwald
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions
00006  * are met:
00007  *
00008  * 1. Redistributions of source code must retain the above copyright
00009  *    notice, this list of conditions and the following disclaimer.
00010  * 2. Redistributions in binary form must reproduce the above copyright
00011  *    notice, this list of conditions and the following disclaimer in the
00012  *    documentation and/or other materials provided with the distribution.
00013  * 3. Neither the name of the copyright holders nor the names of
00014  *    contributors may be used to endorse or promote products derived
00015  *    from this software without specific prior written permission.
00016  * 4. Any redistribution, use, or modification is done solely for
00017  *    personal benefit and not for any commercial purpose or for
00018  *    monetary gain.
00019  *
00020  * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
00021  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
00024  * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
00027  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
00028  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00029  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
00030  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
00031  * SUCH DAMAGE.
00032  *
00033  * Please inquire about commercial licensing options at btstack@ringwald.ch
00034  *
00035  */
00036 
00037 #include <stdio.h>
00038 #include <string.h>
00039 
00040 #include "att.h"
00041 #include "debug.h"
00042 
00043 // from src/utils.
00044 #define READ_BT_16( buffer, pos) ( ((uint16_t) buffer[pos]) | (((uint16_t)buffer[pos+1]) << 8))
00045 
00046 // Buetooth Base UUID 00000000-0000-1000-8000-00805F9B34FB in little endian
00047 static const uint8_t bluetooth_base_uuid[] = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
00048 
00049 static void bt_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){
00050     buffer[pos++] = value;
00051     buffer[pos++] = value >> 8;
00052 }
00053 
00054 static void hexdump2(void const *data, int size){
00055     int i;
00056     for (i=0; i<size;i++){
00057         log_info("%02X ", ((uint8_t *)data)[i]);
00058     }
00059     log_info("\n");
00060 }
00061 
00062 static void printUUID128(const uint8_t * uuid){
00063     int i;
00064     for (i=15; i >= 0 ; i--){
00065         log_info("%02X", uuid[i]);
00066         switch (i){
00067             case 4:
00068             case 6:
00069             case 8:
00070             case 10:
00071                 log_info("-");
00072                 break;
00073             default:
00074                 break;
00075         }
00076     }
00077 }
00078 
00079 static int is_Bluetooth_Base_UUID(uint8_t const *uuid){
00080     if (memcmp(&uuid[0],  &bluetooth_base_uuid[0], 12)) return 0;
00081     if (memcmp(&uuid[14], &bluetooth_base_uuid[14], 2)) return 0;
00082     return 1;
00083     
00084 }
00085 
00086 // ATT Database
00087 static uint8_t const * att_db = NULL;
00088 static att_read_callback_t  att_read_callback  = NULL;
00089 static att_write_callback_t att_write_callback = NULL;
00090 
00091 // new java-style iterator
00092 typedef struct att_iterator {
00093     // private
00094     uint8_t const * att_ptr;
00095     // public
00096     uint16_t size;
00097     uint16_t flags;
00098     uint16_t handle;
00099     uint8_t  const * uuid;
00100     uint16_t value_len;
00101     uint8_t  const * value;
00102 } att_iterator_t;
00103 
00104 void att_iterator_init(att_iterator_t *it){
00105     it->att_ptr = att_db;
00106 }
00107 
00108 int att_iterator_has_next(att_iterator_t *it){
00109     return it->att_ptr != NULL;
00110 }
00111 
00112 void att_iterator_fetch_next(att_iterator_t *it){
00113     it->size   = READ_BT_16(it->att_ptr, 0);
00114     if (it->size == 0){
00115         it->flags = 0;
00116         it->handle = 0;
00117         it->uuid = NULL;
00118         it->value_len = 0;
00119         it->value = NULL;
00120         it->att_ptr = NULL;
00121         return;
00122     }
00123     it->flags  = READ_BT_16(it->att_ptr, 2);
00124     it->handle = READ_BT_16(it->att_ptr, 4);
00125     it->uuid   = &it->att_ptr[6];
00126     // handle 128 bit UUIDs
00127     if (it->flags & ATT_PROPERTY_UUID128){
00128         it->value_len = it->size - 22;
00129         it->value  = &it->att_ptr[22];
00130     } else {
00131         it->value_len = it->size - 8;
00132         it->value  = &it->att_ptr[8];
00133     }
00134     // advance AFTER setting values
00135     it->att_ptr += it->size;
00136 }
00137 
00138 int att_iterator_match_uuid16(att_iterator_t *it, uint16_t uuid){
00139     if (it->handle == 0) return 0;
00140     if (it->flags & ATT_PROPERTY_UUID128){
00141         if (!is_Bluetooth_Base_UUID(it->uuid)) return 0;
00142         return READ_BT_16(it->uuid, 12) == uuid;
00143     }
00144     return READ_BT_16(it->uuid, 0)  == uuid;
00145 }
00146 
00147 int att_iterator_match_uuid(att_iterator_t *it, uint8_t *uuid, uint16_t uuid_len){
00148     if (it->handle == 0) return 0;
00149     // input: UUID16
00150     if (uuid_len == 2) {
00151         return att_iterator_match_uuid16(it, READ_BT_16(uuid, 0));
00152     }
00153     // input and db: UUID128 
00154     if (it->flags & ATT_PROPERTY_UUID128){
00155         return memcmp(it->uuid, uuid, 16) == 0;
00156     }
00157     // input: UUID128, db: UUID16
00158     if (!is_Bluetooth_Base_UUID(uuid)) return 0;
00159     return READ_BT_16(uuid, 12) == READ_BT_16(it->uuid, 0);
00160 }
00161 
00162 
00163 int att_find_handle(att_iterator_t *it, uint16_t handle){
00164     att_iterator_init(it);
00165     while (att_iterator_has_next(it)){
00166         att_iterator_fetch_next(it);
00167         if (it->handle != handle) continue;
00168         return 1;
00169     }
00170     return 0;
00171 }
00172 
00173 static void att_update_value_len(att_iterator_t *it){
00174     if ((it->flags & ATT_PROPERTY_DYNAMIC) == 0 || !att_read_callback) return;
00175     it->value_len = (*att_read_callback)(it->handle, 0, NULL, 0);
00176     return;
00177 }
00178 
00179 static int att_copy_value(att_iterator_t *it, uint16_t offset, uint8_t * buffer, uint16_t buffer_size){
00180     
00181     // DYNAMIC 
00182     if ((it->flags & ATT_PROPERTY_DYNAMIC) && att_read_callback) {
00183         return (*att_read_callback)(it->handle, offset, buffer, buffer_size);
00184     }
00185     
00186     // STATIC
00187     uint16_t bytes_to_copy = it->value_len;
00188     if (bytes_to_copy > buffer_size){
00189         bytes_to_copy = buffer_size;
00190     }
00191     memcpy(buffer, it->value, bytes_to_copy);
00192     return bytes_to_copy;
00193 }
00194 
00195 void att_set_db(uint8_t const * db){
00196     att_db = db;
00197 }
00198 
00199 void att_set_read_callback(att_read_callback_t callback){
00200     att_read_callback = callback;
00201 }
00202 
00203 void att_set_write_callback(att_write_callback_t callback){
00204     att_write_callback = callback;
00205 }
00206 
00207 void att_dump_attributes(void){
00208     att_iterator_t it;
00209     att_iterator_init(&it);
00210     while (att_iterator_has_next(&it)){
00211         att_iterator_fetch_next(&it);
00212         if (it.handle == 0) {
00213             log_info("Handle: END\n");
00214             return;
00215         }
00216         log_info("Handle: 0x%04x, flags: 0x%04x, uuid: ", it.handle, it.flags);
00217         if (it.flags & ATT_PROPERTY_UUID128){
00218             printUUID128(it.uuid);
00219         } else {
00220             log_info("%04x", READ_BT_16(it.uuid, 0));
00221         }
00222         log_info(", value_len: %u, value: ", it.value_len);
00223         hexdump2(it.value, it.value_len);
00224     }
00225 }
00226 
00227 static uint16_t setup_error(uint8_t * response_buffer, uint16_t request, uint16_t handle, uint8_t error_code){
00228     response_buffer[0] = ATT_ERROR_RESPONSE;
00229     response_buffer[1] = request;
00230     bt_store_16(response_buffer, 2, handle);
00231     response_buffer[4] = error_code;
00232     return 5;
00233 }
00234 
00235 static uint16_t setup_error_atribute_not_found(uint8_t * response_buffer, uint16_t request, uint16_t start_handle){
00236     return setup_error(response_buffer, request, start_handle, ATT_ERROR_ATTRIBUTE_NOT_FOUND);
00237 }
00238 
00239 static uint16_t setup_error_invalid_handle(uint8_t * response_buffer, uint16_t request, uint16_t handle){
00240     return setup_error(response_buffer, request, handle, ATT_ERROR_ATTRIBUTE_INVALID);
00241 }
00242 
00243 static uint16_t setup_error_invalid_offset(uint8_t * response_buffer, uint16_t request, uint16_t handle){
00244     return setup_error(response_buffer, request, handle, ATT_ERROR_INVALID_OFFSET);
00245 }
00246 
00247 //
00248 // MARK: ATT_EXCHANGE_MTU_REQUEST
00249 //
00250 static uint16_t handle_exchange_mtu_request(att_connection_t * att_connection, uint8_t * request_buffer,  uint16_t request_len,
00251                                          uint8_t * response_buffer){
00252 
00253     uint16_t client_rx_mtu = READ_BT_16(request_buffer, 1);
00254     if (client_rx_mtu < att_connection->mtu){
00255         att_connection->mtu = client_rx_mtu;
00256     }
00257     
00258     response_buffer[0] = ATT_EXCHANGE_MTU_RESPONSE;
00259     bt_store_16(response_buffer, 1, att_connection->mtu);
00260     return 3;
00261 }
00262 
00263 
00264 //
00265 // MARK: ATT_FIND_INFORMATION_REQUEST
00266 //
00267 // TODO: handle other types then GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID
00268 //
00269 static uint16_t handle_find_information_request2(uint8_t * response_buffer, uint16_t response_buffer_size,
00270                                            uint16_t start_handle, uint16_t end_handle){
00271     
00272     log_info("ATT_FIND_INFORMATION_REQUEST: from %04X to %04X\n", start_handle, end_handle);
00273     
00274     uint16_t offset   = 1;
00275     uint16_t pair_len = 0;
00276     
00277     att_iterator_t it;
00278     att_iterator_init(&it);
00279     while (att_iterator_has_next(&it)){
00280         att_iterator_fetch_next(&it);
00281         if (!it.handle) break;
00282         if (it.handle > end_handle) break;
00283         if (it.handle < start_handle) continue;
00284         
00285         att_update_value_len(&it);
00286         
00287         // log_debug("Handle 0x%04x\n", it.handle);
00288         
00289         // check if value has same len as last one
00290         uint16_t this_pair_len = 2 + it.value_len;
00291         if (offset > 1){
00292             if (pair_len != this_pair_len) {
00293                 break;
00294             }
00295         }
00296         
00297         // first
00298         if (offset == 1) {
00299             pair_len = this_pair_len;
00300             if (it.value_len == 2) {
00301                 response_buffer[offset] = 0x01; // format
00302             } else {
00303                 response_buffer[offset] = 0x02;
00304             }
00305             offset++;
00306         }
00307         
00308         // space?
00309         if (offset + pair_len > response_buffer_size) {
00310             if (offset > 2) break;
00311             it.value_len = response_buffer_size - 4;
00312         }
00313         
00314         // store
00315         bt_store_16(response_buffer, offset, it.handle);
00316         offset += 2;
00317         uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len);
00318         offset += bytes_copied;
00319     }
00320     
00321     if (offset == 1){
00322         return setup_error_atribute_not_found(response_buffer, ATT_FIND_INFORMATION_REQUEST, start_handle);
00323     }
00324     
00325     response_buffer[0] = ATT_FIND_INFORMATION_REPLY;
00326     return offset;
00327 }
00328 
00329 static uint16_t handle_find_information_request(uint8_t * request_buffer,  uint16_t request_len,
00330                                          uint8_t * response_buffer, uint16_t response_buffer_size){
00331     return handle_find_information_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1), READ_BT_16(request_buffer, 3));
00332 }
00333 
00334 //
00335 // MARK: ATT_FIND_BY_TYPE_VALUE
00336 //
00337 // "Only attributes with attribute handles between and including the Starting Handle parameter
00338 // and the Ending Handle parameter that match the requested attri- bute type and the attribute
00339 // value that have sufficient permissions to allow reading will be returned" -> (1)
00340 //
00341 // TODO: handle other types then GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID
00342 //
00343 // NOTE: doesn't handle DYNAMIC values
00344 // NOTE: only supports 16 bit UUIDs
00345 // 
00346 static uint16_t handle_find_by_type_value_request2(uint8_t * response_buffer, uint16_t response_buffer_size,
00347                                            uint16_t start_handle, uint16_t end_handle,
00348                                            uint16_t attribute_type, uint16_t attribute_len, uint8_t* attribute_value){
00349     
00350     log_info("ATT_FIND_BY_TYPE_VALUE_REQUEST: from %04X to %04X, type %04X, value: ", start_handle, end_handle, attribute_type);
00351     hexdump2(attribute_value, attribute_len);
00352     
00353     uint16_t offset      = 1;
00354     uint16_t in_group    = 0;
00355     uint16_t prev_handle = 0;
00356     
00357     att_iterator_t it;
00358     att_iterator_init(&it);
00359     while (att_iterator_has_next(&it)){
00360         att_iterator_fetch_next(&it);
00361         
00362         if (it.handle && it.handle < start_handle) continue;
00363         if (it.handle > end_handle) break;  // (1)
00364         
00365         // close current tag, if within a group and a new service definition starts or we reach end of att db
00366         if (in_group &&
00367             (it.handle == 0 || att_iterator_match_uuid16(&it, GATT_PRIMARY_SERVICE_UUID) || att_iterator_match_uuid16(&it, GATT_SECONDARY_SERVICE_UUID))){
00368             
00369             log_info("End of group, handle 0x%04x\n", prev_handle);
00370             bt_store_16(response_buffer, offset, prev_handle);
00371             offset += 2;
00372             in_group = 0;
00373             
00374             // check if space for another handle pair available
00375             if (offset + 4 > response_buffer_size){
00376                 break;
00377             }
00378         }
00379         
00380         // keep track of previous handle
00381         prev_handle = it.handle;
00382         
00383         // does current attribute match
00384         if (it.handle && att_iterator_match_uuid16(&it, attribute_type) && attribute_len == it.value_len && memcmp(attribute_value, it.value, it.value_len) == 0){
00385             log_info("Begin of group, handle 0x%04x\n", it.handle);
00386             bt_store_16(response_buffer, offset, it.handle);
00387             offset += 2;
00388             in_group = 1;
00389         }
00390     }
00391     
00392     if (offset == 1){
00393         return setup_error_atribute_not_found(response_buffer, ATT_FIND_BY_TYPE_VALUE_REQUEST, start_handle);
00394     }
00395     
00396     response_buffer[0] = ATT_FIND_BY_TYPE_VALUE_RESPONSE;
00397     return offset;
00398 }
00399                                          
00400 static uint16_t handle_find_by_type_value_request(uint8_t * request_buffer,  uint16_t request_len,
00401                                            uint8_t * response_buffer, uint16_t response_buffer_size){
00402     int attribute_len = request_len - 7;
00403     return handle_find_by_type_value_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1),
00404                                               READ_BT_16(request_buffer, 3), READ_BT_16(request_buffer, 5), attribute_len, &request_buffer[7]);
00405 }
00406                                                                                   
00407 //
00408 // MARK: ATT_READ_BY_TYPE_REQUEST
00409 //
00410 static uint16_t handle_read_by_type_request2(uint8_t * response_buffer, uint16_t response_buffer_size,
00411                                       uint16_t start_handle, uint16_t end_handle,
00412                                       uint16_t attribute_type_len, uint8_t * attribute_type){
00413     
00414     log_info("ATT_READ_BY_TYPE_REQUEST: from %04X to %04X, type: ", start_handle, end_handle); 
00415     hexdump2(attribute_type, attribute_type_len);
00416     
00417     uint16_t offset   = 1;
00418     uint16_t pair_len = 0;
00419 
00420     att_iterator_t it;
00421     att_iterator_init(&it);
00422     while (att_iterator_has_next(&it)){
00423         att_iterator_fetch_next(&it);
00424         
00425         if (!it.handle) break;
00426         if (it.handle < start_handle) continue;
00427         if (it.handle > end_handle) break;  // (1)
00428 
00429         // does current attribute match
00430         if (!att_iterator_match_uuid(&it, attribute_type, attribute_type_len)) continue;
00431         
00432         att_update_value_len(&it);
00433         
00434         // check if value has same len as last one
00435         uint16_t this_pair_len = 2 + it.value_len;
00436         if (offset > 1){
00437             if (pair_len != this_pair_len) {
00438                 break;
00439             }
00440         }
00441         
00442         // first
00443         if (offset == 1) {
00444             pair_len = this_pair_len;
00445             response_buffer[offset] = pair_len;
00446             offset++;
00447         }
00448         
00449         // space?
00450         if (offset + pair_len > response_buffer_size) {
00451             if (offset > 2) break;
00452             it.value_len = response_buffer_size - 4;
00453         }
00454         
00455         // store
00456         bt_store_16(response_buffer, offset, it.handle);
00457         offset += 2;
00458         uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len);
00459         offset += bytes_copied;
00460     }
00461     
00462     if (offset == 1){
00463         return setup_error_atribute_not_found(response_buffer, ATT_READ_BY_TYPE_REQUEST, start_handle);
00464     }
00465     
00466     response_buffer[0] = ATT_READ_BY_TYPE_RESPONSE;
00467     return offset;
00468 }
00469 
00470 static uint16_t handle_read_by_type_request(uint8_t * request_buffer,  uint16_t request_len,
00471                                      uint8_t * response_buffer, uint16_t response_buffer_size){
00472     int attribute_type_len;
00473     if (request_len <= 7){
00474         attribute_type_len = 2;
00475     } else {
00476         attribute_type_len = 16;
00477     }
00478     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]);
00479 }
00480 
00481 //
00482 // MARK: ATT_READ_BY_TYPE_REQUEST
00483 //
00484 static uint16_t handle_read_request2(uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t handle){
00485     
00486     log_info("ATT_READ_REQUEST: handle %04x\n", handle);
00487     
00488     att_iterator_t it;
00489     int ok = att_find_handle(&it, handle);
00490     if (!ok){
00491         return setup_error_atribute_not_found(response_buffer, ATT_READ_REQUEST, handle);
00492     }
00493 
00494     att_update_value_len(&it);
00495 
00496     uint16_t offset   = 1;
00497     // limit data
00498     if (offset + it.value_len > response_buffer_size) {
00499         it.value_len = response_buffer_size - 1;
00500     }
00501     
00502     // store
00503     uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len);
00504     offset += bytes_copied;
00505     
00506     response_buffer[0] = ATT_READ_RESPONSE;
00507     return offset;
00508 }
00509 
00510 static uint16_t handle_read_request(uint8_t * request_buffer,  uint16_t request_len,
00511                              uint8_t * response_buffer, uint16_t response_buffer_size){
00512     return handle_read_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1));
00513 }
00514 
00515 //
00516 // MARK: ATT_READ_BLOB_REQUEST 0x0c
00517 //
00518 static uint16_t handle_read_blob_request2(uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t handle, uint16_t value_offset){
00519     log_info("ATT_READ_BLOB_REQUEST: handle %04x, offset %u\n", handle, value_offset);
00520 
00521     att_iterator_t it;
00522     int ok = att_find_handle(&it, handle);
00523     if (!ok){
00524         return setup_error_atribute_not_found(response_buffer, ATT_READ_BLOB_REQUEST, handle);
00525     }
00526     
00527     att_update_value_len(&it);
00528 
00529     if (value_offset >= it.value_len){
00530         return setup_error_invalid_offset(response_buffer, ATT_READ_BLOB_REQUEST, handle);
00531     }
00532     
00533     // limit data
00534     uint16_t offset   = 1;
00535     if (offset + it.value_len - value_offset > response_buffer_size) {
00536         it.value_len = response_buffer_size - 1 + value_offset;
00537     }
00538     
00539     // store
00540     uint16_t bytes_copied = att_copy_value(&it, value_offset, response_buffer + offset, it.value_len - value_offset);
00541     offset += bytes_copied;
00542     
00543     response_buffer[0] = ATT_READ_BLOB_RESPONSE;
00544     return offset;
00545 }
00546 
00547 uint16_t handle_read_blob_request(uint8_t * request_buffer,  uint16_t request_len,
00548                                   uint8_t * response_buffer, uint16_t response_buffer_size){
00549     return handle_read_blob_request2(response_buffer, response_buffer_size, READ_BT_16(request_buffer, 1), READ_BT_16(request_buffer, 3));
00550 }
00551 
00552 //
00553 // MARK: ATT_READ_MULTIPLE_REQUEST 0x0e
00554 //
00555 static uint16_t handle_read_multiple_request2(uint8_t * response_buffer, uint16_t response_buffer_size, uint16_t num_handles, uint16_t * handles){
00556     log_info("ATT_READ_MULTIPLE_REQUEST: num handles %u\n", num_handles);
00557     
00558     uint16_t offset   = 1;
00559 
00560     int i;
00561     for (i=0;i<num_handles;i++){
00562         uint16_t handle = handles[i];
00563         
00564         if (handle == 0){
00565             return setup_error_invalid_handle(response_buffer, ATT_READ_MULTIPLE_REQUEST, handle);
00566         }
00567         
00568         att_iterator_t it;
00569 
00570         int ok = att_find_handle(&it, handle);
00571         if (!ok){
00572             return setup_error_invalid_handle(response_buffer, ATT_READ_MULTIPLE_REQUEST, handle);
00573         }
00574 
00575         att_update_value_len(&it);
00576         
00577         // limit data
00578         if (offset + it.value_len > response_buffer_size) {
00579             it.value_len = response_buffer_size - 1;
00580         }
00581         
00582         // store
00583         uint16_t bytes_copied = att_copy_value(&it, 0, response_buffer + offset, it.value_len);
00584         offset += bytes_copied;
00585     }
00586     
00587     response_buffer[0] = ATT_READ_MULTIPLE_RESPONSE;
00588     return offset;
00589 }
00590 uint16_t handle_read_multiple_request(uint8_t * request_buffer,  uint16_t request_len,
00591                                       uint8_t * response_buffer, uint16_t response_buffer_size){
00592     int num_handles = (request_len - 1) >> 1;
00593     return handle_read_multiple_request2(response_buffer, response_buffer_size, num_handles, (uint16_t*) &request_buffer[1]);
00594 }
00595 
00596 //
00597 // MARK: ATT_READ_BY_GROUP_TYPE_REQUEST 0x10
00598 //
00599 // TODO: handle other types then GATT_PRIMARY_SERVICE_UUID and GATT_SECONDARY_SERVICE_UUID
00600 //
00601 // NOTE: doesn't handle DYNAMIC values
00602 //
00603 static uint16_t handle_read_by_group_type_request2(uint8_t * response_buffer, uint16_t response_buffer_size,
00604                                             uint16_t start_handle, uint16_t end_handle,
00605                                             uint16_t attribute_type_len, uint8_t * attribute_type){
00606     
00607     log_info("ATT_READ_BY_GROUP_TYPE_REQUEST: from %04X to %04X, buffer size %u, type: ", start_handle, end_handle, response_buffer_size);
00608     hexdump2(attribute_type, attribute_type_len);
00609     
00610     uint16_t offset   = 1;
00611     uint16_t pair_len = 0;
00612     uint16_t in_group = 0;
00613     uint16_t group_start_handle = 0;
00614     uint8_t const * group_start_value = NULL;
00615     uint16_t prev_handle = 0;
00616 
00617     att_iterator_t it;
00618     att_iterator_init(&it);
00619     while (att_iterator_has_next(&it)){
00620         att_iterator_fetch_next(&it);
00621         
00622         if (it.handle && it.handle < start_handle) continue;
00623         if (it.handle > end_handle) break;  // (1)
00624         
00625         // close current tag, if within a group and a new service definition starts or we reach end of att db
00626         if (in_group &&
00627             (it.handle == 0 || att_iterator_match_uuid16(&it, GATT_PRIMARY_SERVICE_UUID) || att_iterator_match_uuid16(&it, GATT_SECONDARY_SERVICE_UUID))){
00628             // TODO: check if handle is included in start/end range
00629             // log_debug("End of group, handle 0x%04x, val_len: %u\n", prev_handle, pair_len - 4);
00630             
00631             bt_store_16(response_buffer, offset, group_start_handle);
00632             offset += 2;
00633             bt_store_16(response_buffer, offset, prev_handle);
00634             offset += 2;
00635             memcpy(response_buffer + offset, group_start_value, pair_len - 4);
00636             offset += pair_len - 4;
00637             in_group = 0;
00638             
00639             // check if space for another handle pair available
00640             if (offset + pair_len > response_buffer_size){
00641                 break;
00642             }
00643         }
00644         
00645         // keep track of previous handle
00646         prev_handle = it.handle;
00647         
00648         // does current attribute match
00649         // log_debug("compare: %04x == %04x\n", *(uint16_t*) context->attribute_type, *(uint16_t*) uuid);
00650         if (it.handle && att_iterator_match_uuid(&it, attribute_type, attribute_type_len)) {
00651             
00652             // check if value has same len as last one
00653             uint16_t this_pair_len = 4 + it.value_len;
00654             if (offset > 1){
00655                 if (this_pair_len != pair_len) {
00656                     break;
00657                 }
00658             }
00659             
00660             // log_debug("Begin of group, handle 0x%04x\n", it.handle);
00661             
00662             // first
00663             if (offset == 1) {
00664                 pair_len = this_pair_len;
00665                 response_buffer[offset] = this_pair_len;
00666                 offset++;
00667             }
00668             
00669             group_start_handle = it.handle;
00670             group_start_value  = it.value;
00671             in_group = 1;
00672         }
00673     }        
00674     
00675     if (offset == 1){
00676         return setup_error_atribute_not_found(response_buffer, ATT_READ_BY_GROUP_TYPE_REQUEST, start_handle);
00677     }
00678     
00679     response_buffer[0] = ATT_READ_BY_GROUP_TYPE_RESPONSE;
00680     return offset;
00681 }
00682 uint16_t handle_read_by_group_type_request(uint8_t * request_buffer,  uint16_t request_len,
00683                                            uint8_t * response_buffer, uint16_t response_buffer_size){
00684     int attribute_type_len;
00685     if (request_len <= 7){
00686         attribute_type_len = 2;
00687     } else {
00688         attribute_type_len = 16;
00689     }
00690     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]);
00691 }
00692 
00693 //
00694 // MARK: ATT_WRITE_REQUEST 0x12
00695 static uint16_t handle_write_request(uint8_t * request_buffer,  uint16_t request_len,
00696                               uint8_t * response_buffer, uint16_t response_buffer_size){
00697     uint16_t handle = READ_BT_16(request_buffer, 1);
00698     if (!att_write_callback) {
00699         // TODO: Use "Write Not Permitted"
00700         return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle);
00701     }
00702     att_iterator_t it;
00703     int ok = att_find_handle(&it, handle);
00704     if (!ok) {
00705         return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle);
00706     }
00707     if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) {
00708         // TODO: Use "Write Not Permitted"
00709         return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle);
00710     }
00711     (*att_write_callback)(handle, ATT_TRANSACTION_MODE_NONE, 0, request_buffer + 3, request_len - 3, NULL);
00712     response_buffer[0] = ATT_WRITE_RESPONSE;
00713     return 1;
00714 }
00715 
00716 //
00717 // MARK: ATT_PREPARE_WRITE_REQUEST 0x16
00718 static uint16_t handle_prepare_write_request(uint8_t * request_buffer,  uint16_t request_len,
00719                                       uint8_t * response_buffer, uint16_t response_buffer_size){
00720     uint16_t handle = READ_BT_16(request_buffer, 1);
00721     if (!att_write_callback) {
00722         // TODO: Use "Write Not Permitted"
00723         return setup_error_atribute_not_found(response_buffer, ATT_PREPARE_WRITE_REQUEST, handle);
00724     }
00725     att_iterator_t it;
00726     int ok = att_find_handle(&it, handle);
00727     if (!ok) {
00728         return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle);
00729     }
00730     if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) {
00731         // TODO: Use "Write Not Permitted"
00732         return setup_error_atribute_not_found(response_buffer, ATT_WRITE_REQUEST, handle);
00733     }
00734     (*att_write_callback)(handle, ATT_TRANSACTION_MODE_ACTIVE, 0, request_buffer + 3, request_len - 3, NULL);
00735     
00736     // response: echo request
00737     memcpy(response_buffer, request_buffer, request_len);
00738     response_buffer[0] = ATT_PREPARE_WRITE_RESPONSE;
00739     return request_len;
00740 }
00741 
00742 // MARK: ATT_EXECUTE_WRITE_REQUEST 0x18
00743 static uint16_t handle_execute_write_request(uint8_t * request_buffer,  uint16_t request_len,
00744                                       uint8_t * response_buffer, uint16_t response_buffer_size){
00745     if (!att_write_callback) {
00746         // TODO: Use "Write Not Permitted"
00747         return setup_error_atribute_not_found(response_buffer, ATT_EXECUTE_WRITE_REQUEST, 0);
00748     }
00749     if (request_buffer[1]) {
00750         (*att_write_callback)(0, ATT_TRANSACTION_MODE_EXECUTE, 0, request_buffer + 3, request_len - 3, NULL);
00751     } else {
00752         (*att_write_callback)(0, ATT_TRANSACTION_MODE_CANCEL, 0, request_buffer + 3, request_len - 3, NULL);
00753     }
00754     response_buffer[0] = ATT_EXECUTE_WRITE_RESPONSE;
00755     return 1;
00756 }
00757 
00758 // MARK: ATT_WRITE_COMMAND 0x52
00759 static void handle_write_command(uint8_t * request_buffer,  uint16_t request_len,
00760                                            uint8_t * response_buffer, uint16_t response_buffer_size){
00761     if (!att_write_callback) return;
00762     uint16_t handle = READ_BT_16(request_buffer, 1);
00763     att_iterator_t it;
00764     int ok = att_find_handle(&it, handle);
00765     if (!ok) return;
00766     if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) return;
00767     (*att_write_callback)(handle, ATT_TRANSACTION_MODE_NONE, 0, request_buffer + 3, request_len - 3, NULL);
00768 }
00769 
00770 // MARK: ATT_SIGNED_WRITE_COMAND 0xD2
00771 static void handle_signed_write_command(uint8_t * request_buffer,  uint16_t request_len,
00772                                  uint8_t * response_buffer, uint16_t response_buffer_size){
00773 
00774     if (request_len < 15) return;
00775     if (!att_write_callback) return;
00776     uint16_t handle = READ_BT_16(request_buffer, 1);
00777     att_iterator_t it;
00778     int ok = att_find_handle(&it, handle);
00779     if (!ok) return;
00780     if ((it.flags & ATT_PROPERTY_DYNAMIC) == 0) return;
00781     (*att_write_callback)(handle, ATT_TRANSACTION_MODE_NONE, 0, request_buffer + 3, request_len - 3 - 12, (signature_t *) request_buffer + request_len - 12);
00782 }
00783 
00784 // MARK: helper for ATT_HANDLE_VALUE_NOTIFICATION and ATT_HANDLE_VALUE_INDICATION
00785 static uint16_t prepare_handle_value(att_connection_t * att_connection,
00786                                      uint16_t handle,
00787                                      uint8_t *value,
00788                                      uint16_t value_len, 
00789                                      uint8_t * response_buffer){
00790     bt_store_16(response_buffer, 1, handle);
00791     if (value_len > att_connection->mtu - 3){
00792         value_len = att_connection->mtu - 3;
00793     }
00794     memcpy(&response_buffer[3], value, value_len);
00795     return value_len + 3;
00796 }
00797 
00798 // MARK: ATT_HANDLE_VALUE_NOTIFICATION 0x1b
00799 uint16_t att_prepare_handle_value_notification(att_connection_t * att_connection,
00800                                                uint16_t handle,
00801                                                uint8_t *value,
00802                                                uint16_t value_len, 
00803                                                uint8_t * response_buffer){
00804 
00805     response_buffer[0] = ATT_HANDLE_VALUE_NOTIFICATION;
00806     return prepare_handle_value(att_connection, handle, value, value_len, response_buffer);
00807 }
00808 
00809 // MARK: ATT_HANDLE_VALUE_INDICATION 0x1d
00810 uint16_t att_prepare_handle_value_indication(att_connection_t * att_connection,
00811                                              uint16_t handle,
00812                                              uint8_t *value,
00813                                              uint16_t value_len, 
00814                                              uint8_t * response_buffer){
00815 
00816     response_buffer[0] = ATT_HANDLE_VALUE_INDICATION;
00817     return prepare_handle_value(att_connection, handle, value, value_len, response_buffer);
00818 }
00819     
00820 // MARK: Dispatcher
00821 uint16_t att_handle_request(att_connection_t * att_connection,
00822                             uint8_t * request_buffer,
00823                             uint16_t request_len,
00824                             uint8_t * response_buffer){
00825     uint16_t response_len = 0;
00826     uint16_t response_buffer_size = att_connection->mtu;
00827     
00828     switch (request_buffer[0]){
00829         case ATT_EXCHANGE_MTU_REQUEST:
00830             response_len = handle_exchange_mtu_request(att_connection, request_buffer, request_len, response_buffer);
00831             break;
00832         case ATT_FIND_INFORMATION_REQUEST:
00833             response_len = handle_find_information_request(request_buffer, request_len,response_buffer, response_buffer_size);
00834             break;
00835         case ATT_FIND_BY_TYPE_VALUE_REQUEST:
00836             response_len = handle_find_by_type_value_request(request_buffer, request_len, response_buffer, response_buffer_size);
00837             break;
00838         case ATT_READ_BY_TYPE_REQUEST:  
00839             response_len = handle_read_by_type_request(request_buffer, request_len, response_buffer, response_buffer_size);
00840             break;
00841         case ATT_READ_REQUEST:  
00842             response_len = handle_read_request(request_buffer, request_len, response_buffer, response_buffer_size);
00843             break;
00844         case ATT_READ_BLOB_REQUEST:  
00845             response_len = handle_read_blob_request(request_buffer, request_len, response_buffer, response_buffer_size);
00846             break;
00847         case ATT_READ_MULTIPLE_REQUEST:  
00848             response_len = handle_read_multiple_request(request_buffer, request_len, response_buffer, response_buffer_size);
00849             break;
00850         case ATT_READ_BY_GROUP_TYPE_REQUEST:  
00851             response_len = handle_read_by_group_type_request(request_buffer, request_len, response_buffer, response_buffer_size);
00852             break;
00853         case ATT_WRITE_REQUEST:
00854             response_len = handle_write_request(request_buffer, request_len, response_buffer, response_buffer_size);
00855             break;
00856         case ATT_PREPARE_WRITE_REQUEST:
00857             response_len = handle_prepare_write_request(request_buffer, request_len, response_buffer, response_buffer_size);
00858             break;
00859         case ATT_EXECUTE_WRITE_REQUEST:
00860             response_len = handle_execute_write_request(request_buffer, request_len, response_buffer, response_buffer_size);
00861             break;
00862         case ATT_WRITE_COMMAND:
00863             handle_write_command(request_buffer, request_len, response_buffer, response_buffer_size);
00864             break;
00865         case ATT_SIGNED_WRITE_COMAND:
00866             handle_signed_write_command(request_buffer, request_len, response_buffer, response_buffer_size);
00867             break;
00868         default:
00869             log_info("Unhandled ATT Command: %02X, DATA: ", request_buffer[0]);
00870             hexdump2(&request_buffer[9], request_len-9);
00871             break;
00872     }
00873     return response_len;
00874 }
00875 
00876 #if 0
00877 
00878 // test profile
00879 #include "profile.h"
00880 
00881 int main(){
00882     int acl_buffer_size;
00883     uint8_t acl_buffer[27];
00884     att_set_db(profile_data);
00885     att_dump_attributes();
00886 
00887     uint8_t uuid_1[] = { 0x00, 0x18};
00888     acl_buffer_size = handle_find_by_type_value_request2(acl_buffer, 19, 0, 0xffff, 0x2800, 2, (uint8_t *) &uuid_1);
00889     hexdump2(acl_buffer, acl_buffer_size);
00890     
00891     uint8_t uuid_3[] = { 0x00, 0x2a};
00892     acl_buffer_size = handle_read_by_type_request2(acl_buffer, 19, 0, 0xffff, 2, (uint8_t *) &uuid_3);
00893     hexdump2(acl_buffer, acl_buffer_size);
00894         
00895     acl_buffer_size = handle_find_by_type_value_request2(acl_buffer, 19, 0, 0xffff, 0x2800, 2, (uint8_t *) &uuid_1);
00896     hexdump2(acl_buffer, acl_buffer_size);
00897 
00898     uint8_t uuid_4[] = { 0x00, 0x28};
00899     acl_buffer_size = handle_read_by_group_type_request2(acl_buffer, 20, 0, 0xffff, 2, (uint8_t *) &uuid_4);
00900     hexdump2(acl_buffer, acl_buffer_size);
00901     
00902     acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 0, 0xffff);
00903     hexdump2(acl_buffer, acl_buffer_size);
00904     acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 3, 0xffff);
00905     hexdump2(acl_buffer, acl_buffer_size);
00906     acl_buffer_size = handle_find_information_request2(acl_buffer, 20, 5, 0xffff);
00907     hexdump2(acl_buffer, acl_buffer_size);
00908 
00909     acl_buffer_size = handle_read_request2(acl_buffer, 19, 0x0003);
00910     hexdump2(acl_buffer, acl_buffer_size);
00911 
00912     return 0;
00913 }
00914 #endif