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 * contact@bluekitchen-gmbh.com 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