1 /*
2 * Copyright (C) 2020 BlueKitchen GmbH
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 BLUEKITCHEN GMBH 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 BLUEKITCHEN
24 * GMBH 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
34 * [email protected]
35 *
36 */
37
38 #define BTSTACK_FILE__ "ll_nrf5.c"
39
40 #define DEBUG
41
42 #include <string.h>
43 #include <inttypes.h>
44 #include <stdio.h>
45
46 #include "ll.h"
47
48 #include "btstack_config.h"
49 #include "btstack_debug.h"
50 #include "btstack_memory.h"
51 #include "btstack_memory_pool.h"
52 #include "btstack_linked_queue.h"
53 #include "bluetooth_company_id.h"
54 #include "hal_cpu.h"
55 #include "hci_event.h"
56 #include "hopping.h"
57 #include "hal_timer.h"
58 #include "radio.h"
59 #include "nrf.h"
60
61 //
62 // configuration
63 //
64
65 // bluetooth
66 // bluetooth.h
67 #define ADVERTISING_RADIO_ACCESS_ADDRESS 0x8E89BED6
68 #define ADVERTISING_CRC_INIT 0x555555
69
70 #define ACL_LE_MAX_PAYLOAD 31
71 #define ADV_MAX_PAYLOAD (6+6+22)
72 #define LL_MAX_PAYLOAD 37
73
74 // sync hop delay - time we prepare for next connection event
75 #define SYNC_HOP_DELAY_US 700
76
77 // timeout between RX complete and next RX packet
78 #define TX_TO_RX_TIMEOUT_US 250
79
80 // num tx buffers for use by link layer
81 #define HCI_NUM_TX_BUFFERS_LL 4
82
83 // num rx buffers
84 #define HCI_NUM_RX_BUFFERS 16
85
86 // total number PDU buffers
87 #define MAX_NUM_LL_PDUS (HCI_NUM_TX_BUFFERS_STACK + HCI_NUM_TX_BUFFERS_LL + HCI_NUM_RX_BUFFERS)
88
89 // HCI Connection Handle used for all HCI events/connections
90 #define HCI_CON_HANDLE 0x0001
91
92 // convert us to ticks, rounding to the closest tick count
93 // @note us must be <= 1000000 us = 1 s
94 #define US_TO_TICKS(US) (((((uint32_t)(US)) * 4096) + 6125) / 125000L)
95
96 // ADV PDU Types
97 enum pdu_adv_type {
98 PDU_ADV_TYPE_ADV_IND = 0x00,
99 PDU_ADV_TYPE_DIRECT_IND = 0x01,
100 PDU_ADV_TYPE_NONCONN_IND = 0x02,
101 PDU_ADV_TYPE_SCAN_REQ = 0x03,
102 PDU_ADV_TYPE_AUX_SCAN_REQ = PDU_ADV_TYPE_SCAN_REQ,
103 PDU_ADV_TYPE_SCAN_RSP = 0x04,
104 PDU_ADV_TYPE_CONNECT_IND = 0x05,
105 PDU_ADV_TYPE_AUX_CONNECT_REQ = PDU_ADV_TYPE_CONNECT_IND,
106 PDU_ADV_TYPE_SCAN_IND = 0x06,
107 PDU_ADV_TYPE_EXT_IND = 0x07,
108 PDU_ADV_TYPE_AUX_ADV_IND = PDU_ADV_TYPE_EXT_IND,
109 PDU_ADV_TYPE_AUX_SCAN_RSP = PDU_ADV_TYPE_EXT_IND,
110 PDU_ADV_TYPE_AUX_SYNC_IND = PDU_ADV_TYPE_EXT_IND,
111 PDU_ADV_TYPE_AUX_CHAIN_IND = PDU_ADV_TYPE_EXT_IND,
112 PDU_ADV_TYPE_AUX_CONNECT_RSP = 0x08,
113 };
114
115 // DATA PDU Types
116 enum pdu_data_llid {
117 PDU_DATA_LLID_RESV = 0x00,
118 PDU_DATA_LLID_DATA_CONTINUE = 0x01,
119 PDU_DATA_LLID_DATA_START = 0x02,
120 PDU_DATA_LLID_CTRL = 0x03,
121 };
122
123 // DATA Link Layer Control Types
124 enum pdu_data_llctrl_type {
125 PDU_DATA_LLCTRL_TYPE_CONN_UPDATE_IND = 0x00,
126 PDU_DATA_LLCTRL_TYPE_CHAN_MAP_IND = 0x01,
127 PDU_DATA_LLCTRL_TYPE_TERMINATE_IND = 0x02,
128 PDU_DATA_LLCTRL_TYPE_ENC_REQ = 0x03,
129 PDU_DATA_LLCTRL_TYPE_ENC_RSP = 0x04,
130 PDU_DATA_LLCTRL_TYPE_START_ENC_REQ = 0x05,
131 PDU_DATA_LLCTRL_TYPE_START_ENC_RSP = 0x06,
132 PDU_DATA_LLCTRL_TYPE_UNKNOWN_RSP = 0x07,
133 PDU_DATA_LLCTRL_TYPE_FEATURE_REQ = 0x08,
134 PDU_DATA_LLCTRL_TYPE_FEATURE_RSP = 0x09,
135 PDU_DATA_LLCTRL_TYPE_PAUSE_ENC_REQ = 0x0A,
136 PDU_DATA_LLCTRL_TYPE_PAUSE_ENC_RSP = 0x0B,
137 PDU_DATA_LLCTRL_TYPE_VERSION_IND = 0x0C,
138 PDU_DATA_LLCTRL_TYPE_REJECT_IND = 0x0D,
139 PDU_DATA_LLCTRL_TYPE_SLAVE_FEATURE_REQ = 0x0E,
140 PDU_DATA_LLCTRL_TYPE_CONN_PARAM_REQ = 0x0F,
141 PDU_DATA_LLCTRL_TYPE_CONN_PARAM_RSP = 0x10,
142 PDU_DATA_LLCTRL_TYPE_REJECT_EXT_IND = 0x11,
143 PDU_DATA_LLCTRL_TYPE_PING_REQ = 0x12,
144 PDU_DATA_LLCTRL_TYPE_PING_RSP = 0x13,
145 PDU_DATA_LLCTRL_TYPE_LENGTH_REQ = 0x14,
146 PDU_DATA_LLCTRL_TYPE_LENGTH_RSP = 0x15,
147 PDU_DATA_LLCTRL_TYPE_PHY_REQ = 0x16,
148 PDU_DATA_LLCTRL_TYPE_PHY_RSP = 0x17,
149 PDU_DATA_LLCTRL_TYPE_PHY_UPD_IND = 0x18,
150 PDU_DATA_LLCTRL_TYPE_MIN_USED_CHAN_IND = 0x19,
151 };
152
153 // Link Layer State
154 typedef enum {
155 LL_STATE_STANDBY,
156 LL_STATE_SCANNING,
157 LL_STATE_ADVERTISING,
158 LL_STATE_INITIATING,
159 LL_STATE_CONNECTED
160 } ll_state_t;
161
162 // Link Layer PDU Flags
163 typedef enum {
164 LL_PDU_FLAG_DATA_PDU = 1,
165 } ll_pdu_flags;
166
167 // Link Layer PDU, used in linked list
168 typedef struct {
169 // header
170 void * item;
171 hci_con_handle_t con_handle;
172 uint8_t flags;
173 int8_t rssi;
174 uint16_t connection_event;
175 uint16_t packet_nr;
176 // over the air data
177 uint8_t header;
178 uint8_t len;
179 uint8_t payload[LL_MAX_PAYLOAD];
180 } ll_pdu_t;
181
182
183 // hopping context
184 static hopping_t h;
185
186 static struct {
187
188 volatile bool synced;
189
190 volatile uint16_t packet_nr_in_connection_event;
191
192 volatile uint16_t conn_interval_1250us;
193 volatile uint32_t conn_interval_us;
194 volatile uint16_t conn_interval_ticks;
195
196 volatile uint16_t conn_latency;
197
198 volatile uint16_t supervision_timeout_10ms;
199 volatile uint32_t supervision_timeout_us;
200
201 //
202 volatile uint32_t time_without_any_packets_us;
203
204 // access address
205 volatile uint32_t aa;
206
207 // start of current connection event
208 volatile uint32_t anchor_ticks;
209
210 // custom anchor delta to apply on next sync hop (if != 0)
211 volatile uint16_t anchor_delta_ticks;
212
213 // latest time to send tx packet before sync hop
214 volatile uint16_t conn_latest_tx_ticks;
215
216 // timeout for sync relative to anchor
217 volatile uint16_t conn_sync_hop_ticks;
218
219 // current channel
220 volatile uint8_t channel;
221
222 // CSA #2 supported
223 uint8_t csa2_support;
224
225 // channels selection algorithm index (1 for csa #2)
226 volatile uint8_t channel_selection_algorithm;
227
228 // current connection event, first one starts with 0
229 // - needed for connection param and channel map updates as well as encryption
230 volatile uint16_t connection_event;
231
232 // pending channel map update
233 volatile bool channel_map_update_pending;
234 volatile uint16_t channel_map_update_instant;
235 volatile uint8_t channel_map_update_map[5];
236
237 // pending connection param update
238 volatile bool conn_param_update_pending;
239 volatile uint16_t conn_param_update_instant;
240 volatile uint8_t conn_param_update_win_size;
241 volatile uint16_t conn_param_update_win_offset;
242 volatile uint16_t conn_param_update_interval_1250us;
243 volatile uint16_t conn_param_update_latency;
244 volatile uint32_t conn_param_update_timeout_us;
245
246 // our bd_addr as little endian
247 uint8_t bd_addr_le[6];
248
249 // peer addr
250 uint8_t peer_addr_type;
251 uint8_t peer_addr[6];
252
253 // adv data
254 uint8_t adv_len;
255 uint8_t adv_data[31];
256
257 // adv param
258 uint8_t adv_map;
259 uint32_t adv_interval_us;
260 uint8_t adv_type;
261
262 // adv data
263 uint8_t scan_resp_len;
264 uint8_t scan_resp_data[31];
265
266 // transmit window size in us
267 volatile uint32_t transmit_window_size_us;
268
269 // transmit window offset in us
270 volatile uint32_t transmit_window_offset_us;
271
272 // next expected sequence number
273 volatile uint8_t next_expected_sequence_number;
274
275 // transmit sequence number
276 volatile uint8_t transmit_sequence_number;
277
278 // remote active: more data or non-empty packet
279 volatile bool remote_active;
280
281
282 // rx queue
283 btstack_linked_queue_t rx_queue;
284
285 // current incoming packet
286 ll_pdu_t * rx_pdu;
287
288 // tx queue of outgoing pdus
289 btstack_linked_queue_t tx_queue;
290
291 // current outgoing packet
292 ll_pdu_t * tx_pdu;
293
294 // num completed packets
295 volatile uint8_t num_completed;
296
297 // used for controller events
298 volatile uint8_t error_code;
299
300 volatile bool ll_send_disconnected;
301
302 volatile bool ll_send_connection_complete;
303
304 } ctx;
305
306 // Buffer pool
307 static ll_pdu_t ll_pdu_pool_storage[MAX_NUM_LL_PDUS];
308 static btstack_memory_pool_t ll_pdu_pool;
309
310 // prepared adv + scan packets
311 static uint8_t adv_packet_data[39];
312 static uint8_t adv_packet_len;
313 static uint8_t scan_packet_data[39];
314 static uint8_t scan_packet_len;
315
316 // single ll empty pdu
317 static uint8_t ll_empty_pdu[2];
318
319 // single ll control response
320 static ll_pdu_t ll_tx_packet;
321
322 // Link Layer State
323 static ll_state_t ll_state;
324 static uint32_t ll_scan_interval_us;
325 static uint32_t ll_scan_window_us;
326
327 static ll_pdu_t * ll_reserved_acl_buffer;
328
329 // Controller interface
330 static uint8_t ll_outgoing_hci_event[258];
331 static void (*controller_packet_handler)(uint8_t packet_type, uint8_t * packet, uint16_t size);
332
333 // Memory Pool for acl-le pdus
334
btstack_memory_ll_pdu_get(void)335 static ll_pdu_t * btstack_memory_ll_pdu_get(void){
336 void * buffer = btstack_memory_pool_get(&ll_pdu_pool);
337 if (buffer){
338 memset(buffer, 0, sizeof(ll_pdu_t));
339 }
340 return (ll_pdu_t *) buffer;
341 }
342
btstack_memory_ll_pdu_free(ll_pdu_t * acl_le_pdu)343 static void btstack_memory_ll_pdu_free(ll_pdu_t *acl_le_pdu){
344 btstack_memory_pool_free(&ll_pdu_pool, acl_le_pdu);
345 }
346
347 // Link Layer
348
349 // prototypes
350
ll_prepare_rx_buffer(void)351 static bool ll_prepare_rx_buffer(void){
352 if (ctx.rx_pdu == NULL){
353 ctx.rx_pdu = btstack_memory_ll_pdu_get();
354 }
355 if (ctx.rx_pdu == NULL){
356 printf("No free RX buffer\n");
357 return false;
358 } else {
359 return true;
360 }
361 }
362
ll_stop_timer(void)363 static void ll_stop_timer(void){
364 hal_timer_stop();
365 }
366
ll_set_timer_ticks(uint32_t anchor_offset_ticks)367 static void ll_set_timer_ticks(uint32_t anchor_offset_ticks){
368 ll_stop_timer();
369 // set timer for next radio event relative to anchor
370 uint32_t timeout_ticks = ctx.anchor_ticks + anchor_offset_ticks;
371 hal_timer_start(timeout_ticks);
372 }
373
374 // preamble (1) + aa (4) + header (1) + len (1) + payload (len) + crc (3) -- ISR handler ca. 5 us (educated guess)
ll_start_ticks_for_end_time_and_len(uint32_t packet_end_ticks,uint16_t len)375 static uint32_t ll_start_ticks_for_end_time_and_len(uint32_t packet_end_ticks, uint16_t len){
376 uint32_t timestamp_delay = (10 + len) * 8 - 5;
377 uint32_t packet_start_ticks = packet_end_ticks - US_TO_TICKS(timestamp_delay);
378 return packet_start_ticks;
379 }
380
ll_emit_hci_event(const hci_event_t * event,...)381 static void ll_emit_hci_event(const hci_event_t * event, ...){
382 va_list argptr;
383 va_start(argptr, event);
384 uint16_t length = hci_event_create_from_template_and_arglist(ll_outgoing_hci_event, sizeof(ll_outgoing_hci_event), event, argptr);
385 va_end(argptr);
386 controller_packet_handler(HCI_EVENT_PACKET, ll_outgoing_hci_event, length);
387 }
388
389 // ll adv prototypes
390
391 static void ll_advertising_timer_handler(void);
392 static void ll_advertising_tx_done(radio_result_t result);
393 static void ll_advertising_tx_to_rx(radio_result_t result);
394
ll_advertising_statemachine(void)395 static void ll_advertising_statemachine(void){
396 // find next channel
397 while (ctx.channel < 40){
398 ctx.channel++;
399 if ((ctx.adv_map & (1 << (ctx.channel - 37))) != 0) {
400 // Set Channel
401 radio_set_channel(ctx.channel);
402 // Expect response?
403 radio_transition_t transition;
404 radio_callback_t callback;
405 if (ctx.adv_type == 3){
406 // Non connectable undirected advertising (ADV_NONCONN_IND)
407 transition = RADIO_TRANSITION_TX_ONLY;
408 callback = &ll_advertising_tx_done;
409 } else {
410 // All other are either connectable and/or scannable
411 transition = RADIO_TRANSITION_TX_TO_RX;
412 callback = &ll_advertising_tx_to_rx;
413 }
414 // log_info("Send adv on #%u", ctx.channel);
415 radio_transmit(callback, transition, adv_packet_data, adv_packet_len);
416 break;
417 }
418 // adv sent on all active channels
419 if (ctx.channel >= 40){
420 // Disable HF Clock
421 radio_hf_clock_disable();
422
423 // Set timer
424 uint32_t adv_interval_ticks = US_TO_TICKS(ctx.adv_interval_us);
425 hal_timer_set_callback(&ll_advertising_timer_handler);
426 ll_set_timer_ticks(adv_interval_ticks);
427 }
428 }
429 }
430
ll_advertising_tx_done(radio_result_t result)431 static void ll_advertising_tx_done(radio_result_t result){
432 UNUSED(result);
433 ll_advertising_statemachine();
434 }
435
ll_advertising_disabled(radio_result_t result)436 static void ll_advertising_disabled(radio_result_t result){
437 UNUSED(result);
438 ll_advertising_statemachine();
439 }
440
ll_advertising_conn_ind_received(radio_result_t result)441 static void ll_advertising_conn_ind_received(radio_result_t result){
442 UNUSED(result);
443 ll_pdu_t * rx_packet = ctx.rx_pdu;
444 // packet used
445 ctx.rx_pdu = NULL;
446 // mark as adv packet
447 rx_packet->flags = 0;
448 // queue received packet -> ll_execute_once
449 btstack_linked_queue_enqueue(&ctx.rx_queue, (btstack_linked_item_t *) rx_packet);
450 }
451
ll_advertising_rx_done(radio_result_t result)452 static void ll_advertising_rx_done(radio_result_t result){
453 uint8_t pdu_type;
454 switch (result){
455 case RADIO_RESULT_OK:
456 // check for Scan and Connect requests
457 pdu_type = ctx.rx_pdu->header & 0x0f;
458 switch (pdu_type){
459 case PDU_ADV_TYPE_SCAN_REQ:
460 switch (ctx.adv_type) {
461 case 3:
462 // ignore for ADV_NONCONN_IND
463 radio_stop(&ll_advertising_disabled);
464 break;
465 default:
466 radio_transmit(&ll_advertising_tx_done, RADIO_TRANSITION_TX_ONLY, scan_packet_data, scan_packet_len);
467 break;
468 }
469 break;
470 case PDU_ADV_TYPE_CONNECT_IND:
471 switch (ctx.adv_type){
472 case 2: // ADV_SCAN_IND
473 case 3: // ADV_NONCONN_IND
474 radio_stop(&ll_advertising_disabled);
475 break;
476 default:
477 // store ticks as anchor
478 ctx.anchor_ticks = hal_timer_get_ticks();
479 // stop radio and
480 radio_stop(&ll_advertising_conn_ind_received);
481 break;
482 }
483 break;
484 default:
485 radio_stop(&ll_advertising_disabled);
486 break;
487 }
488 break;
489 case RADIO_RESULT_TIMEOUT:
490 ll_advertising_statemachine();
491 break;
492 case RADIO_RESULT_CRC_ERROR:
493 radio_stop(&ll_advertising_disabled);
494 break;
495 default:
496 btstack_assert(false);
497 break;
498 }
499 }
500
ll_advertising_tx_to_rx(radio_result_t result)501 static void ll_advertising_tx_to_rx(radio_result_t result){
502 UNUSED(result);
503 if (ll_prepare_rx_buffer()){
504 radio_receive(&ll_advertising_rx_done, TX_TO_RX_TIMEOUT_US, &ctx.rx_pdu->header, 2 + LL_MAX_PAYLOAD, NULL);
505 } else {
506 // TODO: stop radio
507 btstack_assert(false);
508 }
509 }
510
ll_advertising_timer_handler(void)511 static void ll_advertising_timer_handler(void){
512
513 uint32_t t0 = hal_timer_get_ticks();
514
515 // enable HF Clock
516 radio_hf_clock_enable(true);
517
518 // send adv on all configured channels
519 ctx.channel = 36;
520 ctx.anchor_ticks = t0;
521 ll_advertising_statemachine();
522 }
523
ll_advertising_setup_pdu(uint8_t * buffer,uint8_t header,uint8_t len,const uint8_t * data)524 static uint16_t ll_advertising_setup_pdu(uint8_t * buffer, uint8_t header, uint8_t len, const uint8_t * data){
525 buffer[0] = header;
526 buffer[1] = 6 + len;
527 memcpy(&buffer[2], ctx.bd_addr_le, 6);
528 memcpy(&buffer[8], data, len);
529 uint16_t packet_size = 2 + buffer[1];
530 return packet_size;
531 }
532
ll_advertising_start(void)533 static uint8_t ll_advertising_start(void){
534 // COMMAND DISALLOWED if wrong state.
535 if (ll_state != LL_STATE_STANDBY) return ERROR_CODE_COMMAND_DISALLOWED;
536 log_info("Start Advertising on channels 0x%0x, interval %lu us", ctx.adv_map, ctx.adv_interval_us);
537
538 radio_set_access_address(ADVERTISING_RADIO_ACCESS_ADDRESS);
539 radio_set_crc_init(ADVERTISING_CRC_INIT);
540
541 ll_state = LL_STATE_ADVERTISING;
542
543 // prepare adv and scan data in tx0 and tx1
544 enum pdu_adv_type adv_type;
545 switch (ctx.adv_type){
546 case 0:
547 // Connectable and scannable undirected advertising
548 adv_type = PDU_ADV_TYPE_ADV_IND;
549 break;
550 case 1:
551 // Connectable high duty cycle directed advertising
552 adv_type = PDU_ADV_TYPE_DIRECT_IND;
553 break;
554 case 2:
555 // Scannable undirected advertising (ADV_SCAN_IND);
556 adv_type = PDU_ADV_TYPE_SCAN_IND;
557 break;
558 case 3:
559 // Non connectable undirected advertising (ADV_NONCONN_IND)
560 adv_type = PDU_ADV_TYPE_NONCONN_IND;
561 break;
562 case 4:
563 // Connectable low duty cycle directed advertising
564 adv_type = PDU_ADV_TYPE_DIRECT_IND;
565 break;
566 default:
567 adv_type = PDU_ADV_TYPE_ADV_IND;
568 break;
569 }
570 adv_packet_len = ll_advertising_setup_pdu(adv_packet_data, adv_type, ctx.adv_len, ctx.adv_data);
571 scan_packet_len = ll_advertising_setup_pdu(scan_packet_data, PDU_ADV_TYPE_SCAN_RSP, ctx.scan_resp_len, ctx.scan_resp_data);
572
573 ctx.channel = 36;
574 ctx.anchor_ticks = hal_timer_get_ticks();
575
576 // and get started
577 radio_hf_clock_enable(true);
578 ll_advertising_statemachine();
579
580 return ERROR_CODE_SUCCESS;
581 }
582
ll_advertising_stop(void)583 static uint8_t ll_advertising_stop(void){
584 // COMMAND DISALLOWED if wrong state.
585 if (ll_state != LL_STATE_ADVERTISING) return ERROR_CODE_COMMAND_DISALLOWED;
586
587 // TODO:
588 return ERROR_CODE_SUCCESS;
589 }
590
591 // ll scanning
592
593 static void ll_scanning_statemachine(void);
594
ll_scanning_for_window(void)595 static void ll_scanning_for_window(void){
596 radio_hf_clock_enable(true);
597 // next channel
598 ctx.channel++;
599 if (ctx.channel >= 40){
600 ctx.channel = 37;
601 }
602 radio_set_channel(ctx.channel);
603 ctx.anchor_ticks = hal_timer_get_ticks();
604 log_info("Scan channel %u", ctx.channel);
605 ll_scanning_statemachine();
606 }
607
ll_scanning_tx_interrupted(radio_result_t result)608 static void ll_scanning_tx_interrupted(radio_result_t result){
609 UNUSED(result);
610 ll_scanning_statemachine();
611 }
612
ll_scanning_rx_done(radio_result_t result)613 static void ll_scanning_rx_done(radio_result_t result){
614 ll_pdu_t * rx_packet;
615 switch (result){
616 case RADIO_RESULT_OK:
617 rx_packet = ctx.rx_pdu;
618 btstack_assert(rx_packet != NULL);
619 // packet used
620 ctx.rx_pdu = NULL;
621 // mark as adv packet
622 rx_packet->flags = 0;
623 // queue received packet
624 btstack_linked_queue_enqueue(&ctx.rx_queue, (btstack_linked_item_t *) rx_packet);
625 // stop rx->tx transition
626 radio_stop(&ll_scanning_tx_interrupted);
627 break;
628 case RADIO_RESULT_CRC_ERROR:
629 // stop rx->tx transition
630 radio_stop(&ll_scanning_tx_interrupted);
631 break;
632 case RADIO_RESULT_TIMEOUT:
633 ll_scanning_statemachine();
634 break;
635 default:
636 break;
637 }
638 }
639
ll_scanning_statemachine(void)640 static void ll_scanning_statemachine(void){
641 uint32_t now = hal_timer_get_ticks();
642 uint32_t scanning_active_ticks = now - ctx.anchor_ticks;
643 uint32_t scanning_active_us = (scanning_active_ticks * 70) / 2;
644
645 ll_prepare_rx_buffer();
646
647 if ((scanning_active_us < ll_scan_window_us) && (ctx.rx_pdu != NULL)){
648 uint32_t scan_interval_remaining_us = ll_scan_window_us - scanning_active_us;
649 // start receiving
650 radio_receive(&ll_scanning_rx_done, scan_interval_remaining_us, &ctx.rx_pdu->header, 2 + LL_MAX_PAYLOAD, &ctx.rx_pdu->rssi);
651 } else {
652 // scan window over or no buffer
653
654 // disable radio if there is enough time
655 uint32_t pause_us = ll_scan_interval_us - scanning_active_us;
656 if (pause_us < 500){
657 // almost 100% scanning, keep hf clock on
658 ll_scanning_for_window();
659 } else {
660 radio_hf_clock_disable();
661 hal_timer_set_callback(&ll_scanning_for_window);
662 hal_timer_start(now + US_TO_TICKS(pause_us));
663 }
664 }
665 }
666
ll_scanning_start(uint8_t filter_duplicates)667 static uint8_t ll_scanning_start(uint8_t filter_duplicates){
668
669 // COMMAND DISALLOWED if wrong state.
670 if (ll_state != LL_STATE_STANDBY) return ERROR_CODE_COMMAND_DISALLOWED;
671
672 ll_state = LL_STATE_SCANNING;
673 ctx.channel = 36;
674
675 radio_set_access_address(ADVERTISING_RADIO_ACCESS_ADDRESS);
676 radio_set_crc_init(ADVERTISING_CRC_INIT);
677
678 log_info("LE Scan Start: window %lu, interval %lu ms", ll_scan_interval_us, ll_scan_window_us);
679
680 ll_scanning_for_window();
681
682 return ERROR_CODE_SUCCESS;
683 }
684
ll_scanning_stop_done(radio_result_t result)685 static void ll_scanning_stop_done(radio_result_t result){
686 UNUSED(result);
687 ll_state = LL_STATE_STANDBY;
688 radio_hf_clock_disable();
689 }
690
ll_scanning_stop(void)691 static uint8_t ll_scanning_stop(void){
692 // COMMAND DISALLOWED if wrong state.
693 if (ll_state != LL_STATE_SCANNING) return 0x0c;
694
695 // TODO: post-pone result until scanning actually stopped
696 log_info("LE Scan Stop");
697
698 // stop radio
699 radio_stop(&ll_scanning_stop_done);
700 return ERROR_CODE_SUCCESS;
701 }
702
703 // ll connected
704
ll_connected_one_more_packet(void)705 static bool ll_connected_one_more_packet(void){
706 uint32_t now = hal_timer_get_ticks();
707 int32_t connection_ticks = now - ctx.anchor_ticks;
708 return connection_ticks < ctx.conn_latest_tx_ticks;
709 }
710
ll_connected_next_channel(void)711 static void ll_connected_next_channel(void){
712 switch (ctx.channel_selection_algorithm){
713 case 0:
714 ctx.channel = hopping_csa1_get_next_channel( &h );
715 break;
716 case 1:
717 ctx.channel = hopping_csa2_get_channel_for_counter( &h, ctx.connection_event);
718 break;
719 default:
720 break;
721 }
722 radio_set_channel(ctx.channel);
723 }
724
ll_connected_ctx_set_conn_interval(uint16_t conn_interval_1250us)725 static void ll_connected_ctx_set_conn_interval(uint16_t conn_interval_1250us){
726 ctx.conn_interval_1250us = conn_interval_1250us;
727 ctx.conn_interval_us = ctx.conn_interval_1250us * 1250;
728 ctx.conn_interval_ticks = US_TO_TICKS(ctx.conn_interval_us);
729 ctx.conn_sync_hop_ticks = US_TO_TICKS(ctx.conn_interval_us - SYNC_HOP_DELAY_US);
730
731 // latest time to send a packet before getting ready for next cnonection event
732 uint16_t max_packet_time_incl_ifs_us = 500;
733 ctx.conn_latest_tx_ticks = US_TO_TICKS(ctx.conn_interval_us - SYNC_HOP_DELAY_US - max_packet_time_incl_ifs_us);
734 }
735
ll_connected_terminate(uint8_t error_code)736 static void ll_connected_terminate(uint8_t error_code){
737 ll_state = LL_STATE_STANDBY;
738 ctx.conn_param_update_pending = false;
739 ctx.channel_map_update_pending = false;
740 // turn off clock
741 radio_hf_clock_disable();
742 // stop sync hop timer
743 ll_stop_timer();
744 // free outgoing tx packets
745 if ((ctx.tx_pdu != NULL) && (ctx.tx_pdu != &ll_tx_packet)){
746 btstack_memory_ll_pdu_free(ctx.tx_pdu);
747 ctx.tx_pdu = NULL;
748 }
749 // free queued tx packets
750 while (true){
751 ll_pdu_t * tx_packet = (ll_pdu_t *) btstack_linked_queue_dequeue(&ctx.tx_queue);
752 if (tx_packet != NULL) {
753 btstack_memory_ll_pdu_free(tx_packet);
754 } else {
755 break;
756 }
757 }
758 // notify host stack
759 ctx.error_code = error_code;
760 ctx.ll_send_disconnected = true;
761 }
762
763 static void ll_connected_timer_handler(void);
764 static void lL_connected_rx_done(radio_result_t result);
765
ll_connected_connection_event_complete(void)766 static void ll_connected_connection_event_complete(void){
767 radio_hf_clock_disable();
768 }
769
ll_connected_radio_stopped(radio_result_t result)770 static void ll_connected_radio_stopped(radio_result_t result){
771 UNUSED(result);
772 ll_connected_connection_event_complete();
773 }
774
ll_connected_tx_only_done(radio_result_t result)775 static void ll_connected_tx_only_done(radio_result_t result){
776 UNUSED(result);
777 ll_connected_connection_event_complete();
778 }
779
ll_connected_tx_to_rx_done(radio_result_t result)780 static void ll_connected_tx_to_rx_done(radio_result_t result) {
781 UNUSED(result);
782 if (!ll_connected_one_more_packet()){
783 // stop tx
784 radio_stop(&ll_connected_radio_stopped);
785 return;
786 }
787
788 // receive next packet
789 (void) ll_prepare_rx_buffer();
790 if (ctx.rx_pdu == NULL){
791 radio_stop(&ll_connected_radio_stopped);
792 return;
793 }
794
795 // receive master packet
796 radio_receive(&lL_connected_rx_done, TX_TO_RX_TIMEOUT_US, &ctx.rx_pdu->header, 2 + LL_MAX_PAYLOAD, NULL);
797 }
798
ll_connected_terminate_received(radio_result_t result)799 static void ll_connected_terminate_received(radio_result_t result){
800 UNUSED(result);
801 ll_connected_terminate(ERROR_CODE_REMOTE_USER_TERMINATED_CONNECTION);
802 }
803
lL_connected_rx_done(radio_result_t result)804 static void lL_connected_rx_done(radio_result_t result){
805
806 uint32_t packet_end_ticks = hal_timer_get_ticks();
807
808 if (result == RADIO_RESULT_TIMEOUT){
809 ll_connected_connection_event_complete();
810 return;
811 }
812
813 // Handle RX
814
815 // set anchor on first packet in connection event
816 if (ctx.packet_nr_in_connection_event == 0){
817 ctx.anchor_ticks = ll_start_ticks_for_end_time_and_len(packet_end_ticks, ctx.rx_pdu->len);
818 ctx.synced = true;
819
820 // set timer for sync-hop
821 hal_timer_set_callback(&ll_connected_timer_handler);
822 ll_set_timer_ticks( ctx.conn_sync_hop_ticks);
823 }
824
825 // packet received
826 if (result == RADIO_RESULT_OK) {
827
828 // parse rx pdu header
829 uint8_t rx_header = ctx.rx_pdu->header;
830 uint8_t next_expected_sequence_number = (rx_header >> 2) & 1;
831 uint8_t sequence_number = (rx_header >> 3) & 1;
832 uint8_t more_data = (rx_header >> 4) & 1;
833
834 // remote active if md or len > 0
835 ctx.remote_active = (more_data != 0) || ctx.rx_pdu->len > 0;
836
837 // only accept packets with new sequence number and len <= payload size
838 if ((sequence_number == ctx.next_expected_sequence_number) && (ctx.rx_pdu->len <= LL_MAX_PAYLOAD)) {
839
840 // update state
841 ctx.next_expected_sequence_number = 1 - sequence_number;
842
843 // queue if not empty
844 ll_pdu_t *rx_packet = ctx.rx_pdu;
845 if (rx_packet->len != 0) {
846
847 // handle terminate immediately
848 uint8_t ll_id = rx_packet->header & 3;
849 if (ll_id == PDU_DATA_LLID_CTRL) {
850 if (rx_packet->payload[0] == PDU_DATA_LLCTRL_TYPE_TERMINATE_IND){
851 ll_stop_timer();
852 radio_stop(&ll_connected_terminate_received);
853 return;
854 }
855 }
856
857 // packet used
858 ctx.rx_pdu = NULL;
859
860 // mark as data packet and add meta data
861 rx_packet->flags |= LL_PDU_FLAG_DATA_PDU;
862 rx_packet->connection_event = ctx.connection_event;
863 rx_packet->packet_nr = ctx.packet_nr_in_connection_event;
864
865 // queue received packet
866 btstack_linked_queue_enqueue(&ctx.rx_queue, (btstack_linked_item_t *) rx_packet);
867 }
868 }
869
870 ctx.packet_nr_in_connection_event++;
871
872 // report outgoing packet as ack'ed and free if confirmed by peer
873 bool tx_acked = ctx.transmit_sequence_number != next_expected_sequence_number;
874 if (tx_acked) {
875 // if non link-layer packet, free buffer and report as completed
876 if ((ctx.tx_pdu != NULL) && (ctx.tx_pdu != &ll_tx_packet)) {
877 btstack_memory_ll_pdu_free(ctx.tx_pdu);
878 ctx.num_completed++;
879 }
880 ctx.tx_pdu = NULL;
881 ctx.transmit_sequence_number = next_expected_sequence_number;
882 }
883 }
884
885 // restart supervision timeout
886 ctx.time_without_any_packets_us = 0;
887
888 // Prepare TX
889
890 // check if we can sent a full packet before sync hop
891 if (!ll_connected_one_more_packet()){
892 // stop tx
893 radio_stop(&ll_connected_radio_stopped);
894 return;
895 }
896
897 // fetch next packet
898 if (ctx.tx_pdu == NULL){
899 ctx.tx_pdu = (ll_pdu_t *) btstack_linked_queue_dequeue(&ctx.tx_queue);
900 }
901
902 // setup empty packet if no tx packet ready
903 uint8_t * tx_buffer;
904 if (ctx.tx_pdu == NULL){
905 ll_empty_pdu[0] = PDU_DATA_LLID_DATA_CONTINUE;
906 ll_empty_pdu[1] = 0;
907 tx_buffer = ll_empty_pdu;
908 } else {
909 tx_buffer = &ctx.tx_pdu->header;
910 }
911
912 // setup pdu header
913 uint8_t md = btstack_linked_queue_empty(&ctx.tx_queue) ? 0 : 1;
914 tx_buffer[0] |= (md << 4) | (ctx.transmit_sequence_number << 3) | (ctx.next_expected_sequence_number << 2);
915
916 // send packet
917 bool tx_to_rx = ctx.remote_active || (tx_buffer[1] > 0);
918 if (tx_to_rx){
919 radio_transmit(&ll_connected_tx_to_rx_done, RADIO_TRANSITION_TX_TO_RX, tx_buffer, 2 + tx_buffer[1]);
920 } else {
921 radio_transmit(&ll_connected_tx_only_done, RADIO_TRANSITION_TX_ONLY, tx_buffer, 2 + tx_buffer[1]);
922 }
923 }
924
ll_connected_handle_conn_ind(ll_pdu_t * rx_packet)925 static void ll_connected_handle_conn_ind(ll_pdu_t * rx_packet){
926 // parse packet
927 uint8_t * init_addr = &rx_packet->payload[0];
928 uint8_t * adv_addr = &rx_packet->payload[6];
929 uint8_t chan_sel = (rx_packet->header >> 5) & 1;
930
931 // verify AdvA
932 if (memcmp(ctx.bd_addr_le, adv_addr, 6) != 0){
933 // differs, go back to adv sending
934 ll_advertising_statemachine();
935 return;
936 }
937
938 // next event is > 1.25 ms away
939 radio_hf_clock_disable();
940
941 // TODO: get remote addr type
942 ctx.peer_addr_type = 0;
943 memcpy(ctx.peer_addr, init_addr, 6);
944
945 // get params for HCI event
946 const uint8_t * ll_data = &rx_packet->payload[12];
947
948 ctx.aa = little_endian_read_32(ll_data, 0);
949 uint32_t crc_init = little_endian_read_24(ll_data, 4);
950 uint8_t transmit_window_size = ll_data[7];
951 uint16_t transmit_window_offset = little_endian_read_16(ll_data, 8);
952 uint16_t conn_interval_1250us = little_endian_read_16(ll_data, 10);
953 ctx.conn_latency = little_endian_read_16(ll_data, 12);
954 ctx.supervision_timeout_10ms = little_endian_read_16(ll_data, 14);
955 const uint8_t * channel_map = &ll_data[16];
956 uint8_t hop = ll_data[21] & 0x1f;
957 uint8_t sca = ll_data[21] >> 5;
958
959 // TODO: handle sleep clock accuracy of initiator
960 UNUSED(sca);
961
962 ll_connected_ctx_set_conn_interval(conn_interval_1250us);
963
964 // convert to us
965 ctx.supervision_timeout_us = ctx.supervision_timeout_10ms * 10000;
966 ctx.transmit_window_size_us = transmit_window_size * 1250;
967 ctx.transmit_window_offset_us = transmit_window_offset * 1250;
968
969 // init connection state
970 ctx.connection_event = 0;
971 ctx.packet_nr_in_connection_event = 0;
972 ctx.next_expected_sequence_number = 0;
973 ctx.transmit_sequence_number = 0;
974
975 // set AA
976 radio_set_access_address(ctx.aa);
977
978 // set CRC init value
979 radio_set_crc_init(crc_init);
980
981 printf("Transmit window offset %u us\n", (int) ctx.transmit_window_offset_us);
982 printf("Transmit window size %u us\n", (int) ctx.transmit_window_size_us);
983 printf("Connection interval %u us\n", (int) ctx.conn_interval_us);
984 printf("Connection timeout %u us\n", (int) ctx.supervision_timeout_us);
985 printf("AA %08x\n", (int) ctx.aa);
986 printf("CRC Init 0x%06" PRIx32 "x\n", crc_init);
987
988 // init hopping
989 hopping_init( &h );
990 hopping_set_channel_map( &h, channel_map);
991 ctx.channel_selection_algorithm = ctx.csa2_support & chan_sel;
992 switch (ctx.channel_selection_algorithm){
993 case 0:
994 hopping_csa1_set_hop_increment( &h, hop );
995 break;
996 case 1:
997 hopping_csa2_set_access_address( &h, ctx.aa);
998 break;
999 default:
1000 break;
1001 }
1002
1003 // connected -> notify controller
1004 ll_state = LL_STATE_CONNECTED;
1005 ctx.synced = false;
1006 ctx.ll_send_connection_complete = true;
1007
1008 // sleep until transmit window
1009 hal_timer_set_callback(&ll_connected_timer_handler);
1010 ctx.anchor_delta_ticks = US_TO_TICKS(ctx.transmit_window_offset_us + 1250);
1011 ll_set_timer_ticks(US_TO_TICKS(ctx.transmit_window_offset_us + 1250 - SYNC_HOP_DELAY_US));
1012 }
1013
ll_connected_timer_handler(void)1014 static void ll_connected_timer_handler(void){
1015
1016 // Check supervision timeout
1017 if (ctx.synced){
1018 // check supervision timeout if connection was established
1019 ctx.time_without_any_packets_us += ctx.conn_interval_us;
1020 if (ctx.time_without_any_packets_us > ctx.supervision_timeout_us) {
1021 printf("Supervision timeout (regular)\n\n");
1022 ll_connected_terminate(ERROR_CODE_CONNECTION_TIMEOUT);
1023 return;
1024 }
1025 } else {
1026 // give up on receiving first packet after 6 tries
1027 if (ctx.connection_event > 6){
1028 printf("Supervision timeout(establishment)\n\n");
1029 ll_connected_terminate(ERROR_CODE_CONNECTION_FAILED_TO_BE_ESTABLISHED);
1030 return;
1031 }
1032 }
1033
1034 // update anchor using custom value when transmit windows was used
1035 // connection event counter is only incremented for regular connection interval
1036 if (ctx.anchor_delta_ticks == 0){
1037 ctx.anchor_ticks += ctx.conn_interval_ticks;
1038 ctx.connection_event++;
1039 } else {
1040 ctx.anchor_ticks += ctx.anchor_delta_ticks;
1041 ctx.anchor_delta_ticks = 0;
1042 }
1043
1044 if (ctx.channel_map_update_pending && (ctx.channel_map_update_instant == ctx.connection_event)) {
1045 ctx.channel_map_update_pending = false;
1046
1047 log_info("Chan map update now");
1048
1049 hopping_set_channel_map( &h, (const uint8_t *) &ctx.channel_map_update_map );
1050 }
1051
1052 if (ctx.conn_param_update_pending && ((ctx.conn_param_update_instant) == ctx.connection_event) ) {
1053 ctx.conn_param_update_pending = false;
1054
1055 log_info("Conn param update now");
1056
1057 ll_connected_ctx_set_conn_interval(ctx.conn_param_update_interval_1250us);
1058 ctx.conn_latency = ctx.conn_param_update_latency;
1059 ctx.supervision_timeout_us = ctx.conn_param_update_timeout_us;
1060 ctx.transmit_window_offset_us = ctx.conn_param_update_win_offset * 1250;
1061 ctx.transmit_window_size_us = ctx.conn_param_update_win_size * 1250;
1062 ctx.synced = false;
1063
1064 // See Core V5.2, Vol 6B, 5.1.1, Figure 5.1
1065 // if transmit window == 0, continue. If not sleep again
1066 if (ctx.conn_param_update_win_offset > 0){
1067 ctx.anchor_delta_ticks = US_TO_TICKS(ctx.transmit_window_offset_us);
1068 ll_set_timer_ticks(US_TO_TICKS(ctx.transmit_window_offset_us - SYNC_HOP_DELAY_US));
1069 return;
1070 }
1071 }
1072
1073 // prepare connection event
1074 ctx.packet_nr_in_connection_event = 0;
1075
1076 radio_hf_clock_enable(true);
1077
1078 ll_connected_next_channel();
1079
1080 // set radio timer (should get updated by first packet)
1081 ll_set_timer_ticks( ctx.conn_sync_hop_ticks);
1082
1083 (void) ll_prepare_rx_buffer();
1084 if (ctx.rx_pdu == NULL) {
1085 log_info("No RX PDU for first packet, skip connection event");
1086 } else {
1087 if (ctx.synced){
1088 radio_receive(&lL_connected_rx_done, SYNC_HOP_DELAY_US + 100, &ctx.rx_pdu->header, 2 + LL_MAX_PAYLOAD, NULL);
1089 } else {
1090 radio_receive(&lL_connected_rx_done, SYNC_HOP_DELAY_US + ctx.transmit_window_size_us, &ctx.rx_pdu->header, 2 + LL_MAX_PAYLOAD, NULL);
1091 }
1092 }
1093
1094 // printf("--SYNC-Ch %02u-Event %04u - t %08" PRIu32 "--\n", ctx.channel, ctx.connection_event, t0);
1095 }
1096
ll_connected_handle_control(ll_pdu_t * rx_packet)1097 static void ll_connected_handle_control(ll_pdu_t * rx_packet){
1098 ll_pdu_t * tx_packet = &ll_tx_packet;
1099 uint8_t opcode = rx_packet->payload[0];
1100 switch (opcode){
1101 case PDU_DATA_LLCTRL_TYPE_VERSION_IND:
1102 tx_packet->len = 6;
1103 tx_packet->header = PDU_DATA_LLID_CTRL;
1104 tx_packet->payload[0] = PDU_DATA_LLCTRL_TYPE_VERSION_IND;
1105 tx_packet->payload[1] = 0x06; // VersNr = Bluetooth Core V4.0
1106 little_endian_store_16(tx_packet->payload, 2, BLUETOOTH_COMPANY_ID_BLUEKITCHEN_GMBH);
1107 little_endian_store_16(tx_packet->payload, 4, 0);
1108 btstack_linked_queue_enqueue(&ctx.tx_queue, (btstack_linked_item_t *) tx_packet);
1109 printf("Queue Version Ind\n");
1110 break;
1111 case PDU_DATA_LLCTRL_TYPE_FEATURE_REQ:
1112 tx_packet->len = 9;
1113 tx_packet->header = PDU_DATA_LLID_CTRL;
1114 tx_packet->payload[0] = PDU_DATA_LLCTRL_TYPE_FEATURE_RSP;
1115 // TODO: set features of our controller
1116 memset(&tx_packet->payload[1], 0, 8);
1117 btstack_linked_queue_enqueue(&ctx.tx_queue, (btstack_linked_item_t *) tx_packet);
1118 printf("Queue Feature Rsp\n");
1119 break;
1120 case PDU_DATA_LLCTRL_TYPE_CHAN_MAP_IND:
1121 memcpy((uint8_t *) ctx.channel_map_update_map, &rx_packet->payload[1], 5);
1122 ctx.channel_map_update_instant = little_endian_read_16(rx_packet->payload, 6);
1123 ctx.channel_map_update_pending = true;
1124 break;
1125 case PDU_DATA_LLCTRL_TYPE_CONN_UPDATE_IND:
1126 ctx.conn_param_update_win_size = rx_packet->payload[1];
1127 ctx.conn_param_update_win_offset = little_endian_read_16(rx_packet->payload, 2);
1128 ctx.conn_param_update_interval_1250us = little_endian_read_16(rx_packet->payload, 4);
1129 ctx.conn_param_update_latency = little_endian_read_16(rx_packet->payload, 6);
1130 ctx.conn_param_update_timeout_us = little_endian_read_16(rx_packet->payload, 8) * 10000;
1131 ctx.conn_param_update_instant = little_endian_read_16(rx_packet->payload, 10);
1132 ctx.conn_param_update_pending = true;
1133 log_info("PDU_DATA_LLCTRL_TYPE_CONN_UPDATE_IND, conn interval %u 1250us at instant %u",
1134 (unsigned int) ctx.conn_param_update_interval_1250us, ctx.conn_param_update_instant);
1135 break;
1136 default:
1137 break;
1138 }
1139 }
1140
ll_connected_handle_data(ll_pdu_t * rx_packet)1141 static void ll_connected_handle_data(ll_pdu_t * rx_packet){
1142 btstack_assert(rx_packet->len <= LL_MAX_PAYLOAD);
1143 log_debug("CE: %u, nr %u, header 0x%02x, len %u", rx_packet->connection_event, rx_packet->packet_nr, rx_packet->header, rx_packet->len);
1144 uint8_t acl_packet[4 + LL_MAX_PAYLOAD];
1145 // ACL Header
1146 uint8_t ll_id = rx_packet->header & 3;
1147 acl_packet[0] = 0x01;
1148 acl_packet[1] = ll_id << 4;
1149 little_endian_store_16(acl_packet, 2, rx_packet->len);
1150 memcpy(&acl_packet[4], rx_packet->payload, rx_packet->len);
1151 (*controller_packet_handler)(HCI_ACL_DATA_PACKET, acl_packet, rx_packet->len + 4);
1152 }
1153
ll_handle_adv(ll_pdu_t * rx_packet)1154 static void ll_handle_adv(ll_pdu_t * rx_packet) {
1155 // Map PDU_ADV_TYPE to HCI Event_Type
1156 uint8_t event_type = 0;
1157 switch (rx_packet->header & 0x0f){
1158 case PDU_ADV_TYPE_ADV_IND:
1159 event_type = 0;
1160 break;
1161 case PDU_ADV_TYPE_DIRECT_IND:
1162 event_type = 1;
1163 break;
1164 case PDU_ADV_TYPE_NONCONN_IND:
1165 event_type = 3;
1166 break;
1167 case PDU_ADV_TYPE_SCAN_RSP:
1168 event_type = 3;
1169 break;
1170 default:
1171 return;
1172 }
1173 uint8_t advertiser_addr_type = ((rx_packet->header & 0x40) != 0) ? 1 : 0;
1174 uint8_t adv_data_len = rx_packet->len - 6;
1175
1176 uint16_t pos = 0;
1177 ll_outgoing_hci_event[pos++] = HCI_EVENT_LE_META;
1178 pos++;
1179 ll_outgoing_hci_event[pos++] = HCI_SUBEVENT_LE_ADVERTISING_REPORT;
1180 ll_outgoing_hci_event[pos++] = 1;
1181 ll_outgoing_hci_event[pos++] = event_type;
1182 ll_outgoing_hci_event[pos++] = advertiser_addr_type;
1183 memcpy(&ll_outgoing_hci_event[pos], &rx_packet->payload[0], 6);
1184 pos += 6;
1185 ll_outgoing_hci_event[pos++] = adv_data_len;
1186 memcpy(&ll_outgoing_hci_event[pos], &rx_packet->payload[6], adv_data_len);
1187 pos += adv_data_len;
1188 ll_outgoing_hci_event[pos++] = (uint8_t) rx_packet->rssi;
1189 ll_outgoing_hci_event[1] = pos - 2;
1190 (*controller_packet_handler)(HCI_EVENT_PACKET, ll_outgoing_hci_event, pos);
1191 }
1192
1193 // public API
1194
ll_init(void)1195 void ll_init(void){
1196 // setup memory pools
1197 btstack_memory_pool_create(&ll_pdu_pool, ll_pdu_pool_storage, MAX_NUM_LL_PDUS, sizeof(ll_pdu_t));
1198
1199 // set test bd addr 33:33:33:33:33:33
1200 memset(ctx.bd_addr_le, 0x33, 6);
1201
1202 // default channels, advertising interval
1203 ctx.adv_map = 0x7;
1204 ctx.adv_interval_us = 1280000;
1205
1206 // init radio
1207 radio_init();
1208 }
1209
ll_radio_on(void)1210 void ll_radio_on(void){
1211 ll_state = LL_STATE_STANDBY;
1212 }
1213
ll_set_scan_parameters(uint8_t le_scan_type,uint16_t le_scan_interval,uint16_t le_scan_window,uint8_t own_address_type,uint8_t scanning_filter_policy)1214 void ll_set_scan_parameters(uint8_t le_scan_type, uint16_t le_scan_interval, uint16_t le_scan_window, uint8_t own_address_type, uint8_t scanning_filter_policy){
1215 // TODO .. store other params
1216 ll_scan_interval_us = ((uint32_t) le_scan_interval) * 625;
1217 ll_scan_window_us = ((uint32_t) le_scan_window) * 625;
1218 log_info("LE Scan Params: window %lu, interval %lu ms", ll_scan_interval_us, ll_scan_window_us);
1219 }
1220
ll_set_scan_enable(uint8_t le_scan_enable,uint8_t filter_duplicates)1221 uint8_t ll_set_scan_enable(uint8_t le_scan_enable, uint8_t filter_duplicates){
1222 if (le_scan_enable){
1223 return ll_scanning_start(filter_duplicates);
1224 } else {
1225 return ll_scanning_stop();
1226 }
1227 }
1228
ll_set_advertise_enable(uint8_t le_adv_enable)1229 uint8_t ll_set_advertise_enable(uint8_t le_adv_enable){
1230 if (le_adv_enable){
1231 return ll_advertising_start();
1232 } else {
1233 return ll_advertising_stop();
1234 }
1235 }
1236
ll_set_advertising_parameters(uint16_t advertising_interval_min,uint16_t advertising_interval_max,uint8_t advertising_type,uint8_t own_address_type,uint8_t peer_address_types,uint8_t * peer_address,uint8_t advertising_channel_map,uint8_t advertising_filter_policy)1237 uint8_t ll_set_advertising_parameters(uint16_t advertising_interval_min, uint16_t advertising_interval_max,
1238 uint8_t advertising_type, uint8_t own_address_type, uint8_t peer_address_types, uint8_t * peer_address,
1239 uint8_t advertising_channel_map, uint8_t advertising_filter_policy){
1240
1241 // validate channel map
1242 if (advertising_channel_map == 0) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1243 if ((advertising_channel_map & 0xf8) != 0) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1244
1245 // validate advertising interval
1246 if (advertising_interval_min < 0x20) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1247 if (advertising_interval_min > 0x4000) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1248 if (advertising_interval_max < 0x20) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1249 if (advertising_interval_max > 0x4000) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1250 if (advertising_interval_min > advertising_interval_max) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1251
1252 ctx.adv_map = advertising_channel_map;
1253 ctx.adv_interval_us = advertising_interval_max * 625;
1254 ctx.adv_type= advertising_type;
1255
1256 // TODO: validate other params
1257 // TODO: process other params
1258
1259 return ERROR_CODE_SUCCESS;
1260 }
1261
ll_set_advertising_data(uint8_t adv_len,const uint8_t * adv_data)1262 uint8_t ll_set_advertising_data(uint8_t adv_len, const uint8_t * adv_data){
1263 // COMMAND DISALLOWED if wrong state.
1264 if (ll_state == LL_STATE_ADVERTISING) return ERROR_CODE_COMMAND_DISALLOWED;
1265 if (adv_len > 31) return ERROR_CODE_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE;
1266 ctx.adv_len = adv_len;
1267 memcpy(ctx.adv_data, adv_data, adv_len);
1268
1269 return ERROR_CODE_SUCCESS;
1270 }
1271
ll_set_scan_response_data(uint8_t adv_len,const uint8_t * adv_data)1272 uint8_t ll_set_scan_response_data(uint8_t adv_len, const uint8_t * adv_data){
1273 // COMMAND DISALLOWED if wrong state.
1274 if (ll_state == LL_STATE_ADVERTISING) return ERROR_CODE_COMMAND_DISALLOWED;
1275 if (adv_len > 31) return ERROR_CODE_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE;
1276 ctx.scan_resp_len = adv_len;
1277 memcpy(ctx.scan_resp_data, adv_data, adv_len);
1278
1279 return ERROR_CODE_SUCCESS;
1280 }
1281
ll_execute_once(void)1282 void ll_execute_once(void){
1283 // process received packets
1284 while (1){
1285 ll_pdu_t * rx_packet;
1286 /** critical section start */
1287 hal_cpu_disable_irqs();
1288 rx_packet = (ll_pdu_t *) btstack_linked_queue_dequeue(&ctx.rx_queue);
1289 hal_cpu_enable_irqs();
1290 /** critical section end */
1291 if (rx_packet == NULL) break;
1292 uint8_t ll_id;
1293 if (rx_packet->len > 0){
1294 switch (ll_state){
1295 case LL_STATE_ADVERTISING:
1296 if ((rx_packet->flags & LL_PDU_FLAG_DATA_PDU) != 0){
1297 break;
1298 }
1299 if ((rx_packet->header & 0x0f) != PDU_ADV_TYPE_CONNECT_IND) {
1300 break;
1301 }
1302 ll_connected_handle_conn_ind(rx_packet);
1303 break;
1304 case LL_STATE_SCANNING:
1305 if ((rx_packet->flags & LL_PDU_FLAG_DATA_PDU) != 0){
1306 break;
1307 }
1308 ll_handle_adv(rx_packet);
1309 break;
1310 case LL_STATE_CONNECTED:
1311 // DATA PDU
1312 if ((rx_packet->flags & LL_PDU_FLAG_DATA_PDU) == 0){
1313 break;
1314 }
1315 ll_id = rx_packet->header & 3;
1316 if (ll_id == PDU_DATA_LLID_CTRL) {
1317 ll_connected_handle_control(rx_packet);
1318 } else {
1319 ll_connected_handle_data(rx_packet);
1320 }
1321 break;
1322 default:
1323 break;
1324 }
1325 }
1326 // free packet
1327 /** critical section start */
1328 hal_cpu_disable_irqs();
1329 btstack_memory_ll_pdu_free(rx_packet);
1330 hal_cpu_enable_irqs();
1331 /** critical section end */
1332 }
1333
1334 // generate HCI events
1335
1336 // report num complete packets
1337 /** critical section start */
1338 hal_cpu_disable_irqs();
1339 uint8_t num_completed = ctx.num_completed;
1340 ctx.num_completed = 0;
1341 hal_cpu_enable_irqs();
1342 /** critical section end */
1343 if (num_completed > 0){
1344 ll_emit_hci_event(&hci_event_number_of_completed_packets_1, 1, HCI_CON_HANDLE, num_completed);
1345 }
1346
1347 // report connection event
1348 if (ctx.ll_send_connection_complete){
1349 ctx.ll_send_connection_complete = false;
1350 ll_emit_hci_event(&hci_subevent_le_connection_complete,
1351 ERROR_CODE_SUCCESS, HCI_CON_HANDLE, 0x01 /* slave */, ctx.peer_addr_type, ctx.peer_addr,
1352 ctx.conn_interval_1250us, ctx.conn_latency, ctx.supervision_timeout_10ms, 0 /* master clock accuracy */);
1353 }
1354
1355 // report disconnection event
1356 if (ctx.ll_send_disconnected){
1357 ctx.ll_send_disconnected = false;
1358 uint8_t error_code = ctx.error_code;
1359 ctx.error_code = ERROR_CODE_SUCCESS;
1360 ll_emit_hci_event(&hci_event_disconnection_complete, ERROR_CODE_SUCCESS, HCI_CON_HANDLE, error_code);
1361 }
1362 }
1363
ll_reserve_acl_packet(void)1364 bool ll_reserve_acl_packet(void){
1365 if (ll_reserved_acl_buffer == NULL){
1366
1367 /** critical section start */
1368 hal_cpu_disable_irqs();
1369 ll_reserved_acl_buffer = btstack_memory_ll_pdu_get();
1370 hal_cpu_enable_irqs();
1371 /** critical section end */
1372
1373 }
1374 return ll_reserved_acl_buffer != NULL;
1375 }
1376
ll_queue_acl_packet(const uint8_t * packet,uint16_t size)1377 void ll_queue_acl_packet(const uint8_t * packet, uint16_t size){
1378 btstack_assert(ll_reserved_acl_buffer != NULL);
1379
1380 ll_pdu_t * tx_packet = ll_reserved_acl_buffer;
1381 ll_reserved_acl_buffer = NULL;
1382
1383 switch ((packet[1] >> 4) & 0x03){
1384 case 0:
1385 case 2:
1386 tx_packet->header = PDU_DATA_LLID_DATA_START;
1387 break;
1388 case 1:
1389 tx_packet->header = PDU_DATA_LLID_DATA_CONTINUE;
1390 break;
1391 case 3:
1392 while(1);
1393 break;
1394 default:
1395 break;
1396 }
1397 tx_packet->len = size - 4;
1398 memcpy(tx_packet->payload, &packet[4], size - 4);
1399
1400 /** critical section start */
1401 hal_cpu_disable_irqs();
1402 btstack_linked_queue_enqueue(&ctx.tx_queue, (btstack_linked_item_t *) tx_packet);
1403 hal_cpu_enable_irqs();
1404 /** critical section end */
1405 }
1406
ll_register_packet_handler(void (* packet_handler)(uint8_t packet_type,uint8_t * packet,uint16_t size))1407 void ll_register_packet_handler(void (*packet_handler)(uint8_t packet_type, uint8_t * packet, uint16_t size)){
1408 controller_packet_handler = packet_handler;
1409 }
1410