1 | /*
|
---|
2 | * Copyright (C) 2009-2012 by Matthias Ringwald
|
---|
3 | *
|
---|
4 | * Redistribution and use in source and binary forms, with or without
|
---|
5 | * modification, are permitted provided that the following conditions
|
---|
6 | * are met:
|
---|
7 | *
|
---|
8 | * 1. Redistributions of source code must retain the above copyright
|
---|
9 | * notice, this list of conditions and the following disclaimer.
|
---|
10 | * 2. Redistributions in binary form must reproduce the above copyright
|
---|
11 | * notice, this list of conditions and the following disclaimer in the
|
---|
12 | * documentation and/or other materials provided with the distribution.
|
---|
13 | * 3. Neither the name of the copyright holders nor the names of
|
---|
14 | * contributors may be used to endorse or promote products derived
|
---|
15 | * from this software without specific prior written permission.
|
---|
16 | * 4. Any redistribution, use, or modification is done solely for
|
---|
17 | * personal benefit and not for any commercial purpose or for
|
---|
18 | * monetary gain.
|
---|
19 | *
|
---|
20 | * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS
|
---|
21 | * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
---|
22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
---|
23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
|
---|
24 | * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
---|
25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
---|
26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
---|
27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
---|
28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
---|
29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
|
---|
30 | * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
---|
31 | * SUCH DAMAGE.
|
---|
32 | *
|
---|
33 | * Please inquire about commercial licensing options at btstack@ringwald.ch
|
---|
34 | *
|
---|
35 | */
|
---|
36 |
|
---|
37 | /*
|
---|
38 | * l2cap.c
|
---|
39 | *
|
---|
40 | * Logical Link Control and Adaption Protocl (L2CAP)
|
---|
41 | *
|
---|
42 | * Created by Matthias Ringwald on 5/16/09.
|
---|
43 | */
|
---|
44 |
|
---|
45 | #include <stdarg.h>
|
---|
46 | #include <string.h>
|
---|
47 | #include <stdio.h>
|
---|
48 |
|
---|
49 | #include "l2cap.h"
|
---|
50 | #include "hci.h"
|
---|
51 | #include "hci_dump.h"
|
---|
52 | #include "debug.h"
|
---|
53 | #include "btstack_memory.h"
|
---|
54 |
|
---|
55 | // nr of buffered acl packets in outgoing queue to get max performance
|
---|
56 | #define NR_BUFFERED_ACL_PACKETS 3
|
---|
57 |
|
---|
58 | // used to cache l2cap rejects, echo, and informational requests
|
---|
59 | #define NR_PENDING_SIGNALING_RESPONSES 3
|
---|
60 |
|
---|
61 | // offsets for L2CAP SIGNALING COMMANDS
|
---|
62 | #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET 0
|
---|
63 | #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET 1
|
---|
64 | #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2
|
---|
65 | #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET 4
|
---|
66 |
|
---|
67 | static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
|
---|
68 | static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size);
|
---|
69 |
|
---|
70 | // used to cache l2cap rejects, echo, and informational requests
|
---|
71 | static l2cap_signaling_response_t signaling_responses[NR_PENDING_SIGNALING_RESPONSES];
|
---|
72 | static int signaling_responses_pending;
|
---|
73 |
|
---|
74 | /*static*/ linked_list_t l2cap_channels;
|
---|
75 | /*static*/ linked_list_t l2cap_services;
|
---|
76 | static void (*packet_handler) (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) = null_packet_handler;
|
---|
77 | static int new_credits_blocked = 0;
|
---|
78 |
|
---|
79 | static btstack_packet_handler_t attribute_protocol_packet_handler;
|
---|
80 | static btstack_packet_handler_t security_protocol_packet_handler;
|
---|
81 | static uint8_t require_security_level2_for_outgoing_sdp;
|
---|
82 |
|
---|
83 | // prototypes
|
---|
84 | static void l2cap_finialize_channel_close(l2cap_channel_t *channel);
|
---|
85 | static l2cap_service_t * l2cap_get_service(uint16_t psm);
|
---|
86 | static void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status);
|
---|
87 | static void l2cap_emit_channel_closed(l2cap_channel_t *channel);
|
---|
88 | static void l2cap_emit_connection_request(l2cap_channel_t *channel);
|
---|
89 | static int l2cap_channel_ready_for_open(l2cap_channel_t *channel);
|
---|
90 |
|
---|
91 |
|
---|
92 | void l2cap_init(){
|
---|
93 | new_credits_blocked = 0;
|
---|
94 | signaling_responses_pending = 0;
|
---|
95 |
|
---|
96 | l2cap_channels = NULL;
|
---|
97 | l2cap_services = NULL;
|
---|
98 |
|
---|
99 | packet_handler = null_packet_handler;
|
---|
100 | attribute_protocol_packet_handler = NULL;
|
---|
101 | security_protocol_packet_handler = NULL;
|
---|
102 |
|
---|
103 | require_security_level2_for_outgoing_sdp = 0;
|
---|
104 |
|
---|
105 | //
|
---|
106 | // register callback with HCI
|
---|
107 | //
|
---|
108 | hci_register_packet_handler(&l2cap_packet_handler);
|
---|
109 | hci_connectable_control(0); // no services yet
|
---|
110 | }
|
---|
111 |
|
---|
112 |
|
---|
113 | /** Register L2CAP packet handlers */
|
---|
114 | static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
|
---|
115 | }
|
---|
116 | void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){
|
---|
117 | packet_handler = handler;
|
---|
118 | }
|
---|
119 |
|
---|
120 | // notify client/protocol handler
|
---|
121 | void l2cap_dispatch(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){
|
---|
122 | if (channel->packet_handler) {
|
---|
123 | (* (channel->packet_handler))(type, channel->local_cid, data, size);
|
---|
124 | } else {
|
---|
125 | (*packet_handler)(channel->connection, type, channel->local_cid, data, size);
|
---|
126 | }
|
---|
127 | }
|
---|
128 |
|
---|
129 | void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) {
|
---|
130 | uint8_t event[23];
|
---|
131 | log_info("L2CAP_EVENT_CHANNEL_OPENED status 0x%x addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x local_mtu %u, remote_mtu %u, flush_timeout %u",
|
---|
132 | status, bd_addr_to_str(channel->address), channel->handle, channel->psm,
|
---|
133 | channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu, channel->flush_timeout);
|
---|
134 | event[0] = L2CAP_EVENT_CHANNEL_OPENED;
|
---|
135 | event[1] = sizeof(event) - 2;
|
---|
136 | event[2] = status;
|
---|
137 | reverse_bd_addr(channel->address, &event[3]);
|
---|
138 | little_endian_store_16(event, 9, channel->handle);
|
---|
139 | little_endian_store_16(event, 11, channel->psm);
|
---|
140 | little_endian_store_16(event, 13, channel->local_cid);
|
---|
141 | little_endian_store_16(event, 15, channel->remote_cid);
|
---|
142 | little_endian_store_16(event, 17, channel->local_mtu);
|
---|
143 | little_endian_store_16(event, 19, channel->remote_mtu);
|
---|
144 | little_endian_store_16(event, 21, channel->flush_timeout);
|
---|
145 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
146 | l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
|
---|
147 | }
|
---|
148 |
|
---|
149 | void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
|
---|
150 | uint8_t event[4];
|
---|
151 | log_info("L2CAP_EVENT_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid);
|
---|
152 | event[0] = L2CAP_EVENT_CHANNEL_CLOSED;
|
---|
153 | event[1] = sizeof(event) - 2;
|
---|
154 | little_endian_store_16(event, 2, channel->local_cid);
|
---|
155 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
156 | l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
|
---|
157 | }
|
---|
158 |
|
---|
159 | void l2cap_emit_connection_request(l2cap_channel_t *channel) {
|
---|
160 | uint8_t event[16];
|
---|
161 | log_info("L2CAP_EVENT_INCOMING_CONNECTION addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x",
|
---|
162 | bd_addr_to_str(channel->address), channel->handle, channel->psm, channel->local_cid, channel->remote_cid);
|
---|
163 | event[0] = L2CAP_EVENT_INCOMING_CONNECTION;
|
---|
164 | event[1] = sizeof(event) - 2;
|
---|
165 | reverse_bd_addr(channel->address, &event[2]);
|
---|
166 | little_endian_store_16(event, 8, channel->handle);
|
---|
167 | little_endian_store_16(event, 10, channel->psm);
|
---|
168 | little_endian_store_16(event, 12, channel->local_cid);
|
---|
169 | little_endian_store_16(event, 14, channel->remote_cid);
|
---|
170 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
171 | l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
|
---|
172 | }
|
---|
173 |
|
---|
174 | void l2cap_emit_connection_parameter_update_response(uint16_t handle, uint16_t result){
|
---|
175 | uint8_t event[6];
|
---|
176 | event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE;
|
---|
177 | event[1] = 4;
|
---|
178 | little_endian_store_16(event, 2, handle);
|
---|
179 | little_endian_store_16(event, 4, result);
|
---|
180 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
181 | (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
182 | }
|
---|
183 |
|
---|
184 | static void l2cap_emit_service_registered(void *connection, uint8_t status, uint16_t psm){
|
---|
185 | uint8_t event[5];
|
---|
186 | log_info("L2CAP_EVENT_SERVICE_REGISTERED status 0x%x psm 0x%x", status, psm);
|
---|
187 | event[0] = L2CAP_EVENT_SERVICE_REGISTERED;
|
---|
188 | event[1] = sizeof(event) - 2;
|
---|
189 | event[2] = status;
|
---|
190 | little_endian_store_16(event, 3, psm);
|
---|
191 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
192 | (*packet_handler)(connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
193 | }
|
---|
194 |
|
---|
195 | void l2cap_emit_credits(l2cap_channel_t *channel, uint8_t credits) {
|
---|
196 | uint8_t event[5];
|
---|
197 | log_info("L2CAP_EVENT_CREDITS local_cid 0x%x credits %u", channel->local_cid, credits);
|
---|
198 | // track credits
|
---|
199 | channel->packets_granted += credits;
|
---|
200 |
|
---|
201 | event[0] = L2CAP_EVENT_CREDITS;
|
---|
202 | event[1] = sizeof(event) - 2;
|
---|
203 | little_endian_store_16(event, 2, channel->local_cid);
|
---|
204 | event[4] = credits;
|
---|
205 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
206 | l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
|
---|
207 | }
|
---|
208 |
|
---|
209 | void l2cap_block_new_credits(uint8_t blocked){
|
---|
210 | new_credits_blocked = blocked;
|
---|
211 | }
|
---|
212 |
|
---|
213 | void l2cap_hand_out_credits(void){
|
---|
214 | linked_list_iterator_t it;
|
---|
215 |
|
---|
216 | if (new_credits_blocked) return; // we're told not to. used by daemon
|
---|
217 |
|
---|
218 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
219 | while (linked_list_iterator_has_next(&it)){
|
---|
220 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
221 | if (!hci_number_free_acl_slots_for_handle(channel->handle)) return;
|
---|
222 | if (channel->state != L2CAP_STATE_OPEN) continue;
|
---|
223 | if (hci_number_outgoing_packets(channel->handle) < NR_BUFFERED_ACL_PACKETS && channel->packets_granted == 0) {
|
---|
224 | l2cap_emit_credits(channel, 1);
|
---|
225 | }
|
---|
226 | }
|
---|
227 | }
|
---|
228 |
|
---|
229 | l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){
|
---|
230 | linked_list_iterator_t it;
|
---|
231 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
232 | while (linked_list_iterator_has_next(&it)){
|
---|
233 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
234 | if ( channel->local_cid == local_cid) {
|
---|
235 | return channel;
|
---|
236 | }
|
---|
237 | }
|
---|
238 | return NULL;
|
---|
239 | }
|
---|
240 |
|
---|
241 | int l2cap_can_send_packet_now(uint16_t local_cid){
|
---|
242 | l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid);
|
---|
243 | if (!channel) return 0;
|
---|
244 | if (!channel->packets_granted) return 0;
|
---|
245 | return hci_can_send_acl_packet_now(channel->handle);
|
---|
246 | }
|
---|
247 |
|
---|
248 | // @deprecated
|
---|
249 | int l2cap_can_send_connectionless_packet_now(void){
|
---|
250 | // TODO provide real handle
|
---|
251 | return l2cap_can_send_fixed_channel_packet_now(0x1234);
|
---|
252 | }
|
---|
253 |
|
---|
254 | int l2cap_can_send_fixed_channel_packet_now(uint16_t handle){
|
---|
255 | return hci_can_send_acl_packet_now(handle);
|
---|
256 | }
|
---|
257 |
|
---|
258 | uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){
|
---|
259 | l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
|
---|
260 | if (channel) {
|
---|
261 | return channel->remote_mtu;
|
---|
262 | }
|
---|
263 | return 0;
|
---|
264 | }
|
---|
265 |
|
---|
266 | static l2cap_channel_t * l2cap_channel_for_rtx_timer(timer_source_t * ts){
|
---|
267 | linked_list_iterator_t it;
|
---|
268 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
269 | while (linked_list_iterator_has_next(&it)){
|
---|
270 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
271 | if ( &channel->rtx == ts) {
|
---|
272 | return channel;
|
---|
273 | }
|
---|
274 | }
|
---|
275 | return NULL;
|
---|
276 | }
|
---|
277 |
|
---|
278 | static void l2cap_rtx_timeout(timer_source_t * ts){
|
---|
279 | l2cap_channel_t * channel = l2cap_channel_for_rtx_timer(ts);
|
---|
280 | if (!ts) return;
|
---|
281 |
|
---|
282 | log_info("l2cap_rtx_timeout for local cid 0x%02x", channel->local_cid);
|
---|
283 |
|
---|
284 | // "When terminating the channel, it is not necessary to send a L2CAP_DisconnectReq
|
---|
285 | // and enter WAIT_DISCONNECT state. Channels can be transitioned directly to the CLOSED state."
|
---|
286 | // notify client
|
---|
287 | l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_RTX_TIMEOUT);
|
---|
288 |
|
---|
289 | // discard channel
|
---|
290 | // no need to stop timer here, it is removed from list during timer callback
|
---|
291 | linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
|
---|
292 | btstack_memory_l2cap_channel_free(channel);
|
---|
293 | }
|
---|
294 |
|
---|
295 | static void l2cap_stop_rtx(l2cap_channel_t * channel){
|
---|
296 | log_info("l2cap_stop_rtx for local cid 0x%02x", channel->local_cid);
|
---|
297 | run_loop_remove_timer(&channel->rtx);
|
---|
298 | }
|
---|
299 |
|
---|
300 | static void l2cap_start_rtx(l2cap_channel_t * channel){
|
---|
301 | l2cap_stop_rtx(channel);
|
---|
302 | log_info("l2cap_start_rtx for local cid 0x%02x", channel->local_cid);
|
---|
303 | run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout);
|
---|
304 | run_loop_set_timer(&channel->rtx, L2CAP_RTX_TIMEOUT_MS);
|
---|
305 | run_loop_add_timer(&channel->rtx);
|
---|
306 | }
|
---|
307 |
|
---|
308 | static void l2cap_start_ertx(l2cap_channel_t * channel){
|
---|
309 | log_info("l2cap_start_ertx for local cid 0x%02x", channel->local_cid);
|
---|
310 | l2cap_stop_rtx(channel);
|
---|
311 | run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout);
|
---|
312 | run_loop_set_timer(&channel->rtx, L2CAP_ERTX_TIMEOUT_MS);
|
---|
313 | run_loop_add_timer(&channel->rtx);
|
---|
314 | }
|
---|
315 |
|
---|
316 | void l2cap_require_security_level_2_for_outgoing_sdp(){
|
---|
317 | require_security_level2_for_outgoing_sdp = 1;
|
---|
318 | }
|
---|
319 |
|
---|
320 | static int l2cap_security_level_0_allowed_for_PSM(uint16_t psm){
|
---|
321 | return (psm == PSM_SDP) && (!require_security_level2_for_outgoing_sdp);
|
---|
322 | }
|
---|
323 |
|
---|
324 | int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){
|
---|
325 | uint8_t *acl_buffer;
|
---|
326 | va_list argptr;
|
---|
327 | uint16_t len;
|
---|
328 |
|
---|
329 | if (!hci_can_send_acl_packet_now(handle)){
|
---|
330 | log_info("l2cap_send_signaling_packet, cannot send");
|
---|
331 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
332 | }
|
---|
333 |
|
---|
334 | // log_info("l2cap_send_signaling_packet type %u", cmd);
|
---|
335 | hci_reserve_packet_buffer();
|
---|
336 | acl_buffer = hci_get_outgoing_packet_buffer();
|
---|
337 | va_start(argptr, identifier);
|
---|
338 | len = l2cap_create_signaling_classic(acl_buffer, handle, cmd, identifier, argptr);
|
---|
339 | va_end(argptr);
|
---|
340 | // log_info("l2cap_send_signaling_packet con %u!", handle);
|
---|
341 | return hci_send_acl_packet_buffer(len);
|
---|
342 | }
|
---|
343 |
|
---|
344 | #ifdef HAVE_BLE
|
---|
345 | int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){
|
---|
346 |
|
---|
347 | if (!hci_can_send_acl_packet_now(handle)){
|
---|
348 | log_info("l2cap_send_signaling_packet, cannot send");
|
---|
349 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
350 | }
|
---|
351 |
|
---|
352 | // log_info("l2cap_send_signaling_packet type %u", cmd);
|
---|
353 | hci_reserve_packet_buffer();
|
---|
354 | uint8_t *acl_buffer = hci_get_outgoing_packet_buffer();
|
---|
355 | va_list argptr;
|
---|
356 | va_start(argptr, identifier);
|
---|
357 | uint16_t len = l2cap_create_signaling_le(acl_buffer, handle, cmd, identifier, argptr);
|
---|
358 | va_end(argptr);
|
---|
359 | // log_info("l2cap_send_signaling_packet con %u!", handle);
|
---|
360 | return hci_send_acl_packet_buffer(len);
|
---|
361 | }
|
---|
362 | #endif
|
---|
363 |
|
---|
364 | uint8_t *l2cap_get_outgoing_buffer(void){
|
---|
365 | return hci_get_outgoing_packet_buffer() + COMPLETE_L2CAP_HEADER; // 8 bytes
|
---|
366 | }
|
---|
367 |
|
---|
368 | int l2cap_reserve_packet_buffer(void){
|
---|
369 | return hci_reserve_packet_buffer();
|
---|
370 | }
|
---|
371 |
|
---|
372 | void l2cap_release_packet_buffer(void){
|
---|
373 | hci_release_packet_buffer();
|
---|
374 | }
|
---|
375 |
|
---|
376 |
|
---|
377 | int l2cap_send_prepared(uint16_t local_cid, uint16_t len){
|
---|
378 | l2cap_channel_t * channel;
|
---|
379 | uint8_t *acl_buffer;
|
---|
380 | int pb;
|
---|
381 | int err;
|
---|
382 |
|
---|
383 | if (!hci_is_packet_buffer_reserved()){
|
---|
384 | log_error("l2cap_send_prepared called without reserving packet first");
|
---|
385 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
386 | }
|
---|
387 |
|
---|
388 | channel = l2cap_get_channel_for_local_cid(local_cid);
|
---|
389 | if (!channel) {
|
---|
390 | log_error("l2cap_send_prepared no channel for cid 0x%02x", local_cid);
|
---|
391 | return -1; // TODO: define error
|
---|
392 | }
|
---|
393 |
|
---|
394 | if (channel->packets_granted == 0){
|
---|
395 | log_error("l2cap_send_prepared cid 0x%02x, no credits!", local_cid);
|
---|
396 | return -1; // TODO: define error
|
---|
397 | }
|
---|
398 |
|
---|
399 | if (!hci_can_send_prepared_acl_packet_now(channel->handle)){
|
---|
400 | log_info("l2cap_send_prepared cid 0x%02x, cannot send", local_cid);
|
---|
401 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
402 | }
|
---|
403 |
|
---|
404 | --channel->packets_granted;
|
---|
405 |
|
---|
406 | log_debug("l2cap_send_prepared cid 0x%02x, handle %u, 1 credit used, credits left %u;",
|
---|
407 | local_cid, channel->handle, channel->packets_granted);
|
---|
408 |
|
---|
409 | acl_buffer = hci_get_outgoing_packet_buffer();
|
---|
410 |
|
---|
411 | pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02;
|
---|
412 |
|
---|
413 | // 0 - Connection handle : PB=pb : BC=00
|
---|
414 | little_endian_store_16(acl_buffer, 0, channel->handle | (pb << 12) | (0 << 14));
|
---|
415 | // 2 - ACL length
|
---|
416 | little_endian_store_16(acl_buffer, 2, len + 4);
|
---|
417 | // 4 - L2CAP packet length
|
---|
418 | little_endian_store_16(acl_buffer, 4, len + 0);
|
---|
419 | // 6 - L2CAP channel DEST
|
---|
420 | little_endian_store_16(acl_buffer, 6, channel->remote_cid);
|
---|
421 | // send
|
---|
422 | err = hci_send_acl_packet_buffer(len+8);
|
---|
423 |
|
---|
424 | l2cap_hand_out_credits();
|
---|
425 |
|
---|
426 | return err;
|
---|
427 | }
|
---|
428 |
|
---|
429 | int l2cap_send_prepared_connectionless(uint16_t handle, uint16_t cid, uint16_t len){
|
---|
430 | uint8_t *acl_buffer;
|
---|
431 | int pb;
|
---|
432 | int err;
|
---|
433 |
|
---|
434 | if (!hci_is_packet_buffer_reserved()){
|
---|
435 | log_error("l2cap_send_prepared_connectionless called without reserving packet first");
|
---|
436 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
437 | }
|
---|
438 |
|
---|
439 | if (!hci_can_send_prepared_acl_packet_now(handle)){
|
---|
440 | log_info("l2cap_send_prepared_connectionless handle 0x%02x, cid 0x%02x, cannot send", handle, cid);
|
---|
441 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
442 | }
|
---|
443 |
|
---|
444 | log_debug("l2cap_send_prepared_connectionless handle %u, cid 0x%02x", handle, cid);
|
---|
445 |
|
---|
446 | acl_buffer = hci_get_outgoing_packet_buffer();
|
---|
447 |
|
---|
448 | pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02;
|
---|
449 |
|
---|
450 | // 0 - Connection handle : PB=pb : BC=00
|
---|
451 | little_endian_store_16(acl_buffer, 0, handle | (pb << 12) | (0 << 14));
|
---|
452 | // 2 - ACL length
|
---|
453 | little_endian_store_16(acl_buffer, 2, len + 4);
|
---|
454 | // 4 - L2CAP packet length
|
---|
455 | little_endian_store_16(acl_buffer, 4, len + 0);
|
---|
456 | // 6 - L2CAP channel DEST
|
---|
457 | little_endian_store_16(acl_buffer, 6, cid);
|
---|
458 | // send
|
---|
459 | err = hci_send_acl_packet_buffer(len+8);
|
---|
460 |
|
---|
461 | l2cap_hand_out_credits();
|
---|
462 |
|
---|
463 | return err;
|
---|
464 | }
|
---|
465 |
|
---|
466 | int l2cap_send_internal(uint16_t local_cid, uint8_t *data, uint16_t len){
|
---|
467 |
|
---|
468 | l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
|
---|
469 | uint8_t *acl_buffer;
|
---|
470 |
|
---|
471 | if (!channel) {
|
---|
472 | log_error("l2cap_send_internal no channel for cid 0x%02x", local_cid);
|
---|
473 | return -1; // TODO: define error
|
---|
474 | }
|
---|
475 |
|
---|
476 | if (len > channel->remote_mtu){
|
---|
477 | log_error("l2cap_send_internal cid 0x%02x, data length exceeds remote MTU.", local_cid);
|
---|
478 | return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU;
|
---|
479 | }
|
---|
480 |
|
---|
481 | if (!hci_can_send_acl_packet_now(channel->handle)){
|
---|
482 | log_info("l2cap_send_internal cid 0x%02x, cannot send", local_cid);
|
---|
483 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
484 | }
|
---|
485 |
|
---|
486 | hci_reserve_packet_buffer();
|
---|
487 | acl_buffer = hci_get_outgoing_packet_buffer();
|
---|
488 |
|
---|
489 | memcpy(&acl_buffer[8], data, len);
|
---|
490 |
|
---|
491 | return l2cap_send_prepared(local_cid, len);
|
---|
492 | }
|
---|
493 |
|
---|
494 | int l2cap_send_connectionless(uint16_t handle, uint16_t cid, uint8_t *data, uint16_t len){
|
---|
495 | uint8_t *acl_buffer;
|
---|
496 |
|
---|
497 | if (!hci_can_send_acl_packet_now(handle)){
|
---|
498 | log_info("l2cap_send_internal cid 0x%02x, cannot send", cid);
|
---|
499 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
500 | }
|
---|
501 |
|
---|
502 | hci_reserve_packet_buffer();
|
---|
503 | acl_buffer = hci_get_outgoing_packet_buffer();
|
---|
504 |
|
---|
505 | memcpy(&acl_buffer[8], data, len);
|
---|
506 |
|
---|
507 | return l2cap_send_prepared_connectionless(handle, cid, len);
|
---|
508 | }
|
---|
509 |
|
---|
510 | int l2cap_send_echo_request(uint16_t handle, uint8_t *data, uint16_t len){
|
---|
511 | return l2cap_send_signaling_packet(handle, ECHO_REQUEST, 0x77, len, data);
|
---|
512 | }
|
---|
513 |
|
---|
514 | static /*inline*/ void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){
|
---|
515 | channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag);
|
---|
516 | }
|
---|
517 |
|
---|
518 | static /*inline*/ void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){
|
---|
519 | channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag);
|
---|
520 | }
|
---|
521 |
|
---|
522 |
|
---|
523 |
|
---|
524 | // MARK: L2CAP_RUN
|
---|
525 | // process outstanding signaling tasks
|
---|
526 | void l2cap_run(void){
|
---|
527 | uint8_t config_options[4];
|
---|
528 | linked_list_iterator_t it;
|
---|
529 |
|
---|
530 | // check pending signaling responses
|
---|
531 | while (signaling_responses_pending){
|
---|
532 |
|
---|
533 | hci_con_handle_t handle = signaling_responses[0].handle;
|
---|
534 | uint8_t sig_id;
|
---|
535 | uint16_t infoType;
|
---|
536 | uint16_t result;
|
---|
537 | uint8_t response_code;
|
---|
538 | int i;
|
---|
539 |
|
---|
540 | if (!hci_can_send_acl_packet_now(handle)) break;
|
---|
541 |
|
---|
542 | sig_id = signaling_responses[0].sig_id;
|
---|
543 | infoType = signaling_responses[0].data; // INFORMATION_REQUEST
|
---|
544 | result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT
|
---|
545 | response_code = signaling_responses[0].code;
|
---|
546 |
|
---|
547 | // remove first item before sending (to avoid sending response mutliple times)
|
---|
548 | signaling_responses_pending--;
|
---|
549 | for (i=0; i < signaling_responses_pending; i++){
|
---|
550 | memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t));
|
---|
551 | }
|
---|
552 |
|
---|
553 | switch (response_code){
|
---|
554 | case CONNECTION_REQUEST:
|
---|
555 | l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, 0, 0, result, 0);
|
---|
556 | // also disconnect if result is 0x0003 - security blocked
|
---|
557 | if (result == 0x0003){
|
---|
558 | hci_disconnect_security_block(handle);
|
---|
559 | }
|
---|
560 | break;
|
---|
561 | case ECHO_REQUEST:
|
---|
562 | l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL);
|
---|
563 | break;
|
---|
564 | case INFORMATION_REQUEST:
|
---|
565 | switch (infoType){
|
---|
566 | case 1: { // Connectionless MTU
|
---|
567 | uint16_t connectionless_mtu = hci_max_acl_data_packet_length();
|
---|
568 | l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(connectionless_mtu), &connectionless_mtu);
|
---|
569 | break;
|
---|
570 | }
|
---|
571 | case 2: { // Extended Features Supported
|
---|
572 | // extended features request supported, only supporing fixed channel map
|
---|
573 | uint32_t features = 0x80;
|
---|
574 | l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(features), &features);
|
---|
575 | break;
|
---|
576 | }
|
---|
577 | case 3: { // Fixed Channels Supported
|
---|
578 | uint8_t map[8];
|
---|
579 | memset(map, 0, 8);
|
---|
580 | map[0] = 0x01; // L2CAP Signaling Channel
|
---|
581 | l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(map), &map);
|
---|
582 | break;
|
---|
583 | }
|
---|
584 | default:
|
---|
585 | // all other types are not supported
|
---|
586 | l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL);
|
---|
587 | break;
|
---|
588 | }
|
---|
589 | break;
|
---|
590 | case COMMAND_REJECT:
|
---|
591 | l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL);
|
---|
592 | #ifdef HAVE_BLE
|
---|
593 | case COMMAND_REJECT_LE:
|
---|
594 | l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL);
|
---|
595 | break;
|
---|
596 | #endif
|
---|
597 | default:
|
---|
598 | // should not happen
|
---|
599 | break;
|
---|
600 | }
|
---|
601 | }
|
---|
602 |
|
---|
603 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
604 | while (linked_list_iterator_has_next(&it)){
|
---|
605 |
|
---|
606 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
607 | // log_info("l2cap_run: state %u, var 0x%02x", channel->state, channel->state_var);
|
---|
608 | switch (channel->state){
|
---|
609 |
|
---|
610 | case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE:
|
---|
611 | case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT:
|
---|
612 | if (!hci_can_send_acl_packet_now(channel->handle)) break;
|
---|
613 | if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) {
|
---|
614 | channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND);
|
---|
615 | l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0);
|
---|
616 | }
|
---|
617 | break;
|
---|
618 |
|
---|
619 | case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION:
|
---|
620 | if (!hci_can_send_command_packet_now()) break;
|
---|
621 | // send connection request - set state first
|
---|
622 | channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE;
|
---|
623 | // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch
|
---|
624 | hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1);
|
---|
625 | break;
|
---|
626 |
|
---|
627 | case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE:
|
---|
628 | if (!hci_can_send_acl_packet_now(channel->handle)) break;
|
---|
629 | l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0);
|
---|
630 | // discard channel - l2cap_finialize_channel_close without sending l2cap close event
|
---|
631 | l2cap_stop_rtx(channel);
|
---|
632 | linked_list_iterator_remove(&it);
|
---|
633 | btstack_memory_l2cap_channel_free(channel);
|
---|
634 | break;
|
---|
635 |
|
---|
636 | case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT:
|
---|
637 | if (!hci_can_send_acl_packet_now(channel->handle)) break;
|
---|
638 | channel->state = L2CAP_STATE_CONFIG;
|
---|
639 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ);
|
---|
640 | l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0);
|
---|
641 | break;
|
---|
642 |
|
---|
643 | case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST:
|
---|
644 | if (!hci_can_send_acl_packet_now(channel->handle)) break;
|
---|
645 | // success, start l2cap handshake
|
---|
646 | channel->local_sig_id = l2cap_next_sig_id();
|
---|
647 | channel->state = L2CAP_STATE_WAIT_CONNECT_RSP;
|
---|
648 | l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid);
|
---|
649 | l2cap_start_rtx(channel);
|
---|
650 | break;
|
---|
651 |
|
---|
652 | case L2CAP_STATE_CONFIG:
|
---|
653 | if (!hci_can_send_acl_packet_now(channel->handle)) break;
|
---|
654 | if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){
|
---|
655 | uint16_t flags = 0;
|
---|
656 | channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP);
|
---|
657 | if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) {
|
---|
658 | flags = 1;
|
---|
659 | } else {
|
---|
660 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP);
|
---|
661 | }
|
---|
662 | if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){
|
---|
663 | l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL);
|
---|
664 | } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){
|
---|
665 | config_options[0] = 1; // MTU
|
---|
666 | config_options[1] = 2; // len param
|
---|
667 | little_endian_store_16( (uint8_t*)&config_options, 2, channel->remote_mtu);
|
---|
668 | l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, 0, 4, &config_options);
|
---|
669 | channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU);
|
---|
670 | } else {
|
---|
671 | l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, 0, 0, NULL);
|
---|
672 | }
|
---|
673 | channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT);
|
---|
674 | }
|
---|
675 | else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){
|
---|
676 | channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ);
|
---|
677 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ);
|
---|
678 | channel->local_sig_id = l2cap_next_sig_id();
|
---|
679 | config_options[0] = 1; // MTU
|
---|
680 | config_options[1] = 2; // len param
|
---|
681 | little_endian_store_16( (uint8_t*)&config_options, 2, channel->local_mtu);
|
---|
682 | l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, 4, &config_options);
|
---|
683 | l2cap_start_rtx(channel);
|
---|
684 | }
|
---|
685 | if (l2cap_channel_ready_for_open(channel)){
|
---|
686 | channel->state = L2CAP_STATE_OPEN;
|
---|
687 | l2cap_emit_channel_opened(channel, 0); // success
|
---|
688 | l2cap_emit_credits(channel, 1);
|
---|
689 | }
|
---|
690 | break;
|
---|
691 |
|
---|
692 | case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE:
|
---|
693 | if (!hci_can_send_acl_packet_now(channel->handle)) break;
|
---|
694 | l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid);
|
---|
695 | // we don't start an RTX timer for a disconnect - there's no point in closing the channel if the other side doesn't respond :)
|
---|
696 | l2cap_finialize_channel_close(channel); // -- remove from list
|
---|
697 | break;
|
---|
698 |
|
---|
699 | case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST:
|
---|
700 | if (!hci_can_send_acl_packet_now(channel->handle)) break;
|
---|
701 | channel->local_sig_id = l2cap_next_sig_id();
|
---|
702 | channel->state = L2CAP_STATE_WAIT_DISCONNECT;
|
---|
703 | l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid);
|
---|
704 | break;
|
---|
705 | default:
|
---|
706 | break;
|
---|
707 | }
|
---|
708 | }
|
---|
709 |
|
---|
710 | #ifdef HAVE_BLE
|
---|
711 | // send l2cap con paramter update if necessary
|
---|
712 | hci_connections_get_iterator(&it);
|
---|
713 | while(linked_list_iterator_has_next(&it)){
|
---|
714 | hci_connection_t * connection = (hci_connection_t *) linked_list_iterator_next(&it);
|
---|
715 | int result;
|
---|
716 |
|
---|
717 | switch (connection->le_con_parameter_update_state){
|
---|
718 | case CON_PARAMETER_UPDATE_SEND_RESPONSE:
|
---|
719 | result = 0;
|
---|
720 | break;
|
---|
721 | case CON_PARAMETER_UPDATE_DENY:
|
---|
722 | result = 1;
|
---|
723 | break;
|
---|
724 | default:
|
---|
725 | result = -1;
|
---|
726 | break;
|
---|
727 | }
|
---|
728 | if (result < 0) break;
|
---|
729 |
|
---|
730 | if (!hci_can_send_acl_packet_now(connection->con_handle)) break;
|
---|
731 | hci_reserve_packet_buffer();
|
---|
732 | uint8_t *acl_buffer = hci_get_outgoing_packet_buffer();
|
---|
733 | connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS;
|
---|
734 | uint16_t len = l2cap_le_create_connection_parameter_update_response(acl_buffer, connection->con_handle, 0);
|
---|
735 | hci_send_acl_packet_buffer(len);
|
---|
736 | }
|
---|
737 | #endif
|
---|
738 |
|
---|
739 | }
|
---|
740 |
|
---|
741 | uint16_t l2cap_max_mtu(void){
|
---|
742 | return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE;
|
---|
743 | }
|
---|
744 |
|
---|
745 | uint16_t l2cap_max_le_mtu(){
|
---|
746 | return l2cap_max_mtu();
|
---|
747 | }
|
---|
748 |
|
---|
749 | static void l2cap_handle_connection_complete(uint16_t handle, l2cap_channel_t * channel){
|
---|
750 | if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) {
|
---|
751 | log_info("l2cap_handle_connection_complete expected state");
|
---|
752 | // success, start l2cap handshake
|
---|
753 | channel->handle = handle;
|
---|
754 | channel->local_cid = l2cap_next_local_cid();
|
---|
755 | // check remote SSP feature first
|
---|
756 | channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES;
|
---|
757 | }
|
---|
758 | }
|
---|
759 |
|
---|
760 | static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){
|
---|
761 | if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return;
|
---|
762 |
|
---|
763 | // we have been waiting for remote supported features, if both support SSP,
|
---|
764 | log_info("l2cap received remote supported features, sec_level_0_allowed for psm %u = %u", channel->psm, l2cap_security_level_0_allowed_for_PSM(channel->psm));
|
---|
765 | if (hci_ssp_supported_on_both_sides(channel->handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){
|
---|
766 | // request security level 2
|
---|
767 | channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE;
|
---|
768 | gap_request_security_level(channel->handle, LEVEL_2);
|
---|
769 | return;
|
---|
770 | }
|
---|
771 | // fine, go ahead
|
---|
772 | channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST;
|
---|
773 | }
|
---|
774 |
|
---|
775 | // open outgoing L2CAP channel
|
---|
776 | void l2cap_create_channel_internal(void * connection, btstack_packet_handler_t packet_handler,
|
---|
777 | bd_addr_t address, uint16_t psm, uint16_t mtu){
|
---|
778 | l2cap_channel_t * chan;
|
---|
779 | hci_connection_t * conn;
|
---|
780 |
|
---|
781 | log_info("L2CAP_CREATE_CHANNEL_MTU addr %s psm 0x%x mtu %u", bd_addr_to_str(address), psm, mtu);
|
---|
782 |
|
---|
783 | // alloc structure
|
---|
784 | chan = btstack_memory_l2cap_channel_get();
|
---|
785 | if (!chan) {
|
---|
786 | // emit error event
|
---|
787 | l2cap_channel_t dummy_channel;
|
---|
788 | memset(&dummy_channel, 0, sizeof(dummy_channel));
|
---|
789 | BD_ADDR_COPY(dummy_channel.address, address);
|
---|
790 | dummy_channel.psm = psm;
|
---|
791 | l2cap_emit_channel_opened(&dummy_channel, BTSTACK_MEMORY_ALLOC_FAILED);
|
---|
792 | return;
|
---|
793 | }
|
---|
794 | // Init memory (make valgrind happy)
|
---|
795 | memset(chan, 0, sizeof(l2cap_channel_t));
|
---|
796 | // limit local mtu to max acl packet length - l2cap header
|
---|
797 | if (mtu > l2cap_max_mtu()) {
|
---|
798 | mtu = l2cap_max_mtu();
|
---|
799 | }
|
---|
800 |
|
---|
801 | // fill in
|
---|
802 | BD_ADDR_COPY(chan->address, address);
|
---|
803 | chan->psm = psm;
|
---|
804 | chan->handle = 0;
|
---|
805 | chan->connection = connection;
|
---|
806 | chan->packet_handler = packet_handler;
|
---|
807 | chan->remote_mtu = L2CAP_MINIMAL_MTU;
|
---|
808 | chan->local_mtu = mtu;
|
---|
809 | chan->packets_granted = 0;
|
---|
810 |
|
---|
811 | // set initial state
|
---|
812 | chan->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION;
|
---|
813 | chan->state_var = L2CAP_CHANNEL_STATE_VAR_NONE;
|
---|
814 | chan->remote_sig_id = L2CAP_SIG_ID_INVALID;
|
---|
815 | chan->local_sig_id = L2CAP_SIG_ID_INVALID;
|
---|
816 | chan->required_security_level = LEVEL_0;
|
---|
817 |
|
---|
818 | // add to connections list
|
---|
819 | linked_list_add(&l2cap_channels, (linked_item_t *) chan);
|
---|
820 |
|
---|
821 | // check if hci connection is already usable
|
---|
822 | conn = hci_connection_for_bd_addr_and_type((bd_addr_t*)address, BD_ADDR_TYPE_CLASSIC);
|
---|
823 | if (conn){
|
---|
824 | log_info("l2cap_create_channel_internal, hci connection already exists");
|
---|
825 | l2cap_handle_connection_complete(conn->con_handle, chan);
|
---|
826 | // check ir remote supported fearures are already received
|
---|
827 | if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) {
|
---|
828 | l2cap_handle_remote_supported_features_received(chan);
|
---|
829 | }
|
---|
830 | }
|
---|
831 |
|
---|
832 | l2cap_run();
|
---|
833 | }
|
---|
834 |
|
---|
835 | void l2cap_disconnect_internal(uint16_t local_cid, uint8_t reason){
|
---|
836 | l2cap_channel_t * channel;
|
---|
837 |
|
---|
838 | log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason);
|
---|
839 | // find channel for local_cid
|
---|
840 | channel = l2cap_get_channel_for_local_cid(local_cid);
|
---|
841 | if (channel) {
|
---|
842 | channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
|
---|
843 | }
|
---|
844 | // process
|
---|
845 | l2cap_run();
|
---|
846 | }
|
---|
847 |
|
---|
848 | static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){
|
---|
849 | linked_list_iterator_t it;
|
---|
850 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
851 | while (linked_list_iterator_has_next(&it)){
|
---|
852 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
853 | if ( BD_ADDR_CMP( channel->address, address) != 0) continue;
|
---|
854 | // channel for this address found
|
---|
855 | switch (channel->state){
|
---|
856 | case L2CAP_STATE_WAIT_CONNECTION_COMPLETE:
|
---|
857 | case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION:
|
---|
858 | // failure, forward error code
|
---|
859 | l2cap_emit_channel_opened(channel, status);
|
---|
860 | // discard channel
|
---|
861 | l2cap_stop_rtx(channel);
|
---|
862 | linked_list_iterator_remove(&it);
|
---|
863 | btstack_memory_l2cap_channel_free(channel);
|
---|
864 | break;
|
---|
865 | default:
|
---|
866 | break;
|
---|
867 | }
|
---|
868 | }
|
---|
869 | }
|
---|
870 |
|
---|
871 | static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){
|
---|
872 | linked_list_iterator_t it;
|
---|
873 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
874 | while (linked_list_iterator_has_next(&it)){
|
---|
875 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
876 | if ( ! BD_ADDR_CMP( channel->address, address) ){
|
---|
877 | l2cap_handle_connection_complete(handle, channel);
|
---|
878 | }
|
---|
879 | }
|
---|
880 | // process
|
---|
881 | l2cap_run();
|
---|
882 | }
|
---|
883 |
|
---|
884 | void l2cap_event_handler(uint8_t *packet, uint16_t size){
|
---|
885 |
|
---|
886 | bd_addr_t address;
|
---|
887 | hci_con_handle_t handle;
|
---|
888 | linked_list_iterator_t it;
|
---|
889 | int hci_con_used;
|
---|
890 |
|
---|
891 | switch(packet[0]){
|
---|
892 |
|
---|
893 | // handle connection complete events
|
---|
894 | case HCI_EVENT_CONNECTION_COMPLETE:
|
---|
895 | reverse_bd_addr(&packet[5], address);
|
---|
896 | if (packet[2] == 0){
|
---|
897 | handle = little_endian_read_16(packet, 3);
|
---|
898 | l2cap_handle_connection_success_for_addr(address, handle);
|
---|
899 | } else {
|
---|
900 | l2cap_handle_connection_failed_for_addr(address, packet[2]);
|
---|
901 | }
|
---|
902 | break;
|
---|
903 |
|
---|
904 | // handle successful create connection cancel command
|
---|
905 | case HCI_EVENT_COMMAND_COMPLETE:
|
---|
906 | if ( HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel) ) {
|
---|
907 | if (packet[5] == 0){
|
---|
908 | reverse_bd_addr(&packet[6], address);
|
---|
909 | // CONNECTION TERMINATED BY LOCAL HOST (0X16)
|
---|
910 | l2cap_handle_connection_failed_for_addr(address, 0x16);
|
---|
911 | }
|
---|
912 | }
|
---|
913 | l2cap_run(); // try sending signaling packets first
|
---|
914 | break;
|
---|
915 |
|
---|
916 | case HCI_EVENT_COMMAND_STATUS:
|
---|
917 | l2cap_run(); // try sending signaling packets first
|
---|
918 | break;
|
---|
919 |
|
---|
920 | // handle disconnection complete events
|
---|
921 | case HCI_EVENT_DISCONNECTION_COMPLETE:
|
---|
922 | // send l2cap disconnect events for all channels on this handle and free them
|
---|
923 | handle = little_endian_read_16(packet, 3);
|
---|
924 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
925 | while (linked_list_iterator_has_next(&it)){
|
---|
926 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
927 | if (channel->handle != handle) continue;
|
---|
928 | l2cap_emit_channel_closed(channel);
|
---|
929 | l2cap_stop_rtx(channel);
|
---|
930 | linked_list_iterator_remove(&it);
|
---|
931 | btstack_memory_l2cap_channel_free(channel);
|
---|
932 | }
|
---|
933 | break;
|
---|
934 |
|
---|
935 | case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS:
|
---|
936 | l2cap_run(); // try sending signaling packets first
|
---|
937 | l2cap_hand_out_credits();
|
---|
938 | break;
|
---|
939 |
|
---|
940 | // HCI Connection Timeouts
|
---|
941 | case L2CAP_EVENT_TIMEOUT_CHECK:
|
---|
942 | handle = little_endian_read_16(packet, 2);
|
---|
943 | if (hci_authentication_active_for_handle(handle)) break;
|
---|
944 | hci_con_used = 0;
|
---|
945 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
946 | while (linked_list_iterator_has_next(&it)){
|
---|
947 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
948 | if (channel->handle != handle) continue;
|
---|
949 | hci_con_used = 1;
|
---|
950 | break;
|
---|
951 | }
|
---|
952 | if (hci_con_used) break;
|
---|
953 | if (!hci_can_send_command_packet_now()) break;
|
---|
954 | hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection
|
---|
955 | break;
|
---|
956 |
|
---|
957 | case DAEMON_EVENT_HCI_PACKET_SENT:
|
---|
958 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
959 | while (linked_list_iterator_has_next(&it)){
|
---|
960 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
961 | if (!channel->packet_handler) continue;
|
---|
962 | (* (channel->packet_handler))(HCI_EVENT_PACKET, channel->local_cid, packet, size);
|
---|
963 | }
|
---|
964 | if (attribute_protocol_packet_handler) {
|
---|
965 | (*attribute_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size);
|
---|
966 | }
|
---|
967 | if (security_protocol_packet_handler) {
|
---|
968 | (*security_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size);
|
---|
969 | }
|
---|
970 | break;
|
---|
971 |
|
---|
972 | case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE:
|
---|
973 | handle = little_endian_read_16(packet, 3);
|
---|
974 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
975 | while (linked_list_iterator_has_next(&it)){
|
---|
976 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
977 | if (channel->handle != handle) continue;
|
---|
978 | l2cap_handle_remote_supported_features_received(channel);
|
---|
979 | break;
|
---|
980 | }
|
---|
981 | break;
|
---|
982 |
|
---|
983 | case GAP_SECURITY_LEVEL:
|
---|
984 | handle = little_endian_read_16(packet, 2);
|
---|
985 | log_info("l2cap - security level update");
|
---|
986 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
987 | while (linked_list_iterator_has_next(&it)){
|
---|
988 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
989 | gap_security_level_t actual_level;
|
---|
990 | gap_security_level_t required_level;
|
---|
991 |
|
---|
992 | if (channel->handle != handle) continue;
|
---|
993 |
|
---|
994 | log_info("l2cap - state %u", channel->state);
|
---|
995 |
|
---|
996 | actual_level = packet[4];
|
---|
997 | required_level = channel->required_security_level;
|
---|
998 |
|
---|
999 | switch (channel->state){
|
---|
1000 | case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE:
|
---|
1001 | if (actual_level >= required_level){
|
---|
1002 | channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT;
|
---|
1003 | l2cap_emit_connection_request(channel);
|
---|
1004 | } else {
|
---|
1005 | channel->reason = 0x03; // security block
|
---|
1006 | channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE;
|
---|
1007 | }
|
---|
1008 | break;
|
---|
1009 |
|
---|
1010 | case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE:
|
---|
1011 | if (actual_level >= required_level){
|
---|
1012 | channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST;
|
---|
1013 | } else {
|
---|
1014 | // disconnnect, authentication not good enough
|
---|
1015 | hci_disconnect_security_block(handle);
|
---|
1016 | }
|
---|
1017 | break;
|
---|
1018 |
|
---|
1019 | default:
|
---|
1020 | break;
|
---|
1021 | }
|
---|
1022 | }
|
---|
1023 | break;
|
---|
1024 |
|
---|
1025 | default:
|
---|
1026 | break;
|
---|
1027 | }
|
---|
1028 |
|
---|
1029 | // pass on: main packet handler, att and sm packet handlers
|
---|
1030 | (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, packet, size);
|
---|
1031 | if (attribute_protocol_packet_handler){
|
---|
1032 | (*attribute_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size);
|
---|
1033 | }
|
---|
1034 | if (security_protocol_packet_handler) {
|
---|
1035 | (*security_protocol_packet_handler)(HCI_EVENT_PACKET, 0, packet, size);
|
---|
1036 | }
|
---|
1037 |
|
---|
1038 | l2cap_run();
|
---|
1039 | }
|
---|
1040 |
|
---|
1041 | static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){
|
---|
1042 | channel->remote_sig_id = identifier;
|
---|
1043 | channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE;
|
---|
1044 | l2cap_run();
|
---|
1045 | }
|
---|
1046 |
|
---|
1047 | static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t data){
|
---|
1048 | // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused."
|
---|
1049 | if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) {
|
---|
1050 | signaling_responses[signaling_responses_pending].handle = handle;
|
---|
1051 | signaling_responses[signaling_responses_pending].code = code;
|
---|
1052 | signaling_responses[signaling_responses_pending].sig_id = sig_id;
|
---|
1053 | signaling_responses[signaling_responses_pending].data = data;
|
---|
1054 | signaling_responses_pending++;
|
---|
1055 | l2cap_run();
|
---|
1056 | }
|
---|
1057 | }
|
---|
1058 |
|
---|
1059 | static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){
|
---|
1060 |
|
---|
1061 | // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid);
|
---|
1062 | l2cap_service_t *service = l2cap_get_service(psm);
|
---|
1063 | hci_connection_t * hci_connection;
|
---|
1064 | l2cap_channel_t * channel;
|
---|
1065 | if (!service) {
|
---|
1066 | // 0x0002 PSM not supported
|
---|
1067 | l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0002);
|
---|
1068 | return;
|
---|
1069 | }
|
---|
1070 |
|
---|
1071 | hci_connection = hci_connection_for_handle( handle );
|
---|
1072 | if (!hci_connection) {
|
---|
1073 | //
|
---|
1074 | log_error("no hci_connection for handle %u", handle);
|
---|
1075 | return;
|
---|
1076 | }
|
---|
1077 |
|
---|
1078 | // reject connection (0x03 security block) and disconnect if both have SSP, connection is not encrypted and PSM != SDP
|
---|
1079 | if ( hci_ssp_supported_on_both_sides(handle)
|
---|
1080 | && gap_security_level(handle) == LEVEL_0
|
---|
1081 | && !l2cap_security_level_0_allowed_for_PSM(psm)){
|
---|
1082 |
|
---|
1083 | // 0x0003 Security Block
|
---|
1084 | l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0003);
|
---|
1085 | return;
|
---|
1086 | }
|
---|
1087 |
|
---|
1088 |
|
---|
1089 | // alloc structure
|
---|
1090 | // log_info("l2cap_handle_connection_request register channel");
|
---|
1091 | channel = btstack_memory_l2cap_channel_get();
|
---|
1092 | if (!channel){
|
---|
1093 | // 0x0004 No resources available
|
---|
1094 | l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0004);
|
---|
1095 | return;
|
---|
1096 | }
|
---|
1097 | // Init memory (make valgrind happy)
|
---|
1098 | memset(channel, 0, sizeof(l2cap_channel_t));
|
---|
1099 | // fill in
|
---|
1100 | BD_ADDR_COPY(channel->address, hci_connection->address);
|
---|
1101 | channel->psm = psm;
|
---|
1102 | channel->handle = handle;
|
---|
1103 | channel->connection = service->connection;
|
---|
1104 | channel->packet_handler = service->packet_handler;
|
---|
1105 | channel->local_cid = l2cap_next_local_cid();
|
---|
1106 | channel->remote_cid = source_cid;
|
---|
1107 | channel->local_mtu = service->mtu;
|
---|
1108 | channel->remote_mtu = L2CAP_DEFAULT_MTU;
|
---|
1109 | channel->packets_granted = 0;
|
---|
1110 | channel->remote_sig_id = sig_id;
|
---|
1111 | channel->required_security_level = service->required_security_level;
|
---|
1112 |
|
---|
1113 | // limit local mtu to max acl packet length - l2cap header
|
---|
1114 | if (channel->local_mtu > l2cap_max_mtu()) {
|
---|
1115 | channel->local_mtu = l2cap_max_mtu();
|
---|
1116 | }
|
---|
1117 |
|
---|
1118 | // set initial state
|
---|
1119 | channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE;
|
---|
1120 | channel->state_var = L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND;
|
---|
1121 |
|
---|
1122 | // add to connections list
|
---|
1123 | linked_list_add(&l2cap_channels, (linked_item_t *) channel);
|
---|
1124 |
|
---|
1125 | // assert security requirements
|
---|
1126 | gap_request_security_level(handle, channel->required_security_level);
|
---|
1127 | }
|
---|
1128 |
|
---|
1129 | void l2cap_accept_connection_internal(uint16_t local_cid){
|
---|
1130 | l2cap_channel_t * channel;
|
---|
1131 | log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid);
|
---|
1132 | channel = l2cap_get_channel_for_local_cid(local_cid);
|
---|
1133 | if (!channel) {
|
---|
1134 | log_error("l2cap_accept_connection_internal called but local_cid 0x%x not found", local_cid);
|
---|
1135 | return;
|
---|
1136 | }
|
---|
1137 |
|
---|
1138 | channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT;
|
---|
1139 |
|
---|
1140 | // process
|
---|
1141 | l2cap_run();
|
---|
1142 | }
|
---|
1143 |
|
---|
1144 | void l2cap_decline_connection_internal(uint16_t local_cid, uint8_t reason){
|
---|
1145 | l2cap_channel_t * channel;
|
---|
1146 | log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x, reason %x", local_cid, reason);
|
---|
1147 | channel = l2cap_get_channel_for_local_cid( local_cid);
|
---|
1148 | if (!channel) {
|
---|
1149 | log_error( "l2cap_decline_connection_internal called but local_cid 0x%x not found", local_cid);
|
---|
1150 | return;
|
---|
1151 | }
|
---|
1152 | channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE;
|
---|
1153 | channel->reason = reason;
|
---|
1154 | l2cap_run();
|
---|
1155 | }
|
---|
1156 |
|
---|
1157 | void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){
|
---|
1158 |
|
---|
1159 | uint16_t flags;
|
---|
1160 | uint16_t end_pos;
|
---|
1161 | uint16_t pos;
|
---|
1162 |
|
---|
1163 | channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
|
---|
1164 |
|
---|
1165 | flags = little_endian_read_16(command, 6);
|
---|
1166 | if (flags & 1) {
|
---|
1167 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT);
|
---|
1168 | }
|
---|
1169 |
|
---|
1170 | // accept the other's configuration options
|
---|
1171 | end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
|
---|
1172 | pos = 8;
|
---|
1173 | while (pos < end_pos){
|
---|
1174 | uint8_t option_hint = command[pos] >> 7;
|
---|
1175 | uint8_t option_type = command[pos] & 0x7f;
|
---|
1176 | uint8_t length;
|
---|
1177 | log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type);
|
---|
1178 | pos++;
|
---|
1179 | length = command[pos++];
|
---|
1180 | // MTU { type(8): 1, len(8):2, MTU(16) }
|
---|
1181 | if (option_type == 1 && length == 2){
|
---|
1182 | channel->remote_mtu = little_endian_read_16(command, pos);
|
---|
1183 | // log_info("l2cap cid 0x%02x, remote mtu %u", channel->local_cid, channel->remote_mtu);
|
---|
1184 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU);
|
---|
1185 | }
|
---|
1186 | // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)}
|
---|
1187 | if (option_type == 2 && length == 2){
|
---|
1188 | channel->flush_timeout = little_endian_read_16(command, pos);
|
---|
1189 | }
|
---|
1190 | // check for unknown options
|
---|
1191 | if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){
|
---|
1192 | log_info("l2cap cid %u, unknown options", channel->local_cid);
|
---|
1193 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID);
|
---|
1194 | }
|
---|
1195 | pos += length;
|
---|
1196 | }
|
---|
1197 | }
|
---|
1198 |
|
---|
1199 | static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){
|
---|
1200 | // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var);
|
---|
1201 | if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0;
|
---|
1202 | if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0;
|
---|
1203 | return 1;
|
---|
1204 | }
|
---|
1205 |
|
---|
1206 |
|
---|
1207 | void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){
|
---|
1208 |
|
---|
1209 | uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
|
---|
1210 | uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
|
---|
1211 | uint16_t result = 0;
|
---|
1212 |
|
---|
1213 | log_info("L2CAP signaling handler code %u, state %u", code, channel->state);
|
---|
1214 |
|
---|
1215 | // handle DISCONNECT REQUESTS seperately
|
---|
1216 | if (code == DISCONNECTION_REQUEST){
|
---|
1217 | switch (channel->state){
|
---|
1218 | case L2CAP_STATE_CONFIG:
|
---|
1219 | case L2CAP_STATE_OPEN:
|
---|
1220 | case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST:
|
---|
1221 | case L2CAP_STATE_WAIT_DISCONNECT:
|
---|
1222 | l2cap_handle_disconnect_request(channel, identifier);
|
---|
1223 | break;
|
---|
1224 |
|
---|
1225 | default:
|
---|
1226 | // ignore in other states
|
---|
1227 | break;
|
---|
1228 | }
|
---|
1229 | return;
|
---|
1230 | }
|
---|
1231 |
|
---|
1232 | // @STATEMACHINE(l2cap)
|
---|
1233 | switch (channel->state) {
|
---|
1234 |
|
---|
1235 | case L2CAP_STATE_WAIT_CONNECT_RSP:
|
---|
1236 | switch (code){
|
---|
1237 | case CONNECTION_RESPONSE:
|
---|
1238 | l2cap_stop_rtx(channel);
|
---|
1239 | result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4);
|
---|
1240 | switch (result) {
|
---|
1241 | case 0:
|
---|
1242 | // successful connection
|
---|
1243 | channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
|
---|
1244 | channel->state = L2CAP_STATE_CONFIG;
|
---|
1245 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ);
|
---|
1246 | break;
|
---|
1247 | case 1:
|
---|
1248 | // connection pending. get some coffee, but start the ERTX
|
---|
1249 | l2cap_start_ertx(channel);
|
---|
1250 | break;
|
---|
1251 | default:
|
---|
1252 | // channel closed
|
---|
1253 | channel->state = L2CAP_STATE_CLOSED;
|
---|
1254 | // map l2cap connection response result to BTstack status enumeration
|
---|
1255 | l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result);
|
---|
1256 |
|
---|
1257 | // drop link key if security block
|
---|
1258 | if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){
|
---|
1259 | hci_drop_link_key_for_bd_addr(&channel->address);
|
---|
1260 | }
|
---|
1261 |
|
---|
1262 | // discard channel
|
---|
1263 | linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
|
---|
1264 | btstack_memory_l2cap_channel_free(channel);
|
---|
1265 | break;
|
---|
1266 | }
|
---|
1267 | break;
|
---|
1268 |
|
---|
1269 | default:
|
---|
1270 | //@TODO: implement other signaling packets
|
---|
1271 | break;
|
---|
1272 | }
|
---|
1273 | break;
|
---|
1274 |
|
---|
1275 | case L2CAP_STATE_CONFIG:
|
---|
1276 | result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4);
|
---|
1277 | switch (code) {
|
---|
1278 | case CONFIGURE_REQUEST:
|
---|
1279 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP);
|
---|
1280 | l2cap_signaling_handle_configure_request(channel, command);
|
---|
1281 | if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){
|
---|
1282 | // only done if continuation not set
|
---|
1283 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ);
|
---|
1284 | }
|
---|
1285 | break;
|
---|
1286 | case CONFIGURE_RESPONSE:
|
---|
1287 | l2cap_stop_rtx(channel);
|
---|
1288 | switch (result){
|
---|
1289 | case 0: // success
|
---|
1290 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP);
|
---|
1291 | break;
|
---|
1292 | case 4: // pending
|
---|
1293 | l2cap_start_ertx(channel);
|
---|
1294 | break;
|
---|
1295 | default:
|
---|
1296 | // retry on negative result
|
---|
1297 | channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ);
|
---|
1298 | break;
|
---|
1299 | }
|
---|
1300 | break;
|
---|
1301 | default:
|
---|
1302 | break;
|
---|
1303 | }
|
---|
1304 | if (l2cap_channel_ready_for_open(channel)){
|
---|
1305 | // for open:
|
---|
1306 | channel->state = L2CAP_STATE_OPEN;
|
---|
1307 | l2cap_emit_channel_opened(channel, 0);
|
---|
1308 | l2cap_emit_credits(channel, 1);
|
---|
1309 | }
|
---|
1310 | break;
|
---|
1311 |
|
---|
1312 | case L2CAP_STATE_WAIT_DISCONNECT:
|
---|
1313 | switch (code) {
|
---|
1314 | case DISCONNECTION_RESPONSE:
|
---|
1315 | l2cap_finialize_channel_close(channel);
|
---|
1316 | break;
|
---|
1317 | default:
|
---|
1318 | //@TODO: implement other signaling packets
|
---|
1319 | break;
|
---|
1320 | }
|
---|
1321 | break;
|
---|
1322 |
|
---|
1323 | case L2CAP_STATE_CLOSED:
|
---|
1324 | // @TODO handle incoming requests
|
---|
1325 | break;
|
---|
1326 |
|
---|
1327 | case L2CAP_STATE_OPEN:
|
---|
1328 | //@TODO: implement other signaling packets, e.g. re-configure
|
---|
1329 | break;
|
---|
1330 | default:
|
---|
1331 | break;
|
---|
1332 | }
|
---|
1333 | // log_info("new state %u", channel->state);
|
---|
1334 | }
|
---|
1335 |
|
---|
1336 |
|
---|
1337 | void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){
|
---|
1338 |
|
---|
1339 | // get code, signalind identifier and command len
|
---|
1340 | uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET];
|
---|
1341 | uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET];
|
---|
1342 | uint16_t dest_cid;
|
---|
1343 | linked_list_iterator_t it;
|
---|
1344 |
|
---|
1345 | // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_REQUEST
|
---|
1346 | if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_REQUEST){
|
---|
1347 | l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, L2CAP_REJ_CMD_UNKNOWN);
|
---|
1348 | return;
|
---|
1349 | }
|
---|
1350 |
|
---|
1351 | // general commands without an assigned channel
|
---|
1352 | switch(code) {
|
---|
1353 |
|
---|
1354 | case CONNECTION_REQUEST: {
|
---|
1355 | uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
|
---|
1356 | uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2);
|
---|
1357 | l2cap_handle_connection_request(handle, sig_id, psm, source_cid);
|
---|
1358 | return;
|
---|
1359 | }
|
---|
1360 |
|
---|
1361 | case ECHO_REQUEST:
|
---|
1362 | l2cap_register_signaling_response(handle, code, sig_id, 0);
|
---|
1363 | return;
|
---|
1364 |
|
---|
1365 | case INFORMATION_REQUEST: {
|
---|
1366 | uint16_t infoType = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
|
---|
1367 | l2cap_register_signaling_response(handle, code, sig_id, infoType);
|
---|
1368 | return;
|
---|
1369 | }
|
---|
1370 |
|
---|
1371 | default:
|
---|
1372 | break;
|
---|
1373 | }
|
---|
1374 |
|
---|
1375 |
|
---|
1376 | // Get potential destination CID
|
---|
1377 | dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET);
|
---|
1378 |
|
---|
1379 | // Find channel for this sig_id and connection handle
|
---|
1380 | linked_list_iterator_init(&it, &l2cap_channels);
|
---|
1381 | while (linked_list_iterator_has_next(&it)){
|
---|
1382 | l2cap_channel_t * channel = (l2cap_channel_t *) linked_list_iterator_next(&it);
|
---|
1383 | if (channel->handle != handle) continue;
|
---|
1384 | if (code & 1) {
|
---|
1385 | // match odd commands (responses) by previous signaling identifier
|
---|
1386 | if (channel->local_sig_id == sig_id) {
|
---|
1387 | l2cap_signaling_handler_channel(channel, command);
|
---|
1388 | break;
|
---|
1389 | }
|
---|
1390 | } else {
|
---|
1391 | // match even commands (requests) by local channel id
|
---|
1392 | if (channel->local_cid == dest_cid) {
|
---|
1393 | l2cap_signaling_handler_channel(channel, command);
|
---|
1394 | break;
|
---|
1395 | }
|
---|
1396 | }
|
---|
1397 | }
|
---|
1398 | }
|
---|
1399 |
|
---|
1400 | void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
|
---|
1401 |
|
---|
1402 | // Get Channel ID
|
---|
1403 | uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
|
---|
1404 | hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet);
|
---|
1405 |
|
---|
1406 | switch (channel_id) {
|
---|
1407 |
|
---|
1408 | case L2CAP_CID_SIGNALING: {
|
---|
1409 |
|
---|
1410 | uint16_t command_offset = 8;
|
---|
1411 | while (command_offset < size) {
|
---|
1412 |
|
---|
1413 | // handle signaling commands
|
---|
1414 | l2cap_signaling_handler_dispatch(handle, &packet[command_offset]);
|
---|
1415 |
|
---|
1416 | // increment command_offset
|
---|
1417 | command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
|
---|
1418 | }
|
---|
1419 | break;
|
---|
1420 | }
|
---|
1421 |
|
---|
1422 | case L2CAP_CID_ATTRIBUTE_PROTOCOL:
|
---|
1423 | if (attribute_protocol_packet_handler) {
|
---|
1424 | (*attribute_protocol_packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
|
---|
1425 | }
|
---|
1426 | break;
|
---|
1427 |
|
---|
1428 | case L2CAP_CID_SECURITY_MANAGER_PROTOCOL:
|
---|
1429 | if (security_protocol_packet_handler) {
|
---|
1430 | (*security_protocol_packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
|
---|
1431 | }
|
---|
1432 | break;
|
---|
1433 |
|
---|
1434 | case L2CAP_CID_SIGNALING_LE: {
|
---|
1435 | switch (packet[8]){
|
---|
1436 | case CONNECTION_PARAMETER_UPDATE_RESPONSE: {
|
---|
1437 | uint16_t result = little_endian_read_16(packet, 12);
|
---|
1438 | l2cap_emit_connection_parameter_update_response(handle, result);
|
---|
1439 | break;
|
---|
1440 | }
|
---|
1441 | case CONNECTION_PARAMETER_UPDATE_REQUEST: {
|
---|
1442 | uint8_t event[10];
|
---|
1443 | hci_connection_t * connection;
|
---|
1444 | event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST;
|
---|
1445 | event[1] = 8;
|
---|
1446 | memcpy(&event[2], &packet[12], 8);
|
---|
1447 |
|
---|
1448 | connection = hci_connection_for_handle(handle);
|
---|
1449 | if (connection){
|
---|
1450 | int update_parameter = 1;
|
---|
1451 | le_connection_parameter_range_t existing_range = gap_le_get_connection_parameter_range();
|
---|
1452 | uint16_t le_conn_interval_min = little_endian_read_16(packet,12);
|
---|
1453 | uint16_t le_conn_interval_max = little_endian_read_16(packet,14);
|
---|
1454 | uint16_t le_conn_latency = little_endian_read_16(packet,16);
|
---|
1455 | uint16_t le_supervision_timeout = little_endian_read_16(packet,18);
|
---|
1456 |
|
---|
1457 | if (le_conn_interval_min < existing_range.le_conn_interval_min) update_parameter = 0;
|
---|
1458 | if (le_conn_interval_max > existing_range.le_conn_interval_max) update_parameter = 0;
|
---|
1459 |
|
---|
1460 | if (le_conn_latency < existing_range.le_conn_latency_min) update_parameter = 0;
|
---|
1461 | if (le_conn_latency > existing_range.le_conn_latency_max) update_parameter = 0;
|
---|
1462 |
|
---|
1463 | if (le_supervision_timeout < existing_range.le_supervision_timeout_min) update_parameter = 0;
|
---|
1464 | if (le_supervision_timeout > existing_range.le_supervision_timeout_max) update_parameter = 0;
|
---|
1465 |
|
---|
1466 | if (update_parameter){
|
---|
1467 | connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE;
|
---|
1468 | connection->le_conn_interval_min = le_conn_interval_min;
|
---|
1469 | connection->le_conn_interval_max = le_conn_interval_max;
|
---|
1470 | connection->le_conn_latency = le_conn_latency;
|
---|
1471 | connection->le_supervision_timeout = le_supervision_timeout;
|
---|
1472 | } else {
|
---|
1473 | connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY;
|
---|
1474 | }
|
---|
1475 | }
|
---|
1476 |
|
---|
1477 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
1478 | (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
1479 |
|
---|
1480 | break;
|
---|
1481 | }
|
---|
1482 | default: {
|
---|
1483 | uint8_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1];
|
---|
1484 | l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, L2CAP_REJ_CMD_UNKNOWN);
|
---|
1485 | break;
|
---|
1486 | }
|
---|
1487 | }
|
---|
1488 | break;
|
---|
1489 | }
|
---|
1490 |
|
---|
1491 | default: {
|
---|
1492 | // Find channel for this channel_id and connection handle
|
---|
1493 | l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id);
|
---|
1494 | if (channel) {
|
---|
1495 | l2cap_dispatch(channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
|
---|
1496 | }
|
---|
1497 | break;
|
---|
1498 | }
|
---|
1499 | }
|
---|
1500 | }
|
---|
1501 |
|
---|
1502 | static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size){
|
---|
1503 | switch (packet_type) {
|
---|
1504 | case HCI_EVENT_PACKET:
|
---|
1505 | l2cap_event_handler(packet, size);
|
---|
1506 | break;
|
---|
1507 | case HCI_ACL_DATA_PACKET:
|
---|
1508 | l2cap_acl_handler(packet, size);
|
---|
1509 | break;
|
---|
1510 | default:
|
---|
1511 | break;
|
---|
1512 | }
|
---|
1513 | l2cap_run();
|
---|
1514 | }
|
---|
1515 |
|
---|
1516 | // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE
|
---|
1517 | void l2cap_finialize_channel_close(l2cap_channel_t *channel){
|
---|
1518 | channel->state = L2CAP_STATE_CLOSED;
|
---|
1519 | l2cap_emit_channel_closed(channel);
|
---|
1520 | // discard channel
|
---|
1521 | l2cap_stop_rtx(channel);
|
---|
1522 | linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
|
---|
1523 | btstack_memory_l2cap_channel_free(channel);
|
---|
1524 | }
|
---|
1525 |
|
---|
1526 | l2cap_service_t * l2cap_get_service(uint16_t psm){
|
---|
1527 | linked_list_iterator_t it;
|
---|
1528 | linked_list_iterator_init(&it, &l2cap_services);
|
---|
1529 | while (linked_list_iterator_has_next(&it)){
|
---|
1530 | l2cap_service_t * service = (l2cap_service_t *) linked_list_iterator_next(&it);
|
---|
1531 | if ( service->psm == psm){
|
---|
1532 | return service;
|
---|
1533 | };
|
---|
1534 | }
|
---|
1535 | return NULL;
|
---|
1536 | }
|
---|
1537 |
|
---|
1538 | void l2cap_register_service_internal(void *connection, btstack_packet_handler_t packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){
|
---|
1539 | l2cap_service_t *service;
|
---|
1540 |
|
---|
1541 | log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u connection %p", psm, mtu, connection);
|
---|
1542 |
|
---|
1543 | // check for alread registered psm
|
---|
1544 | // TODO: emit error event
|
---|
1545 | service = l2cap_get_service(psm);
|
---|
1546 | if (service) {
|
---|
1547 | log_error("l2cap_register_service_internal: PSM %u already registered", psm);
|
---|
1548 | l2cap_emit_service_registered(connection, L2CAP_SERVICE_ALREADY_REGISTERED, psm);
|
---|
1549 | return;
|
---|
1550 | }
|
---|
1551 |
|
---|
1552 | // alloc structure
|
---|
1553 | // TODO: emit error event
|
---|
1554 | service = btstack_memory_l2cap_service_get();
|
---|
1555 | if (!service) {
|
---|
1556 | log_error("l2cap_register_service_internal: no memory for l2cap_service_t");
|
---|
1557 | l2cap_emit_service_registered(connection, BTSTACK_MEMORY_ALLOC_FAILED, psm);
|
---|
1558 | return;
|
---|
1559 | }
|
---|
1560 |
|
---|
1561 | // fill in
|
---|
1562 | service->psm = psm;
|
---|
1563 | service->mtu = mtu;
|
---|
1564 | service->connection = connection;
|
---|
1565 | service->packet_handler = packet_handler;
|
---|
1566 | service->required_security_level = security_level;
|
---|
1567 |
|
---|
1568 | // add to services list
|
---|
1569 | linked_list_add(&l2cap_services, (linked_item_t *) service);
|
---|
1570 |
|
---|
1571 | // enable page scan
|
---|
1572 | hci_connectable_control(1);
|
---|
1573 |
|
---|
1574 | // done
|
---|
1575 | l2cap_emit_service_registered(connection, 0, psm);
|
---|
1576 | }
|
---|
1577 |
|
---|
1578 | void l2cap_unregister_service_internal(void *connection, uint16_t psm){
|
---|
1579 | l2cap_service_t *service;
|
---|
1580 |
|
---|
1581 | log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm);
|
---|
1582 |
|
---|
1583 | service = l2cap_get_service(psm);
|
---|
1584 | if (!service) return;
|
---|
1585 | linked_list_remove(&l2cap_services, (linked_item_t *) service);
|
---|
1586 | btstack_memory_l2cap_service_free(service);
|
---|
1587 |
|
---|
1588 | // disable page scan when no services registered
|
---|
1589 | if (!linked_list_empty(&l2cap_services)) return;
|
---|
1590 | hci_connectable_control(0);
|
---|
1591 | }
|
---|
1592 |
|
---|
1593 |
|
---|
1594 | // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol
|
---|
1595 | void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) {
|
---|
1596 | switch(channel_id){
|
---|
1597 | case L2CAP_CID_ATTRIBUTE_PROTOCOL:
|
---|
1598 | attribute_protocol_packet_handler = packet_handler;
|
---|
1599 | break;
|
---|
1600 | case L2CAP_CID_SECURITY_MANAGER_PROTOCOL:
|
---|
1601 | security_protocol_packet_handler = packet_handler;
|
---|
1602 | break;
|
---|
1603 | }
|
---|
1604 | }
|
---|
1605 |
|
---|
1606 | #ifdef HAVE_BLE
|
---|
1607 |
|
---|
1608 | // Request LE connection parameter update
|
---|
1609 | int l2cap_le_request_connection_parameter_update(uint16_t handle, uint16_t interval_min, uint16_t interval_max, uint16_t slave_latency, uint16_t timeout_multiplier){
|
---|
1610 | if (!hci_can_send_acl_packet_now(handle)){
|
---|
1611 | log_info("l2cap_send_signaling_packet, cannot send");
|
---|
1612 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
1613 | }
|
---|
1614 | // log_info("l2cap_send_signaling_packet type %u", cmd);
|
---|
1615 | hci_reserve_packet_buffer();
|
---|
1616 | uint8_t *acl_buffer = hci_get_outgoing_packet_buffer();
|
---|
1617 | uint16_t len = l2cap_le_create_connection_parameter_update_request(acl_buffer, handle, interval_min, interval_max, slave_latency, timeout_multiplier);
|
---|
1618 | return hci_send_acl_packet_buffer(len);
|
---|
1619 | }
|
---|
1620 | #endif
|
---|
1621 |
|
---|