Ken Todotani
BLE demo for mbed Ported RunningElectronics's SBDBT firmware for BLE. It can communicate with iOS
Dependencies: FatFileSystem mbed
Fork of
BTstack
by 
Norimasa Okamoto
Embed:
(wiki syntax)
Show/hide line numbers
l2cap.c
00001 /* 00002 * Copyright (C) 2009-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 /* 00038 * l2cap.c 00039 * 00040 * Logical Link Control and Adaption Protocl (L2CAP) 00041 * 00042 * Created by Matthias Ringwald on 5/16/09. 00043 */ 00044 00045 #include "l2cap.h" 00046 #include "hci.h" 00047 #include "hci_dump.h" 00048 #include "debug.h" 00049 #include "btstack_memory.h" 00050 00051 #include <stdarg.h> 00052 #include <string.h> 00053 00054 #include <stdio.h> 00055 00056 // nr of buffered acl packets in outgoing queue to get max performance 00057 #define NR_BUFFERED_ACL_PACKETS 3 00058 00059 // used to cache l2cap rejects, echo, and informational requests 00060 #define NR_PENDING_SIGNALING_RESPONSES 3 00061 00062 // offsets for L2CAP SIGNALING COMMANDS 00063 #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET 0 00064 #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET 1 00065 #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2 00066 #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET 4 00067 00068 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 00069 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size); 00070 00071 // used to cache l2cap rejects, echo, and informational requests 00072 static l2cap_signaling_response_t signaling_responses[NR_PENDING_SIGNALING_RESPONSES]; 00073 static int signaling_responses_pending; 00074 00075 static linked_list_t l2cap_channels = NULL; 00076 static linked_list_t l2cap_services = NULL; 00077 static void (*packet_handler) (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) = null_packet_handler; 00078 static int new_credits_blocked = 0; 00079 00080 static btstack_packet_handler_t attribute_protocol_packet_handler = NULL; 00081 static btstack_packet_handler_t security_protocol_packet_handler = NULL; 00082 00083 // prototypes 00084 static void l2cap_finialize_channel_close(l2cap_channel_t *channel); 00085 static l2cap_service_t * l2cap_get_service(uint16_t psm); 00086 static void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status); 00087 static void l2cap_emit_channel_closed(l2cap_channel_t *channel); 00088 static void l2cap_emit_connection_request(l2cap_channel_t *channel); 00089 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel); 00090 00091 00092 void l2cap_init(){ 00093 new_credits_blocked = 0; 00094 signaling_responses_pending = 0; 00095 00096 l2cap_channels = NULL; 00097 l2cap_services = NULL; 00098 00099 packet_handler = null_packet_handler; 00100 00101 // 00102 // register callback with HCI 00103 // 00104 hci_register_packet_handler(&l2cap_packet_handler); 00105 hci_connectable_control(0); // no services yet 00106 } 00107 00108 00109 /** Register L2CAP packet handlers */ 00110 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 00111 } 00112 void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 00113 packet_handler = handler; 00114 } 00115 00116 // notify client/protocol handler 00117 void l2cap_dispatch(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){ 00118 if (channel->packet_handler) { 00119 (* (channel->packet_handler))(type, channel->local_cid, data, size); 00120 } else { 00121 (*packet_handler)(channel->connection, type, channel->local_cid, data, size); 00122 } 00123 } 00124 00125 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 00126 uint8_t event[21]; 00127 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 00128 event[1] = sizeof(event) - 2; 00129 event[2] = status; 00130 bt_flip_addr(&event[3], channel->address); 00131 bt_store_16(event, 9, channel->handle); 00132 bt_store_16(event, 11, channel->psm); 00133 bt_store_16(event, 13, channel->local_cid); 00134 bt_store_16(event, 15, channel->remote_cid); 00135 bt_store_16(event, 17, channel->local_mtu); 00136 bt_store_16(event, 19, channel->remote_mtu); 00137 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 00138 l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); 00139 } 00140 00141 void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 00142 uint8_t event[4]; 00143 event[0] = L2CAP_EVENT_CHANNEL_CLOSED; 00144 event[1] = sizeof(event) - 2; 00145 bt_store_16(event, 2, channel->local_cid); 00146 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 00147 l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); 00148 } 00149 00150 void l2cap_emit_connection_request(l2cap_channel_t *channel) { 00151 uint8_t event[16]; 00152 event[0] = L2CAP_EVENT_INCOMING_CONNECTION; 00153 event[1] = sizeof(event) - 2; 00154 bt_flip_addr(&event[2], channel->address); 00155 bt_store_16(event, 8, channel->handle); 00156 bt_store_16(event, 10, channel->psm); 00157 bt_store_16(event, 12, channel->local_cid); 00158 bt_store_16(event, 14, channel->remote_cid); 00159 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 00160 l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); 00161 } 00162 00163 static void l2cap_emit_service_registered(void *connection, uint8_t status, uint16_t psm){ 00164 uint8_t event[5]; 00165 event[0] = L2CAP_EVENT_SERVICE_REGISTERED; 00166 event[1] = sizeof(event) - 2; 00167 event[2] = status; 00168 bt_store_16(event, 3, psm); 00169 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 00170 (*packet_handler)(connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 00171 } 00172 00173 void l2cap_emit_credits(l2cap_channel_t *channel, uint8_t credits) { 00174 // track credits 00175 channel->packets_granted += credits; 00176 // log_info("l2cap_emit_credits for cid %u, credits given: %u (+%u)\n", channel->local_cid, channel->packets_granted, credits); 00177 00178 uint8_t event[5]; 00179 event[0] = L2CAP_EVENT_CREDITS; 00180 event[1] = sizeof(event) - 2; 00181 bt_store_16(event, 2, channel->local_cid); 00182 event[4] = credits; 00183 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 00184 l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); 00185 } 00186 00187 void l2cap_block_new_credits(uint8_t blocked){ 00188 new_credits_blocked = blocked; 00189 } 00190 00191 void l2cap_hand_out_credits(void){ 00192 00193 if (new_credits_blocked) return; // we're told not to. used by daemon 00194 00195 linked_item_t *it; 00196 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 00197 if (!hci_number_free_acl_slots()) return; 00198 l2cap_channel_t * channel = (l2cap_channel_t *) it; 00199 if (channel->state != L2CAP_STATE_OPEN) continue; 00200 if (hci_number_outgoing_packets(channel->handle) < NR_BUFFERED_ACL_PACKETS && channel->packets_granted == 0) { 00201 l2cap_emit_credits(channel, 1); 00202 } 00203 } 00204 } 00205 00206 l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){ 00207 linked_item_t *it; 00208 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 00209 l2cap_channel_t * channel = (l2cap_channel_t *) it; 00210 if ( channel->local_cid == local_cid) { 00211 return channel; 00212 } 00213 } 00214 return NULL; 00215 } 00216 00217 int l2cap_can_send_packet_now(uint16_t local_cid){ 00218 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 00219 if (!channel) return 0; 00220 if (!channel->packets_granted) return 0; 00221 return hci_can_send_packet_now(HCI_ACL_DATA_PACKET); 00222 } 00223 00224 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){ 00225 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 00226 if (channel) { 00227 return channel->remote_mtu; 00228 } 00229 return 0; 00230 } 00231 00232 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 00233 00234 if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){ 00235 log_info("l2cap_send_signaling_packet, cannot send\n"); 00236 return BTSTACK_ACL_BUFFERS_FULL; 00237 } 00238 00239 // log_info("l2cap_send_signaling_packet type %u\n", cmd); 00240 uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer(); 00241 va_list argptr; 00242 va_start(argptr, identifier); 00243 uint16_t len = l2cap_create_signaling_internal(acl_buffer, handle, cmd, identifier, argptr); 00244 va_end(argptr); 00245 // log_info("l2cap_send_signaling_packet con %u!\n", handle); 00246 return hci_send_acl_packet(acl_buffer, len); 00247 } 00248 00249 uint8_t *l2cap_get_outgoing_buffer(void){ 00250 return hci_get_outgoing_acl_packet_buffer() + COMPLETE_L2CAP_HEADER; // 8 bytes 00251 } 00252 00253 int l2cap_send_prepared(uint16_t local_cid, uint16_t len){ 00254 00255 if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){ 00256 log_info("l2cap_send_internal cid %u, cannot send\n", local_cid); 00257 return BTSTACK_ACL_BUFFERS_FULL; 00258 } 00259 00260 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 00261 if (!channel) { 00262 log_error("l2cap_send_internal no channel for cid %u\n", local_cid); 00263 return -1; // TODO: define error 00264 } 00265 00266 if (channel->packets_granted == 0){ 00267 log_error("l2cap_send_internal cid %u, no credits!\n", local_cid); 00268 return -1; // TODO: define error 00269 } 00270 00271 --channel->packets_granted; 00272 00273 log_debug("l2cap_send_internal cid %u, handle %u, 1 credit used, credits left %u;\n", 00274 local_cid, channel->handle, channel->packets_granted); 00275 00276 uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer(); 00277 00278 // 0 - Connection handle : PB=10 : BC=00 00279 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 00280 // 2 - ACL length 00281 bt_store_16(acl_buffer, 2, len + 4); 00282 // 4 - L2CAP packet length 00283 bt_store_16(acl_buffer, 4, len + 0); 00284 // 6 - L2CAP channel DEST 00285 bt_store_16(acl_buffer, 6, channel->remote_cid); 00286 // send 00287 int err = hci_send_acl_packet(acl_buffer, len+8); 00288 00289 l2cap_hand_out_credits(); 00290 00291 return err; 00292 } 00293 00294 int l2cap_send_prepared_connectionless(uint16_t handle, uint16_t cid, uint16_t len){ 00295 00296 if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){ 00297 log_info("l2cap_send_prepared_to_handle cid %u, cannot send\n", cid); 00298 return BTSTACK_ACL_BUFFERS_FULL; 00299 } 00300 00301 log_debug("l2cap_send_prepared_to_handle cid %u, handle %u\n", cid, handle); 00302 00303 uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer(); 00304 00305 // 0 - Connection handle : PB=10 : BC=00 00306 bt_store_16(acl_buffer, 0, handle | (2 << 12) | (0 << 14)); 00307 // 2 - ACL length 00308 bt_store_16(acl_buffer, 2, len + 4); 00309 // 4 - L2CAP packet length 00310 bt_store_16(acl_buffer, 4, len + 0); 00311 // 6 - L2CAP channel DEST 00312 bt_store_16(acl_buffer, 6, cid); 00313 // send 00314 int err = hci_send_acl_packet(acl_buffer, len+8); 00315 00316 l2cap_hand_out_credits(); 00317 00318 return err; 00319 } 00320 00321 int l2cap_send_internal(uint16_t local_cid, uint8_t *data, uint16_t len){ 00322 00323 if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){ 00324 log_info("l2cap_send_internal cid %u, cannot send\n", local_cid); 00325 return BTSTACK_ACL_BUFFERS_FULL; 00326 } 00327 00328 uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer(); 00329 00330 memcpy(&acl_buffer[8], data, len); 00331 00332 return l2cap_send_prepared(local_cid, len); 00333 } 00334 00335 int l2cap_send_connectionless(uint16_t handle, uint16_t cid, uint8_t *data, uint16_t len){ 00336 00337 if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)){ 00338 log_info("l2cap_send_internal cid %u, cannot send\n", cid); 00339 return BTSTACK_ACL_BUFFERS_FULL; 00340 } 00341 00342 uint8_t *acl_buffer = hci_get_outgoing_acl_packet_buffer(); 00343 00344 memcpy(&acl_buffer[8], data, len); 00345 00346 return l2cap_send_prepared_connectionless(handle, cid, len); 00347 } 00348 00349 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 00350 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag); 00351 } 00352 00353 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 00354 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag); 00355 } 00356 00357 00358 00359 // MARK: L2CAP_RUN 00360 // process outstanding signaling tasks 00361 void l2cap_run(void){ 00362 00363 // check pending signaling responses 00364 while (signaling_responses_pending){ 00365 00366 if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)) break; 00367 00368 hci_con_handle_t handle = signaling_responses[0].handle; 00369 uint8_t sig_id = signaling_responses[0].sig_id; 00370 uint16_t infoType = signaling_responses[0].data; // INFORMATION_REQUEST 00371 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST 00372 00373 switch (signaling_responses[0].code){ 00374 case CONNECTION_REQUEST: 00375 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, 0, 0, result, 0); 00376 break; 00377 case ECHO_REQUEST: 00378 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 00379 break; 00380 case INFORMATION_REQUEST: 00381 if (infoType == 2) { 00382 uint32_t features = 0; 00383 // extended features request supported, however no features present 00384 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, 4, &features); 00385 } else { 00386 // all other types are not supported 00387 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL); 00388 } 00389 break; 00390 default: 00391 // should not happen 00392 break; 00393 } 00394 00395 // remove first item 00396 signaling_responses_pending--; 00397 int i; 00398 for (i=0; i < signaling_responses_pending; i++){ 00399 memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t)); 00400 } 00401 } 00402 00403 uint8_t config_options[4]; 00404 linked_item_t *it; 00405 linked_item_t *next; 00406 for (it = (linked_item_t *) l2cap_channels; it ; it = next){ 00407 next = it->next; // cache next item as current item might get freed 00408 00409 if (!hci_can_send_packet_now(HCI_COMMAND_DATA_PACKET)) break; 00410 if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)) break; 00411 00412 l2cap_channel_t * channel = (l2cap_channel_t *) it; 00413 00414 // log_info("l2cap_run: state %u, var 0x%02x\n", channel->state, channel->state_var); 00415 00416 00417 switch (channel->state){ 00418 00419 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 00420 // send connection request - set state first 00421 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 00422 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 00423 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1); 00424 break; 00425 00426 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 00427 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, channel->reason, 0); 00428 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 00429 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); // -- remove from list 00430 btstack_memory_l2cap_channel_free(channel); 00431 break; 00432 00433 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 00434 channel->state = L2CAP_STATE_CONFIG; 00435 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 00436 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 00437 break; 00438 00439 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 00440 // success, start l2cap handshake 00441 channel->local_sig_id = l2cap_next_sig_id(); 00442 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 00443 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 00444 break; 00445 00446 case L2CAP_STATE_CONFIG: 00447 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 00448 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 00449 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 00450 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, 0, 0, 0, NULL); 00451 } 00452 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 00453 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 00454 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 00455 channel->local_sig_id = l2cap_next_sig_id(); 00456 config_options[0] = 1; // MTU 00457 config_options[1] = 2; // len param 00458 bt_store_16( (uint8_t*)&config_options, 2, channel->local_mtu); 00459 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, 4, &config_options); 00460 } 00461 if (l2cap_channel_ready_for_open(channel)){ 00462 channel->state = L2CAP_STATE_OPEN; 00463 l2cap_emit_channel_opened(channel, 0); // success 00464 l2cap_emit_credits(channel, 1); 00465 } 00466 break; 00467 00468 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 00469 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 00470 l2cap_finialize_channel_close(channel); // -- remove from list 00471 break; 00472 00473 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 00474 channel->local_sig_id = l2cap_next_sig_id(); 00475 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 00476 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 00477 break; 00478 default: 00479 break; 00480 } 00481 } 00482 } 00483 00484 uint16_t l2cap_max_mtu(void){ 00485 return hci_max_acl_data_packet_length() - L2CAP_HEADER_SIZE; 00486 } 00487 00488 // open outgoing L2CAP channel 00489 void l2cap_create_channel_internal(void * connection, btstack_packet_handler_t packet_handler, 00490 bd_addr_t address, uint16_t psm, uint16_t mtu){ 00491 00492 // alloc structure 00493 l2cap_channel_t * chan = (l2cap_channel_t*) btstack_memory_l2cap_channel_get(); 00494 if (!chan) { 00495 // emit error event 00496 l2cap_channel_t dummy_channel; 00497 BD_ADDR_COPY(dummy_channel.address, address); 00498 dummy_channel.psm = psm; 00499 l2cap_emit_channel_opened(&dummy_channel, BTSTACK_MEMORY_ALLOC_FAILED); 00500 return; 00501 } 00502 // limit local mtu to max acl packet length 00503 if (mtu > l2cap_max_mtu()) { 00504 mtu = l2cap_max_mtu(); 00505 } 00506 00507 // fill in 00508 BD_ADDR_COPY(chan->address, address); 00509 chan->psm = psm; 00510 chan->handle = 0; 00511 chan->connection = connection; 00512 chan->packet_handler = packet_handler; 00513 chan->remote_mtu = L2CAP_MINIMAL_MTU; 00514 chan->local_mtu = mtu; 00515 chan->packets_granted = 0; 00516 00517 // set initial state 00518 chan->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 00519 chan->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 00520 chan->remote_sig_id = L2CAP_SIG_ID_INVALID; 00521 chan->local_sig_id = L2CAP_SIG_ID_INVALID; 00522 00523 // add to connections list 00524 linked_list_add(&l2cap_channels, (linked_item_t *) chan); 00525 00526 l2cap_run(); 00527 } 00528 00529 void l2cap_disconnect_internal(uint16_t local_cid, uint8_t reason){ 00530 // find channel for local_cid 00531 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 00532 if (channel) { 00533 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 00534 } 00535 // process 00536 l2cap_run(); 00537 } 00538 00539 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 00540 linked_item_t *it = (linked_item_t *) &l2cap_channels; 00541 while (it->next){ 00542 l2cap_channel_t * channel = (l2cap_channel_t *) it->next; 00543 if ( ! BD_ADDR_CMP( channel->address, address) ){ 00544 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 00545 // failure, forward error code 00546 l2cap_emit_channel_opened(channel, status); 00547 // discard channel 00548 it->next = it->next->next; 00549 btstack_memory_l2cap_channel_free(channel); 00550 } 00551 } else { 00552 it = it->next; 00553 } 00554 } 00555 } 00556 00557 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 00558 linked_item_t *it; 00559 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 00560 l2cap_channel_t * channel = (l2cap_channel_t *) it; 00561 if ( ! BD_ADDR_CMP( channel->address, address) ){ 00562 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 00563 // success, start l2cap handshake 00564 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 00565 channel->handle = handle; 00566 channel->local_cid = l2cap_next_local_cid(); 00567 } 00568 } 00569 } 00570 // process 00571 l2cap_run(); 00572 } 00573 00574 void l2cap_event_handler( uint8_t *packet, uint16_t size ){ 00575 00576 bd_addr_t address; 00577 hci_con_handle_t handle; 00578 l2cap_channel_t * channel; 00579 linked_item_t *it; 00580 int hci_con_used; 00581 00582 switch(packet[0]){ 00583 00584 // handle connection complete events 00585 case HCI_EVENT_CONNECTION_COMPLETE: 00586 bt_flip_addr(address, &packet[5]); 00587 if (packet[2] == 0){ 00588 handle = READ_BT_16(packet, 3); 00589 l2cap_handle_connection_success_for_addr(address, handle); 00590 } else { 00591 l2cap_handle_connection_failed_for_addr(address, packet[2]); 00592 } 00593 break; 00594 00595 // handle successful create connection cancel command 00596 case HCI_EVENT_COMMAND_COMPLETE: 00597 if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) { 00598 if (packet[5] == 0){ 00599 bt_flip_addr(address, &packet[6]); 00600 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 00601 l2cap_handle_connection_failed_for_addr(address, 0x16); 00602 } 00603 } 00604 l2cap_run(); // try sending signaling packets first 00605 break; 00606 00607 case HCI_EVENT_COMMAND_STATUS: 00608 l2cap_run(); // try sending signaling packets first 00609 break; 00610 00611 // handle disconnection complete events 00612 case HCI_EVENT_DISCONNECTION_COMPLETE: 00613 // send l2cap disconnect events for all channels on this handle 00614 handle = READ_BT_16(packet, 3); 00615 it = (linked_item_t *) &l2cap_channels; 00616 while (it->next){ 00617 l2cap_channel_t * channel = (l2cap_channel_t *) it->next; 00618 if ( channel->handle == handle ){ 00619 // update prev item before free'ing next element - don't call l2cap_finalize_channel_close 00620 it->next = it->next->next; 00621 l2cap_emit_channel_closed(channel); 00622 btstack_memory_l2cap_channel_free(channel); 00623 } else { 00624 it = it->next; 00625 } 00626 } 00627 break; 00628 00629 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 00630 l2cap_run(); // try sending signaling packets first 00631 l2cap_hand_out_credits(); 00632 break; 00633 00634 // HCI Connection Timeouts 00635 case L2CAP_EVENT_TIMEOUT_CHECK: 00636 handle = READ_BT_16(packet, 2); 00637 if (hci_authentication_active_for_handle(handle)) break; 00638 hci_con_used = 0; 00639 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 00640 channel = (l2cap_channel_t *) it; 00641 if (channel->handle == handle) { 00642 hci_con_used = 1; 00643 } 00644 } 00645 if (hci_con_used) break; 00646 if (!hci_can_send_packet_now(HCI_COMMAND_DATA_PACKET)) break; 00647 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 00648 break; 00649 00650 case DAEMON_EVENT_HCI_PACKET_SENT: 00651 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 00652 channel = (l2cap_channel_t *) it; 00653 if (channel->packet_handler) { 00654 (* (channel->packet_handler))(HCI_EVENT_PACKET, channel->local_cid, packet, size); 00655 } 00656 } 00657 if (attribute_protocol_packet_handler) { 00658 (*attribute_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size); 00659 } 00660 if (security_protocol_packet_handler) { 00661 (*security_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size); 00662 } 00663 break; 00664 00665 default: 00666 break; 00667 } 00668 00669 // pass on 00670 (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, packet, size); 00671 } 00672 00673 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 00674 channel->remote_sig_id = identifier; 00675 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 00676 l2cap_run(); 00677 } 00678 00679 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t data){ 00680 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 00681 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 00682 signaling_responses[signaling_responses_pending].handle = handle; 00683 signaling_responses[signaling_responses_pending].code = code; 00684 signaling_responses[signaling_responses_pending].sig_id = sig_id; 00685 signaling_responses[signaling_responses_pending].data = data; 00686 signaling_responses_pending++; 00687 l2cap_run(); 00688 } 00689 } 00690 00691 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 00692 00693 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid %u\n", handle, psm, source_cid); 00694 l2cap_service_t *service = l2cap_get_service(psm); 00695 if (!service) { 00696 // 0x0002 PSM not supported 00697 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0002); 00698 return; 00699 } 00700 00701 hci_connection_t * hci_connection = connection_for_handle( handle ); 00702 if (!hci_connection) { 00703 // 00704 log_error("no hci_connection for handle %u\n", handle); 00705 return; 00706 } 00707 // alloc structure 00708 // log_info("l2cap_handle_connection_request register channel\n"); 00709 l2cap_channel_t * channel = (l2cap_channel_t*) btstack_memory_l2cap_channel_get(); 00710 if (!channel){ 00711 // 0x0004 No resources available 00712 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0004); 00713 return; 00714 } 00715 00716 // fill in 00717 BD_ADDR_COPY(channel->address, hci_connection->address); 00718 channel->psm = psm; 00719 channel->handle = handle; 00720 channel->connection = service->connection; 00721 channel->packet_handler = service->packet_handler; 00722 channel->local_cid = l2cap_next_local_cid(); 00723 channel->remote_cid = source_cid; 00724 channel->local_mtu = service->mtu; 00725 channel->remote_mtu = L2CAP_DEFAULT_MTU; 00726 channel->packets_granted = 0; 00727 channel->remote_sig_id = sig_id; 00728 00729 // limit local mtu to max acl packet length 00730 if (channel->local_mtu > l2cap_max_mtu()) { 00731 channel->local_mtu = l2cap_max_mtu(); 00732 } 00733 00734 // set initial state 00735 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 00736 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 00737 00738 // add to connections list 00739 linked_list_add(&l2cap_channels, (linked_item_t *) channel); 00740 00741 // emit incoming connection request 00742 l2cap_emit_connection_request(channel); 00743 } 00744 00745 void l2cap_accept_connection_internal(uint16_t local_cid){ 00746 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 00747 if (!channel) { 00748 log_error("l2cap_accept_connection_internal called but local_cid 0x%x not found", local_cid); 00749 return; 00750 } 00751 00752 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 00753 00754 // process 00755 l2cap_run(); 00756 } 00757 00758 void l2cap_decline_connection_internal(uint16_t local_cid, uint8_t reason){ 00759 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 00760 if (!channel) { 00761 log_error( "l2cap_decline_connection_internal called but local_cid 0x%x not found", local_cid); 00762 return; 00763 } 00764 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 00765 channel->reason = reason; 00766 l2cap_run(); 00767 } 00768 00769 void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 00770 00771 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 00772 00773 // accept the other's configuration options 00774 uint16_t end_pos = 4 + READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 00775 uint16_t pos = 8; 00776 while (pos < end_pos){ 00777 uint8_t type = command[pos++]; 00778 uint8_t length = command[pos++]; 00779 // MTU { type(8): 1, len(8):2, MTU(16) } 00780 if ((type & 0x7f) == 1 && length == 2){ 00781 channel->remote_mtu = READ_BT_16(command, pos); 00782 // log_info("l2cap cid %u, remote mtu %u\n", channel->local_cid, channel->remote_mtu); 00783 } 00784 pos += length; 00785 } 00786 } 00787 00788 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 00789 // log_info("l2cap_channel_ready_for_open 0x%02x\n", channel->state_var); 00790 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 00791 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 00792 return 1; 00793 } 00794 00795 00796 void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 00797 00798 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 00799 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 00800 uint16_t result = 0; 00801 00802 log_info("L2CAP signaling handler code %u, state %u\n", code, channel->state); 00803 00804 // handle DISCONNECT REQUESTS seperately 00805 if (code == DISCONNECTION_REQUEST){ 00806 switch (channel->state){ 00807 case L2CAP_STATE_CONFIG: 00808 case L2CAP_STATE_OPEN: 00809 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 00810 case L2CAP_STATE_WAIT_DISCONNECT: 00811 l2cap_handle_disconnect_request(channel, identifier); 00812 break; 00813 00814 default: 00815 // ignore in other states 00816 break; 00817 } 00818 return; 00819 } 00820 00821 // @STATEMACHINE(l2cap) 00822 switch (channel->state) { 00823 00824 case L2CAP_STATE_WAIT_CONNECT_RSP: 00825 switch (code){ 00826 case CONNECTION_RESPONSE: 00827 result = READ_BT_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 00828 switch (result) { 00829 case 0: 00830 // successful connection 00831 channel->remote_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 00832 channel->state = L2CAP_STATE_CONFIG; 00833 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 00834 break; 00835 case 1: 00836 // connection pending. get some coffee 00837 break; 00838 default: 00839 // channel closed 00840 channel->state = L2CAP_STATE_CLOSED; 00841 00842 // map l2cap connection response result to BTstack status enumeration 00843 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 00844 00845 // drop link key if security block 00846 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 00847 hci_drop_link_key_for_bd_addr(&channel->address); 00848 } 00849 00850 // discard channel 00851 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 00852 btstack_memory_l2cap_channel_free(channel); 00853 break; 00854 } 00855 break; 00856 00857 default: 00858 //@TODO: implement other signaling packets 00859 break; 00860 } 00861 break; 00862 00863 case L2CAP_STATE_CONFIG: 00864 switch (code) { 00865 case CONFIGURE_REQUEST: 00866 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 00867 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 00868 l2cap_signaling_handle_configure_request(channel, command); 00869 break; 00870 case CONFIGURE_RESPONSE: 00871 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 00872 break; 00873 default: 00874 break; 00875 } 00876 if (l2cap_channel_ready_for_open(channel)){ 00877 // for open: 00878 channel->state = L2CAP_STATE_OPEN; 00879 l2cap_emit_channel_opened(channel, 0); 00880 l2cap_emit_credits(channel, 1); 00881 } 00882 break; 00883 00884 case L2CAP_STATE_WAIT_DISCONNECT: 00885 switch (code) { 00886 case DISCONNECTION_RESPONSE: 00887 l2cap_finialize_channel_close(channel); 00888 break; 00889 default: 00890 //@TODO: implement other signaling packets 00891 break; 00892 } 00893 break; 00894 00895 case L2CAP_STATE_CLOSED: 00896 // @TODO handle incoming requests 00897 break; 00898 00899 case L2CAP_STATE_OPEN: 00900 //@TODO: implement other signaling packets, e.g. re-configure 00901 break; 00902 default: 00903 break; 00904 } 00905 // log_info("new state %u\n", channel->state); 00906 } 00907 00908 00909 void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){ 00910 00911 // get code, signalind identifier and command len 00912 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 00913 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 00914 00915 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_REQUEST 00916 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_REQUEST){ 00917 return; 00918 } 00919 00920 // general commands without an assigned channel 00921 switch(code) { 00922 00923 case CONNECTION_REQUEST: { 00924 uint16_t psm = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 00925 uint16_t source_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 00926 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 00927 return; 00928 } 00929 00930 case ECHO_REQUEST: 00931 l2cap_register_signaling_response(handle, code, sig_id, 0); 00932 return; 00933 00934 case INFORMATION_REQUEST: { 00935 uint16_t infoType = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 00936 l2cap_register_signaling_response(handle, code, sig_id, infoType); 00937 return; 00938 } 00939 00940 default: 00941 break; 00942 } 00943 00944 00945 // Get potential destination CID 00946 uint16_t dest_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 00947 00948 // Find channel for this sig_id and connection handle 00949 linked_item_t *it; 00950 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 00951 l2cap_channel_t * channel = (l2cap_channel_t *) it; 00952 if (channel->handle == handle) { 00953 if (code & 1) { 00954 // match odd commands (responses) by previous signaling identifier 00955 if (channel->local_sig_id == sig_id) { 00956 l2cap_signaling_handler_channel(channel, command); 00957 break; 00958 } 00959 } else { 00960 // match even commands (requests) by local channel id 00961 if (channel->local_cid == dest_cid) { 00962 l2cap_signaling_handler_channel(channel, command); 00963 break; 00964 } 00965 } 00966 } 00967 } 00968 } 00969 00970 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 00971 00972 // Get Channel ID 00973 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 00974 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 00975 00976 switch (channel_id) { 00977 00978 case L2CAP_CID_SIGNALING: { 00979 00980 uint16_t command_offset = 8; 00981 while (command_offset < size) { 00982 00983 // handle signaling commands 00984 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 00985 00986 // increment command_offset 00987 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + READ_BT_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 00988 } 00989 break; 00990 } 00991 00992 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 00993 if (attribute_protocol_packet_handler) { 00994 (*attribute_protocol_packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 00995 } 00996 break; 00997 00998 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 00999 if (security_protocol_packet_handler) { 01000 (*security_protocol_packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 01001 } 01002 break; 01003 01004 default: { 01005 // Find channel for this channel_id and connection handle 01006 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id); 01007 if (channel) { 01008 l2cap_dispatch(channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 01009 } 01010 break; 01011 } 01012 } 01013 01014 l2cap_run(); 01015 } 01016 01017 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size){ 01018 switch (packet_type) { 01019 case HCI_EVENT_PACKET: 01020 l2cap_event_handler(packet, size); 01021 break; 01022 case HCI_ACL_DATA_PACKET: 01023 l2cap_acl_handler(packet, size); 01024 break; 01025 default: 01026 break; 01027 } 01028 } 01029 01030 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 01031 void l2cap_finialize_channel_close(l2cap_channel_t *channel){ 01032 channel->state = L2CAP_STATE_CLOSED; 01033 l2cap_emit_channel_closed(channel); 01034 // discard channel 01035 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 01036 btstack_memory_l2cap_channel_free(channel); 01037 } 01038 01039 l2cap_service_t * l2cap_get_service(uint16_t psm){ 01040 linked_item_t *it; 01041 01042 // close open channels 01043 for (it = (linked_item_t *) l2cap_services; it ; it = it->next){ 01044 l2cap_service_t * service = ((l2cap_service_t *) it); 01045 if ( service->psm == psm){ 01046 return service; 01047 }; 01048 } 01049 return NULL; 01050 } 01051 01052 void l2cap_register_service_internal(void *connection, btstack_packet_handler_t packet_handler, uint16_t psm, uint16_t mtu){ 01053 // check for alread registered psm 01054 // TODO: emit error event 01055 l2cap_service_t *service = l2cap_get_service(psm); 01056 if (service) { 01057 log_error("l2cap_register_service_internal: PSM %u already registered\n", psm); 01058 l2cap_emit_service_registered(connection, L2CAP_SERVICE_ALREADY_REGISTERED, psm); 01059 return; 01060 } 01061 01062 // alloc structure 01063 // TODO: emit error event 01064 service = (l2cap_service_t *) btstack_memory_l2cap_service_get(); 01065 if (!service) { 01066 log_error("l2cap_register_service_internal: no memory for l2cap_service_t\n"); 01067 l2cap_emit_service_registered(connection, BTSTACK_MEMORY_ALLOC_FAILED, psm); 01068 return; 01069 } 01070 01071 // fill in 01072 service->psm = psm; 01073 service->mtu = mtu; 01074 service->connection = connection; 01075 service->packet_handler = packet_handler; 01076 01077 // add to services list 01078 linked_list_add(&l2cap_services, (linked_item_t *) service); 01079 01080 // enable page scan 01081 hci_connectable_control(1); 01082 01083 // done 01084 l2cap_emit_service_registered(connection, 0, psm); 01085 } 01086 01087 void l2cap_unregister_service_internal(void *connection, uint16_t psm){ 01088 l2cap_service_t *service = l2cap_get_service(psm); 01089 if (!service) return; 01090 linked_list_remove(&l2cap_services, (linked_item_t *) service); 01091 btstack_memory_l2cap_service_free(service); 01092 01093 // disable page scan when no services registered 01094 if (!linked_list_empty(&l2cap_services)) return; 01095 hci_connectable_control(0); 01096 } 01097 01098 // 01099 void l2cap_close_connection(void *connection){ 01100 linked_item_t *it; 01101 01102 // close open channels - note to myself: no channel is freed, so no new for fancy iterator tricks 01103 l2cap_channel_t * channel; 01104 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 01105 channel = (l2cap_channel_t *) it; 01106 if (channel->connection == connection) { 01107 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 01108 } 01109 } 01110 01111 // unregister services 01112 it = (linked_item_t *) &l2cap_services; 01113 while (it->next) { 01114 l2cap_service_t * service = (l2cap_service_t *) it->next; 01115 if (service->connection == connection){ 01116 it->next = it->next->next; 01117 btstack_memory_l2cap_service_free(service); 01118 } else { 01119 it = it->next; 01120 } 01121 } 01122 01123 // process 01124 l2cap_run(); 01125 } 01126 01127 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 01128 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 01129 switch(channel_id){ 01130 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 01131 attribute_protocol_packet_handler = packet_handler; 01132 break; 01133 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 01134 security_protocol_packet_handler = packet_handler; 01135 break; 01136 } 01137 } 01138
Generated on Thu Jul 14 2022 15:03:48 by
1.7.2