|
20 | 20 | #include <syscfg/syscfg.h>
|
21 | 21 | #if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
|
22 | 22 | #include <stdint.h>
|
| 23 | +#include "controller/ble_ll_utils.h" |
23 | 24 | #include "controller/ble_ll_conn.h"
|
24 | 25 | #include "controller/ble_ll_sched.h"
|
25 | 26 | #include "controller/ble_ll_tmr.h"
|
26 | 27 | #include "ble_ll_cs_priv.h"
|
27 | 28 |
|
| 29 | +extern struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap; |
28 | 30 | extern uint8_t g_ble_ll_cs_chan_count;
|
29 | 31 | extern uint8_t g_ble_ll_cs_chan_indices[72];
|
30 | 32 | struct ble_ll_cs_sm *g_ble_ll_cs_sm_current;
|
@@ -54,10 +56,18 @@ struct ble_ll_cs_sm *g_ble_ll_cs_sm_current;
|
54 | 56 |
|
55 | 57 | #define BLE_LL_CONN_ITVL_USECS (1250)
|
56 | 58 |
|
| 59 | +/* The ramp-down window in µs */ |
| 60 | +#define T_RD (5) |
| 61 | +/* The guard time duration in µs */ |
| 62 | +#define T_GD (10) |
| 63 | +/* The requency measurement period in µs */ |
| 64 | +#define T_FM (80) |
| 65 | + |
57 | 66 | static struct ble_ll_cs_aci aci_table[] = {
|
58 | 67 | {1, 1, 1}, {2, 2, 1}, {3, 3, 1}, {4, 4, 1},
|
59 | 68 | {2, 1, 2}, {3, 1, 3}, {4, 1, 4}, {4, 2, 2}
|
60 | 69 | };
|
| 70 | +static const uint8_t rtt_seq_len[] = {0, 4, 12, 4, 8, 12, 16}; |
61 | 71 |
|
62 | 72 | static int
|
63 | 73 | ble_ll_cs_generate_channel(struct ble_ll_cs_sm *cssm)
|
@@ -113,10 +123,110 @@ ble_ll_cs_proc_rm_from_sched(void *cb_args)
|
113 | 123 | {
|
114 | 124 | }
|
115 | 125 |
|
| 126 | +static uint8_t |
| 127 | +ble_ll_cs_proc_set_t_sw(struct ble_ll_cs_sm *cssm) |
| 128 | +{ |
| 129 | + uint8_t t_sw; |
| 130 | + uint8_t t_sw_i; |
| 131 | + uint8_t t_sw_r; |
| 132 | + uint8_t aci = cssm->active_config->proc_params.aci; |
| 133 | + |
| 134 | + if (cssm->active_config->role == BLE_LL_CS_ROLE_INITIATOR) { |
| 135 | + t_sw_i = g_ble_ll_cs_local_cap.t_sw; |
| 136 | + t_sw_r = cssm->remote_cap.t_sw; |
| 137 | + } else { /* BLE_LL_CS_ROLE_REFLECTOR */ |
| 138 | + t_sw_i = cssm->remote_cap.t_sw; |
| 139 | + t_sw_r = g_ble_ll_cs_local_cap.t_sw; |
| 140 | + } |
| 141 | + |
| 142 | + if (aci == 0) { |
| 143 | + t_sw = 0; |
| 144 | + } else if (IN_RANGE(aci, 1, 3)) { |
| 145 | + t_sw = t_sw_i; |
| 146 | + } else if (IN_RANGE(aci, 4, 6)) { |
| 147 | + t_sw = t_sw_r; |
| 148 | + } else { /* ACI == 7 */ |
| 149 | + if (g_ble_ll_cs_local_cap.t_sw > cssm->remote_cap.t_sw) { |
| 150 | + t_sw = g_ble_ll_cs_local_cap.t_sw; |
| 151 | + } else { |
| 152 | + t_sw = cssm->remote_cap.t_sw; |
| 153 | + } |
| 154 | + } |
| 155 | + |
| 156 | + cssm->t_sw = t_sw; |
| 157 | + |
| 158 | + return t_sw; |
| 159 | +} |
| 160 | + |
| 161 | +static int |
| 162 | +ble_ll_cs_proc_calculate_timing(struct ble_ll_cs_sm *cssm) |
| 163 | +{ |
| 164 | + struct ble_ll_cs_config *conf = cssm->active_config; |
| 165 | + uint8_t t_ip1 = conf->t_ip1; |
| 166 | + uint8_t t_ip2 = conf->t_ip2; |
| 167 | + uint8_t t_pm = conf->t_pm; |
| 168 | + uint8_t t_sw; |
| 169 | + uint8_t n_ap = cssm->n_ap; |
| 170 | + uint8_t t_sy; |
| 171 | + uint8_t t_sy_seq; |
| 172 | + uint8_t sequence_len; |
| 173 | + |
| 174 | + t_sw = ble_ll_cs_proc_set_t_sw(cssm); |
| 175 | + |
| 176 | + /* CS packets with no Sounding Sequence or Random Sequence fields take 44 µs |
| 177 | + * to transmit when sent using the LE 1M PHY and 26 µs to transmit when using |
| 178 | + * the LE 2M and the LE 2M 2BT PHYs. CS packets that include a Sounding Sequence |
| 179 | + * or Random Sequence field take proportionally longer to transmit based on |
| 180 | + * the length of the field and the PHY selection. |
| 181 | + */ |
| 182 | + |
| 183 | + sequence_len = rtt_seq_len[conf->rtt_type]; |
| 184 | + |
| 185 | + switch (conf->cs_sync_phy) { |
| 186 | + case BLE_LL_CS_SYNC_PHY_1M: |
| 187 | + t_sy = BLE_LL_CS_SYNC_TIME_1M; |
| 188 | + t_sy_seq = sequence_len; |
| 189 | + break; |
| 190 | + case BLE_LL_CS_SYNC_PHY_2M: |
| 191 | + t_sy = BLE_LL_CS_SYNC_TIME_2M; |
| 192 | + t_sy_seq = sequence_len / 2; |
| 193 | + break; |
| 194 | + } |
| 195 | + |
| 196 | + cssm->mode0_step_duration_usecs = t_ip1 + T_GD + T_FM + 2 * (t_sy + T_RD); |
| 197 | + |
| 198 | + cssm->mode1_step_duration_usecs = t_ip1 + 2 * (t_sy + t_sy_seq + T_RD); |
| 199 | + |
| 200 | + cssm->mode2_step_duration_usecs = t_ip2 + 2 * ((t_sw + t_pm) * (n_ap + 1) + T_RD); |
| 201 | + |
| 202 | + cssm->mode3_step_duration_usecs = t_ip2 + 2 * ((t_sy + t_sy_seq + T_GD + T_RD) + |
| 203 | + (t_sw + t_pm) * (n_ap + 1)); |
| 204 | + |
| 205 | + cssm->t_sy = t_sy; |
| 206 | + cssm->t_sy_seq = t_sy_seq; |
| 207 | + |
| 208 | + return 0; |
| 209 | +} |
| 210 | + |
116 | 211 | static void
|
117 | 212 | ble_ll_cs_set_step_duration(struct ble_ll_cs_sm *cssm)
|
118 | 213 | {
|
119 |
| - /* TODO: cssm->step_duration_usecs = ? */ |
| 214 | + switch (cssm->step_mode) { |
| 215 | + case BLE_LL_CS_MODE0: |
| 216 | + cssm->step_duration_usecs = cssm->mode0_step_duration_usecs; |
| 217 | + break; |
| 218 | + case BLE_LL_CS_MODE1: |
| 219 | + cssm->step_duration_usecs = cssm->mode1_step_duration_usecs; |
| 220 | + break; |
| 221 | + case BLE_LL_CS_MODE2: |
| 222 | + cssm->step_duration_usecs = cssm->mode2_step_duration_usecs; |
| 223 | + break; |
| 224 | + case BLE_LL_CS_MODE3: |
| 225 | + cssm->step_duration_usecs = cssm->mode3_step_duration_usecs; |
| 226 | + break; |
| 227 | + default: |
| 228 | + assert(0); |
| 229 | + } |
120 | 230 | }
|
121 | 231 |
|
122 | 232 | static int
|
@@ -324,6 +434,7 @@ ble_ll_cs_setup_next_step(struct ble_ll_cs_sm *cssm)
|
324 | 434 | int rc;
|
325 | 435 | const struct ble_ll_cs_config *conf = cssm->active_config;
|
326 | 436 |
|
| 437 | + cssm->step_anchor_usecs = cssm->anchor_usecs; |
327 | 438 | ++cssm->steps_in_procedure_count;
|
328 | 439 | ++cssm->steps_in_subevent_count;
|
329 | 440 |
|
@@ -367,123 +478,190 @@ ble_ll_cs_setup_next_step(struct ble_ll_cs_sm *cssm)
|
367 | 478 | }
|
368 | 479 |
|
369 | 480 | cssm->antenna_path_count = cssm->n_ap;
|
| 481 | + cssm->anchor_usecs = cssm->step_anchor_usecs; |
| 482 | + |
370 | 483 | return 0;
|
371 | 484 | }
|
372 | 485 |
|
373 | 486 | static void
|
374 | 487 | ble_ll_cs_proc_mode0_next_state(struct ble_ll_cs_sm *cssm)
|
375 | 488 | {
|
| 489 | + uint32_t duration = 0; |
| 490 | + uint32_t delay = 0; |
376 | 491 | uint8_t state = cssm->step_state;
|
| 492 | + uint8_t t_ip1 = cssm->active_config->t_ip1; |
| 493 | + uint8_t t_fcs = cssm->active_config->t_fcs; |
| 494 | + uint8_t t_sy = cssm->t_sy; |
377 | 495 |
|
378 | 496 | switch (state) {
|
379 | 497 | case STEP_STATE_INIT:
|
380 | 498 | state = STEP_STATE_CS_SYNC_I;
|
| 499 | + duration = t_sy; |
| 500 | + delay = 0; |
381 | 501 | break;
|
382 | 502 | case STEP_STATE_CS_SYNC_I:
|
383 | 503 | state = STEP_STATE_CS_SYNC_R;
|
| 504 | + duration = t_sy; |
| 505 | + delay = T_RD + t_ip1; |
384 | 506 | break;
|
385 | 507 | case STEP_STATE_CS_SYNC_R:
|
386 | 508 | state = STEP_STATE_CS_TONE_R;
|
| 509 | + duration = T_FM; |
| 510 | + delay = T_GD; |
387 | 511 | break;
|
388 | 512 | case STEP_STATE_CS_TONE_R:
|
389 | 513 | state = STEP_STATE_COMPLETE;
|
| 514 | + duration = 0; |
| 515 | + delay = T_RD + t_fcs; |
390 | 516 | break;
|
391 | 517 | default:
|
392 | 518 | assert(0);
|
393 | 519 | }
|
394 | 520 |
|
| 521 | + cssm->duration_usecs = duration; |
| 522 | + cssm->anchor_usecs += delay; |
395 | 523 | cssm->step_state = state;
|
396 | 524 | }
|
397 | 525 |
|
398 | 526 | static void
|
399 | 527 | ble_ll_cs_proc_mode1_next_state(struct ble_ll_cs_sm *cssm)
|
400 | 528 | {
|
| 529 | + uint32_t duration = 0; |
| 530 | + uint32_t delay = 0; |
401 | 531 | uint8_t state = cssm->step_state;
|
| 532 | + uint8_t t_ip1 = cssm->active_config->t_ip1; |
| 533 | + uint8_t t_fcs = cssm->active_config->t_fcs; |
| 534 | + uint8_t t_sy = cssm->t_sy + cssm->t_sy_seq; |
402 | 535 |
|
403 | 536 | switch (state) {
|
404 | 537 | case STEP_STATE_INIT:
|
405 | 538 | state = STEP_STATE_CS_SYNC_I;
|
| 539 | + duration = t_sy; |
| 540 | + delay = 0; |
406 | 541 | break;
|
407 | 542 | case STEP_STATE_CS_SYNC_I:
|
408 | 543 | state = STEP_STATE_CS_SYNC_R;
|
| 544 | + duration = t_sy; |
| 545 | + delay = T_RD + t_ip1; |
409 | 546 | break;
|
410 | 547 | case STEP_STATE_CS_SYNC_R:
|
411 | 548 | state = STEP_STATE_COMPLETE;
|
| 549 | + duration = 0; |
| 550 | + delay = T_RD + t_fcs; |
412 | 551 | break;
|
413 | 552 | default:
|
414 | 553 | assert(0);
|
415 | 554 | }
|
416 | 555 |
|
| 556 | + cssm->duration_usecs = duration; |
| 557 | + cssm->anchor_usecs += delay; |
417 | 558 | cssm->step_state = state;
|
418 | 559 | }
|
419 | 560 |
|
420 | 561 | static void
|
421 | 562 | ble_ll_cs_proc_mode2_next_state(struct ble_ll_cs_sm *cssm)
|
422 | 563 | {
|
| 564 | + uint32_t duration = 0; |
| 565 | + uint32_t delay = 0; |
423 | 566 | uint8_t state = cssm->step_state;
|
| 567 | + uint8_t t_ip2 = cssm->active_config->t_ip2; |
| 568 | + uint8_t t_fcs = cssm->active_config->t_fcs; |
| 569 | + uint8_t t_pm = cssm->active_config->t_pm; |
424 | 570 |
|
425 | 571 | switch (state) {
|
426 | 572 | case STEP_STATE_INIT:
|
427 | 573 | state = STEP_STATE_CS_TONE_I;
|
| 574 | + duration = t_pm; |
| 575 | + delay = 0; |
428 | 576 | break;
|
429 | 577 | case STEP_STATE_CS_TONE_I:
|
| 578 | + duration = t_pm; |
430 | 579 | if (cssm->antenna_path_count != 0) {
|
431 | 580 | --cssm->antenna_path_count;
|
| 581 | + delay = cssm->t_sw; |
432 | 582 | } else {
|
433 | 583 | state = STEP_STATE_CS_TONE_R;
|
434 | 584 | cssm->antenna_path_count = cssm->n_ap;
|
| 585 | + delay = T_RD + t_ip2; |
435 | 586 | }
|
436 | 587 | break;
|
437 | 588 | case STEP_STATE_CS_TONE_R:
|
438 | 589 | if (cssm->antenna_path_count != 0) {
|
439 | 590 | --cssm->antenna_path_count;
|
| 591 | + duration = t_pm; |
| 592 | + delay = cssm->t_sw; |
440 | 593 | } else {
|
441 | 594 | state = STEP_STATE_COMPLETE;
|
442 | 595 | cssm->antenna_path_count = cssm->n_ap;
|
| 596 | + duration = 0; |
| 597 | + delay = T_RD + t_fcs; |
443 | 598 | }
|
444 | 599 | break;
|
445 | 600 | default:
|
446 | 601 | assert(0);
|
447 | 602 | }
|
448 | 603 |
|
| 604 | + cssm->duration_usecs = duration; |
| 605 | + cssm->anchor_usecs += delay; |
449 | 606 | cssm->step_state = state;
|
450 | 607 | }
|
451 | 608 |
|
452 | 609 | static void
|
453 | 610 | ble_ll_cs_proc_mode3_next_state(struct ble_ll_cs_sm *cssm)
|
454 | 611 | {
|
| 612 | + uint32_t duration = 0; |
| 613 | + uint32_t delay = 0; |
455 | 614 | uint8_t state = cssm->step_state;
|
| 615 | + uint8_t t_ip2 = cssm->active_config->t_ip2; |
| 616 | + uint8_t t_fcs = cssm->active_config->t_fcs; |
| 617 | + uint8_t t_sy = cssm->t_sy + cssm->t_sy_seq; |
| 618 | + uint8_t t_pm = cssm->active_config->t_pm; |
456 | 619 |
|
457 | 620 | switch (state) {
|
458 | 621 | case STEP_STATE_INIT:
|
459 | 622 | state = STEP_STATE_CS_SYNC_I;
|
| 623 | + duration = t_sy; |
| 624 | + delay = 0; |
460 | 625 | break;
|
461 | 626 | case STEP_STATE_CS_SYNC_I:
|
462 | 627 | state = STEP_STATE_CS_TONE_I;
|
| 628 | + duration = t_pm; |
| 629 | + delay = T_GD; |
463 | 630 | break;
|
464 | 631 | case STEP_STATE_CS_TONE_I:
|
| 632 | + duration = t_pm; |
465 | 633 | if (cssm->antenna_path_count != 0) {
|
466 | 634 | --cssm->antenna_path_count;
|
| 635 | + delay = cssm->t_sw; |
467 | 636 | } else {
|
468 | 637 | state = STEP_STATE_CS_TONE_R;
|
469 | 638 | cssm->antenna_path_count = cssm->n_ap;
|
| 639 | + delay = T_RD + t_ip2; |
470 | 640 | }
|
471 | 641 | break;
|
472 | 642 | case STEP_STATE_CS_TONE_R:
|
473 | 643 | if (cssm->antenna_path_count != 0) {
|
474 | 644 | --cssm->antenna_path_count;
|
| 645 | + duration = t_pm; |
| 646 | + delay = cssm->t_sw; |
475 | 647 | } else {
|
476 | 648 | state = STEP_STATE_CS_SYNC_R;
|
477 | 649 | cssm->antenna_path_count = cssm->n_ap;
|
| 650 | + duration = t_sy; |
| 651 | + delay = T_RD; |
478 | 652 | }
|
479 | 653 | break;
|
480 | 654 | case STEP_STATE_CS_SYNC_R:
|
481 | 655 | state = STEP_STATE_COMPLETE;
|
| 656 | + duration = 0; |
| 657 | + delay = T_RD + t_fcs; |
482 | 658 | break;
|
483 | 659 | default:
|
484 | 660 | assert(0);
|
485 | 661 | }
|
486 | 662 |
|
| 663 | + cssm->duration_usecs = duration; |
| 664 | + cssm->anchor_usecs += delay; |
487 | 665 | cssm->step_state = state;
|
488 | 666 | }
|
489 | 667 |
|
@@ -647,6 +825,8 @@ ble_ll_cs_proc_scheduling_start(struct ble_ll_conn_sm *connsm,
|
647 | 825 | cssm->mode0_next_chan_id = 0xFF;
|
648 | 826 | cssm->non_mode0_next_chan_id = 0xFF;
|
649 | 827 |
|
| 828 | + ble_ll_cs_proc_calculate_timing(cssm); |
| 829 | + |
650 | 830 | rc = ble_ll_cs_proc_schedule_next_tx_or_rx(cssm);
|
651 | 831 | if (rc) {
|
652 | 832 | return BLE_ERR_UNSPECIFIED;
|
|
0 commit comments