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