M tamaki
mbed base bard check program for BlueTooth USB dongle module (3 switches, 6 leds, I2C LCD, A/D)
Fork of
BTstack
by 
Norimasa Okamoto
BTstack/sdp.c@3:7b7d1273e2d5, 2016-10-17 (annotated)
- Committer:
- tamaki
- Date:
- Mon Oct 17 00:25:18 2016 +0000
- Revision:
- 3:7b7d1273e2d5
- Parent:
- 0:1ed23ab1345f
mbed base bard check program
; for BlueTooth USB dongle module
; (3 switches, 6 leds, I2C LCD, A/D)
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| va009039 | 0:1ed23ab1345f | 1 | /* |
| va009039 | 0:1ed23ab1345f | 2 | * Copyright (C) 2009-2012 by Matthias Ringwald |
| va009039 | 0:1ed23ab1345f | 3 | * |
| va009039 | 0:1ed23ab1345f | 4 | * Redistribution and use in source and binary forms, with or without |
| va009039 | 0:1ed23ab1345f | 5 | * modification, are permitted provided that the following conditions |
| va009039 | 0:1ed23ab1345f | 6 | * are met: |
| va009039 | 0:1ed23ab1345f | 7 | * |
| va009039 | 0:1ed23ab1345f | 8 | * 1. Redistributions of source code must retain the above copyright |
| va009039 | 0:1ed23ab1345f | 9 | * notice, this list of conditions and the following disclaimer. |
| va009039 | 0:1ed23ab1345f | 10 | * 2. Redistributions in binary form must reproduce the above copyright |
| va009039 | 0:1ed23ab1345f | 11 | * notice, this list of conditions and the following disclaimer in the |
| va009039 | 0:1ed23ab1345f | 12 | * documentation and/or other materials provided with the distribution. |
| va009039 | 0:1ed23ab1345f | 13 | * 3. Neither the name of the copyright holders nor the names of |
| va009039 | 0:1ed23ab1345f | 14 | * contributors may be used to endorse or promote products derived |
| va009039 | 0:1ed23ab1345f | 15 | * from this software without specific prior written permission. |
| va009039 | 0:1ed23ab1345f | 16 | * 4. Any redistribution, use, or modification is done solely for |
| va009039 | 0:1ed23ab1345f | 17 | * personal benefit and not for any commercial purpose or for |
| va009039 | 0:1ed23ab1345f | 18 | * monetary gain. |
| va009039 | 0:1ed23ab1345f | 19 | * |
| va009039 | 0:1ed23ab1345f | 20 | * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS |
| va009039 | 0:1ed23ab1345f | 21 | * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| va009039 | 0:1ed23ab1345f | 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| va009039 | 0:1ed23ab1345f | 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS |
| va009039 | 0:1ed23ab1345f | 24 | * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| va009039 | 0:1ed23ab1345f | 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| va009039 | 0:1ed23ab1345f | 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
| va009039 | 0:1ed23ab1345f | 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
| va009039 | 0:1ed23ab1345f | 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
| va009039 | 0:1ed23ab1345f | 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF |
| va009039 | 0:1ed23ab1345f | 30 | * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
| va009039 | 0:1ed23ab1345f | 31 | * SUCH DAMAGE. |
| va009039 | 0:1ed23ab1345f | 32 | * |
| va009039 | 0:1ed23ab1345f | 33 | * Please inquire about commercial licensing options at btstack@ringwald.ch |
| va009039 | 0:1ed23ab1345f | 34 | * |
| va009039 | 0:1ed23ab1345f | 35 | */ |
| va009039 | 0:1ed23ab1345f | 36 | |
| va009039 | 0:1ed23ab1345f | 37 | /* |
| va009039 | 0:1ed23ab1345f | 38 | * Implementation of the Service Discovery Protocol Server |
| va009039 | 0:1ed23ab1345f | 39 | */ |
| va009039 | 0:1ed23ab1345f | 40 | |
| va009039 | 0:1ed23ab1345f | 41 | #include "sdp.h" |
| va009039 | 0:1ed23ab1345f | 42 | |
| va009039 | 0:1ed23ab1345f | 43 | |
| va009039 | 0:1ed23ab1345f | 44 | #include <stdio.h> |
| va009039 | 0:1ed23ab1345f | 45 | #include <string.h> |
| va009039 | 0:1ed23ab1345f | 46 | |
| va009039 | 0:1ed23ab1345f | 47 | #include <btstack/sdp_util.h> |
| va009039 | 0:1ed23ab1345f | 48 | |
| va009039 | 0:1ed23ab1345f | 49 | #include "hci_dump.h" |
| va009039 | 0:1ed23ab1345f | 50 | #include "l2cap.h" |
| va009039 | 0:1ed23ab1345f | 51 | |
| va009039 | 0:1ed23ab1345f | 52 | #include "debug.h" |
| va009039 | 0:1ed23ab1345f | 53 | |
| va009039 | 0:1ed23ab1345f | 54 | // max reserved ServiceRecordHandle |
| va009039 | 0:1ed23ab1345f | 55 | #define maxReservedServiceRecordHandle 0xffff |
| va009039 | 0:1ed23ab1345f | 56 | |
| va009039 | 0:1ed23ab1345f | 57 | // max SDP response |
| va009039 | 0:1ed23ab1345f | 58 | #define SDP_RESPONSE_BUFFER_SIZE (HCI_ACL_BUFFER_SIZE-HCI_ACL_HEADER_SIZE) |
| va009039 | 0:1ed23ab1345f | 59 | |
| va009039 | 0:1ed23ab1345f | 60 | static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); |
| va009039 | 0:1ed23ab1345f | 61 | |
| va009039 | 0:1ed23ab1345f | 62 | // registered service records |
| va009039 | 0:1ed23ab1345f | 63 | static linked_list_t sdp_service_records = NULL; |
| va009039 | 0:1ed23ab1345f | 64 | |
| va009039 | 0:1ed23ab1345f | 65 | // our handles start after the reserved range |
| va009039 | 0:1ed23ab1345f | 66 | static uint32_t sdp_next_service_record_handle = maxReservedServiceRecordHandle + 2; |
| va009039 | 0:1ed23ab1345f | 67 | |
| va009039 | 0:1ed23ab1345f | 68 | static uint8_t sdp_response_buffer[SDP_RESPONSE_BUFFER_SIZE]; |
| va009039 | 0:1ed23ab1345f | 69 | |
| va009039 | 0:1ed23ab1345f | 70 | static void (*app_packet_handler)(void * connection, uint8_t packet_type, |
| va009039 | 0:1ed23ab1345f | 71 | uint16_t channel, uint8_t *packet, uint16_t size) = NULL; |
| va009039 | 0:1ed23ab1345f | 72 | |
| va009039 | 0:1ed23ab1345f | 73 | static uint16_t l2cap_cid = 0; |
| va009039 | 0:1ed23ab1345f | 74 | static uint16_t sdp_response_size = 0; |
| va009039 | 0:1ed23ab1345f | 75 | |
| va009039 | 0:1ed23ab1345f | 76 | void sdp_init(){ |
| va009039 | 0:1ed23ab1345f | 77 | // register with l2cap psm sevices - max MTU |
| va009039 | 0:1ed23ab1345f | 78 | l2cap_register_service_internal(NULL, sdp_packet_handler, PSM_SDP, 0xffff); |
| va009039 | 0:1ed23ab1345f | 79 | } |
| va009039 | 0:1ed23ab1345f | 80 | |
| va009039 | 0:1ed23ab1345f | 81 | // register packet handler |
| va009039 | 0:1ed23ab1345f | 82 | void sdp_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, |
| va009039 | 0:1ed23ab1345f | 83 | uint16_t channel, uint8_t *packet, uint16_t size)){ |
| va009039 | 0:1ed23ab1345f | 84 | app_packet_handler = handler; |
| va009039 | 0:1ed23ab1345f | 85 | l2cap_cid = 0; |
| va009039 | 0:1ed23ab1345f | 86 | } |
| va009039 | 0:1ed23ab1345f | 87 | |
| va009039 | 0:1ed23ab1345f | 88 | uint32_t sdp_get_service_record_handle(uint8_t * record){ |
| va009039 | 0:1ed23ab1345f | 89 | uint8_t * serviceRecordHandleAttribute = sdp_get_attribute_value_for_attribute_id(record, SDP_ServiceRecordHandle); |
| va009039 | 0:1ed23ab1345f | 90 | if (!serviceRecordHandleAttribute) return 0; |
| va009039 | 0:1ed23ab1345f | 91 | if (de_get_element_type(serviceRecordHandleAttribute) != DE_UINT) return 0; |
| va009039 | 0:1ed23ab1345f | 92 | if (de_get_size_type(serviceRecordHandleAttribute) != DE_SIZE_32) return 0; |
| va009039 | 0:1ed23ab1345f | 93 | return READ_NET_32(serviceRecordHandleAttribute, 1); |
| va009039 | 0:1ed23ab1345f | 94 | } |
| va009039 | 0:1ed23ab1345f | 95 | |
| va009039 | 0:1ed23ab1345f | 96 | // data: event(8), len(8), status(8), service_record_handle(32) |
| va009039 | 0:1ed23ab1345f | 97 | static void sdp_emit_service_registered(void *connection, uint32_t handle, uint8_t status) { |
| va009039 | 0:1ed23ab1345f | 98 | if (!app_packet_handler) return; |
| va009039 | 0:1ed23ab1345f | 99 | uint8_t event[7]; |
| va009039 | 0:1ed23ab1345f | 100 | event[0] = SDP_SERVICE_REGISTERED; |
| va009039 | 0:1ed23ab1345f | 101 | event[1] = sizeof(event) - 2; |
| va009039 | 0:1ed23ab1345f | 102 | event[2] = status; |
| va009039 | 0:1ed23ab1345f | 103 | bt_store_32(event, 3, handle); |
| va009039 | 0:1ed23ab1345f | 104 | hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); |
| va009039 | 0:1ed23ab1345f | 105 | (*app_packet_handler)(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event)); |
| va009039 | 0:1ed23ab1345f | 106 | } |
| va009039 | 0:1ed23ab1345f | 107 | |
| va009039 | 0:1ed23ab1345f | 108 | service_record_item_t * sdp_get_record_for_handle(uint32_t handle){ |
| va009039 | 0:1ed23ab1345f | 109 | linked_item_t *it; |
| va009039 | 0:1ed23ab1345f | 110 | for (it = (linked_item_t *) sdp_service_records; it ; it = it->next){ |
| va009039 | 0:1ed23ab1345f | 111 | service_record_item_t * item = (service_record_item_t *) it; |
| va009039 | 0:1ed23ab1345f | 112 | if (item->service_record_handle == handle){ |
| va009039 | 0:1ed23ab1345f | 113 | return item; |
| va009039 | 0:1ed23ab1345f | 114 | } |
| va009039 | 0:1ed23ab1345f | 115 | } |
| va009039 | 0:1ed23ab1345f | 116 | return NULL; |
| va009039 | 0:1ed23ab1345f | 117 | } |
| va009039 | 0:1ed23ab1345f | 118 | |
| va009039 | 0:1ed23ab1345f | 119 | // get next free, unregistered service record handle |
| va009039 | 0:1ed23ab1345f | 120 | uint32_t sdp_create_service_record_handle(void){ |
| va009039 | 0:1ed23ab1345f | 121 | uint32_t handle = 0; |
| va009039 | 0:1ed23ab1345f | 122 | do { |
| va009039 | 0:1ed23ab1345f | 123 | handle = sdp_next_service_record_handle++; |
| va009039 | 0:1ed23ab1345f | 124 | if (sdp_get_record_for_handle(handle)) handle = 0; |
| va009039 | 0:1ed23ab1345f | 125 | } while (handle == 0); |
| va009039 | 0:1ed23ab1345f | 126 | return handle; |
| va009039 | 0:1ed23ab1345f | 127 | } |
| va009039 | 0:1ed23ab1345f | 128 | |
| va009039 | 0:1ed23ab1345f | 129 | #ifdef EMBEDDED |
| va009039 | 0:1ed23ab1345f | 130 | |
| va009039 | 0:1ed23ab1345f | 131 | // register service record internally - this special version doesn't copy the record, it should not be freeed |
| va009039 | 0:1ed23ab1345f | 132 | // pre: AttributeIDs are in ascending order |
| va009039 | 0:1ed23ab1345f | 133 | // pre: ServiceRecordHandle is first attribute and valid |
| va009039 | 0:1ed23ab1345f | 134 | // pre: record |
| va009039 | 0:1ed23ab1345f | 135 | // @returns ServiceRecordHandle or 0 if registration failed |
| va009039 | 0:1ed23ab1345f | 136 | uint32_t sdp_register_service_internal(void *connection, service_record_item_t * record_item){ |
| va009039 | 0:1ed23ab1345f | 137 | // get user record handle |
| va009039 | 0:1ed23ab1345f | 138 | uint32_t record_handle = record_item->service_record_handle; |
| va009039 | 0:1ed23ab1345f | 139 | // get actual record |
| va009039 | 0:1ed23ab1345f | 140 | uint8_t *record = record_item->service_record; |
| va009039 | 0:1ed23ab1345f | 141 | |
| va009039 | 0:1ed23ab1345f | 142 | // check for ServiceRecordHandle attribute, returns pointer or null |
| va009039 | 0:1ed23ab1345f | 143 | uint8_t * req_record_handle = sdp_get_attribute_value_for_attribute_id(record, SDP_ServiceRecordHandle); |
| va009039 | 0:1ed23ab1345f | 144 | if (!req_record_handle) { |
| va009039 | 0:1ed23ab1345f | 145 | log_error("SDP Error - record does not contain ServiceRecordHandle attribute\n"); |
| va009039 | 0:1ed23ab1345f | 146 | return 0; |
| va009039 | 0:1ed23ab1345f | 147 | } |
| va009039 | 0:1ed23ab1345f | 148 | |
| va009039 | 0:1ed23ab1345f | 149 | // validate service record handle is not in reserved range |
| va009039 | 0:1ed23ab1345f | 150 | if (record_handle <= maxReservedServiceRecordHandle) record_handle = 0; |
| va009039 | 0:1ed23ab1345f | 151 | |
| va009039 | 0:1ed23ab1345f | 152 | // check if already in use |
| va009039 | 0:1ed23ab1345f | 153 | if (record_handle) { |
| va009039 | 0:1ed23ab1345f | 154 | if (sdp_get_record_for_handle(record_handle)) { |
| va009039 | 0:1ed23ab1345f | 155 | record_handle = 0; |
| va009039 | 0:1ed23ab1345f | 156 | } |
| va009039 | 0:1ed23ab1345f | 157 | } |
| va009039 | 0:1ed23ab1345f | 158 | |
| va009039 | 0:1ed23ab1345f | 159 | // create new handle if needed |
| va009039 | 0:1ed23ab1345f | 160 | if (!record_handle){ |
| va009039 | 0:1ed23ab1345f | 161 | record_handle = sdp_create_service_record_handle(); |
| va009039 | 0:1ed23ab1345f | 162 | // Write the handle back into the record too |
| va009039 | 0:1ed23ab1345f | 163 | record_item->service_record_handle = record_handle; |
| va009039 | 0:1ed23ab1345f | 164 | sdp_set_attribute_value_for_attribute_id(record, SDP_ServiceRecordHandle, record_handle); |
| va009039 | 0:1ed23ab1345f | 165 | } |
| va009039 | 0:1ed23ab1345f | 166 | |
| va009039 | 0:1ed23ab1345f | 167 | // add to linked list |
| va009039 | 0:1ed23ab1345f | 168 | linked_list_add(&sdp_service_records, (linked_item_t *) record_item); |
| va009039 | 0:1ed23ab1345f | 169 | |
| va009039 | 0:1ed23ab1345f | 170 | sdp_emit_service_registered(connection, 0, record_item->service_record_handle); |
| va009039 | 0:1ed23ab1345f | 171 | |
| va009039 | 0:1ed23ab1345f | 172 | return record_handle; |
| va009039 | 0:1ed23ab1345f | 173 | } |
| va009039 | 0:1ed23ab1345f | 174 | |
| va009039 | 0:1ed23ab1345f | 175 | #else |
| va009039 | 0:1ed23ab1345f | 176 | |
| va009039 | 0:1ed23ab1345f | 177 | // AttributeIDList used to remove ServiceRecordHandle |
| va009039 | 0:1ed23ab1345f | 178 | static const uint8_t removeServiceRecordHandleAttributeIDList[] = { 0x36, 0x00, 0x05, 0x0A, 0x00, 0x01, 0xFF, 0xFF }; |
| va009039 | 0:1ed23ab1345f | 179 | |
| va009039 | 0:1ed23ab1345f | 180 | // register service record internally - the normal version creates a copy of the record |
| va009039 | 0:1ed23ab1345f | 181 | // pre: AttributeIDs are in ascending order => ServiceRecordHandle is first attribute if present |
| va009039 | 0:1ed23ab1345f | 182 | // @returns ServiceRecordHandle or 0 if registration failed |
| va009039 | 0:1ed23ab1345f | 183 | uint32_t sdp_register_service_internal(void *connection, uint8_t * record){ |
| va009039 | 0:1ed23ab1345f | 184 | |
| va009039 | 0:1ed23ab1345f | 185 | // dump for now |
| va009039 | 0:1ed23ab1345f | 186 | // printf("Register service record\n"); |
| va009039 | 0:1ed23ab1345f | 187 | // de_dump_data_element(record); |
| va009039 | 0:1ed23ab1345f | 188 | |
| va009039 | 0:1ed23ab1345f | 189 | // get user record handle |
| va009039 | 0:1ed23ab1345f | 190 | uint32_t record_handle = sdp_get_service_record_handle(record); |
| va009039 | 0:1ed23ab1345f | 191 | |
| va009039 | 0:1ed23ab1345f | 192 | // validate service record handle is not in reserved range |
| va009039 | 0:1ed23ab1345f | 193 | if (record_handle <= maxReservedServiceRecordHandle) record_handle = 0; |
| va009039 | 0:1ed23ab1345f | 194 | |
| va009039 | 0:1ed23ab1345f | 195 | // check if already in use |
| va009039 | 0:1ed23ab1345f | 196 | if (record_handle) { |
| va009039 | 0:1ed23ab1345f | 197 | if (sdp_get_record_for_handle(record_handle)) { |
| va009039 | 0:1ed23ab1345f | 198 | record_handle = 0; |
| va009039 | 0:1ed23ab1345f | 199 | } |
| va009039 | 0:1ed23ab1345f | 200 | } |
| va009039 | 0:1ed23ab1345f | 201 | |
| va009039 | 0:1ed23ab1345f | 202 | // create new handle if needed |
| va009039 | 0:1ed23ab1345f | 203 | if (!record_handle){ |
| va009039 | 0:1ed23ab1345f | 204 | record_handle = sdp_create_service_record_handle(); |
| va009039 | 0:1ed23ab1345f | 205 | } |
| va009039 | 0:1ed23ab1345f | 206 | |
| va009039 | 0:1ed23ab1345f | 207 | // calculate size of new service record: DES (2 byte len) |
| va009039 | 0:1ed23ab1345f | 208 | // + ServiceRecordHandle attribute (UINT16 UINT32) + size of existing attributes |
| va009039 | 0:1ed23ab1345f | 209 | uint16_t recordSize = 3 + (3 + 5) + de_get_data_size(record); |
| va009039 | 0:1ed23ab1345f | 210 | |
| va009039 | 0:1ed23ab1345f | 211 | // alloc memory for new service_record_item |
| va009039 | 0:1ed23ab1345f | 212 | service_record_item_t * newRecordItem = (service_record_item_t *) malloc(recordSize + sizeof(service_record_item_t)); |
| va009039 | 0:1ed23ab1345f | 213 | if (!newRecordItem) { |
| va009039 | 0:1ed23ab1345f | 214 | sdp_emit_service_registered(connection, 0, BTSTACK_MEMORY_ALLOC_FAILED); |
| va009039 | 0:1ed23ab1345f | 215 | return 0; |
| va009039 | 0:1ed23ab1345f | 216 | } |
| va009039 | 0:1ed23ab1345f | 217 | // link new service item to client connection |
| va009039 | 0:1ed23ab1345f | 218 | newRecordItem->connection = connection; |
| va009039 | 0:1ed23ab1345f | 219 | |
| va009039 | 0:1ed23ab1345f | 220 | // set new handle |
| va009039 | 0:1ed23ab1345f | 221 | newRecordItem->service_record_handle = record_handle; |
| va009039 | 0:1ed23ab1345f | 222 | |
| va009039 | 0:1ed23ab1345f | 223 | // create updated service record |
| va009039 | 0:1ed23ab1345f | 224 | uint8_t * newRecord = (uint8_t *) &(newRecordItem->service_record); |
| va009039 | 0:1ed23ab1345f | 225 | |
| va009039 | 0:1ed23ab1345f | 226 | // create DES for new record |
| va009039 | 0:1ed23ab1345f | 227 | de_create_sequence(newRecord); |
| va009039 | 0:1ed23ab1345f | 228 | |
| va009039 | 0:1ed23ab1345f | 229 | // set service record handle |
| va009039 | 0:1ed23ab1345f | 230 | de_add_number(newRecord, DE_UINT, DE_SIZE_16, 0); |
| va009039 | 0:1ed23ab1345f | 231 | de_add_number(newRecord, DE_UINT, DE_SIZE_32, record_handle); |
| va009039 | 0:1ed23ab1345f | 232 | |
| va009039 | 0:1ed23ab1345f | 233 | // add other attributes |
| va009039 | 0:1ed23ab1345f | 234 | sdp_append_attributes_in_attributeIDList(record, (uint8_t *) removeServiceRecordHandleAttributeIDList, 0, recordSize, newRecord); |
| va009039 | 0:1ed23ab1345f | 235 | |
| va009039 | 0:1ed23ab1345f | 236 | // dump for now |
| va009039 | 0:1ed23ab1345f | 237 | // de_dump_data_element(newRecord); |
| va009039 | 0:1ed23ab1345f | 238 | // printf("reserved size %u, actual size %u\n", recordSize, de_get_len(newRecord)); |
| va009039 | 0:1ed23ab1345f | 239 | |
| va009039 | 0:1ed23ab1345f | 240 | // add to linked list |
| va009039 | 0:1ed23ab1345f | 241 | linked_list_add(&sdp_service_records, (linked_item_t *) newRecordItem); |
| va009039 | 0:1ed23ab1345f | 242 | |
| va009039 | 0:1ed23ab1345f | 243 | sdp_emit_service_registered(connection, 0, newRecordItem->service_record_handle); |
| va009039 | 0:1ed23ab1345f | 244 | |
| va009039 | 0:1ed23ab1345f | 245 | return record_handle; |
| va009039 | 0:1ed23ab1345f | 246 | } |
| va009039 | 0:1ed23ab1345f | 247 | |
| va009039 | 0:1ed23ab1345f | 248 | #endif |
| va009039 | 0:1ed23ab1345f | 249 | |
| va009039 | 0:1ed23ab1345f | 250 | // unregister service record internally |
| va009039 | 0:1ed23ab1345f | 251 | // |
| va009039 | 0:1ed23ab1345f | 252 | // makes sure one client cannot remove service records of other clients |
| va009039 | 0:1ed23ab1345f | 253 | // |
| va009039 | 0:1ed23ab1345f | 254 | void sdp_unregister_service_internal(void *connection, uint32_t service_record_handle){ |
| va009039 | 0:1ed23ab1345f | 255 | service_record_item_t * record_item = sdp_get_record_for_handle(service_record_handle); |
| va009039 | 0:1ed23ab1345f | 256 | if (record_item && record_item->connection == connection) { |
| va009039 | 0:1ed23ab1345f | 257 | linked_list_remove(&sdp_service_records, (linked_item_t *) record_item); |
| va009039 | 0:1ed23ab1345f | 258 | } |
| va009039 | 0:1ed23ab1345f | 259 | } |
| va009039 | 0:1ed23ab1345f | 260 | |
| va009039 | 0:1ed23ab1345f | 261 | // remove all service record for a client connection |
| va009039 | 0:1ed23ab1345f | 262 | void sdp_unregister_services_for_connection(void *connection){ |
| va009039 | 0:1ed23ab1345f | 263 | linked_item_t *it = (linked_item_t *) &sdp_service_records; |
| va009039 | 0:1ed23ab1345f | 264 | while (it->next){ |
| va009039 | 0:1ed23ab1345f | 265 | service_record_item_t *record_item = (service_record_item_t *) it->next; |
| va009039 | 0:1ed23ab1345f | 266 | if (record_item->connection == connection){ |
| va009039 | 0:1ed23ab1345f | 267 | it->next = it->next->next; |
| va009039 | 0:1ed23ab1345f | 268 | #ifndef EMBEDDED |
| va009039 | 0:1ed23ab1345f | 269 | free(record_item); |
| va009039 | 0:1ed23ab1345f | 270 | #endif |
| va009039 | 0:1ed23ab1345f | 271 | } else { |
| va009039 | 0:1ed23ab1345f | 272 | it = it->next; |
| va009039 | 0:1ed23ab1345f | 273 | } |
| va009039 | 0:1ed23ab1345f | 274 | } |
| va009039 | 0:1ed23ab1345f | 275 | } |
| va009039 | 0:1ed23ab1345f | 276 | |
| va009039 | 0:1ed23ab1345f | 277 | // PDU |
| va009039 | 0:1ed23ab1345f | 278 | // PDU ID (1), Transaction ID (2), Param Length (2), Param 1, Param 2, .. |
| va009039 | 0:1ed23ab1345f | 279 | |
| va009039 | 0:1ed23ab1345f | 280 | int sdp_create_error_response(uint16_t transaction_id, uint16_t error_code){ |
| va009039 | 0:1ed23ab1345f | 281 | sdp_response_buffer[0] = SDP_ErrorResponse; |
| va009039 | 0:1ed23ab1345f | 282 | net_store_16(sdp_response_buffer, 1, transaction_id); |
| va009039 | 0:1ed23ab1345f | 283 | net_store_16(sdp_response_buffer, 3, 2); |
| va009039 | 0:1ed23ab1345f | 284 | net_store_16(sdp_response_buffer, 5, error_code); // invalid syntax |
| va009039 | 0:1ed23ab1345f | 285 | return 7; |
| va009039 | 0:1ed23ab1345f | 286 | } |
| va009039 | 0:1ed23ab1345f | 287 | |
| va009039 | 0:1ed23ab1345f | 288 | int sdp_handle_service_search_request(uint8_t * packet, uint16_t remote_mtu){ |
| va009039 | 0:1ed23ab1345f | 289 | |
| va009039 | 0:1ed23ab1345f | 290 | // get request details |
| va009039 | 0:1ed23ab1345f | 291 | uint16_t transaction_id = READ_NET_16(packet, 1); |
| va009039 | 0:1ed23ab1345f | 292 | // not used yet - uint16_t param_len = READ_NET_16(packet, 3); |
| va009039 | 0:1ed23ab1345f | 293 | uint8_t * serviceSearchPattern = &packet[5]; |
| va009039 | 0:1ed23ab1345f | 294 | uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern); |
| va009039 | 0:1ed23ab1345f | 295 | uint16_t maximumServiceRecordCount = READ_NET_16(packet, 5 + serviceSearchPatternLen); |
| va009039 | 0:1ed23ab1345f | 296 | uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2]; |
| va009039 | 0:1ed23ab1345f | 297 | |
| va009039 | 0:1ed23ab1345f | 298 | // calc maxumumServiceRecordCount based on remote MTU |
| va009039 | 0:1ed23ab1345f | 299 | uint16_t maxNrServiceRecordsPerResponse = (remote_mtu - (9+3))/4; |
| va009039 | 0:1ed23ab1345f | 300 | |
| va009039 | 0:1ed23ab1345f | 301 | // continuation state contains index of next service record to examine |
| va009039 | 0:1ed23ab1345f | 302 | int continuation = 0; |
| va009039 | 0:1ed23ab1345f | 303 | uint16_t continuation_index = 0; |
| va009039 | 0:1ed23ab1345f | 304 | if (continuationState[0] == 2){ |
| va009039 | 0:1ed23ab1345f | 305 | continuation_index = READ_NET_16(continuationState, 1); |
| va009039 | 0:1ed23ab1345f | 306 | } |
| va009039 | 0:1ed23ab1345f | 307 | |
| va009039 | 0:1ed23ab1345f | 308 | // get and limit total count |
| va009039 | 0:1ed23ab1345f | 309 | linked_item_t *it; |
| va009039 | 0:1ed23ab1345f | 310 | uint16_t total_service_count = 0; |
| va009039 | 0:1ed23ab1345f | 311 | for (it = (linked_item_t *) sdp_service_records; it ; it = it->next){ |
| va009039 | 0:1ed23ab1345f | 312 | service_record_item_t * item = (service_record_item_t *) it; |
| va009039 | 0:1ed23ab1345f | 313 | if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; |
| va009039 | 0:1ed23ab1345f | 314 | total_service_count++; |
| va009039 | 0:1ed23ab1345f | 315 | } |
| va009039 | 0:1ed23ab1345f | 316 | if (total_service_count > maximumServiceRecordCount){ |
| va009039 | 0:1ed23ab1345f | 317 | total_service_count = maximumServiceRecordCount; |
| va009039 | 0:1ed23ab1345f | 318 | } |
| va009039 | 0:1ed23ab1345f | 319 | |
| va009039 | 0:1ed23ab1345f | 320 | // ServiceRecordHandleList at 9 |
| va009039 | 0:1ed23ab1345f | 321 | uint16_t pos = 9; |
| va009039 | 0:1ed23ab1345f | 322 | uint16_t current_service_count = 0; |
| va009039 | 0:1ed23ab1345f | 323 | uint16_t current_service_index = 0; |
| va009039 | 0:1ed23ab1345f | 324 | uint16_t matching_service_count = 0; |
| va009039 | 0:1ed23ab1345f | 325 | for (it = (linked_item_t *) sdp_service_records; it ; it = it->next, ++current_service_index){ |
| va009039 | 0:1ed23ab1345f | 326 | service_record_item_t * item = (service_record_item_t *) it; |
| va009039 | 0:1ed23ab1345f | 327 | |
| va009039 | 0:1ed23ab1345f | 328 | if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; |
| va009039 | 0:1ed23ab1345f | 329 | matching_service_count++; |
| va009039 | 0:1ed23ab1345f | 330 | |
| va009039 | 0:1ed23ab1345f | 331 | if (current_service_index < continuation_index) continue; |
| va009039 | 0:1ed23ab1345f | 332 | |
| va009039 | 0:1ed23ab1345f | 333 | net_store_32(sdp_response_buffer, pos, item->service_record_handle); |
| va009039 | 0:1ed23ab1345f | 334 | pos += 4; |
| va009039 | 0:1ed23ab1345f | 335 | current_service_count++; |
| va009039 | 0:1ed23ab1345f | 336 | |
| va009039 | 0:1ed23ab1345f | 337 | if (matching_service_count >= total_service_count) break; |
| va009039 | 0:1ed23ab1345f | 338 | |
| va009039 | 0:1ed23ab1345f | 339 | if (current_service_count >= maxNrServiceRecordsPerResponse){ |
| va009039 | 0:1ed23ab1345f | 340 | continuation = 1; |
| va009039 | 0:1ed23ab1345f | 341 | continuation_index = current_service_index + 1; |
| va009039 | 0:1ed23ab1345f | 342 | break; |
| va009039 | 0:1ed23ab1345f | 343 | } |
| va009039 | 0:1ed23ab1345f | 344 | } |
| va009039 | 0:1ed23ab1345f | 345 | |
| va009039 | 0:1ed23ab1345f | 346 | // Store continuation state |
| va009039 | 0:1ed23ab1345f | 347 | if (continuation) { |
| va009039 | 0:1ed23ab1345f | 348 | sdp_response_buffer[pos++] = 2; |
| va009039 | 0:1ed23ab1345f | 349 | net_store_16(sdp_response_buffer, pos, continuation_index); |
| va009039 | 0:1ed23ab1345f | 350 | pos += 2; |
| va009039 | 0:1ed23ab1345f | 351 | } else { |
| va009039 | 0:1ed23ab1345f | 352 | sdp_response_buffer[pos++] = 0; |
| va009039 | 0:1ed23ab1345f | 353 | } |
| va009039 | 0:1ed23ab1345f | 354 | |
| va009039 | 0:1ed23ab1345f | 355 | // header |
| va009039 | 0:1ed23ab1345f | 356 | sdp_response_buffer[0] = SDP_ServiceSearchResponse; |
| va009039 | 0:1ed23ab1345f | 357 | net_store_16(sdp_response_buffer, 1, transaction_id); |
| va009039 | 0:1ed23ab1345f | 358 | net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload |
| va009039 | 0:1ed23ab1345f | 359 | net_store_16(sdp_response_buffer, 5, total_service_count); |
| va009039 | 0:1ed23ab1345f | 360 | net_store_16(sdp_response_buffer, 7, current_service_count); |
| va009039 | 0:1ed23ab1345f | 361 | |
| va009039 | 0:1ed23ab1345f | 362 | return pos; |
| va009039 | 0:1ed23ab1345f | 363 | } |
| va009039 | 0:1ed23ab1345f | 364 | |
| va009039 | 0:1ed23ab1345f | 365 | int sdp_handle_service_attribute_request(uint8_t * packet, uint16_t remote_mtu){ |
| va009039 | 0:1ed23ab1345f | 366 | |
| va009039 | 0:1ed23ab1345f | 367 | // get request details |
| va009039 | 0:1ed23ab1345f | 368 | uint16_t transaction_id = READ_NET_16(packet, 1); |
| va009039 | 0:1ed23ab1345f | 369 | // not used yet - uint16_t param_len = READ_NET_16(packet, 3); |
| va009039 | 0:1ed23ab1345f | 370 | uint32_t serviceRecordHandle = READ_NET_32(packet, 5); |
| va009039 | 0:1ed23ab1345f | 371 | uint16_t maximumAttributeByteCount = READ_NET_16(packet, 9); |
| va009039 | 0:1ed23ab1345f | 372 | uint8_t * attributeIDList = &packet[11]; |
| va009039 | 0:1ed23ab1345f | 373 | uint16_t attributeIDListLen = de_get_len(attributeIDList); |
| va009039 | 0:1ed23ab1345f | 374 | uint8_t * continuationState = &packet[11+attributeIDListLen]; |
| va009039 | 0:1ed23ab1345f | 375 | |
| va009039 | 0:1ed23ab1345f | 376 | // calc maximumAttributeByteCount based on remote MTU |
| va009039 | 0:1ed23ab1345f | 377 | uint16_t maximumAttributeByteCount2 = remote_mtu - (7+3); |
| va009039 | 0:1ed23ab1345f | 378 | if (maximumAttributeByteCount2 < maximumAttributeByteCount) { |
| va009039 | 0:1ed23ab1345f | 379 | maximumAttributeByteCount = maximumAttributeByteCount2; |
| va009039 | 0:1ed23ab1345f | 380 | } |
| va009039 | 0:1ed23ab1345f | 381 | |
| va009039 | 0:1ed23ab1345f | 382 | // continuation state contains the offset into the complete response |
| va009039 | 0:1ed23ab1345f | 383 | uint16_t continuation_offset = 0; |
| va009039 | 0:1ed23ab1345f | 384 | if (continuationState[0] == 2){ |
| va009039 | 0:1ed23ab1345f | 385 | continuation_offset = READ_NET_16(continuationState, 1); |
| va009039 | 0:1ed23ab1345f | 386 | } |
| va009039 | 0:1ed23ab1345f | 387 | |
| va009039 | 0:1ed23ab1345f | 388 | // get service record |
| va009039 | 0:1ed23ab1345f | 389 | service_record_item_t * item = sdp_get_record_for_handle(serviceRecordHandle); |
| va009039 | 0:1ed23ab1345f | 390 | if (!item){ |
| va009039 | 0:1ed23ab1345f | 391 | // service record handle doesn't exist |
| va009039 | 0:1ed23ab1345f | 392 | return sdp_create_error_response(transaction_id, 0x0002); /// invalid Service Record Handle |
| va009039 | 0:1ed23ab1345f | 393 | } |
| va009039 | 0:1ed23ab1345f | 394 | |
| va009039 | 0:1ed23ab1345f | 395 | |
| va009039 | 0:1ed23ab1345f | 396 | // AttributeList - starts at offset 7 |
| va009039 | 0:1ed23ab1345f | 397 | uint16_t pos = 7; |
| va009039 | 0:1ed23ab1345f | 398 | |
| va009039 | 0:1ed23ab1345f | 399 | if (continuation_offset == 0){ |
| va009039 | 0:1ed23ab1345f | 400 | |
| va009039 | 0:1ed23ab1345f | 401 | // get size of this record |
| va009039 | 0:1ed23ab1345f | 402 | uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList); |
| va009039 | 0:1ed23ab1345f | 403 | |
| va009039 | 0:1ed23ab1345f | 404 | // store DES |
| va009039 | 0:1ed23ab1345f | 405 | de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size); |
| va009039 | 0:1ed23ab1345f | 406 | maximumAttributeByteCount -= 3; |
| va009039 | 0:1ed23ab1345f | 407 | pos += 3; |
| va009039 | 0:1ed23ab1345f | 408 | } |
| va009039 | 0:1ed23ab1345f | 409 | |
| va009039 | 0:1ed23ab1345f | 410 | // copy maximumAttributeByteCount from record |
| va009039 | 0:1ed23ab1345f | 411 | uint16_t bytes_used; |
| va009039 | 0:1ed23ab1345f | 412 | int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]); |
| va009039 | 0:1ed23ab1345f | 413 | pos += bytes_used; |
| va009039 | 0:1ed23ab1345f | 414 | |
| va009039 | 0:1ed23ab1345f | 415 | uint16_t attributeListByteCount = pos - 7; |
| va009039 | 0:1ed23ab1345f | 416 | |
| va009039 | 0:1ed23ab1345f | 417 | if (complete) { |
| va009039 | 0:1ed23ab1345f | 418 | sdp_response_buffer[pos++] = 0; |
| va009039 | 0:1ed23ab1345f | 419 | } else { |
| va009039 | 0:1ed23ab1345f | 420 | continuation_offset += bytes_used; |
| va009039 | 0:1ed23ab1345f | 421 | sdp_response_buffer[pos++] = 2; |
| va009039 | 0:1ed23ab1345f | 422 | net_store_16(sdp_response_buffer, pos, continuation_offset); |
| va009039 | 0:1ed23ab1345f | 423 | pos += 2; |
| va009039 | 0:1ed23ab1345f | 424 | } |
| va009039 | 0:1ed23ab1345f | 425 | |
| va009039 | 0:1ed23ab1345f | 426 | // header |
| va009039 | 0:1ed23ab1345f | 427 | sdp_response_buffer[0] = SDP_ServiceAttributeResponse; |
| va009039 | 0:1ed23ab1345f | 428 | net_store_16(sdp_response_buffer, 1, transaction_id); |
| va009039 | 0:1ed23ab1345f | 429 | net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload |
| va009039 | 0:1ed23ab1345f | 430 | net_store_16(sdp_response_buffer, 5, attributeListByteCount); |
| va009039 | 0:1ed23ab1345f | 431 | |
| va009039 | 0:1ed23ab1345f | 432 | return pos; |
| va009039 | 0:1ed23ab1345f | 433 | } |
| va009039 | 0:1ed23ab1345f | 434 | |
| va009039 | 0:1ed23ab1345f | 435 | static uint16_t sdp_get_size_for_service_search_attribute_response(uint8_t * serviceSearchPattern, uint8_t * attributeIDList){ |
| va009039 | 0:1ed23ab1345f | 436 | uint16_t total_response_size = 0; |
| va009039 | 0:1ed23ab1345f | 437 | linked_item_t *it; |
| va009039 | 0:1ed23ab1345f | 438 | for (it = (linked_item_t *) sdp_service_records; it ; it = it->next){ |
| va009039 | 0:1ed23ab1345f | 439 | service_record_item_t * item = (service_record_item_t *) it; |
| va009039 | 0:1ed23ab1345f | 440 | |
| va009039 | 0:1ed23ab1345f | 441 | if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; |
| va009039 | 0:1ed23ab1345f | 442 | |
| va009039 | 0:1ed23ab1345f | 443 | // for all service records that match |
| va009039 | 0:1ed23ab1345f | 444 | total_response_size += 3 + spd_get_filtered_size(item->service_record, attributeIDList); |
| va009039 | 0:1ed23ab1345f | 445 | } |
| va009039 | 0:1ed23ab1345f | 446 | return total_response_size; |
| va009039 | 0:1ed23ab1345f | 447 | } |
| va009039 | 0:1ed23ab1345f | 448 | |
| va009039 | 0:1ed23ab1345f | 449 | int sdp_handle_service_search_attribute_request(uint8_t * packet, uint16_t remote_mtu){ |
| va009039 | 0:1ed23ab1345f | 450 | |
| va009039 | 0:1ed23ab1345f | 451 | // SDP header before attribute sevice list: 7 |
| va009039 | 0:1ed23ab1345f | 452 | // Continuation, worst case: 5 |
| va009039 | 0:1ed23ab1345f | 453 | |
| va009039 | 0:1ed23ab1345f | 454 | // get request details |
| va009039 | 0:1ed23ab1345f | 455 | uint16_t transaction_id = READ_NET_16(packet, 1); |
| va009039 | 0:1ed23ab1345f | 456 | // not used yet - uint16_t param_len = READ_NET_16(packet, 3); |
| va009039 | 0:1ed23ab1345f | 457 | uint8_t * serviceSearchPattern = &packet[5]; |
| va009039 | 0:1ed23ab1345f | 458 | uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern); |
| va009039 | 0:1ed23ab1345f | 459 | uint16_t maximumAttributeByteCount = READ_NET_16(packet, 5 + serviceSearchPatternLen); |
| va009039 | 0:1ed23ab1345f | 460 | uint8_t * attributeIDList = &packet[5+serviceSearchPatternLen+2]; |
| va009039 | 0:1ed23ab1345f | 461 | uint16_t attributeIDListLen = de_get_len(attributeIDList); |
| va009039 | 0:1ed23ab1345f | 462 | uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2+attributeIDListLen]; |
| va009039 | 0:1ed23ab1345f | 463 | |
| va009039 | 0:1ed23ab1345f | 464 | // calc maximumAttributeByteCount based on remote MTU, SDP header and reserved Continuation block |
| va009039 | 0:1ed23ab1345f | 465 | uint16_t maximumAttributeByteCount2 = remote_mtu - 12; |
| va009039 | 0:1ed23ab1345f | 466 | if (maximumAttributeByteCount2 < maximumAttributeByteCount) { |
| va009039 | 0:1ed23ab1345f | 467 | maximumAttributeByteCount = maximumAttributeByteCount2; |
| va009039 | 0:1ed23ab1345f | 468 | } |
| va009039 | 0:1ed23ab1345f | 469 | |
| va009039 | 0:1ed23ab1345f | 470 | // continuation state contains: index of next service record to examine |
| va009039 | 0:1ed23ab1345f | 471 | // continuation state contains: byte offset into this service record |
| va009039 | 0:1ed23ab1345f | 472 | uint16_t continuation_service_index = 0; |
| va009039 | 0:1ed23ab1345f | 473 | uint16_t continuation_offset = 0; |
| va009039 | 0:1ed23ab1345f | 474 | if (continuationState[0] == 4){ |
| va009039 | 0:1ed23ab1345f | 475 | continuation_service_index = READ_NET_16(continuationState, 1); |
| va009039 | 0:1ed23ab1345f | 476 | continuation_offset = READ_NET_16(continuationState, 3); |
| va009039 | 0:1ed23ab1345f | 477 | } |
| va009039 | 0:1ed23ab1345f | 478 | |
| va009039 | 0:1ed23ab1345f | 479 | // printf("--> sdp_handle_service_search_attribute_request, cont %u/%u, max %u\n", continuation_service_index, continuation_offset, maximumAttributeByteCount); |
| va009039 | 0:1ed23ab1345f | 480 | |
| va009039 | 0:1ed23ab1345f | 481 | // AttributeLists - starts at offset 7 |
| va009039 | 0:1ed23ab1345f | 482 | uint16_t pos = 7; |
| va009039 | 0:1ed23ab1345f | 483 | |
| va009039 | 0:1ed23ab1345f | 484 | // add DES with total size for first request |
| va009039 | 0:1ed23ab1345f | 485 | if (continuation_service_index == 0 && continuation_offset == 0){ |
| va009039 | 0:1ed23ab1345f | 486 | uint16_t total_response_size = sdp_get_size_for_service_search_attribute_response(serviceSearchPattern, attributeIDList); |
| va009039 | 0:1ed23ab1345f | 487 | de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, total_response_size); |
| va009039 | 0:1ed23ab1345f | 488 | // log_info("total response size %u\n", total_response_size); |
| va009039 | 0:1ed23ab1345f | 489 | pos += 3; |
| va009039 | 0:1ed23ab1345f | 490 | maximumAttributeByteCount -= 3; |
| va009039 | 0:1ed23ab1345f | 491 | } |
| va009039 | 0:1ed23ab1345f | 492 | |
| va009039 | 0:1ed23ab1345f | 493 | // create attribute list |
| va009039 | 0:1ed23ab1345f | 494 | int first_answer = 1; |
| va009039 | 0:1ed23ab1345f | 495 | int continuation = 0; |
| va009039 | 0:1ed23ab1345f | 496 | uint16_t current_service_index = 0; |
| va009039 | 0:1ed23ab1345f | 497 | linked_item_t *it = (linked_item_t *) sdp_service_records; |
| va009039 | 0:1ed23ab1345f | 498 | for ( ; it ; it = it->next, ++current_service_index){ |
| va009039 | 0:1ed23ab1345f | 499 | service_record_item_t * item = (service_record_item_t *) it; |
| va009039 | 0:1ed23ab1345f | 500 | |
| va009039 | 0:1ed23ab1345f | 501 | if (current_service_index < continuation_service_index ) continue; |
| va009039 | 0:1ed23ab1345f | 502 | if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue; |
| va009039 | 0:1ed23ab1345f | 503 | |
| va009039 | 0:1ed23ab1345f | 504 | if (continuation_offset == 0){ |
| va009039 | 0:1ed23ab1345f | 505 | |
| va009039 | 0:1ed23ab1345f | 506 | // get size of this record |
| va009039 | 0:1ed23ab1345f | 507 | uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList); |
| va009039 | 0:1ed23ab1345f | 508 | |
| va009039 | 0:1ed23ab1345f | 509 | // stop if complete record doesn't fits into response but we already have a partial response |
| va009039 | 0:1ed23ab1345f | 510 | if ((filtered_attributes_size + 3 > maximumAttributeByteCount) && !first_answer) { |
| va009039 | 0:1ed23ab1345f | 511 | continuation = 1; |
| va009039 | 0:1ed23ab1345f | 512 | break; |
| va009039 | 0:1ed23ab1345f | 513 | } |
| va009039 | 0:1ed23ab1345f | 514 | |
| va009039 | 0:1ed23ab1345f | 515 | // store DES |
| va009039 | 0:1ed23ab1345f | 516 | de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size); |
| va009039 | 0:1ed23ab1345f | 517 | pos += 3; |
| va009039 | 0:1ed23ab1345f | 518 | maximumAttributeByteCount -= 3; |
| va009039 | 0:1ed23ab1345f | 519 | } |
| va009039 | 0:1ed23ab1345f | 520 | |
| va009039 | 0:1ed23ab1345f | 521 | first_answer = 0; |
| va009039 | 0:1ed23ab1345f | 522 | |
| va009039 | 0:1ed23ab1345f | 523 | // copy maximumAttributeByteCount from record |
| va009039 | 0:1ed23ab1345f | 524 | uint16_t bytes_used; |
| va009039 | 0:1ed23ab1345f | 525 | int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]); |
| va009039 | 0:1ed23ab1345f | 526 | pos += bytes_used; |
| va009039 | 0:1ed23ab1345f | 527 | maximumAttributeByteCount -= bytes_used; |
| va009039 | 0:1ed23ab1345f | 528 | |
| va009039 | 0:1ed23ab1345f | 529 | if (complete) { |
| va009039 | 0:1ed23ab1345f | 530 | continuation_offset = 0; |
| va009039 | 0:1ed23ab1345f | 531 | continue; |
| va009039 | 0:1ed23ab1345f | 532 | } |
| va009039 | 0:1ed23ab1345f | 533 | |
| va009039 | 0:1ed23ab1345f | 534 | continuation = 1; |
| va009039 | 0:1ed23ab1345f | 535 | continuation_offset += bytes_used; |
| va009039 | 0:1ed23ab1345f | 536 | break; |
| va009039 | 0:1ed23ab1345f | 537 | } |
| va009039 | 0:1ed23ab1345f | 538 | |
| va009039 | 0:1ed23ab1345f | 539 | uint16_t attributeListsByteCount = pos - 7; |
| va009039 | 0:1ed23ab1345f | 540 | |
| va009039 | 0:1ed23ab1345f | 541 | // Continuation State |
| va009039 | 0:1ed23ab1345f | 542 | if (continuation){ |
| va009039 | 0:1ed23ab1345f | 543 | sdp_response_buffer[pos++] = 4; |
| va009039 | 0:1ed23ab1345f | 544 | net_store_16(sdp_response_buffer, pos, (uint16_t) current_service_index); |
| va009039 | 0:1ed23ab1345f | 545 | pos += 2; |
| va009039 | 0:1ed23ab1345f | 546 | net_store_16(sdp_response_buffer, pos, continuation_offset); |
| va009039 | 0:1ed23ab1345f | 547 | pos += 2; |
| va009039 | 0:1ed23ab1345f | 548 | } else { |
| va009039 | 0:1ed23ab1345f | 549 | // complete |
| va009039 | 0:1ed23ab1345f | 550 | sdp_response_buffer[pos++] = 0; |
| va009039 | 0:1ed23ab1345f | 551 | } |
| va009039 | 0:1ed23ab1345f | 552 | |
| va009039 | 0:1ed23ab1345f | 553 | // create SDP header |
| va009039 | 0:1ed23ab1345f | 554 | sdp_response_buffer[0] = SDP_ServiceSearchAttributeResponse; |
| va009039 | 0:1ed23ab1345f | 555 | net_store_16(sdp_response_buffer, 1, transaction_id); |
| va009039 | 0:1ed23ab1345f | 556 | net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload |
| va009039 | 0:1ed23ab1345f | 557 | net_store_16(sdp_response_buffer, 5, attributeListsByteCount); |
| va009039 | 0:1ed23ab1345f | 558 | |
| va009039 | 0:1ed23ab1345f | 559 | return pos; |
| va009039 | 0:1ed23ab1345f | 560 | } |
| va009039 | 0:1ed23ab1345f | 561 | |
| va009039 | 0:1ed23ab1345f | 562 | static void sdp_try_respond(void){ |
| va009039 | 0:1ed23ab1345f | 563 | if (!sdp_response_size ) return; |
| va009039 | 0:1ed23ab1345f | 564 | if (!l2cap_cid) return; |
| va009039 | 0:1ed23ab1345f | 565 | if (!l2cap_can_send_packet_now(l2cap_cid)) return; |
| va009039 | 0:1ed23ab1345f | 566 | |
| va009039 | 0:1ed23ab1345f | 567 | // update state before sending packet (avoid getting called when new l2cap credit gets emitted) |
| va009039 | 0:1ed23ab1345f | 568 | uint16_t size = sdp_response_size; |
| va009039 | 0:1ed23ab1345f | 569 | sdp_response_size = 0; |
| va009039 | 0:1ed23ab1345f | 570 | l2cap_send_internal(l2cap_cid, sdp_response_buffer, size); |
| va009039 | 0:1ed23ab1345f | 571 | } |
| va009039 | 0:1ed23ab1345f | 572 | |
| va009039 | 0:1ed23ab1345f | 573 | // we assume that we don't get two requests in a row |
| va009039 | 0:1ed23ab1345f | 574 | static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ |
| va009039 | 0:1ed23ab1345f | 575 | uint16_t transaction_id; |
| va009039 | 0:1ed23ab1345f | 576 | SDP_PDU_ID_t pdu_id; |
| va009039 | 0:1ed23ab1345f | 577 | uint16_t remote_mtu; |
| va009039 | 0:1ed23ab1345f | 578 | // uint16_t param_len; |
| va009039 | 0:1ed23ab1345f | 579 | |
| va009039 | 0:1ed23ab1345f | 580 | switch (packet_type) { |
| va009039 | 0:1ed23ab1345f | 581 | |
| va009039 | 0:1ed23ab1345f | 582 | case L2CAP_DATA_PACKET: |
| va009039 | 0:1ed23ab1345f | 583 | pdu_id = (SDP_PDU_ID_t) packet[0]; |
| va009039 | 0:1ed23ab1345f | 584 | transaction_id = READ_NET_16(packet, 1); |
| va009039 | 0:1ed23ab1345f | 585 | // param_len = READ_NET_16(packet, 3); |
| va009039 | 0:1ed23ab1345f | 586 | remote_mtu = l2cap_get_remote_mtu_for_local_cid(channel); |
| va009039 | 0:1ed23ab1345f | 587 | // account for our buffer |
| va009039 | 0:1ed23ab1345f | 588 | if (remote_mtu > SDP_RESPONSE_BUFFER_SIZE){ |
| va009039 | 0:1ed23ab1345f | 589 | remote_mtu = SDP_RESPONSE_BUFFER_SIZE; |
| va009039 | 0:1ed23ab1345f | 590 | } |
| va009039 | 0:1ed23ab1345f | 591 | |
| va009039 | 0:1ed23ab1345f | 592 | // printf("SDP Request: type %u, transaction id %u, len %u, mtu %u\n", pdu_id, transaction_id, param_len, remote_mtu); |
| va009039 | 0:1ed23ab1345f | 593 | switch (pdu_id){ |
| va009039 | 0:1ed23ab1345f | 594 | |
| va009039 | 0:1ed23ab1345f | 595 | case SDP_ServiceSearchRequest: |
| va009039 | 0:1ed23ab1345f | 596 | sdp_response_size = sdp_handle_service_search_request(packet, remote_mtu); |
| va009039 | 0:1ed23ab1345f | 597 | break; |
| va009039 | 0:1ed23ab1345f | 598 | |
| va009039 | 0:1ed23ab1345f | 599 | case SDP_ServiceAttributeRequest: |
| va009039 | 0:1ed23ab1345f | 600 | sdp_response_size = sdp_handle_service_attribute_request(packet, remote_mtu); |
| va009039 | 0:1ed23ab1345f | 601 | break; |
| va009039 | 0:1ed23ab1345f | 602 | |
| va009039 | 0:1ed23ab1345f | 603 | case SDP_ServiceSearchAttributeRequest: |
| va009039 | 0:1ed23ab1345f | 604 | sdp_response_size = sdp_handle_service_search_attribute_request(packet, remote_mtu); |
| va009039 | 0:1ed23ab1345f | 605 | break; |
| va009039 | 0:1ed23ab1345f | 606 | |
| va009039 | 0:1ed23ab1345f | 607 | default: |
| va009039 | 0:1ed23ab1345f | 608 | sdp_response_size = sdp_create_error_response(transaction_id, 0x0003); // invalid syntax |
| va009039 | 0:1ed23ab1345f | 609 | break; |
| va009039 | 0:1ed23ab1345f | 610 | } |
| va009039 | 0:1ed23ab1345f | 611 | |
| va009039 | 0:1ed23ab1345f | 612 | sdp_try_respond(); |
| va009039 | 0:1ed23ab1345f | 613 | |
| va009039 | 0:1ed23ab1345f | 614 | break; |
| va009039 | 0:1ed23ab1345f | 615 | |
| va009039 | 0:1ed23ab1345f | 616 | case HCI_EVENT_PACKET: |
| va009039 | 0:1ed23ab1345f | 617 | |
| va009039 | 0:1ed23ab1345f | 618 | switch (packet[0]) { |
| va009039 | 0:1ed23ab1345f | 619 | |
| va009039 | 0:1ed23ab1345f | 620 | case L2CAP_EVENT_INCOMING_CONNECTION: |
| va009039 | 0:1ed23ab1345f | 621 | if (l2cap_cid) { |
| va009039 | 0:1ed23ab1345f | 622 | // CONNECTION REJECTED DUE TO LIMITED RESOURCES |
| va009039 | 0:1ed23ab1345f | 623 | l2cap_decline_connection_internal(channel, 0x0d); |
| va009039 | 0:1ed23ab1345f | 624 | break; |
| va009039 | 0:1ed23ab1345f | 625 | } |
| va009039 | 0:1ed23ab1345f | 626 | // accept |
| va009039 | 0:1ed23ab1345f | 627 | l2cap_cid = channel; |
| va009039 | 0:1ed23ab1345f | 628 | sdp_response_size = 0; |
| va009039 | 0:1ed23ab1345f | 629 | l2cap_accept_connection_internal(channel); |
| va009039 | 0:1ed23ab1345f | 630 | break; |
| va009039 | 0:1ed23ab1345f | 631 | |
| va009039 | 0:1ed23ab1345f | 632 | case L2CAP_EVENT_CHANNEL_OPENED: |
| va009039 | 0:1ed23ab1345f | 633 | if (packet[2]) { |
| va009039 | 0:1ed23ab1345f | 634 | // open failed -> reset |
| va009039 | 0:1ed23ab1345f | 635 | l2cap_cid = 0; |
| va009039 | 0:1ed23ab1345f | 636 | } |
| va009039 | 0:1ed23ab1345f | 637 | break; |
| va009039 | 0:1ed23ab1345f | 638 | |
| va009039 | 0:1ed23ab1345f | 639 | case L2CAP_EVENT_CREDITS: |
| va009039 | 0:1ed23ab1345f | 640 | case DAEMON_EVENT_HCI_PACKET_SENT: |
| va009039 | 0:1ed23ab1345f | 641 | sdp_try_respond(); |
| va009039 | 0:1ed23ab1345f | 642 | break; |
| va009039 | 0:1ed23ab1345f | 643 | |
| va009039 | 0:1ed23ab1345f | 644 | case L2CAP_EVENT_CHANNEL_CLOSED: |
| va009039 | 0:1ed23ab1345f | 645 | if (channel == l2cap_cid){ |
| va009039 | 0:1ed23ab1345f | 646 | // reset |
| va009039 | 0:1ed23ab1345f | 647 | l2cap_cid = 0; |
| va009039 | 0:1ed23ab1345f | 648 | } |
| va009039 | 0:1ed23ab1345f | 649 | break; |
| va009039 | 0:1ed23ab1345f | 650 | |
| va009039 | 0:1ed23ab1345f | 651 | default: |
| va009039 | 0:1ed23ab1345f | 652 | // other event |
| va009039 | 0:1ed23ab1345f | 653 | break; |
| va009039 | 0:1ed23ab1345f | 654 | } |
| va009039 | 0:1ed23ab1345f | 655 | break; |
| va009039 | 0:1ed23ab1345f | 656 | |
| va009039 | 0:1ed23ab1345f | 657 | default: |
| va009039 | 0:1ed23ab1345f | 658 | // other packet type |
| va009039 | 0:1ed23ab1345f | 659 | break; |
| va009039 | 0:1ed23ab1345f | 660 | } |
| va009039 | 0:1ed23ab1345f | 661 | } |
| va009039 | 0:1ed23ab1345f | 662 |