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_sx1280.c"
39
40 #define DEBUG
41
42 #include <string.h>
43
44 #include "ll.h"
45
46 #include "hw.h"
47 #include "radio.h"
48 #include "sx1280.h"
49 #include "debug.h"
50 #include "btstack_config.h"
51 #include "btstack_debug.h"
52 #include "btstack_memory.h"
53 #include "btstack_memory_pool.h"
54 #include "btstack_linked_queue.h"
55 #include "bluetooth_company_id.h"
56 #include "hal_cpu.h"
57 #include "hci_event.h"
58 #include "hopping.h"
59 #include "hal_timer.h"
60
61
62 //
63 // configuration
64 //
65
66 #define AUTO_RX_TX_TIME_US 86
67
68 #define TX_PARAMS_RAMP_TIME RADIO_RAMP_02_US
69
70 // set output power in dBM, range [-18..+13] dBm - Bluetooth LE max is 10 dBM
71 #define TX_PARAMS_OUTPUT_POWER 10
72
73
74
75 #define ACL_LE_MAX_PAYLOAD 31
76 #define ADV_MAX_PAYLOAD (6+6+22)
77 #define LL_MAX_PAYLOAD 37
78
79 // split 256 bytes data buffer into 2 rx and 2 tx buffers
80 #define SX1280_RX0_OFFSET 0
81 #define SX1280_RX1_OFFSET 64
82 #define SX1280_TX0_OFFSET 128
83 #define SX1280_TX1_OFFSET 192
84
85
86 // Mask of IRQs to listen in tx and rx mode
87 #define RX_TX_IRQ_MASK (IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_CRC_ERROR)
88
89 // sync hop delay - time we prepare for next connection event
90 #define SYNC_HOP_DELAY_US 600
91
92 // num tx buffers for use by link layer
93 #define HCI_NUM_TX_BUFFERS_LL 4
94
95 // num rx buffers
96 #define HCI_NUM_RX_BUFFERS 16
97
98 // total number PDU buffers
99 #define MAX_NUM_LL_PDUS (HCI_NUM_TX_BUFFERS_STACK + HCI_NUM_TX_BUFFERS_LL + HCI_NUM_RX_BUFFERS)
100
101 // HCI Connection Handle used for all HCI events/connections
102 #define HCI_CON_HANDLE 0x0001
103
104 // convert us to ticks, rounding to the closest tick count
105 // @note us must be <= 1000000 us = 1 s
106 #define US_TO_TICKS(US) (((((uint32_t)(US)) * 4096) + 6125) / 125000L)
107
108 // ADV PDU Types
109 enum pdu_adv_type {
110 PDU_ADV_TYPE_ADV_IND = 0x00,
111 PDU_ADV_TYPE_DIRECT_IND = 0x01,
112 PDU_ADV_TYPE_NONCONN_IND = 0x02,
113 PDU_ADV_TYPE_SCAN_REQ = 0x03,
114 PDU_ADV_TYPE_AUX_SCAN_REQ = PDU_ADV_TYPE_SCAN_REQ,
115 PDU_ADV_TYPE_SCAN_RSP = 0x04,
116 PDU_ADV_TYPE_CONNECT_IND = 0x05,
117 PDU_ADV_TYPE_AUX_CONNECT_REQ = PDU_ADV_TYPE_CONNECT_IND,
118 PDU_ADV_TYPE_SCAN_IND = 0x06,
119 PDU_ADV_TYPE_EXT_IND = 0x07,
120 PDU_ADV_TYPE_AUX_ADV_IND = PDU_ADV_TYPE_EXT_IND,
121 PDU_ADV_TYPE_AUX_SCAN_RSP = PDU_ADV_TYPE_EXT_IND,
122 PDU_ADV_TYPE_AUX_SYNC_IND = PDU_ADV_TYPE_EXT_IND,
123 PDU_ADV_TYPE_AUX_CHAIN_IND = PDU_ADV_TYPE_EXT_IND,
124 PDU_ADV_TYPE_AUX_CONNECT_RSP = 0x08,
125 };
126
127 // DATA PDU Types
128 enum pdu_data_llid {
129 PDU_DATA_LLID_RESV = 0x00,
130 PDU_DATA_LLID_DATA_CONTINUE = 0x01,
131 PDU_DATA_LLID_DATA_START = 0x02,
132 PDU_DATA_LLID_CTRL = 0x03,
133 };
134
135 // DATA Link Layer Control Types
136 enum pdu_data_llctrl_type {
137 PDU_DATA_LLCTRL_TYPE_CONN_UPDATE_IND = 0x00,
138 PDU_DATA_LLCTRL_TYPE_CHAN_MAP_IND = 0x01,
139 PDU_DATA_LLCTRL_TYPE_TERMINATE_IND = 0x02,
140 PDU_DATA_LLCTRL_TYPE_ENC_REQ = 0x03,
141 PDU_DATA_LLCTRL_TYPE_ENC_RSP = 0x04,
142 PDU_DATA_LLCTRL_TYPE_START_ENC_REQ = 0x05,
143 PDU_DATA_LLCTRL_TYPE_START_ENC_RSP = 0x06,
144 PDU_DATA_LLCTRL_TYPE_UNKNOWN_RSP = 0x07,
145 PDU_DATA_LLCTRL_TYPE_FEATURE_REQ = 0x08,
146 PDU_DATA_LLCTRL_TYPE_FEATURE_RSP = 0x09,
147 PDU_DATA_LLCTRL_TYPE_PAUSE_ENC_REQ = 0x0A,
148 PDU_DATA_LLCTRL_TYPE_PAUSE_ENC_RSP = 0x0B,
149 PDU_DATA_LLCTRL_TYPE_VERSION_IND = 0x0C,
150 PDU_DATA_LLCTRL_TYPE_REJECT_IND = 0x0D,
151 PDU_DATA_LLCTRL_TYPE_SLAVE_FEATURE_REQ = 0x0E,
152 PDU_DATA_LLCTRL_TYPE_CONN_PARAM_REQ = 0x0F,
153 PDU_DATA_LLCTRL_TYPE_CONN_PARAM_RSP = 0x10,
154 PDU_DATA_LLCTRL_TYPE_REJECT_EXT_IND = 0x11,
155 PDU_DATA_LLCTRL_TYPE_PING_REQ = 0x12,
156 PDU_DATA_LLCTRL_TYPE_PING_RSP = 0x13,
157 PDU_DATA_LLCTRL_TYPE_LENGTH_REQ = 0x14,
158 PDU_DATA_LLCTRL_TYPE_LENGTH_RSP = 0x15,
159 PDU_DATA_LLCTRL_TYPE_PHY_REQ = 0x16,
160 PDU_DATA_LLCTRL_TYPE_PHY_RSP = 0x17,
161 PDU_DATA_LLCTRL_TYPE_PHY_UPD_IND = 0x18,
162 PDU_DATA_LLCTRL_TYPE_MIN_USED_CHAN_IND = 0x19,
163 };
164
165 // Radio State
166 typedef enum {
167 RADIO_LOWPOWER,
168 RADIO_RX_ERROR,
169 RADIO_TX_TIMEOUT,
170 RADIO_W4_TX_DONE_TO_RX,
171 RADIO_W4_TX_ONLY_DONE,
172 RADIO_W4_TIMER,
173 } radio_state_t;
174
175 // Link Layer State
176 typedef enum {
177 LL_STATE_STANDBY,
178 LL_STATE_SCANNING,
179 LL_STATE_ADVERTISING,
180 LL_STATE_INITIATING,
181 LL_STATE_CONNECTED
182 } ll_state_t;
183
184 // Link Layer PDU Flags
185 typedef enum {
186 LL_PDU_FLAG_DATA_PDU = 1,
187 } ll_pdu_flags;
188
189 // Link Layer PDU, used in linked list
190 typedef struct {
191 // header
192 void * item;
193 hci_con_handle_t con_handle;
194 uint8_t flags;
195 // over the air data
196 uint8_t header;
197 uint8_t len;
198 uint8_t payload[LL_MAX_PAYLOAD];
199 } ll_pdu_t;
200
201 // channel table: freq in hertz and whitening seed
202 static const struct {
203 uint32_t freq_hz;
204 uint8_t whitening;
205 } channel_table[] = {
206 { 2404000000, 0x01 /* 00000001 */ },
207 { 2406000000, 0x41 /* 01000001 */ },
208 { 2408000000, 0x21 /* 00100001 */ },
209 { 2410000000, 0x61 /* 01100001 */ },
210 { 2412000000, 0x11 /* 00010001 */ },
211 { 2414000000, 0x51 /* 01010001 */ },
212 { 2416000000, 0x31 /* 00110001 */ },
213 { 2418000000, 0x71 /* 01110001 */ },
214 { 2420000000, 0x09 /* 00001001 */ },
215 { 2422000000, 0x49 /* 01001001 */ },
216 { 2424000000, 0x29 /* 00101001 */ },
217 { 2428000000, 0x69 /* 01101001 */ },
218 { 2430000000, 0x19 /* 00011001 */ },
219 { 2432000000, 0x59 /* 01011001 */ },
220 { 2434000000, 0x39 /* 00111001 */ },
221 { 2436000000, 0x79 /* 01111001 */ },
222 { 2438000000, 0x05 /* 00000101 */ },
223 { 2440000000, 0x45 /* 01000101 */ },
224 { 2442000000, 0x25 /* 00100101 */ },
225 { 2444000000, 0x65 /* 01100101 */ },
226 { 2446000000, 0x15 /* 00010101 */ },
227 { 2448000000, 0x55 /* 01010101 */ },
228 { 2450000000, 0x35 /* 00110101 */ },
229 { 2452000000, 0x75 /* 01110101 */ },
230 { 2454000000, 0x0d /* 00001101 */ },
231 { 2456000000, 0x4d /* 01001101 */ },
232 { 2458000000, 0x2d /* 00101101 */ },
233 { 2460000000, 0x6d /* 01101101 */ },
234 { 2462000000, 0x1d /* 00011101 */ },
235 { 2464000000, 0x5d /* 01011101 */ },
236 { 2466000000, 0x3d /* 00111101 */ },
237 { 2468000000, 0x7d /* 01111101 */ },
238 { 2470000000, 0x03 /* 00000011 */ },
239 { 2472000000, 0x43 /* 01000011 */ },
240 { 2474000000, 0x23 /* 00100011 */ },
241 { 2476000000, 0x63 /* 01100011 */ },
242 { 2478000000, 0x13 /* 00010011 */ },
243 { 2402000000, 0x53 /* 01010011 */ },
244 { 2426000000, 0x33 /* 00110011 */ },
245 { 2480000000, 0x73 /* 01110011 */ },
246 };
247
248 // tx buffer offset
249 static uint8_t tx_buffer_offset[] = {
250 SX1280_TX0_OFFSET,
251 SX1280_TX1_OFFSET
252 };
253
254 // hopping context
255 static hopping_t h;
256
257 static struct {
258
259 volatile bool synced;
260
261 volatile uint16_t packet_nr_in_connection_event;
262
263 volatile uint16_t conn_interval_1250us;
264 volatile uint32_t conn_interval_us;
265 volatile uint16_t conn_interval_ticks;
266
267 volatile uint16_t conn_latency;
268
269 volatile uint16_t supervision_timeout_10ms;
270 volatile uint32_t supervision_timeout_us;
271
272 //
273 volatile uint32_t time_without_any_packets_us;
274
275 // access address
276 volatile uint32_t aa;
277
278 // start of current connection event
279 volatile uint16_t anchor_ticks;
280
281 // latest time to send tx packet before sync hop
282 volatile uint16_t conn_latest_tx_ticks;
283
284 // timeout for sync relative to anchor
285 volatile uint16_t conn_sync_hop_ticks;
286
287 // current channel
288 volatile uint8_t channel;
289
290 // CSA #2 supported
291 uint8_t csa2_support;
292
293 // channels selection algorithm index (1 for csa #2)
294 volatile uint8_t channel_selection_algorithm;
295
296 // current connection event, first one starts with 0
297 // - needed for connection param and channel map updates as well as encryption
298 volatile uint16_t connection_event;
299
300 // pending channel map update
301 volatile bool channel_map_update_pending;
302 volatile uint16_t channel_map_update_instant;
303 volatile uint8_t channel_map_update_map[5];
304
305 // pending connection param update
306 volatile bool conn_param_update_pending;
307 volatile uint16_t conn_param_update_instant;
308 volatile uint8_t conn_param_update_win_size;
309 volatile uint16_t conn_param_update_win_offset;
310 volatile uint16_t conn_param_update_interval_1250us;
311 volatile uint16_t conn_param_update_latency;
312 volatile uint32_t conn_param_update_timeout_us;
313
314 // our bd_addr as little endian
315 uint8_t bd_addr_le[6];
316
317 // peer addr
318 uint8_t peer_addr_type;
319 uint8_t peer_addr[6];
320
321 // adv data
322 uint8_t adv_len;
323 uint8_t adv_data[31];
324
325 // adv param
326 uint8_t adv_map;
327 uint32_t adv_interval_us;
328 uint8_t adv_type;
329
330 // adv data
331 uint8_t scan_resp_len;
332 uint8_t scan_resp_data[31];
333
334 // next expected sequence number
335 volatile uint8_t next_expected_sequence_number;
336
337 // transmit sequence number
338 volatile uint8_t transmit_sequence_number;
339
340 // num completed packets
341 volatile uint8_t num_completed;
342
343 // rx queue
344 btstack_linked_queue_t rx_queue;
345
346 // current incoming packet
347 ll_pdu_t * rx_pdu;
348
349 // rx packet ready
350 bool rx_pdu_received;
351
352 // tx queue of outgoing pdus
353 btstack_linked_queue_t tx_queue;
354
355 // pdus transferred into controller tx buffers
356 ll_pdu_t * tx_buffer_pdu[2];
357
358 // manage tx packets on controller
359 uint8_t num_tx_pdus_on_controller;
360
361 // index of next tx buffer to send
362 uint8_t next_tx_buffer;
363
364 } ctx;
365
366 static radio_state_t radio_state = RADIO_LOWPOWER;
367
368 // Buffer pool
369 static ll_pdu_t ll_pdu_pool_storage[MAX_NUM_LL_PDUS];
370 static btstack_memory_pool_t ll_pdu_pool;
371
372 // single ll control response
373 static ll_pdu_t ll_tx_packet;
374 static ll_pdu_t ll_empty_packet;
375
376 // Link Layer State
377 static ll_state_t ll_state;
378 static uint32_t ll_scan_interval_us;
379 static uint32_t ll_scan_window_us;
380
381 static ll_pdu_t * ll_reserved_acl_buffer;
382 static void (*controller_packet_handler)(uint8_t packet_type, uint8_t * packet, uint16_t size);
383
384 static uint8_t ll_outgoing_hci_event[258];
385 static bool ll_send_disconnected;
386 static bool ll_send_connection_complete;
387
388 // prototypes
389 static void radio_set_timer_ticks(uint32_t anchor_offset_ticks);
390
391
392 // memory pool for acl-le pdus
btstack_memory_ll_pdu_get(void)393 static ll_pdu_t * btstack_memory_ll_pdu_get(void){
394 void * buffer = btstack_memory_pool_get(&ll_pdu_pool);
395 if (buffer){
396 memset(buffer, 0, sizeof(ll_pdu_t));
397 }
398 return (ll_pdu_t *) buffer;
399 }
400
btstack_memory_ll_pdu_free(ll_pdu_t * acl_le_pdu)401 static void btstack_memory_ll_pdu_free(ll_pdu_t *acl_le_pdu){
402 btstack_memory_pool_free(&ll_pdu_pool, acl_le_pdu);
403 }
404
radio_auto_tx_on(void)405 static void radio_auto_tx_on(void){
406 // SetAutoTX(150 ms) - direct write / ignore compensation
407 uint8_t buf[2];
408 big_endian_store_16(buf, 0, AUTO_RX_TX_TIME_US);
409 SX1280HalWriteCommand( RADIO_SET_AUTOTX, buf, 2 );
410 }
411
radio_auto_tx_off(void)412 static void radio_auto_tx_off(void){
413 // SetAutoTX(0) - direct write / ignore compensation
414 uint8_t buf[2] = { 0, 0 };
415 SX1280HalWriteCommand( RADIO_SET_AUTOTX, buf, 2 );
416 }
417
receive_prepare_rx_bufffer(void)418 static bool receive_prepare_rx_bufffer(void){
419 if (ctx.rx_pdu == NULL){
420 ctx.rx_pdu = btstack_memory_ll_pdu_get();
421 }
422 if (ctx.rx_pdu == NULL){
423 printf("No free RX buffer\n");
424 return false;
425 } else {
426 return true;
427 }
428 }
429
receive_response(void)430 static void receive_response(void){
431 if (receive_prepare_rx_bufffer()) {
432 // 150 us would be enough, but the timeout seems to apply for AutoTx as well, so we use 250 us
433 Radio.SetRx( ( TickTime_t ) { RADIO_TICK_SIZE_0015_US, 16 } );
434 }
435 }
436
receive_first_master(void)437 static void receive_first_master(void){
438 if (receive_prepare_rx_bufffer()){
439 Radio.SetRx( ( TickTime_t ) { RADIO_TICK_SIZE_1000_US, 1000 } );
440 }
441 }
442
receive_master(void)443 static void receive_master(void){
444 if (receive_prepare_rx_bufffer()) {
445 Radio.SetRx((TickTime_t) {RADIO_TICK_SIZE_1000_US, 1});
446 }
447 }
448
setup_adv_pdu(uint8_t offset,uint8_t header,uint8_t len,const uint8_t * data)449 static void setup_adv_pdu(uint8_t offset, uint8_t header, uint8_t len, const uint8_t * data){
450 uint8_t buffer[39];
451 buffer[0] = header;
452 buffer[1] = 6 + len;
453 memcpy(&buffer[2], ctx.bd_addr_le, 6);
454 memcpy(&buffer[8], data, len);
455 uint16_t packet_size = 2 + buffer[1];
456 SX1280HalWriteBuffer( offset, buffer, packet_size );
457 }
458
send_adv(void)459 static void send_adv(void){
460
461 // enable AutoTX for potential Scan Response
462 // TODO: only if adv type allows for scanning
463 radio_auto_tx_on();
464
465 SX1280SetBufferBaseAddresses( SX1280_TX0_OFFSET, SX1280_RX0_OFFSET);
466 SX1280SetTx( ( TickTime_t ){ RADIO_TICK_SIZE_1000_US, 1 } );
467 }
468
select_channel(uint8_t channel)469 static void select_channel(uint8_t channel){
470 // Set Whitening seed
471 Radio.SetWhiteningSeed( channel_table[channel].whitening );
472
473 // Sel Frequency
474 Radio.SetRfFrequency( channel_table[channel].freq_hz );
475 }
476
next_channel(void)477 static void next_channel(void){
478 switch (ctx.channel_selection_algorithm){
479 case 0:
480 ctx.channel = hopping_csa1_get_next_channel( &h );
481 break;
482 case 1:
483 ctx.channel = hopping_csa2_get_channel_for_counter( &h, ctx.connection_event);
484 break;
485 default:
486 break;
487 }
488 select_channel(ctx.channel);
489 }
490
ll_advertising_statemachine(void)491 static void ll_advertising_statemachine(void){
492 switch ( radio_state) {
493 case RADIO_RX_ERROR:
494 case RADIO_LOWPOWER:
495 // find next channel
496 while (ctx.channel < 40){
497 ctx.channel++;
498 if ((ctx.adv_map & (1 << (ctx.channel - 37))) != 0) {
499 // Set Channel
500 select_channel(ctx.channel);
501 if (ctx.adv_type == 3) {
502 // Non connectable undirected advertising (ADV_NONCONN_IND)
503 radio_state = RADIO_W4_TX_ONLY_DONE;
504 } else {
505 // All other are either connectable and/or scannable
506 radio_state = RADIO_W4_TX_DONE_TO_RX;
507 }
508 send_adv();
509 break;
510 }
511 if (ctx.channel >= 40){
512 // Set timer
513 radio_state = RADIO_W4_TIMER;
514 uint32_t adv_interval_ticks = US_TO_TICKS(ctx.adv_interval_us);
515 radio_set_timer_ticks(adv_interval_ticks);
516 }
517 }
518 break;
519 default:
520 break;
521 }
522 }
523
start_advertising(void)524 static void start_advertising(void){
525
526 Radio.StopAutoTx();
527
528 PacketParams_t packetParams;
529 packetParams.PacketType = PACKET_TYPE_BLE;
530 packetParams.Params.Ble.BlePacketType = BLE_EYELONG_1_0;
531 packetParams.Params.Ble.ConnectionState = BLE_PAYLOAD_LENGTH_MAX_37_BYTES;
532 packetParams.Params.Ble.CrcField = BLE_CRC_3B;
533 packetParams.Params.Ble.Whitening = RADIO_WHITENING_ON;
534 Radio.SetPacketParams( &packetParams );
535
536 // Set CRC init value 0x555555
537 Radio.WriteRegister(0x9c7, 0x55 );
538 Radio.WriteRegister(0x9c8, 0x55 );
539 Radio.WriteRegister(0x9c9, 0x55 );
540
541 // Set AccessAddress for ADV packets
542 Radio.SetBleAdvertizerAccessAddress( );
543
544 // prepare adv and scan data in tx0 and tx1
545 setup_adv_pdu(SX1280_TX0_OFFSET, PDU_ADV_TYPE_ADV_IND, ctx.adv_len, ctx.adv_data);
546 setup_adv_pdu(SX1280_TX1_OFFSET, PDU_ADV_TYPE_SCAN_RSP, ctx.scan_resp_len, ctx.scan_resp_data);
547
548 radio_state = RADIO_LOWPOWER;
549 ll_state = LL_STATE_ADVERTISING;
550
551 // prepare
552 ctx.channel = 36;
553 ctx.anchor_ticks = hal_timer_get_ticks();
554
555 // and get started
556 ll_advertising_statemachine();
557 }
558
start_hopping(void)559 static void start_hopping(void){
560 PacketParams_t packetParams;
561 packetParams.PacketType = PACKET_TYPE_BLE;
562 packetParams.Params.Ble.BlePacketType = BLE_EYELONG_1_0;
563 packetParams.Params.Ble.ConnectionState = BLE_PAYLOAD_LENGTH_MAX_31_BYTES;
564 packetParams.Params.Ble.CrcField = BLE_CRC_3B;
565 packetParams.Params.Ble.Whitening = RADIO_WHITENING_ON;
566 Radio.SetPacketParams( &packetParams );
567
568 }
569
radio_stop_timer(void)570 static void radio_stop_timer(void){
571 hal_timer_stop();
572 }
573
radio_set_timer_ticks(uint32_t anchor_offset_ticks)574 static void radio_set_timer_ticks(uint32_t anchor_offset_ticks){
575 radio_stop_timer();
576 // set timer for next radio event relative to anchor
577 uint16_t timeout_ticks = (uint16_t) (ctx.anchor_ticks + anchor_offset_ticks);
578 hal_timer_start(timeout_ticks);
579 }
580
ctx_set_conn_interval(uint16_t conn_interval_1250us)581 static void ctx_set_conn_interval(uint16_t conn_interval_1250us){
582 ctx.conn_interval_1250us = conn_interval_1250us;
583 ctx.conn_interval_us = ctx.conn_interval_1250us * 1250;
584 ctx.conn_interval_ticks = US_TO_TICKS(ctx.conn_interval_us);
585 ctx.conn_sync_hop_ticks = US_TO_TICKS(ctx.conn_interval_us - SYNC_HOP_DELAY_US);
586
587 // latest time to send a packet before getting ready for next cnonection event
588 uint16_t max_packet_time_incl_ifs_us = 500;
589 ctx.conn_latest_tx_ticks = US_TO_TICKS(ctx.conn_interval_us - SYNC_HOP_DELAY_US - max_packet_time_incl_ifs_us);
590 }
591
ll_terminate(void)592 static void ll_terminate(void){
593 ll_state = LL_STATE_STANDBY;
594 ctx.conn_param_update_pending = false;
595 ctx.channel_map_update_pending = false;
596 // stop sync hop timer
597 radio_stop_timer();
598 // free outgoing tx packets
599 uint8_t i;
600 for (i=0;i<2;i++){
601 ll_pdu_t * tx_pdu = ctx.tx_buffer_pdu[i];
602 if ((tx_pdu != NULL) && (tx_pdu != &ll_tx_packet) && (tx_pdu != &ll_empty_packet)){
603 btstack_memory_ll_pdu_free(tx_pdu);
604 ctx.tx_buffer_pdu[i] = NULL;
605 }
606 }
607 ctx.num_tx_pdus_on_controller = 0;
608 // free queued tx packets
609 while (true){
610 ll_pdu_t * tx_pdu = (ll_pdu_t *) btstack_linked_queue_dequeue(&ctx.tx_queue);
611 if (tx_pdu != NULL) {
612 btstack_memory_ll_pdu_free(tx_pdu);
613 } else {
614 break;
615 }
616 }
617 // disable auto tx
618 Radio.StopAutoTx();
619 // notify host stack
620 ll_send_disconnected = true;
621 }
622
623 // load queued tx pdu into next free tx buffer
preload_tx_buffer(void)624 static void preload_tx_buffer(void){
625 if (ctx.num_tx_pdus_on_controller >= 2) return;
626
627 ll_pdu_t * tx_pdu = (ll_pdu_t *) btstack_linked_queue_dequeue(&ctx.tx_queue);
628 if (tx_pdu == NULL) return;
629
630 const uint16_t max_packet_len = 2 + 27;
631 uint8_t index = (ctx.next_tx_buffer + ctx.num_tx_pdus_on_controller) & 1;
632 ctx.tx_buffer_pdu[index] = tx_pdu;
633 SX1280HalWriteBuffer( tx_buffer_offset[index], (uint8_t *) &ctx.tx_buffer_pdu[index]->header, max_packet_len);
634
635 ctx.num_tx_pdus_on_controller++;
636 // printf("preload %u bytes into %u\n", ctx.tx_buffer_pdu[index]->len, index);
637 }
638
radio_timer_handler(void)639 static void radio_timer_handler(void){
640
641 uint16_t t0 = hal_timer_get_ticks();
642
643 switch (ll_state){
644 case LL_STATE_CONNECTED:
645 // check supervision timeout
646 ctx.time_without_any_packets_us += ctx.conn_interval_us;
647 if (ctx.time_without_any_packets_us > ctx.supervision_timeout_us) {
648 printf("Supervision timeout\n\n");
649 ll_terminate();
650 return;
651 }
652
653 // prepare next connection event
654 ctx.connection_event++;
655 ctx.anchor_ticks += ctx.conn_interval_ticks;
656
657 ctx.packet_nr_in_connection_event = 0;
658 next_channel();
659
660 if (ctx.channel_map_update_pending && (ctx.channel_map_update_instant == ctx.connection_event)) {
661 hopping_set_channel_map( &h, (const uint8_t *) &ctx.channel_map_update_map );
662 ctx.channel_map_update_pending = false;
663 }
664
665 if (ctx.conn_param_update_pending && ((ctx.conn_param_update_instant) == ctx.connection_event) ) {
666 ctx_set_conn_interval(ctx.conn_param_update_interval_1250us);
667 ctx.conn_latency = ctx.conn_param_update_latency;
668 ctx.supervision_timeout_us = ctx.conn_param_update_timeout_us;
669 ctx.conn_param_update_pending = false;
670
671 log_info("Conn param update now");
672
673 radio_stop_timer();
674 ctx.synced = false;
675 }
676
677 // preload tx pdu
678 preload_tx_buffer();
679
680 if (ctx.synced){
681 // restart radio timer (might get overwritten by first packet)
682 radio_set_timer_ticks(ctx.conn_sync_hop_ticks);
683
684 receive_master();
685 } else {
686 // just wait longer
687 receive_first_master();
688 }
689
690 // printf("--SYNC-Ch %02u-Event %04u - t %08u--\n", ctx.channel, ctx.connection_event, t0);
691 break;
692 case LL_STATE_ADVERTISING:
693 // send adv on all configured channels
694 ctx.channel = 36;
695 ctx.anchor_ticks = t0;
696 radio_stop_timer();
697 ll_advertising_statemachine();
698 radio_state = RADIO_LOWPOWER;
699 break;
700 default:
701 break;
702 }
703
704 }
705
radio_fetch_rx_pdu(void)706 static void radio_fetch_rx_pdu(void){
707
708 if (!ctx.rx_pdu_received) return;
709 ctx.rx_pdu_received = false;
710
711 // fetch reserved rx pdu
712 ll_pdu_t * rx_packet = ctx.rx_pdu;
713 btstack_assert(rx_packet != NULL);
714
715 // read max packet
716 uint16_t max_packet_len = 2 + 27;
717 SX1280HalReadBuffer( SX1280_RX0_OFFSET, &rx_packet->header, max_packet_len);
718
719 // queue if not empty
720 if (rx_packet->len != 0){
721
722 // packet used
723 ctx.rx_pdu = NULL;
724
725 // mark as data packet
726 rx_packet->flags |= LL_PDU_FLAG_DATA_PDU;
727
728 // queue received packet
729 btstack_linked_queue_enqueue(&ctx.rx_queue, (btstack_linked_item_t *) rx_packet);
730 }
731 }
732
733 /** Radio IRQ handlers */
radio_on_tx_done(void)734 static void radio_on_tx_done(void ){
735 switch (ll_state){
736 case LL_STATE_ADVERTISING:
737 switch (radio_state){
738 case RADIO_W4_TX_DONE_TO_RX:
739 receive_response();
740 break;
741 case RADIO_W4_TX_ONLY_DONE:
742 radio_state = RADIO_LOWPOWER;
743 break;
744 default:
745 break;
746 }
747 break;
748 case LL_STATE_CONNECTED:
749 btstack_assert(radio_state == RADIO_W4_TX_DONE_TO_RX);
750 receive_response();
751 radio_fetch_rx_pdu();
752 preload_tx_buffer();
753 break;
754 default:
755 break;
756 }
757 }
radio_prepare_auto_tx(uint16_t packet_end_ticks,uint8_t rx_len)758 static void radio_prepare_auto_tx(uint16_t packet_end_ticks, uint8_t rx_len){
759 // restart supervision timeout
760 ctx.time_without_any_packets_us = 0;
761
762 // check if we can sent a full packet before sync hop
763 int16_t now_ticks = packet_end_ticks - ctx.anchor_ticks;
764 if (ctx.synced && (now_ticks > ctx.conn_latest_tx_ticks)){
765 // disable AutoTX to abort sending of next packet
766 Radio.SetFs();
767 log_info("Close before Sync hop: now %u > %u", now_ticks, ctx.conn_latest_tx_ticks);
768
769 // get rx pdu and
770 radio_fetch_rx_pdu();
771 return;
772 }
773
774 // setup empty packet in ll buffer if no tx packet was preloaded
775 if (ctx.num_tx_pdus_on_controller == 0) {
776 ctx.tx_buffer_pdu[ctx.next_tx_buffer] = &ll_empty_packet;
777 ctx.num_tx_pdus_on_controller++;
778 ll_empty_packet.header = PDU_DATA_LLID_DATA_CONTINUE;
779 ll_empty_packet.len = 0;
780 }
781
782 // setup pdu header
783 uint8_t packet_header[2];
784 uint8_t md = btstack_linked_queue_empty(&ctx.tx_queue) ? 0 : 1;
785 packet_header[0] = (md << 4) | (ctx.transmit_sequence_number << 3) | (ctx.next_expected_sequence_number << 2) | ctx.tx_buffer_pdu[ctx.next_tx_buffer]->header;
786 packet_header[1] = ctx.tx_buffer_pdu[ctx.next_tx_buffer]->len;
787
788 // select outgoing tx buffer and update pdu header
789 SX1280SetBufferBaseAddresses( tx_buffer_offset[ctx.next_tx_buffer], SX1280_RX0_OFFSET);
790 SX1280HalWriteBuffer( tx_buffer_offset[ctx.next_tx_buffer], (uint8_t *) packet_header, sizeof(packet_header));
791
792 // update operating state
793 SX1280AutoTxWillStart();
794
795 // set anchor on first packet in connection event
796 if (ctx.packet_nr_in_connection_event == 0){
797
798 // preamble (1) + aa (4) + header (1) + len (1) + payload (len) + crc (3) -- ISR handler ca. 35 us
799 uint16_t timestamp_delay = (10 + rx_len) * 8 - 35;
800 uint16_t packet_start_ticks = packet_end_ticks - US_TO_TICKS(timestamp_delay);
801
802 ctx.anchor_ticks = packet_start_ticks;
803 ctx.synced = true;
804 radio_set_timer_ticks(ctx.conn_sync_hop_ticks);
805 }
806
807 ctx.packet_nr_in_connection_event++;
808
809 // printf("RX %02x -- tx buffer %u, %02x %02x\n", rx_header, ctx.next_tx_buffer, packet_header[0], packet_header[1]);
810 }
811
radio_on_rx_done(void)812 static void radio_on_rx_done(void ){
813 uint16_t packet_end_ticks = hal_timer_get_ticks();
814
815 if (ll_state == LL_STATE_ADVERTISING){
816
817 // get rx pdu header
818 uint8_t rx_header;
819 SX1280HalReadBuffer( SX1280_RX0_OFFSET, &rx_header, 1);
820
821 // check for Scan Request
822 uint8_t pdu_type = rx_header & 0x0f;
823 if (pdu_type == PDU_ADV_TYPE_SCAN_REQ){
824 // scan request, select TX1 for active AutoTx
825 SX1280SetBufferBaseAddresses( SX1280_TX1_OFFSET, SX1280_RX0_OFFSET);
826 radio_state = RADIO_W4_TX_ONLY_DONE;
827 } else {
828
829 // fetch reserved rx pdu
830 ll_pdu_t * rx_packet = ctx.rx_pdu;
831 btstack_assert(rx_packet != NULL);
832 ctx.rx_pdu = NULL;
833
834 // no data packet
835 rx_packet->flags = 0;
836 uint16_t max_packet_len = 2 + LL_MAX_PAYLOAD;
837
838 // no scan request, disable auto tx and read complete buffer
839 radio_auto_tx_off();
840 SX1280HalReadBuffer( SX1280_RX0_OFFSET, &rx_packet->header, max_packet_len);
841
842 // queue received packet
843 btstack_linked_queue_enqueue(&ctx.rx_queue, (btstack_linked_item_t *) rx_packet);
844 }
845
846 } else if (ll_state == LL_STATE_CONNECTED){
847
848 // get and parse rx pdu header
849 uint8_t rx_buffer[2];
850 SX1280HalReadBuffer( SX1280_RX0_OFFSET, rx_buffer, 2);
851 uint8_t rx_header = rx_buffer[0];
852 uint8_t rx_len = rx_buffer[1];
853 uint8_t next_expected_sequence_number = (rx_header >> 2) & 1;
854 uint8_t sequence_number = (rx_header >> 3) & 1;
855 // more data field not used yet
856 // uint8_t more_data = (rx_packet->header >> 4) & 1;
857
858 // only accept packets with new sequence number and len <= payload size
859 if ((sequence_number == ctx.next_expected_sequence_number) && (rx_len <= LL_MAX_PAYLOAD)) {
860
861 bool rx_buffer_available = receive_prepare_rx_bufffer();
862 if (rx_buffer_available){
863 // update state
864 ctx.next_expected_sequence_number = 1 - sequence_number;
865
866 // register pdu fetch
867 ctx.rx_pdu_received = true;
868 }
869 }
870
871 // report outgoing packet as ack'ed and free if confirmed by peer
872 bool tx_acked = ctx.transmit_sequence_number != next_expected_sequence_number;
873 if (tx_acked){
874 if (ctx.num_tx_pdus_on_controller > 0){
875 ll_pdu_t * acked_pdu = ctx.tx_buffer_pdu[ctx.next_tx_buffer];
876 btstack_assert(acked_pdu != NULL);
877 // if non link-layer packet, free buffer and report as completed
878 if ((acked_pdu != &ll_tx_packet) && (acked_pdu != &ll_empty_packet)){
879 btstack_memory_ll_pdu_free(acked_pdu);
880 ctx.tx_buffer_pdu[ctx.next_tx_buffer] = NULL;
881 ctx.num_completed++;
882 }
883 // next buffer
884 ctx.num_tx_pdus_on_controller--;
885 ctx.next_tx_buffer = (ctx.next_tx_buffer + 1 ) & 1;
886 }
887 ctx.transmit_sequence_number = next_expected_sequence_number;
888 }
889
890 // packet received, now prepare for AutoTX
891 radio_prepare_auto_tx(packet_end_ticks, rx_len);
892 }
893 }
894
radio_on_tx_timeout(void)895 static void radio_on_tx_timeout(void ){
896 radio_state = RADIO_TX_TIMEOUT;
897 printf( "<>>>>>>>>TXE\n\r" );
898 }
899
radio_on_rx_timeout(void)900 static void radio_on_rx_timeout(void ){
901 switch (ll_state){
902 case LL_STATE_ADVERTISING:
903 radio_state = RADIO_RX_ERROR;
904 break;
905 default:
906 break;
907 }
908 }
909
radio_on_rx_error(IrqErrorCode_t errorCode)910 static void radio_on_rx_error(IrqErrorCode_t errorCode ){
911 uint16_t packet_end_ticks = hal_timer_get_ticks();
912 uint8_t rx_buffer[2];
913 uint8_t rx_len;
914 switch (ll_state){
915 case LL_STATE_ADVERTISING:
916 radio_state = RADIO_RX_ERROR;
917 break;
918 case LL_STATE_CONNECTED:
919 // get len from rx pdu header
920 SX1280HalReadBuffer(SX1280_RX0_OFFSET, rx_buffer, 2);
921 rx_len = rx_buffer[1];
922 radio_prepare_auto_tx(packet_end_ticks, rx_len);
923 break;
924 default:
925 break;
926 }
927 }
928
929 const static RadioCallbacks_t Callbacks =
930 {
931 &radio_on_tx_done, // txDone
932 &radio_on_rx_done, // rxDone
933 NULL, // syncWordDone
934 NULL, // headerDone
935 &radio_on_tx_timeout, // txTimeout
936 &radio_on_rx_timeout, // rxTimeout
937 &radio_on_rx_error, // rxError
938 NULL, // rangingDone
939 NULL, // cadDone
940 };
941
942 // Link Layer
943
ll_emit_hci_event(const hci_event_t * event,...)944 static void ll_emit_hci_event(const hci_event_t * event, ...){
945 va_list argptr;
946 va_start(argptr, event);
947 uint16_t length = hci_event_create_from_template_and_arglist(ll_outgoing_hci_event, sizeof(ll_outgoing_hci_event), event, argptr);
948 va_end(argptr);
949 controller_packet_handler(HCI_EVENT_PACKET, ll_outgoing_hci_event, length);
950 }
951
ll_init(void)952 void ll_init(void){
953
954 // setup memory pools
955 btstack_memory_pool_create(&ll_pdu_pool, ll_pdu_pool_storage, MAX_NUM_LL_PDUS, sizeof(ll_pdu_t));
956
957 // set test bd addr 33:33:33:33:33:33
958 memset(ctx.bd_addr_le, 0x33, 6);
959
960 // default channels, advertising interval
961 ctx.adv_map = 0x7;
962 ctx.adv_interval_us = 1280000;
963
964 // init timer
965 hal_timer_init();
966 hal_timer_set_callback(&radio_timer_handler);
967 }
968
ll_radio_on(void)969 void ll_radio_on(void){
970
971 Radio.Init( (RadioCallbacks_t *) &Callbacks );
972 Radio.SetRegulatorMode( USE_DCDC ); // Can also be set in LDO mode but consume more power
973 Radio.SetInterruptMode( );
974 Radio.SetDioIrqParams( RX_TX_IRQ_MASK, RX_TX_IRQ_MASK, IRQ_RADIO_NONE, IRQ_RADIO_NONE );
975
976 ModulationParams_t modulationParams;
977 modulationParams.PacketType = PACKET_TYPE_BLE;
978 modulationParams.Params.Ble.BitrateBandwidth = GFSK_BLE_BR_1_000_BW_1_2;
979 modulationParams.Params.Ble.ModulationIndex = GFSK_BLE_MOD_IND_0_50;
980 modulationParams.Params.Ble.ModulationShaping = RADIO_MOD_SHAPING_BT_0_5;
981
982 Radio.SetStandby( STDBY_RC );
983 Radio.SetPacketType( modulationParams.PacketType );
984 Radio.SetModulationParams( &modulationParams );
985 Radio.SetBufferBaseAddresses( SX1280_TX0_OFFSET, SX1280_RX0_OFFSET );
986 Radio.SetTxParams( TX_PARAMS_OUTPUT_POWER, TX_PARAMS_RAMP_TIME );
987
988 // Go back to Frequcency Synthesis Mode, reduces transition time between Rx<->TX
989 Radio.SetAutoFS(1);
990
991 uint16_t fw_version = SX1280GetFirmwareVersion();
992 printf("FW Version: 0x%04x\n", fw_version);
993
994 // quick test
995 uint8_t data[] = {1, 2, 4, 8, 16, 32, 64, 128, 1, 2, 4, 8, 16, 32, 64, 128, 1, 2, 4, 8, 16, 32, 64, 128, 1, 2, 4, 8, 16, 32, 64, 128 };
996 Radio.WriteBuffer(0, data, sizeof(data));
997 uint8_t check[32];
998 Radio.ReadBuffer(0, check, sizeof(data));
999 if (memcmp(data, check, sizeof(data)) != 0) {
1000 printf("GOOD: "); printf_hexdump(data, sizeof(data));
1001 printf("BAD: "); printf_hexdump(check, sizeof(data));
1002 btstack_assert(false);
1003 }
1004
1005 ll_state = LL_STATE_STANDBY;
1006 }
1007
ll_handle_conn_ind(ll_pdu_t * rx_packet)1008 static void ll_handle_conn_ind(ll_pdu_t * rx_packet){
1009 printf("Connect Req: ");
1010 printf_hexdump(&rx_packet->header, rx_packet->len + 2);
1011
1012 uint8_t * init_addr = &rx_packet->payload[0];
1013 uint8_t * adv_addr = &rx_packet->payload[6];
1014 uint8_t chan_sel = (rx_packet->header >> 5) & 1;
1015
1016 // verify AdvA
1017 if (memcmp(ctx.bd_addr_le, adv_addr, 6) != 0){
1018 // differs, go back to adv sending
1019 radio_state = RADIO_LOWPOWER;
1020 return;
1021 }
1022
1023 // TODO: get remote addr type
1024 ctx.peer_addr_type = 0;
1025 memcpy(ctx.peer_addr, init_addr, 6);
1026
1027 // get params for HCI event
1028 const uint8_t * ll_data = &rx_packet->payload[12];
1029
1030 ctx.aa = little_endian_read_32(ll_data, 0);
1031 uint8_t crc_init_0 = ll_data[4];
1032 uint8_t crc_init_1 = ll_data[5];
1033 uint8_t crc_init_2 = ll_data[6];
1034 uint8_t win_size = ll_data[7];
1035 uint16_t win_offset = little_endian_read_16(ll_data, 8);
1036 uint16_t conn_interval_1250us = little_endian_read_16(ll_data, 10);
1037 ctx.conn_latency = little_endian_read_16(ll_data, 12);
1038 ctx.supervision_timeout_10ms = little_endian_read_16(ll_data, 14);
1039 const uint8_t * channel_map = &ll_data[16];
1040 uint8_t hop = ll_data[21] & 0x1f;
1041 uint8_t sca = ll_data[21] >> 5;
1042
1043 UNUSED(sca);
1044 UNUSED(win_offset);
1045 UNUSED(win_size);
1046
1047 ctx_set_conn_interval(conn_interval_1250us);
1048
1049 // convert to us
1050 ctx.supervision_timeout_us = ctx.supervision_timeout_10ms * 10000;
1051 ctx.connection_event = 0;
1052 ctx.packet_nr_in_connection_event = 0;
1053 ctx.next_expected_sequence_number = 0;
1054 ctx.transmit_sequence_number = 0;
1055
1056 // set AA
1057 Radio.SetBleAccessAddress(ctx.aa);
1058
1059 // set CRC init value
1060 Radio.WriteRegister(0x9c7, crc_init_2);
1061 Radio.WriteRegister(0x9c8, crc_init_1);
1062 Radio.WriteRegister(0x9c9, crc_init_0);
1063
1064 printf("Connection interval %u us\n", ctx.conn_interval_us);
1065 printf("Connection timeout %u us\n", ctx.supervision_timeout_us);
1066 printf("AA %08x\n", ctx.aa);
1067 printf("CRC Init 0x%02x%02x%02x\n", crc_init_2, crc_init_1, crc_init_0);
1068
1069 // init hopping
1070 hopping_init( &h );
1071 hopping_set_channel_map( &h, channel_map);
1072 ctx.channel_selection_algorithm = ctx.csa2_support & chan_sel;
1073 switch (ctx.channel_selection_algorithm){
1074 case 0:
1075 hopping_csa1_set_hop_increment( &h, hop );
1076 break;
1077 case 1:
1078 hopping_csa2_set_access_address( &h, ctx.aa);
1079 break;
1080 default:
1081 break;
1082 }
1083 next_channel();
1084
1085 start_hopping();
1086
1087 radio_auto_tx_on();
1088
1089 // pre-load tx pdu
1090 ctx.num_tx_pdus_on_controller = 0;
1091 ctx.next_tx_buffer = 0;
1092 preload_tx_buffer();
1093
1094 // get next packet
1095 ll_state = LL_STATE_CONNECTED;
1096
1097 receive_first_master();
1098 ll_send_connection_complete = true;
1099 }
1100
ll_queue_control_tx(void)1101 static void ll_queue_control_tx(void){
1102 hal_cpu_disable_irqs();
1103 btstack_linked_queue_enqueue(&ctx.tx_queue, (btstack_linked_item_t *) &ll_tx_packet);
1104 hal_cpu_enable_irqs();
1105 }
1106
ll_handle_control(ll_pdu_t * rx_packet)1107 static void ll_handle_control(ll_pdu_t * rx_packet){
1108 ll_pdu_t * tx_packet = &ll_tx_packet;
1109 uint8_t opcode = rx_packet->payload[0];
1110 switch (opcode){
1111 case PDU_DATA_LLCTRL_TYPE_VERSION_IND:
1112 tx_packet->len = 6;
1113 tx_packet->header = PDU_DATA_LLID_CTRL;
1114 tx_packet->payload[0] = PDU_DATA_LLCTRL_TYPE_VERSION_IND;
1115 tx_packet->payload[1] = 0x06; // VersNr = Bluetooth Core V4.0
1116 little_endian_store_16(tx_packet->payload, 2, BLUETOOTH_COMPANY_ID_BLUEKITCHEN_GMBH);
1117 little_endian_store_16(tx_packet->payload, 4, 0);
1118 ll_queue_control_tx();
1119 printf("Queue Version Ind\n");
1120 break;
1121 case PDU_DATA_LLCTRL_TYPE_FEATURE_REQ:
1122 tx_packet->len = 9;
1123 tx_packet->header = PDU_DATA_LLID_CTRL;
1124 tx_packet->payload[0] = PDU_DATA_LLCTRL_TYPE_FEATURE_RSP;
1125 // TODO: set features of our controller
1126 memset(&tx_packet->payload[1], 0, 8);
1127 ll_queue_control_tx();
1128 printf("Queue Feature Rsp\n");
1129 break;
1130 case PDU_DATA_LLCTRL_TYPE_CHAN_MAP_IND:
1131 memcpy((uint8_t *) ctx.channel_map_update_map, &rx_packet->payload[1], 5);
1132 ctx.channel_map_update_instant = little_endian_read_16(rx_packet->payload, 6);
1133 ctx.channel_map_update_pending = true;
1134 break;
1135 case PDU_DATA_LLCTRL_TYPE_CONN_UPDATE_IND:
1136 ctx.conn_param_update_win_size = rx_packet->payload[1];
1137 ctx.conn_param_update_win_offset = little_endian_read_16(rx_packet->payload, 2);
1138 ctx.conn_param_update_interval_1250us = little_endian_read_16(rx_packet->payload, 4);
1139 ctx.conn_param_update_latency = little_endian_read_16(rx_packet->payload, 6);
1140 ctx.conn_param_update_timeout_us = little_endian_read_16(rx_packet->payload, 8) * 10000;
1141 ctx.conn_param_update_instant = little_endian_read_16(rx_packet->payload, 10);
1142 ctx.conn_param_update_pending = true;
1143 log_info("PDU_DATA_LLCTRL_TYPE_CONN_UPDATE_IND, conn interval %u 1250us at instant %u",
1144 (unsigned int) ctx.conn_param_update_interval_1250us, ctx.conn_param_update_instant);
1145 break;
1146 case PDU_DATA_LLCTRL_TYPE_TERMINATE_IND:
1147 printf("Terminate!\n");
1148 ll_terminate();
1149 break;
1150 default:
1151 btstack_assert(false);
1152 printf("Unhandled LL Control PDU 0x%02x\n", opcode);
1153 break;
1154 }
1155 }
1156
ll_handle_data(ll_pdu_t * rx_packet)1157 static void ll_handle_data(ll_pdu_t * rx_packet){
1158 if (ll_state != LL_STATE_CONNECTED) return;
1159 btstack_assert(rx_packet->len <= LL_MAX_PAYLOAD);
1160 uint8_t acl_packet[4 + LL_MAX_PAYLOAD];
1161 // ACL Header
1162 uint8_t ll_id = rx_packet->header & 3;
1163 acl_packet[0] = 0x01;
1164 acl_packet[1] = ll_id << 4;
1165 little_endian_store_16(acl_packet, 2, rx_packet->len);
1166 memcpy(&acl_packet[4], rx_packet->payload, rx_packet->len);
1167 (*controller_packet_handler)(HCI_ACL_DATA_PACKET, acl_packet, rx_packet->len + 4);
1168 }
1169
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)1170 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){
1171 // TODO .. store other params
1172 ll_scan_interval_us = ((uint32_t) le_scan_interval) * 625;
1173 ll_scan_window_us = ((uint32_t) le_scan_window) * 625;
1174 log_info("LE Scan Params: window %lu, interval %lu ms", ll_scan_interval_us, ll_scan_window_us);
1175 }
1176
ll_start_scanning(uint8_t filter_duplicates)1177 static uint8_t ll_start_scanning(uint8_t filter_duplicates){
1178 #if 0
1179 // COMMAND DISALLOWED if wrong state.
1180 if (ll_state != LL_STATE_STANDBY) return 0x0c;
1181
1182 ll_state = LL_STATE_SCANNING;
1183
1184 log_info("LE Scan Start: window %lu, interval %lu ms", ll_scan_interval_us, ll_scan_window_us);
1185
1186 // reset timer and capature events
1187 NRF_TIMER0->TASKS_CLEAR = 1;
1188 NRF_TIMER0->TASKS_STOP = 1;
1189 NRF_TIMER0->EVENTS_COMPARE[0] = 0;
1190 NRF_TIMER0->EVENTS_COMPARE[1] = 0;
1191
1192 // limit scanning
1193 if (ll_scan_window_us < ll_scan_interval_us){
1194 // setup PPI to disable radio after end of scan_window
1195 NRF_TIMER0->CC[1] = ll_scan_window_us;
1196 NRF_PPI->CHENSET = 1 << 22; // TIMER0->EVENTS_COMPARE[1] -> RADIO->TASKS_DISABLE
1197 }
1198
1199 // set timer to trigger IRQ for next scan interval
1200 NRF_TIMER0->CC[0] = ll_scan_interval_us;
1201 NRF_TIMER0->INTENSET = TIMER_INTENSET_COMPARE0_Enabled << TIMER_INTENSET_COMPARE0_Pos;
1202
1203 // next channel to scan
1204 int adv_channel = (random_generator_next() % 3) + 37;
1205 log_debug("LE Scan Channel: %u", adv_channel);
1206
1207 // start receiving
1208 NRF_TIMER0->TASKS_START = 1;
1209 radio_receive_on_channel(adv_channel);
1210 #endif
1211 return 0;
1212 }
1213
ll_stop_scanning(void)1214 static uint8_t ll_stop_scanning(void){
1215 #if 0
1216 // COMMAND DISALLOWED if wrong state.
1217 if (ll_state != LL_STATE_SCANNING) return 0x0c;
1218
1219 log_info("LE Scan Stop");
1220
1221 ll_state = LL_STATE_STANDBY;
1222
1223 // stop radio
1224 radio_disable();
1225
1226 #endif
1227 return 0;
1228 }
1229
ll_set_scan_enable(uint8_t le_scan_enable,uint8_t filter_duplicates)1230 uint8_t ll_set_scan_enable(uint8_t le_scan_enable, uint8_t filter_duplicates){
1231 if (le_scan_enable){
1232 return ll_start_scanning(filter_duplicates);
1233 } else {
1234 return ll_stop_scanning();
1235 }
1236 }
1237
ll_start_advertising(void)1238 static uint8_t ll_start_advertising(void){
1239 // COMMAND DISALLOWED if wrong state.
1240 if (ll_state != LL_STATE_STANDBY) return ERROR_CODE_COMMAND_DISALLOWED;
1241 log_info("Start Advertising on channels 0x%0x, interval %lu us", ctx.adv_map, ctx.adv_interval_us);
1242 start_advertising();
1243 return ERROR_CODE_SUCCESS;
1244 }
1245
ll_stop_advertising(void)1246 static uint8_t ll_stop_advertising(void){
1247 // COMMAND DISALLOWED if wrong state.
1248 if (ll_state != LL_STATE_ADVERTISING) return ERROR_CODE_COMMAND_DISALLOWED;
1249
1250 // TODO:
1251 return ERROR_CODE_SUCCESS;
1252 }
1253
ll_set_advertise_enable(uint8_t le_adv_enable)1254 uint8_t ll_set_advertise_enable(uint8_t le_adv_enable){
1255 if (le_adv_enable){
1256 return ll_start_advertising();
1257 } else {
1258 return ll_stop_advertising();
1259 }
1260 }
1261
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)1262 uint8_t ll_set_advertising_parameters(uint16_t advertising_interval_min, uint16_t advertising_interval_max,
1263 uint8_t advertising_type, uint8_t own_address_type, uint8_t peer_address_types, uint8_t * peer_address,
1264 uint8_t advertising_channel_map, uint8_t advertising_filter_policy){
1265
1266 // validate channel map
1267 if (advertising_channel_map == 0) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1268 if ((advertising_channel_map & 0xf8) != 0) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1269
1270 // validate advertising interval
1271 if (advertising_interval_min < 0x20) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1272 if (advertising_interval_min > 0x4000) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1273 if (advertising_interval_max < 0x20) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1274 if (advertising_interval_max > 0x4000) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1275 if (advertising_interval_min > advertising_interval_max) return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
1276
1277 ctx.adv_map = advertising_channel_map;
1278 ctx.adv_interval_us = advertising_interval_max * 625;
1279 ctx.adv_type= advertising_type;
1280
1281 // TODO: validate other params
1282 // TODO: process other params
1283
1284 return ERROR_CODE_SUCCESS;
1285 }
1286
ll_set_advertising_data(uint8_t adv_len,const uint8_t * adv_data)1287 uint8_t ll_set_advertising_data(uint8_t adv_len, const uint8_t * adv_data){
1288 // COMMAND DISALLOWED if wrong state.
1289 if (ll_state == LL_STATE_ADVERTISING) return ERROR_CODE_COMMAND_DISALLOWED;
1290 if (adv_len > 31) return ERROR_CODE_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE;
1291 ctx.adv_len = adv_len;
1292 memcpy(ctx.adv_data, adv_data, adv_len);
1293
1294 return ERROR_CODE_SUCCESS;
1295 }
1296
ll_set_scan_response_data(uint8_t adv_len,const uint8_t * adv_data)1297 uint8_t ll_set_scan_response_data(uint8_t adv_len, const uint8_t * adv_data){
1298 // COMMAND DISALLOWED if wrong state.
1299 if (ll_state == LL_STATE_ADVERTISING) return ERROR_CODE_COMMAND_DISALLOWED;
1300 if (adv_len > 31) return ERROR_CODE_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE;
1301 ctx.scan_resp_len = adv_len;
1302 memcpy(ctx.scan_resp_data, adv_data, adv_len);
1303
1304 return ERROR_CODE_SUCCESS;
1305 }
1306
ll_execute_once(void)1307 void ll_execute_once(void){
1308 // process received packets
1309 while (1){
1310 ll_pdu_t * rx_packet = (ll_pdu_t *) btstack_linked_queue_dequeue(&ctx.rx_queue);
1311 if (rx_packet == NULL) break;
1312 if (rx_packet->len > 0){
1313 if ((rx_packet->flags & LL_PDU_FLAG_DATA_PDU) == 0){
1314 // ADV PDU
1315 // connect ind?
1316 if ((rx_packet->header & 0x0f) == PDU_ADV_TYPE_CONNECT_IND){
1317 ll_handle_conn_ind(rx_packet);
1318 }
1319 else {
1320 radio_state = RADIO_LOWPOWER;
1321 }
1322 } else {
1323 // DATA PDU
1324 uint8_t ll_id = rx_packet->header & 3;
1325 if (ll_id == PDU_DATA_LLID_CTRL) {
1326 ll_handle_control(rx_packet);
1327 } else {
1328 ll_handle_data(rx_packet);
1329 }
1330 }
1331 }
1332 // free packet
1333 btstack_memory_ll_pdu_free(rx_packet);
1334 }
1335
1336 switch ( ll_state ){
1337 case LL_STATE_ADVERTISING:
1338 ll_advertising_statemachine();
1339 break;
1340 default:
1341 break;
1342 }
1343
1344 // generate HCI events
1345
1346 // report num complete packets
1347 /** critical section start */
1348 hal_cpu_disable_irqs();
1349 uint8_t num_completed = ctx.num_completed;
1350 ctx.num_completed = 0;
1351 hal_cpu_enable_irqs();
1352 /** critical section end */
1353 if (num_completed > 0){
1354 ll_emit_hci_event(&hci_event_number_of_completed_packets_1, 1, HCI_CON_HANDLE, num_completed);
1355 }
1356
1357 // report connection event
1358 if (ll_send_connection_complete){
1359 ll_send_connection_complete = false;
1360 ll_emit_hci_event(&hci_subevent_le_connection_complete,
1361 ERROR_CODE_SUCCESS, HCI_CON_HANDLE, 0x01 /* slave */, ctx.peer_addr_type, ctx.peer_addr,
1362 ctx.conn_interval_1250us, ctx.conn_latency, ctx.supervision_timeout_10ms, 0 /* master clock accuracy */);
1363 }
1364
1365 // report disconnection event
1366 if (ll_send_disconnected){
1367 ll_send_disconnected = false;
1368 ll_emit_hci_event(&hci_event_disconnection_complete, ERROR_CODE_SUCCESS, HCI_CON_HANDLE, 0);
1369 }
1370 }
ll_reserve_acl_packet(void)1371 bool ll_reserve_acl_packet(void){
1372 if (ll_reserved_acl_buffer == NULL){
1373 ll_reserved_acl_buffer = btstack_memory_ll_pdu_get();
1374 }
1375 return ll_reserved_acl_buffer != NULL;
1376 }
1377
ll_queue_acl_packet(const uint8_t * packet,uint16_t size)1378 void ll_queue_acl_packet(const uint8_t * packet, uint16_t size){
1379 btstack_assert(ll_reserved_acl_buffer != NULL);
1380
1381 ll_pdu_t * tx_packet = ll_reserved_acl_buffer;
1382 ll_reserved_acl_buffer = NULL;
1383
1384 switch ((packet[1] >> 4) & 0x03){
1385 case 0:
1386 case 2:
1387 tx_packet->header = PDU_DATA_LLID_DATA_START;
1388 break;
1389 case 1:
1390 tx_packet->header = PDU_DATA_LLID_DATA_CONTINUE;
1391 break;
1392 case 3:
1393 while(1);
1394 break;
1395 default:
1396 break;
1397 }
1398 tx_packet->len = size - 4;
1399 memcpy(tx_packet->payload, &packet[4], size - 4);
1400 btstack_linked_queue_enqueue(&ctx.tx_queue, (btstack_linked_item_t *) tx_packet);
1401 }
1402
ll_register_packet_handler(void (* packet_handler)(uint8_t packet_type,uint8_t * packet,uint16_t size))1403 void ll_register_packet_handler(void (*packet_handler)(uint8_t packet_type, uint8_t * packet, uint16_t size)){
1404 controller_packet_handler = packet_handler;
1405 }
1406