1 /*
2 * Copyright (c) 2022-2024, Texas Instruments Incorporated
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 *
9 * * Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 *
12 * * Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 *
16 * * Neither the name of Texas Instruments Incorporated nor the names of
17 * its contributors may be used to endorse or promote products derived
18 * from this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32 /*
33 * ======== ble_cs.c ========
34 */
35
36 #include <stdint.h>
37 #include <stdlib.h>
38 #include <string.h>
39
40 #include <ti/log/Log.h>
41
42 #include <ti/drivers/rcl/RCL_Command.h>
43 #include <ti/drivers/rcl/RCL_Buffer.h>
44 #include <ti/drivers/rcl/RCL_Scheduler.h>
45
46 #include <ti/drivers/rcl/hal/hal.h>
47 #include <ti/drivers/rcl/commands/ble_cs.h>
48
49 #include <ti/devices/DeviceFamily.h>
50 #include DeviceFamily_constructPath(inc/hw_lrfdtxf.h)
51 #include DeviceFamily_constructPath(inc/hw_lrfdrxf.h)
52 #include DeviceFamily_constructPath(inc/hw_lrfddbell.h)
53 #include DeviceFamily_constructPath(inc/hw_lrfdpbe.h)
54 #include DeviceFamily_constructPath(inc/hw_lrfdpbe32.h)
55 #include DeviceFamily_constructPath(inc/hw_lrfdmdm.h)
56 #include DeviceFamily_constructPath(inc/hw_lrfdmdm32.h)
57 #include DeviceFamily_constructPath(inc/hw_lrfdrfe.h)
58 #include DeviceFamily_constructPath(inc/hw_lrfdrfe32.h)
59 #include DeviceFamily_constructPath(inc/hw_lrfds2r.h)
60 #include DeviceFamily_constructPath(inc/rfe_common_ram_regs.h)
61 #include DeviceFamily_constructPath(inc/pbe_generic_ram_regs.h)
62 #include DeviceFamily_constructPath(inc/pbe_common_ram_regs.h)
63 #include DeviceFamily_constructPath(inc/pbe_ble5_ram_regs.h)
64 #include DeviceFamily_constructPath(inc/pbe_ble_cs_ram_regs.h)
65 #include DeviceFamily_constructPath(inc/pbe_ble_cs_regdef_regs.h)
66
67 #ifndef OCTOPUS
68 #include DeviceFamily_constructPath(inc/hw_types.h)
69 #include DeviceFamily_constructPath(inc/hw_memmap.h)
70 #include DeviceFamily_constructPath(inc/hw_gpio.h)
71 #include DeviceFamily_constructPath(inc/hw_ioc.h)
72 #endif
73
74 #define BLE_CS_BASE_FREQ_MHZ 2402
75 #define BLE_CS_MAX_CHANNEL 78
76 #define BLE_CS_NUM_CHANNELS 90
77 #define BLE_CS_NUM_INVALID_CHANNELS 7
78 #define BLE_CS_SILENT_CHANNEL 100
79 #define BLE_CS_NUM_PERMUTATION 24
80 #define BLE_CS_S2R_OVERSAMPLE_RATE 4
81 #define BLE_CS_S2R_MEMORY_OFFSET 0x0C00
82 #define BLE_CS_TXFIFO_BASE_ADDR 128
83 #define BLE_CS_TXFIFO_LEN 96
84 #define BLE_CS_RXFIFO_BASE_ADDR 224
85 #define BLE_CS_RXFIFO_LEN 160
86 #define BLE_CS_FIRST_BATCH_SIZE 2
87 #define BLE_CS_TONE_QUALITY_GOOD_THR 25
88 #define BLE_CS_TONE_QUALITY_MEDIUM_THR 50
89 #define BLE_CS_TONE_EXTENSION_INITIATOR_TX 0b01
90 #define BLE_CS_TONE_EXTENSION_REFLECTOR_TX 0b10
91 #define DECODE_ANTENNA(x, y) (((x) >> ((y)*2)) & 0x03)
92 #define ENCODE_ANTENNA(x, y) (((x) << ((y)*4)))
93 #define INT16_MSB(x) (((x) >> 8) & 0xFF)
94 #define INT16_LSB(x) ((x) & 0xFF)
95
96 /* Prepare constant terms for step specific calculations */
97 #define T1US 48 // Constant 1us at 48MHz
98 #define T3US 144 // Constant 3us at 48MHz
99 #define TRD 240 // Constant 5us at 48MHz
100 #define TGD 480 // Constant 10us at 48MHz
101 #define TPLT 960 // Constant 20us at 48MHz
102 #define TPYL 0 // Unknown at this point
103
104 /*
105 * ======== Execution status ========
106 */
107 struct
108 {
109 struct {
110 RCL_CommandStatus endStatus;
111 } common;
112 struct {
113 uint32_t startTime;
114 } ble_cs;
115 } bleCsHandlerState;
116
117 /* Storage to decompress data */
118 RCL_CmdBleCs_Step_Internal step;
119 RCL_CmdBleCs_StepResult_Internal result;
120
121 /* Precalculated base terms */
122 uint32_t tStepLut[RCL_CmdBleCs_StepMode_Length];
123 uint16_t tAdjALut[RCL_CmdBleCs_StepMode_Length];
124 uint16_t tAdjBLut[RCL_CmdBleCs_StepMode_Length];
125 uint16_t tAntALut[RCL_CmdBleCs_StepMode_Length];
126 uint16_t tAntBLut[RCL_CmdBleCs_StepMode_Length];
127 uint16_t tRttAdjustLut[RCL_CmdBleCs_StepMode_Length];
128
129 /*
130 * ======== Type for indexing antenna sequence ========
131 */
132 typedef union {
133 struct {
134 uint8_t a0 : 2;
135 uint8_t a1 : 2;
136 uint8_t a2 : 2;
137 uint8_t a3 : 2;
138 } a;
139 uint8_t apn;
140 } RCL_AntennaPath_t;
141
142 /*
143 * ======== Type for mapping antenna permutation to antenna sequence ========
144 */
145 typedef struct {
146 uint8_t numPath : 3; /*!< Total number of antenna path */
147 uint8_t numPerm : 5; /*!< Total number of permutation possible with this configuration */
148 uint8_t increment; /*!< How to index the permutation table */
149 RCL_AntennaPath_t ap[2];
150 } RCL_AntennaConfig_t;
151
152 /*
153 * ======== Antenna permutation table ========
154 */
155 const RCL_AntennaPath_t antennaPermutation[BLE_CS_NUM_PERMUTATION] = {
156 /*[ 0:5 ]=*/{{0,1,2,3}}, {{0,1,3,2}}, {{0,2,3,1}}, {{0,2,1,3}}, {{0,3,1,2}}, {{0,3,2,1}},
157 /*[ 6:11]=*/{{1,0,2,3}}, {{1,0,3,2}}, {{1,2,3,0}}, {{1,2,0,3}}, {{1,3,0,2}}, {{1,3,2,0}},
158 /*[12:17]=*/{{2,0,1,3}}, {{2,0,3,1}}, {{2,1,3,0}}, {{2,1,0,3}}, {{2,3,0,1}}, {{2,3,1,0}},
159 /*[18:24]=*/{{3,0,2,1}}, {{3,0,1,2}}, {{3,1,2,0}}, {{3,1,0,2}}, {{3,2,0,1}}, {{3,2,1,0}}
160 };
161
162 /*
163 * ======== Antenna permutation selection mapping ========
164 */
165 const RCL_AntennaConfig_t antennaConfig[RCL_CmdBleCs_AntennaConfig_Length] = {
166 /*1x1=*/{.numPath=1, .numPerm=1, .increment=0, .ap={/*initiator=*/{{0,0,0,0}}, /*reflector=*/{{0,0,0,0}}}},
167 /*2x1=*/{.numPath=2, .numPerm=2, .increment=6, .ap={/*initiator=*/{{0,1,0,0}}, /*reflector=*/{{0,0,0,0}}}},
168 /*3x1=*/{.numPath=3, .numPerm=6, .increment=3, .ap={/*initiator=*/{{0,1,2,0}}, /*reflector=*/{{0,0,0,0}}}},
169 /*4x1=*/{.numPath=4, .numPerm=24, .increment=1, .ap={/*initiator=*/{{0,1,2,3}}, /*reflector=*/{{0,0,0,0}}}},
170 /*1x2=*/{.numPath=2, .numPerm=2, .increment=6, .ap={/*initiator=*/{{0,0,0,0}}, /*reflector=*/{{0,1,0,0}}}},
171 /*1x3=*/{.numPath=3, .numPerm=6, .increment=3, .ap={/*initiator=*/{{0,0,0,0}}, /*reflector=*/{{0,1,2,0}}}},
172 /*1x4=*/{.numPath=4, .numPerm=24, .increment=1, .ap={/*initiator=*/{{0,0,0,0}}, /*reflector=*/{{0,1,2,3}}}},
173 /*2x2=*/{.numPath=4, .numPerm=24, .increment=1, .ap={/*initiator=*/{{0,0,1,1}}, /*reflector=*/{{0,1,1,0}}}}
174 };
175
176 /*
177 * ======== Type of configuration of MCE and FIDC accumulator used for PCT measurement ========
178 */
179 typedef union {
180 struct {
181 uint8_t period : 2; /*!< Configures the accumulation period of DEMFIDC module */
182 uint8_t numIteration : 2; /*!< Configures how many times to run the accumulator period */
183 uint8_t fracWaitAdj : 4;
184 uint8_t fracWait;
185 };
186 uint16_t val;
187 } RCL_PctConfig_t;
188
189 typedef union {
190 struct {
191 uint8_t magnIteration;
192 uint8_t reserved;
193 };
194 uint16_t val;
195 } RCL_MagnConfig_t;
196
197 typedef struct {
198 int16_t mode0[2];
199 int16_t mode1[2];
200 int16_t mode2[2];
201 int16_t mode3[2];
202 } RCL_Tadjust_t;
203
204 /*
205 * ======== Type of configuration for phy specific settings ========
206 */
207 typedef struct {
208 /* Modem specific registers */
209 uint16_t baud;
210 uint16_t symmap;
211 uint16_t demmisc2;
212 uint16_t demmisc3;
213 uint16_t rssioffset;
214
215 /* Timing parameters [48 MHz units unless specified otherwise] */
216 uint16_t tPostProcessDiv1; /*!< Duration set aside for post processing */
217 uint16_t tPostProcessDiv12; /*!< Duration set aside for post processing */
218 uint16_t tRxTimeoutI0; /*!< Initiator must terminate packet reception before end of step */
219 uint16_t tRxTimeoutI3; /*!< Initiator must terminate packet reception before end of step */
220 int16_t tPilotAdjust; /*!< Pilot tone need to be adjusted with shaper delay */
221 uint16_t tStartup; /*!< Duration set aside for starting up the topSMs and start to emit a tone on the antenna */
222 uint16_t tPacket; /*!< Duration of packet [preamble + AA + Trailer] */
223 uint16_t tRxTimeoutRn; /*!< Reception timeout on reflector side */
224 uint16_t tInfiniteRx; /*!< Correction of step duration when packet reception is used as anchor point */
225 uint16_t tTr; /*!< Duration of trailer bits */
226 uint16_t tCorr; /*!< Time between signal on antenna and correlation event */
227 uint16_t tDem; /*!< Time between signal on antenna and demodulation completed*/
228 uint16_t tFidc; /*!< Time between signal on antenna and FIDC */
229 uint16_t tTxModDelay; /*!< Time between modulator and signal on antenna (mostly shaper delay) */
230 uint16_t tTimestampDelay1; /*!< Sum of delays between signal on antenna vs capture on timer by HW for mode-1 */
231 uint16_t tTimestampDelay3; /*!< Sum of delays between signal on antenna vs capture on timer by HW for mode-3 */
232
233 /* Accumulator period and iteration number in order to fit into the same TPM period. */
234 RCL_PctConfig_t pctConfig[RCL_CmdBleCs_Tpm_Length];
235
236 /* Magnitude accumulator configuration used by RFE for tone quality estimation */
237 RCL_MagnConfig_t magnConfig[RCL_CmdBleCs_Tpm_Length];
238
239 /* Adjustment of TIP period per mode */
240 RCL_Tadjust_t tAdj[RCL_CmdBleCs_Role_Length];
241
242 /* Adjustment of antenna timings for mode 2 & 3 */
243 RCL_Tadjust_t tAnt[RCL_CmdBleCs_Role_Length];
244 } RCL_PhyConfig_t;
245
246 //#define FE_8MHZ 1
247 /*
248 * ======== Configuration per PHY ========
249 */
250 const RCL_PhyConfig_t phyConfigLut[RCL_CmdBleCs_Phy_Length] = {
251 #ifdef FE_8MHZ
252 { /* Phy_1M @ 8MHZ*/
253 .baud = 0x4000,
254 .symmap = 0x001F,
255 .demmisc2 = 0x15F8,
256 .demmisc3 = 0x10A1,
257 .rssioffset = 0x0049,
258
259 /* Theory */
260 .tPacket = 2112, // 8+32+4 bits = 44us
261 .tTr = 192, // 4us
262
263 /* Tuned */
264 .tStartup = 93, // 1.937us (pbe_timer_event -> mce_timer start)
265 .tTxModDelay = 245, // 5.109us (wait -> signal on shaper)
266 .tPilotAdjust = 264, // 5.5us
267 .tPostProcessDiv1 = 936, // 19.5us
268 .tPostProcessDiv12 = 78, // 19.5us
269 .tInfiniteRx = 60, // 1.25us
270
271 .tFidc = 161, // 3.35us
272 .tCorr = 244, // 5.08us
273 .tDem = 783, // 16.31us
274
275 .tRxTimeoutI0 = RCL_BLE_CS_US_TO_MCE_TIMER(70),
276 .tRxTimeoutI3 = RCL_BLE_CS_US_TO_MCE_TIMER(56),
277 .tRxTimeoutRn = RCL_BLE_CS_US_TO_MCE_TIMER(76),
278
279 /* Sum of demodulation and modulation delay (HW only) */
280 .tTimestampDelay1 = 477,
281 .tTimestampDelay3 = 481,
282
283 .pctConfig = {
284 /*TPM = 10US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL32 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 2, .fracWait = 32, .fracWaitAdj = 9},
285 /*TPM = 20US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL128 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 1, .fracWait = 128, .fracWaitAdj = 0},
286 /*TPM = 40US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL128 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 2, .fracWait = 128, .fracWaitAdj = 0}
287 },
288
289 .magnConfig = {
290 /*TPM = 10US*/{.magnIteration = 5},
291 /*TPM = 20US*/{.magnIteration = 14},
292 /*TPM = 40US*/{.magnIteration = 30}
293 },
294
295 .tAdj = { /* { pkt-tn, tn-tn} */
296 {
297 /* Initiator */
298 .mode0 = {0, 0},
299 .mode1 = {0, 0},
300 .mode2 = {-147, +112},
301 .mode3 = {0, +28}
302 },
303 {
304 /* Reflector */
305 .mode0 = {+11, 0},
306 .mode1 = {+11, 0},
307 .mode2 = {-66, -96},
308 .mode3 = {-50, -54}
309 }
310 },
311
312 .tAnt = {
313 {
314 /* Initiator */
315 .mode2 = {0, 0},
316 .mode3 = {0, 0}
317 },
318 {
319 /* Reflector */
320 .mode2 = {0, 0},
321 .mode3 = {0, 0}
322 }
323 }
324 },
325 #else
326 { /* Phy_1M @ 4MHZ*/
327 .baud = 0x4000,
328 .symmap = 0x001F,
329 .demmisc2 = 0x05F8,
330 .demmisc3 = 0x1082,
331 .rssioffset = 0x0049,
332
333 /* Theory */
334 .tPacket = 2112, // 8+32+4 bits = 44us
335 .tTr = 192, // 4us
336
337 /* Tuned */
338 .tStartup = 93, // 1.937us (pbe_timer_event -> mce_timer start)
339 .tTxModDelay = 245, // 5.109us (wait -> signal on shaper)
340 .tPilotAdjust = 259, // 5.39us
341 .tPostProcessDiv1 = 936, // 19.5us
342 .tPostProcessDiv12 = 78, // 19.5us
343 .tInfiniteRx = 60, // 1.25us
344
345 .tFidc = 264, // 5.5us
346 .tCorr = 377, // 7.85us
347 .tDem = 914, // 19.05us
348
349 .tRxTimeoutI0 = RCL_BLE_CS_US_TO_MCE_TIMER(70),
350 .tRxTimeoutI3 = RCL_BLE_CS_US_TO_MCE_TIMER(56),
351 .tRxTimeoutRn = RCL_BLE_CS_US_TO_MCE_TIMER(76),
352
353 /* Sum of demodulation and modulation delay (HW only) */
354 .tTimestampDelay1 = 603,
355 .tTimestampDelay3 = 601,
356
357 .pctConfig = {
358 /*TPM = 10US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL32 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 1, .fracWait = 32, .fracWaitAdj = 0},
359 /*TPM = 20US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL32 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 2, .fracWait = 32, .fracWaitAdj = 0},
360 /*TPM = 40US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL128 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 1, .fracWait = 128, .fracWaitAdj = 0}
361 },
362
363 .magnConfig = {
364 /*TPM = 10US*/{.magnIteration = 3},
365 /*TPM = 20US*/{.magnIteration = 6},
366 /*TPM = 40US*/{.magnIteration = 14}
367 },
368
369 .tAdj = { /* { pkt-tn, tn-tn} */
370 {
371 /* Initiator */
372 .mode0 = {0, 0},
373 .mode1 = {0, 0},
374 .mode2 = {-147, +64},
375 .mode3 = {0, -22}
376 },
377 {
378 /* Reflector */
379 .mode0 = {+27, 0},
380 .mode1 = {+27, 0},
381 .mode2 = {-94, -48},
382 .mode3 = {-96, +16}
383 }
384 },
385
386 .tAnt = {
387 {
388 /* Initiator */
389 .mode2 = {+60, 0},
390 .mode3 = {+66, 0}
391 },
392 {
393 /* Reflector */
394 .mode2 = {+105, 0},
395 .mode3 = {+105, 0}
396 }
397 }
398 },
399 #endif
400 { /* Phy_2M */
401 .baud = 0x8000,
402 .symmap = 0x002E,
403 .demmisc2 = 0x04F8,
404 .demmisc3 = 0x1081,
405 .rssioffset = 0x0049,
406
407 /* Theory */
408 .tPacket = 1248, // 16+32+4bit = 26us
409 .tTr = 96, // 2us
410
411 /* Tuned */
412 .tStartup = 95, // 1.979us (pbe_timer_event -> mce_timer start)
413 .tTxModDelay = 142, // 2.957us (wait -> signal on shaper)
414 .tPilotAdjust = 156, // 4.02us
415 .tPostProcessDiv1 = 720, // 15us
416 .tPostProcessDiv12 = 60, // 15us
417 .tInfiniteRx = 60, // 1.25us
418
419 .tFidc = 146, // 3.05us
420 .tCorr = 204, // 4.25us
421 .tDem = 565, // 11.77us
422
423 .tRxTimeoutI0 = 2448+24, // 51us MCE
424 .tRxTimeoutI3 = 1776, // 37us MCE
425 .tRxTimeoutRn = 3096, // 64.5us MCE
426
427 /* Sum of demodulation and modulation delay (HW only) */
428 .tTimestampDelay1 = 333,
429 .tTimestampDelay3 = 335,
430
431 .pctConfig = {
432 /*TPM = 10US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL32 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 2, .fracWait = 32, .fracWaitAdj = 9},
433 /*TPM = 20US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL128 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 1, .fracWait = 128, .fracWaitAdj = 0},
434 /*TPM = 40US*/{.period = (LRFDMDM_DEMFIDC0_ACCPERIOD_SMPL128 >> LRFDMDM_DEMFIDC0_ACCPERIOD_S), .numIteration = 2, .fracWait = 128, .fracWaitAdj = 0}
435 },
436
437 .magnConfig = {
438 /*TPM = 10US*/{.magnIteration = 5},
439 /*TPM = 20US*/{.magnIteration = 14},
440 /*TPM = 40US*/{.magnIteration = 30}
441 },
442
443 .tAdj = {
444 {
445 /* Initiator */
446 .mode0 = {0, 0},
447 .mode1 = {0, 0},
448 .mode2 = {-48, +107},
449 .mode3 = {0, 0}
450 },
451 {
452 /* Reflector */
453 .mode0 = {+32, 0},
454 .mode1 = {+32, 0},
455 .mode2 = {-38, -93},
456 .mode3 = {-60, -20}
457 }
458 },
459
460 .tAnt = {
461 {
462 /* Initiator */
463 .mode2 = {+63, 0},
464 .mode3 = {+63, 0}
465 },
466 {
467 /* Reflector */
468 .mode2 = {+129, 0},
469 .mode3 = {+129, 0}
470 }
471 }
472 }
473 };
474
475 /* Frequency compensation value due to the fixed CMIXN value use. Resolution is 4x FOFF unit.
476 Calculated based on the formula:
477 foffError = 4x ((((1<<21) * fif) / (BLE_CS_BASE_FREQ_MHZ + pStepI->channelIdx) / 1e3)
478 - ((1<<21) * (1024 - pStepI->cmixn) / 288 / 1024)); */
479 const int8_t foffErrorLut[BLE_CS_NUM_CHANNELS] = {
480 /*2402 MHz =*/ 38, 34, 34, 34, 30, 30, 30, 26,
481 /*2410 MHz =*/ 26, 26, 26, 22, 22, 22, 18, 18, 18, 14,
482 /*2420 MHz =*/ 14, 14, 10, 10, 10, 10, 6, 6, 6, 2,
483 /*2430 MHz =*/ 2, 2, -2, -2, -2, -2, -6, -6, -6,-10,
484 /*2440 MHz =*/ -10,-10,-14,-14,-14,-14,-18,-18,-18,-22,
485 /*2450 MHz =*/ -22,-22,-26,-26,-26,-26,-30,-30,-30,-34,
486 /*2460 MHz =*/ -34,-34,-34,-38,-38,-38,-42,-42,-42,-46,
487 /*2470 MHz =*/ -46,-46,-46,-50,-50,-50,-54,-54,-54,-54,
488 /*2480 MHz =*/ -58,-58,-58,-62,-62,-62,-62,-66,-66,-66
489 };
490
491 /* Look up table of payload length in units of MCE Timer at rate of [1M]. Scaling dynamically if needed. */
492 const uint16_t payloadLut[RCL_CmdBleCs_Payload_Length] = {
493 /*0 bits =*/ RCL_BLE_CS_US_TO_MCE_TIMER(0),
494 /*32 bits =*/ RCL_BLE_CS_US_TO_MCE_TIMER(32),
495 /*64 bits =*/ RCL_BLE_CS_US_TO_MCE_TIMER(64),
496 /*96 bits =*/ RCL_BLE_CS_US_TO_MCE_TIMER(96),
497 /*128 bits =*/ RCL_BLE_CS_US_TO_MCE_TIMER(128)
498 };
499
500 /* Forward declarations */
501 static RCL_MultiBuffer* RCL_Handler_BLE_CS_findBufferFitNumberOfBytes(List_List *pHead, uint16_t numBytes);
502 static void RCL_Handler_BLE_CS_configureTxRxFifo(RCL_CmdBleCs* pCmd);
503 static void RCL_Handler_BLE_CS_retrieveAndStoreNextResult(RCL_CmdBleCs* pCmd, bool forceBufferToFinishState);
504 static void RCL_Handler_BLE_CS_fetchAndforwardNextStep(RCL_CmdBleCs* pCmd);
505 static RCL_Events RCL_Handler_BLE_CS_preFillTxBuffer(RCL_CmdBleCs* pCmd);
506 static RCL_Events RCL_Handler_BLE_CS_readWriteBuffer(RCL_CmdBleCs* pCmd, bool forceBufferToFinishState);
507 static bool RCL_Handler_BLE_CS_transferFinishedBuffers(List_List *pSrc, List_List *pDst);
508 static void RCL_Handler_BLE_CS_configureS2R(RCL_CmdBleCs *pCmd);
509 static RCL_Events RCL_Handler_BLE_CS_readS2RSamples(RCL_CmdBleCs *pCmd);
510 static void RCL_Handler_BLE_CS_readStatistics(RCL_CmdBleCs *pCmd);
511 static void RCL_Handler_BLE_CS_preprocessCommand(RCL_CmdBleCs *pCmd);
512 static void RCL_Handler_BLE_CS_preprocessStep(RCL_CmdBleCs *pCmd, RCL_CmdBleCs_Step *pStep, RCL_CmdBleCs_Step_Internal *pStepI);
513 static RCL_CmdBleCs_StepResult_Internal* RCL_Handler_BLE_CS_fetchNextStepResult(RCL_CmdBleCs *pCmd);
514 static int16_t RCL_Handler_BLE_CS_convertFreqOffset(int16_t foffMeasured);
515 static int16_t RCL_Handler_BLE_CS_convertRtt(RCL_CmdBleCs *pCmd, uint8_t mode, int8_t channel, int16_t foff, uint8_t payload, bool secondToneExtensionSlot, float toAD, uint16_t corrBefore, uint16_t corrPeak, uint16_t corrAfter);
516 static uint32_t RCL_Handler_BLE_CS_convertPct(int16_t pct_i, int16_t pct_q, uint8_t channelIdx);
517 static uint8_t RCL_Handler_BLE_CS_convertPctQuality(uint16_t qMin, uint16_t qMax, uint16_t qAvg, bool toneExtensionSlot, bool toneExpected);
518 static uint16_t RCL_Handler_BLE_CS_estimateStepResultLength(RCL_CmdBleCs *pCmd,RCL_CmdBleCs_StepResult_Internal* src);
519 static uint16_t RCL_Handler_BLE_CS_convertStepResult(RCL_CmdBleCs* pCmd, uint8_t *dst, RCL_CmdBleCs_StepResult_Internal* src);
520 static RCL_CommandStatus RCL_Handler_BLE_CS_findPbeErrorEndStatus(uint16_t pbeEndStatus);
521
522 /*
523 * ======== Traverse multibuffer list looking for space ========
524 */
RCL_Handler_BLE_CS_findBufferFitNumberOfBytes(List_List * pHead,uint16_t numBytes)525 static RCL_MultiBuffer* RCL_Handler_BLE_CS_findBufferFitNumberOfBytes(List_List *pHead, uint16_t numBytes)
526 {
527 /* Start search from the head of list */
528 RCL_MultiBuffer *pBuffer = RCL_MultiBuffer_head(pHead);
529
530 /* Traverse the chain of buffers and find the first free one */
531 do {
532 pBuffer = RCL_MultiBuffer_findFirstWritableBuffer(pBuffer);
533
534 /* Avoid NULL pointer dereferencing */
535 if (pBuffer)
536 {
537 /* Check for available space */
538 if (pBuffer->length - pBuffer->tailIndex >= numBytes)
539 {
540 /* Mark the buffer to be written */
541 pBuffer->state = RCL_BufferStateInUse;
542
543 break;
544 }
545 else
546 {
547 /* The buffer will not fit the incoming number of bytes */
548 pBuffer->state = RCL_BufferStateFinished;
549 }
550 }
551 } while (pBuffer);
552
553 return (pBuffer);
554 }
555
556 /*
557 * ======== Transfer all buffers marked as finished from one list to another ========
558 */
RCL_Handler_BLE_CS_transferFinishedBuffers(List_List * pSrc,List_List * pDst)559 static bool RCL_Handler_BLE_CS_transferFinishedBuffers(List_List *pSrc, List_List *pDst)
560 {
561 /* Initialize return value */
562 bool callbackEvent = false;
563 RCL_MultiBuffer *pBuffer = NULL;
564
565 /* Transfer the used buffers */
566 do
567 {
568 pBuffer = RCL_MultiBuffer_head(pSrc);
569
570 if (pBuffer && (pBuffer->state == RCL_BufferStateFinished))
571 {
572 /* Pop from list and add to done list */
573 RCL_MultiBuffer *pBuff = RCL_MultiBuffer_get(pSrc);
574 RCL_MultiBuffer_put(pDst, pBuff);
575
576 /* Trigger callback side effect*/
577 callbackEvent = true;
578 }
579 else
580 {
581 break;
582 }
583 } while (pBuffer);
584
585 return (callbackEvent);
586 }
587
588 /*
589 * ======== Configure the BUFRAM size and interrupts for BLE CS ========
590 */
RCL_Handler_BLE_CS_configureTxRxFifo(RCL_CmdBleCs * pCmd)591 static void RCL_Handler_BLE_CS_configureTxRxFifo(RCL_CmdBleCs* pCmd)
592 {
593 /* Override the FIFO location and size */
594 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_FCFG1) = BLE_CS_TXFIFO_BASE_ADDR;
595 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_FCFG2) = BLE_CS_TXFIFO_LEN;
596 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_FCFG3) = BLE_CS_RXFIFO_BASE_ADDR;
597 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_FCFG4) = BLE_CS_RXFIFO_LEN;
598
599 /* Reset FIFOs to have a clean start */
600 /* Writing to FCMD is safe since the PBE is not running, ref. RCL-367 */
601 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_FCMD) = LRFDPBE_FCMD_DATA_FIFO_RESET;
602
603 /* Calculate how many entries can be stored simultaneous in given size of tx/rx fifos */
604 uint16_t nStepsFit = HWREG_READ_LRF(LRFDPBE_BASE + LRFDPBE_O_TXFWRITABLE) / sizeof(RCL_CmdBleCs_Step_Internal);
605 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_TXFWBTHRS) = (nStepsFit - 1) * sizeof(RCL_CmdBleCs_Step_Internal);
606
607 uint16_t nResultsFit = HWREG_READ_LRF(LRFDPBE_BASE + LRFDPBE_O_RXFWRITABLE) / sizeof(RCL_CmdBleCs_StepResult_Internal);
608 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_RXFRBTHRS) = (nResultsFit - 1) * sizeof(RCL_CmdBleCs_StepResult_Internal);
609
610 #ifndef PG2
611 /* CDDS BUG00003 - Temporarily workaround */
612 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_RXFRBTHRS) = sizeof(RCL_CmdBleCs_StepResult_Internal);
613 #endif
614
615 /* FIFO pointers should auto-commit/auto-dealloc, enable threshold events */
616 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_FCFG0) = HWREG_READ_LRF(LRFDPBE_BASE + LRFDPBE_O_FCFG0) | (LRFDPBE_FCFG0_TXACOM_M | LRFDPBE_FCFG0_TXADEAL_M | LRFDPBE_FCFG0_RXACOM_M | LRFDPBE_FCFG0_RXADEAL_M);
617 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_FCFG0) = HWREG_READ_LRF(LRFDPBE_BASE + LRFDPBE_O_FCFG0) & (~(LRFDPBE_FCFG0_RXIRQMET_M | LRFDPBE_FCFG0_TXIRQMET_M));
618
619 /* Clear any interrupts left from uninitialized fifos */
620 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_EVTCLR1) = (LRFDPBE_EVTCLR1_RXRDBTHR_M | LRFDPBE_EVTCLR1_RXWRBTHR_M | LRFDPBE_EVTCLR1_TXRDBTHR_M | LRFDPBE_EVTCLR1_TXWRBTHR_M);
621 HWREG_WRITE_LRF(LRFDDBELL_BASE + LRFDDBELL_O_ICLR0) = HWREG_READ_LRF(LRFDDBELL_BASE + LRFDDBELL_O_ICLR0) | (LRFDDBELL_ICLR0_RXFIFO_M | LRFDDBELL_ICLR0_TXFIFO_M);
622 }
623
624 /*
625 * ======== Fill the BLE CS step descriptors into the BUFRAM TX FIFO ========
626 */
RCL_Handler_BLE_CS_preFillTxBuffer(RCL_CmdBleCs * pCmd)627 static RCL_Events RCL_Handler_BLE_CS_preFillTxBuffer(RCL_CmdBleCs* pCmd)
628 {
629 /* Initialize return value */
630 RCL_Events events = {.value = 0};
631
632 /* Calculate how many additional step will fit (which might be all) */
633 uint16_t nWritable = HWREG_READ_LRF(LRFDPBE_BASE + LRFDPBE_O_TXFWRITABLE) / sizeof(RCL_CmdBleCs_Step_Internal);
634 uint16_t nRemaining = (pCmd->mode.repeatSteps) ? (pCmd->mode.nSteps) : (pCmd->mode.nSteps - pCmd->stats->nStepsWritten);
635 uint16_t nBatch = (nWritable < nRemaining) ? nWritable : nRemaining;
636
637 /* Limited time is available when we start the command (ABS_START_TIME_OFFSET),
638 so the first batch is reduced */
639 if (pCmd->stats->nStepsWritten == 0)
640 {
641 nBatch = (nBatch < BLE_CS_FIRST_BATCH_SIZE) ? nBatch : BLE_CS_FIRST_BATCH_SIZE;
642 }
643
644 /* Transfer the calculated number of steps into the fifo */
645 for (uint16_t i=0; i<nBatch; i++)
646 {
647 RCL_Handler_BLE_CS_fetchAndforwardNextStep(pCmd);
648 }
649
650 /* Transfer the used buffers, should trigger a callback if any */
651 events.txBufferFinished = (uint32_t) RCL_Handler_BLE_CS_transferFinishedBuffers(&pCmd->stepBuffers, &pCmd->stepBuffersDone);
652
653 return (events);
654 }
655
656 /*
657 * ======== Write the BLE CS Steps to the BUFRAM TX fifo ========
658 */
RCL_Handler_BLE_CS_fetchAndforwardNextStep(RCL_CmdBleCs * pCmd)659 static void RCL_Handler_BLE_CS_fetchAndforwardNextStep(RCL_CmdBleCs* pCmd)
660 {
661 /* Default return value */
662 RCL_CmdBleCs_Step *pStep = NULL;
663
664 /* Buffers are served on a FIFO basis */
665 RCL_MultiBuffer *pStepBuffer = RCL_MultiBuffer_head(&pCmd->stepBuffers);
666
667 /* Handle buffer boundaries within a batch */
668 pStepBuffer = RCL_MultiBuffer_findFirstWritableBuffer(pStepBuffer);
669 if (pStepBuffer)
670 {
671 /* Flag the buffer */
672 pStepBuffer->state = RCL_BufferStateInUse;
673
674 /* Point to the beginning of step list */
675 pStep = (RCL_CmdBleCs_Step *) &pStepBuffer->data[0];
676
677 /* Verify that there is enough bytes in the buffer */
678 if ((pStepBuffer->headIndex + sizeof(RCL_CmdBleCs_Step)) <= pStepBuffer->tailIndex)
679 {
680 /* Point to the next entry */
681 pStep += pStepBuffer->headIndex/sizeof(RCL_CmdBleCs_Step);
682
683 /* Decompress and calculate internal parameters on step */
684 RCL_Handler_BLE_CS_preprocessStep(pCmd, pStep, &step);
685
686 /* Send it to PBE */
687 uint32_t *ptr = (uint32_t *) &step;
688 for(uint8_t j=0; j<sizeof(RCL_CmdBleCs_Step_Internal)/sizeof(uint32_t); j++)
689 {
690 HWREG_WRITE_LRF(LRFDTXF_BASE + LRFDTXF_O_TXD) = *(ptr + j);
691 }
692
693 /* The step have been consumed */
694 pCmd->stats->nStepsWritten++;
695 pStepBuffer->headIndex += sizeof(RCL_CmdBleCs_Step);
696
697 /* This buffer is exhausted, or end of subevent */
698 if (pStepBuffer->headIndex == pStepBuffer->tailIndex)
699 {
700 if (pCmd->mode.repeatSteps == false)
701 {
702 /* Update state and move the buffer */
703 pStepBuffer->state = RCL_BufferStateFinished;
704 }
705 else
706 {
707 /* Pretend that this is the beginning of the sequence */
708 pStepBuffer->headIndex = 0;
709 pCmd->stats->nStepsWritten = 0;
710 }
711 }
712 }
713 }
714 }
715
716 /*
717 * ======== Read the BLE CS Step results from the BUFRAM RX fifo ========
718 */
RCL_Handler_BLE_CS_retrieveAndStoreNextResult(RCL_CmdBleCs * pCmd,bool forceBufferToFinishState)719 static void RCL_Handler_BLE_CS_retrieveAndStoreNextResult(RCL_CmdBleCs* pCmd, bool forceBufferToFinishState)
720 {
721 uint32_t *ptr = (uint32_t *) &result;
722 for(uint8_t j=0; j<sizeof(RCL_CmdBleCs_StepResult_Internal)/sizeof(uint32_t); j++)
723 {
724 *(ptr+j) = HWREG_READ_LRF(LRFDRXF_BASE + LRFDRXF_O_RXD);
725 }
726
727 if (result.pktResult == RCL_CmdBleCs_PacketResult_Ok)
728 {
729 pCmd->stats->nRxOk += 1;
730 }
731 else
732 {
733 pCmd->stats->nRxNok += 1;
734 }
735
736 /* Pop and fill the next container */
737 RCL_CmdBleCs_StepResult_Internal *pResult = RCL_Handler_BLE_CS_fetchNextStepResult(pCmd);
738 if (pResult)
739 {
740 memcpy(pResult, (RCL_CmdBleCs_StepResult_Internal *) &result, sizeof(RCL_CmdBleCs_StepResult_Internal));
741 }
742
743 /* Identify HCI compression size */
744 uint16_t requiredSpaceInBytes = RCL_Handler_BLE_CS_estimateStepResultLength(pCmd, (RCL_CmdBleCs_StepResult_Internal *) &result);
745
746 /* Alternative HCI destination */
747 RCL_MultiBuffer *pResultBuffer = RCL_Handler_BLE_CS_findBufferFitNumberOfBytes(&pCmd->resultBuffers, requiredSpaceInBytes);
748
749 /* Increment if either internal or HCI format is used */
750 if (pResult || pResultBuffer)
751 {
752 pCmd->stats->nResultsRead++;
753 }
754
755 /* Avoid NULL pointer dereferencing */
756 if (pResultBuffer)
757 {
758 /* Decode the start of the segment */
759 RCL_CmdBleCs_SubeventResults *pSubeventResults = (RCL_CmdBleCs_SubeventResults *) pResultBuffer->data;
760
761 /* Initialize header in an empty buffer */
762 if (pResultBuffer->tailIndex == 0)
763 {
764 /* No steps reported yet */
765 pSubeventResults->numStepsReported = 0;
766
767 /* Increment tail with header length */
768 RCL_MultiBuffer_commitBytes(pResultBuffer, sizeof(RCL_CmdBleCs_SubeventResults));
769 }
770
771 /* Update the HCI header */
772 pSubeventResults->numStepsReported += 1;
773 pSubeventResults->numAntennaPath = pCmd->stats->numAntennaPath + 1;
774 pSubeventResults->referencePowerLevel = result.gain;
775 pSubeventResults->frequencyCompensation = RCL_Handler_BLE_CS_convertFreqOffset(HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_FOFFCOMP) << 2);
776
777 if (pCmd->stats->nResultsRead == 1)
778 {
779 pCmd->stats->reserved0 = result.gain;
780 }
781
782 /* Decode the offset of data field */
783 uint8_t *pResult = RCL_MultiBuffer_getNextWritableByte(pResultBuffer);
784
785 /* Compress and write the data */
786 uint32_t nBytes = RCL_Handler_BLE_CS_convertStepResult(pCmd, pResult, (RCL_CmdBleCs_StepResult_Internal *) &result);
787
788 /* Commit the pointers in the buffer */
789 RCL_MultiBuffer_commitBytes(pResultBuffer, nBytes);
790
791 /* Force close the multibuffer, even if there is more space left */
792 if ( (pCmd->mode.repeatSteps == false) && (pCmd->mode.nSteps == pCmd->stats->nResultsRead) )
793 {
794 pResultBuffer->state = RCL_BufferStateFinished;
795 }
796 else if (forceBufferToFinishState)
797 {
798 pResultBuffer->state = RCL_BufferStateFinished;
799 }
800 }
801 }
802
803 /*
804 * ======== Read the BLE CS Step results from the BUFRAM RX fifo ========
805 */
RCL_Handler_BLE_CS_readWriteBuffer(RCL_CmdBleCs * pCmd,bool forceBufferToFinishState)806 static RCL_Events RCL_Handler_BLE_CS_readWriteBuffer(RCL_CmdBleCs* pCmd, bool forceBufferToFinishState)
807 {
808 /* Initialize return value */
809 RCL_Events events = {.value = 0};
810
811 /* Calculate how many result entries are available in rx fifo (might be all that has left) */
812 uint16_t nBatch = HWREG_READ_LRF(LRFDPBE_BASE + LRFDPBE_O_RXFREADABLE) / sizeof(RCL_CmdBleCs_StepResult_Internal);
813
814 /* Read out the calculated number of entries */
815 for (uint16_t i=0; i<nBatch; i++)
816 {
817 /* Retrieve all that's available and terminate*/
818 bool forceTerminate = (forceBufferToFinishState && (i == nBatch-1));
819
820 RCL_Handler_BLE_CS_retrieveAndStoreNextResult(pCmd, forceTerminate);
821 RCL_Handler_BLE_CS_fetchAndforwardNextStep(pCmd);
822 }
823
824 /* Transfer the used buffers, should trigger a callback if any */
825 events.rxBufferFinished = (uint32_t) RCL_Handler_BLE_CS_transferFinishedBuffers(&pCmd->resultBuffers, &pCmd->resultBuffersDone);
826 events.txBufferFinished = (uint32_t) RCL_Handler_BLE_CS_transferFinishedBuffers(&pCmd->stepBuffers, &pCmd->stepBuffersDone);
827
828 /* Reduce the batch size if there is only a few entries missing */
829 int16_t nRemaining = pCmd->mode.nSteps - pCmd->stats->nResultsRead;
830 if (nRemaining <= 0)
831 {
832 if (pCmd->mode.repeatSteps == false)
833 {
834 /* No more results expected, set the threshold to the max */
835 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_RXFRBTHRS) = 0x3FF;
836 }
837 }
838 else if (nRemaining < nBatch)
839 {
840 /* Reduce the Fifo threshold to number of remaining steps */
841 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_RXFRBTHRS) = (nRemaining * sizeof(RCL_CmdBleCs_StepResult_Internal));
842 }
843
844 return (events);
845 }
846
847
848 /*
849 * ======== Initialize the S2R engine ========
850 */
RCL_Handler_BLE_CS_configureS2R(RCL_CmdBleCs * pCmd)851 static void RCL_Handler_BLE_CS_configureS2R(RCL_CmdBleCs *pCmd)
852 {
853 LRF_setRclClockEnable(LRFDDBELL_CLKCTL_S2RRAM_M | LRFDDBELL_CLKCTL_S2R_M);
854
855 /* Access the list of multibuffers */
856 RCL_MultiBuffer *pS2rBuffer = RCL_MultiBuffer_head(&pCmd->s2rBuffers);
857
858 /* Only set up S2R, if there is buffer on the list */
859 if (pS2rBuffer)
860 {
861 /* Store 32bit words in S2R (offset 3072) memory; don't arm yet */
862 HWREG_WRITE_LRF(LRFDS2R_BASE + LRFDS2R_O_START) = BLE_CS_S2R_MEMORY_OFFSET;
863 HWREG_WRITE_LRF(LRFDS2R_BASE + LRFDS2R_O_STOP) = HWREG_READ_LRF(LRFDS2R_BASE + LRFDS2R_O_START) + (RCL_BLE_CS_MAX_S2R_LEN >> pCmd->mode.phy);
864 HWREG_WRITE_LRF(LRFDS2R_BASE + LRFDS2R_O_CFG) = LRFDS2R_CFG_CTL_EN
865 | LRFDS2R_CFG_SEL_FRONTEND
866 | LRFDS2R_CFG_TRIGMODE_ONESHOT
867 | LRFDS2R_CFG_LAST0_DIS;
868 }
869 }
870
871 /*
872 * ======== Read the S2R memory ========
873 */
RCL_Handler_BLE_CS_readS2RSamples(RCL_CmdBleCs * pCmd)874 RCL_Events RCL_Handler_BLE_CS_readS2RSamples(RCL_CmdBleCs *pCmd)
875 {
876 RCL_Events events = {.value = 0};
877
878 uint16_t s2rLength = sizeof(S2RSample) * HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTWORDSIZE);
879 uint16_t dataLength = sizeof(RCL_CmdBleCs_S2r) + s2rLength;
880
881 /* Access the list of multibuffers */
882 RCL_MultiBuffer *pS2rBuffer = RCL_Handler_BLE_CS_findBufferFitNumberOfBytes(&pCmd->s2rBuffers, dataLength);
883
884 if (pS2rBuffer)
885 {
886 /* Decode the next free slot in the multibuffer */
887 RCL_CmdBleCs_S2r *pS2rData = (RCL_CmdBleCs_S2r *) RCL_MultiBuffer_getNextWritableByte(pS2rBuffer);
888
889 /* Store the step index */
890 pS2rData->phy = pCmd->mode.phy;
891 pS2rData->idx = HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTIDX);
892 pS2rData->channel = HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTCHIDX);
893 pS2rData->payloadLen = HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTPAYLOADLEN);
894 /* 32 bit access */
895 pS2rData->payload0 = HWREG_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTPAYLOAD0L);
896 pS2rData->payload1 = HWREG_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTPAYLOAD1L);
897 pS2rData->payload2 = HWREG_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTPAYLOAD2L);
898 pS2rData->payload3 = HWREG_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTPAYLOAD3L);
899
900 /* Number of valid samples to be copied out with 32bit operation
901 Need to calculate with 0/32/64/96/128 bit @ 8MHz/4MHz front-end sampling */
902 pS2rData->iqLength = HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_S2ROUTWORDSIZE);
903
904 /* Store the data, unwrap the loop for faster copying */
905 uint32_t *src = (uint32_t *)S2R_RAM_BASE_ADDR;
906 uint32_t *dst = (uint32_t *)&pS2rData->data;
907
908 #ifdef NO_INLINE_ASM
909 for(uint16_t i=0; i<(pS2rData->iqLength >> 3); i++)
910 {
911 *dst++ = *src++;
912 *dst++ = *src++;
913 *dst++ = *src++;
914 *dst++ = *src++;
915 *dst++ = *src++;
916 *dst++ = *src++;
917 *dst++ = *src++;
918 *dst++ = *src++;
919 }
920 #else
921 for(uint16_t i=0; i<(pS2rData->iqLength >> 2); i++)
922 {
923 __asm(
924 "LDMIA %0!, {r4, r5, r6, r7} \n"
925 "STMIA %1!, {r4, r5, r6, r7} \n"
926 : "+r" (src), "+r" (dst) :
927 : "r4", "r5", "r6", "r7");
928 }
929 #endif
930
931 /* Flag the entry */
932 RCL_MultiBuffer_commitBytes(pS2rBuffer, dataLength);
933 pCmd->stats->nS2RDone++;
934
935 /* Force close the buffer, even if there is more space left */
936 pS2rBuffer->state = RCL_BufferStateFinished;
937
938 /* Transfer the used buffers, should trigger a callback if any */
939 events.swTriggered = (uint32_t) RCL_Handler_BLE_CS_transferFinishedBuffers(&pCmd->s2rBuffers, &pCmd->s2rBuffersDone);
940 }
941
942 return (events);
943 }
944
945 /*
946 * ======== Read the statistics from the PBE ========
947 */
RCL_Handler_BLE_CS_readStatistics(RCL_CmdBleCs * pCmd)948 static void RCL_Handler_BLE_CS_readStatistics(RCL_CmdBleCs *pCmd)
949 {
950 if (pCmd->stats)
951 {
952 pCmd->stats->nStepsDone = HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_NSTEPSDONE);
953 pCmd->stats->lastRssi = HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_RSSILAST);
954 pCmd->stats->lastFoff = HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_FOFFLAST);
955 pCmd->stats->foffComp = HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_FOFFCOMP);
956 }
957 }
958
959 /*
960 * ======== Load the command and step descriptors into LRF REGISTERS/BUFRAM ========
961 */
RCL_Handler_BLE_CS_preprocessCommand(RCL_CmdBleCs * pCmd)962 static void RCL_Handler_BLE_CS_preprocessCommand(RCL_CmdBleCs *pCmd)
963 {
964 /* Force antenna switching time to zero for single antenna path per spec */
965 if (pCmd->antennaConfig.select == 0)
966 {
967 pCmd->timing.tSw = 0;
968 }
969
970 /* Decode mapping of configuration to antennas */
971 const RCL_AntennaConfig_t *antennaEntry = &antennaConfig[pCmd->antennaConfig.select];
972
973 /* Decode the Phy specific settings */
974 const RCL_PhyConfig_t *config = &phyConfigLut[pCmd->mode.phy];
975
976 /* Clear output statistics */
977 if (pCmd->stats)
978 {
979 memset((void *)pCmd->stats, 0, sizeof(RCL_CmdBleCs_Stats));
980
981 pCmd->stats->lastRssi = LRF_RSSI_INVALID;
982 pCmd->stats->numAntennaPath = antennaEntry->numPath;
983 }
984
985 /* Mode */
986 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_MODE) = ((pCmd->mode.role << PBE_BLE_CS_RAM_MODE_ROLE_S) & PBE_BLE_CS_RAM_MODE_ROLE_M) |
987 ((pCmd->mode.phy << PBE_BLE_CS_RAM_MODE_PHY_S) & PBE_BLE_CS_RAM_MODE_PHY_M) |
988 ((pCmd->mode.repeatSteps << PBE_BLE_CS_RAM_MODE_INFINIT_S) & PBE_BLE_CS_RAM_MODE_INFINIT_M) |
989 ((pCmd->mode.nSteps << PBE_BLE_CS_RAM_MODE_NUM_STEPS_S) & PBE_BLE_CS_RAM_MODE_NUM_STEPS_M);
990
991 /* Antenna */
992 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_ANTN) = antennaEntry->numPath;
993 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_ANTMSK) = pCmd->antennaConfig.gpoMask;
994 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_ANT0) = pCmd->antennaConfig.gpoVal[0];
995 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_ANT1) = pCmd->antennaConfig.gpoVal[1];
996 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_ANT2) = pCmd->antennaConfig.gpoVal[2];
997 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_ANT3) = pCmd->antennaConfig.gpoVal[3];
998
999 /* Timing */
1000 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TFCS) = pCmd->timing.tFcs - (TPLT + config->tStartup); // Pilot tone + startup time is included into the TFCS budget
1001 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TFM) = pCmd->timing.tFm;
1002 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TPM) = pCmd->timing.tPm;
1003 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TIP1) = pCmd->timing.tIp1;
1004 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TIP2) = pCmd->timing.tIp2;
1005 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSW) = pCmd->timing.tSw;
1006 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSWADJA) = pCmd->timing.tSwAdjustA;
1007 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSWADJB) = pCmd->timing.tSwAdjustB;
1008
1009 /* Timegrid adjustment shall be initialized with maximum unsigned value (ca. 536s @ 4MHz) */
1010 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSTEPACCTHRH)= 0x7FFF;
1011 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSTEPACCTHRL)= 0xFFFF;
1012 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSTEPACCH) = 0;
1013 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSTEPACCL) = 0;
1014 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSTEPCOMP) = 0;
1015
1016 /* Initialize MOD.FOFF values. Usueful for sub-events without mode 0 steps */
1017 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_FOFFSUM) = 0;
1018 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_FOFFNUM) = 0;
1019 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_RSSILAST) = LRF_RSSI_INVALID;
1020 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_RSSISUM0) = 0;
1021 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_RSSINUM0) = 0;
1022 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_NSTEPSDONE) = 0;
1023
1024 /* Apply override if configured */
1025 if (pCmd->frontend.foffOverrideEnable)
1026 {
1027 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_FOFFSUM) = pCmd->frontend.foffOverride;
1028 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_FOFFNUM) = (1 << 15) | 1; // [15]=Enable, [1]=NUM hardcoded to 1
1029 }
1030
1031 /* Program frequency dependent config: hardcode on 2440MHz, to be updated on the fly */
1032 HWREG_WRITE_LRF(LRFDMDM32_BASE + LRFDMDM32_O_DEMFRAC1_DEMFRAC0) = 0x0D800000; // P: Constant
1033 HWREG_WRITE_LRF(LRFDMDM32_BASE + LRFDMDM32_O_DEMFRAC3_DEMFRAC2) = 0x0E4C0000; // Q: Being dynammically scaled by RFE per channel
1034
1035 /* Shaper gain */
1036 HWREG_WRITE_LRF(LRFDRFE_BASE + LRFDRFE_O_MOD0) = 0x1824;
1037
1038 /* Configure AGC with gain=15/7 or a static value=gain/gain */
1039 uint16_t rxGain = pCmd->frontend.rxGain;
1040 HWREG_WRITE_LRF(LRFDRFE_BASE + LRFDRFE_O_SPARE0) = (rxGain == 0)
1041 ? (0x30F7)
1042 : ((rxGain << 4) | rxGain);
1043
1044 uint8_t lutIdx = RCL_CmdBleCs_Tpm_40us;
1045 if (pCmd->timing.tPm <= RCL_BLE_CS_US_TO_MCE_TIMER(20))
1046 {
1047 lutIdx = RCL_CmdBleCs_Tpm_20us;
1048 }
1049 if (pCmd->timing.tPm <= RCL_BLE_CS_US_TO_MCE_TIMER(10))
1050 {
1051 lutIdx = RCL_CmdBleCs_Tpm_10us;
1052 }
1053
1054 /* MAFC gain */
1055 #ifdef DeviceFamily_CC27XX
1056 /* Workaround for IQ swap */
1057 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_DEMMISC2) = config->demmisc2 | LRFDMDM_DEMMISC2_PDIFIQCONJEN;
1058 #else
1059 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_DEMMISC2) = config->demmisc2;
1060 #endif
1061
1062 /* Channel filter = 0.33 */
1063 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_DEMMISC1) = 0x0001;
1064
1065 /* Fife IIR adaptation rate = 64 (provides good mode-0 frequency tolerance for both PHYs) */
1066 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_DEMFIFE0) = 0x0007;
1067
1068 /* Use precalibration values or mode-0 estimates */
1069 if ((pCmd->precalTable) && (pCmd->precalTable->valid))
1070 {
1071 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_SPARE1) = (1<<8);
1072 }
1073 else
1074 {
1075 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_SPARE1) = 0;
1076 }
1077
1078 /* Baudrate specific settings */
1079 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_SPARE0) = config->pctConfig[lutIdx].val;
1080 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_BAUD) = config->baud;
1081 HWREG_WRITE_LRF(LRFDMDM_BASE + LRFDMDM_O_MODSYMMAP0) = config->symmap;
1082 HWREG_WRITE_LRF(LRFDRFE_BASE + LRFDRFE_O_SPARE3) = config->magnConfig[lutIdx].val;
1083 HWREG_WRITE_LRF(LRFDRFE_BASE + LRFDRFE_O_RSSIOFFSET) = config->rssioffset;
1084 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_DEMMISC3) = config->demmisc3;
1085 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TPOSTPROCESSDIV1) = config->tPostProcessDiv1;
1086 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TPOSTPROCESSDIV12) = config->tPostProcessDiv12;
1087 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TRXTIMEOUTI0) = config->tRxTimeoutI0;
1088 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TRXTIMEOUTI3) = config->tRxTimeoutI3;
1089 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TPILOTADJ) = config->tPilotAdjust;
1090
1091 /* Alias for dynamic parameters for calculations */
1092 uint16_t tIp1 = pCmd->timing.tIp1;
1093 uint16_t tIp2 = pCmd->timing.tIp2;
1094 uint16_t tFm = pCmd->timing.tFm;
1095 uint16_t tPm = pCmd->timing.tPm;
1096 uint16_t tFcs = pCmd->timing.tFcs;
1097 uint16_t tSw = pCmd->timing.tSw;
1098 uint16_t nPath = antennaEntry->numPath;
1099
1100 /* Phy specific delays */
1101 uint16_t tStartup = config->tStartup;
1102 uint16_t tPkt = config->tPacket;
1103 uint16_t tCorr = config->tCorr;
1104 uint16_t tDem = config->tDem;
1105 uint16_t tTr = config->tTr;
1106 uint16_t tFidc = config->tFidc;
1107 uint16_t tTxModDelay = config->tTxModDelay;
1108 uint16_t tInfiniteRx = config->tInfiniteRx;
1109
1110 /* Modulation + Demodulation delay for timestamp capturing */
1111 uint16_t tTimestampDelay1 = config->tTimestampDelay1;
1112 uint16_t tTimestampDelay3 = config->tTimestampDelay3;
1113
1114 /* Calculate base terms of step duration for each mode.
1115 Later can be extended with optional payload per step before sending it over to the PBE. */
1116 tStepLut[RCL_CmdBleCs_StepMode_0] = (tFcs + ((tPkt + TRD) << 1) + TGD + tIp1 + tFm);
1117 tStepLut[RCL_CmdBleCs_StepMode_1] = (tFcs + ((tPkt + TPYL + TRD) << 1) + tIp1);
1118 tStepLut[RCL_CmdBleCs_StepMode_2] = (tFcs + ((((tPm + tSw) * (nPath + 1)) + TRD) << 1) + tIp2);
1119 tStepLut[RCL_CmdBleCs_StepMode_3] = (tFcs + ((tPkt + TPYL + TGD + ((tPm + tSw) * (nPath + 1)) + TRD) << 1) + tIp2);
1120
1121 /* Adjustment of MCE timegrid */
1122 const RCL_Tadjust_t *tAdj = &config->tAdj[pCmd->mode.role];
1123
1124 if (pCmd->mode.role == RCL_CmdBleCs_Role_Initiator)
1125 {
1126 /* Calculate base terms of interlude period. Later will scale with payload. */
1127
1128 /* TX */
1129 tAdjALut[RCL_CmdBleCs_StepMode_0] = tAdj->mode0[0] + 0xFFFF;
1130 tAdjALut[RCL_CmdBleCs_StepMode_1] = tAdj->mode1[0] + 0xFFFF;
1131 tAdjALut[RCL_CmdBleCs_StepMode_2] = tAdj->mode2[0] + 0x0000; // No packet, variable used for pilot adjustment
1132 tAdjALut[RCL_CmdBleCs_StepMode_3] = tAdj->mode3[0] + 0xFFFF;
1133
1134 /* RX */
1135 tAdjBLut[RCL_CmdBleCs_StepMode_0] = tAdj->mode0[1] + TRD + tIp1 - TPLT;
1136 tAdjBLut[RCL_CmdBleCs_StepMode_1] = tAdj->mode1[1] + TRD + tIp1 - TPLT;
1137 tAdjBLut[RCL_CmdBleCs_StepMode_2] = tAdj->mode2[1] + TRD + tIp2 + tSw + tFidc;
1138 tAdjBLut[RCL_CmdBleCs_StepMode_3] = tAdj->mode3[1] + TRD + tIp2 + tSw + tFidc;
1139
1140 /* ToAD common term */
1141 tRttAdjustLut[RCL_CmdBleCs_StepMode_0] = 0; /* NA, no RTT defined */
1142 tRttAdjustLut[RCL_CmdBleCs_StepMode_1] = TPYL + TRD + tIp1 + tPkt + tTimestampDelay1;
1143 tRttAdjustLut[RCL_CmdBleCs_StepMode_2] = 0; /* NA, no RTT defined */
1144 tRttAdjustLut[RCL_CmdBleCs_StepMode_3] = TPYL + (TGD << 1) + (((tPm + tSw) * ((nPath << 1) + 1))) + TRD + tIp2 + tPkt + tTimestampDelay3;
1145
1146 /* Initiator: timeout is hardcoded in the MCE in order to comply with the static timegrid.
1147 This parameter is therefor ignored. */
1148 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TRXTIMEOUTR013) = 0;
1149 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TRXWIDENINGR0) = 0;
1150 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSTEPREMAININGR0) = 0;
1151 }
1152 else
1153 {
1154 /* RX */
1155 tAdjALut[RCL_CmdBleCs_StepMode_0] = tAdj->mode0[0] - tCorr + tTr + TRD + tIp1 - TPLT - tTxModDelay;
1156 tAdjALut[RCL_CmdBleCs_StepMode_1] = tAdj->mode1[0] - tCorr + tTr + TRD + tIp1 - TPLT - tTxModDelay;
1157 tAdjALut[RCL_CmdBleCs_StepMode_2] = tAdj->mode2[0] + tStartup + TPLT + tSw + tFidc;
1158 tAdjALut[RCL_CmdBleCs_StepMode_3] = tAdj->mode3[0] - tCorr + tTr + TGD + tSw + tFidc;
1159
1160 /* TX */
1161 tAdjBLut[RCL_CmdBleCs_StepMode_0] = tAdj->mode0[1];
1162 tAdjBLut[RCL_CmdBleCs_StepMode_1] = tAdj->mode1[1];
1163 tAdjBLut[RCL_CmdBleCs_StepMode_2] = tAdj->mode2[1] - tFidc - tSw /* + TRD */ + tIp2 - TPLT; // Ramp-down delay is added by the topSM
1164 tAdjBLut[RCL_CmdBleCs_StepMode_3] = tAdj->mode3[1] - tFidc - tSw /* + TRD */ + tIp2 - TPLT - tTxModDelay;
1165
1166 /* ToAD common term */
1167 tRttAdjustLut[RCL_CmdBleCs_StepMode_0] = 0; /* NA, no RTT defined */
1168 tRttAdjustLut[RCL_CmdBleCs_StepMode_1] = TPYL + TRD + tIp1 + tPkt - tTimestampDelay1;
1169 tRttAdjustLut[RCL_CmdBleCs_StepMode_2] = 0; /* NA, no RTT defined */
1170 tRttAdjustLut[RCL_CmdBleCs_StepMode_3] = TPYL + (TGD << 1) + (((tPm + tSw) * ((nPath << 1) + 1))) + TRD + tIp2 + tPkt - tTimestampDelay3;
1171
1172 /* The miminum value is defined by the static timegrid. Different for each datarate and implemented by the MCE. */
1173 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TRXTIMEOUTR013) = config->tRxTimeoutRn;
1174
1175 /* Handle infinite/extended RX timeout implemented by PBE */
1176 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TRXWIDENINGR0)
1177 = (pCmd->timing.tRxWideningR0 == 0xFFFF)
1178 ? (0xFFFF)
1179 : RCL_BLE_CS_MCE_TIMER_TO_PBE_TIMER(tFcs + (tPkt - tTr) + tDem + T3US + pCmd->timing.tRxWideningR0);
1180
1181 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_TSTEPREMAININGR0) =
1182 RCL_BLE_CS_MCE_TIMER_TO_PBE_TIMER(0 - tDem + tTr + TRD + tIp1 + tPkt + TGD + tFm + TRD - tInfiniteRx);
1183 }
1184
1185 /* Adjustment of antenna control per mode per phy */
1186 const RCL_Tadjust_t *tAnt = &config->tAnt[pCmd->mode.role & 1];
1187
1188 tAntALut[RCL_CmdBleCs_StepMode_0] = 0; /* NA - no antenna */
1189 tAntALut[RCL_CmdBleCs_StepMode_1] = 0; /* NA - no antenna */
1190 tAntALut[RCL_CmdBleCs_StepMode_2] = tAnt->mode2[0] + TPLT;
1191 tAntALut[RCL_CmdBleCs_StepMode_3] = tAnt->mode3[0] + TPLT + tPkt + TGD;
1192
1193 tAntBLut[RCL_CmdBleCs_StepMode_0] = 0; /* NA - no antenna */
1194 tAntBLut[RCL_CmdBleCs_StepMode_1] = 0; /* NA - no antenna */
1195 tAntBLut[RCL_CmdBleCs_StepMode_2] = tAnt->mode2[1];
1196 tAntBLut[RCL_CmdBleCs_StepMode_3] = tAnt->mode3[1];
1197
1198 /* Enforce modulator and demodulator clock. RTL bug with automatic control. */
1199 LRF_setRclClockEnable(LRFDDBELL_CLKCTL_DEM_M | LRFDDBELL_CLKCTL_MOD_M);
1200 }
1201
1202 /*
1203 * ======== Preprocess the step ========
1204 */
RCL_Handler_BLE_CS_preprocessStep(RCL_CmdBleCs * pCmd,RCL_CmdBleCs_Step * pStep,RCL_CmdBleCs_Step_Internal * pStepI)1205 static void RCL_Handler_BLE_CS_preprocessStep(RCL_CmdBleCs *pCmd, RCL_CmdBleCs_Step *pStep, RCL_CmdBleCs_Step_Internal *pStepI)
1206 {
1207 /* Decompress the HCI format into PBE internal format */
1208 pStepI->channelIdx = pStep->channelIdx;
1209 pStepI->mode = pStep->mode;
1210 pStepI->antennaPermIdx = pStep->antennaPermIdx;
1211 pStepI->toneExtension = pStep->toneExtension;
1212 pStepI->payloadLen = pStep->payloadLen;
1213 pStepI->aaTx = pStep->aaTx;
1214 pStepI->aaRx = pStep->aaRx;
1215
1216 for (uint8_t i=0; i<RCL_BLE_CS_MAX_PAYLOAD_SIZE; i++)
1217 {
1218 pStepI->payloadTx[i] = pStep->payloadTx[i];
1219 pStepI->payloadRx[i] = pStep->payloadRx[i];
1220 }
1221
1222 /* Disable restricted channels if filtering is turned on */
1223 if (pCmd->mode.chFilterEnable == true)
1224 {
1225 uint16_t channelIdx = pStepI->channelIdx;
1226
1227 if ( channelIdx > BLE_CS_MAX_CHANNEL
1228 || channelIdx == 0 /* Advertisement frequency (channel 37)*/
1229 || channelIdx == 1
1230 || channelIdx == 23
1231 || channelIdx == 24 /* Advertisement frequency (channel 38)*/
1232 || channelIdx == 25
1233 || channelIdx == 77
1234 || channelIdx == 78 /* Advertisement frequency (channel 39)*/)
1235 {
1236 /* The user configured a channel outside the allowed domain or
1237 a restricted channel. Guard channels also covered!
1238 We still keep the timegrid but silence this step. */
1239 pStepI->channelIdx = BLE_CS_SILENT_CHANNEL;
1240 }
1241 }
1242
1243 /* Decode the base term for the complete duration of the step */
1244 uint32_t tStep = tStepLut[pStepI->mode];
1245 uint16_t tAdjA = tAdjALut[pStepI->mode];
1246 uint16_t tAdjB = tAdjBLut[pStepI->mode];
1247
1248 /* Scale with bit length and 1M/2M rate */
1249 uint16_t tPyl = (payloadLut[pStepI->payloadLen]) >> pCmd->mode.phy;
1250
1251 /* Extend with timing of optional payload */
1252 if (pStepI->payloadLen)
1253 {
1254 /* Adjustment only on Reflector */
1255 uint16_t tAdj = (pCmd->mode.role == RCL_CmdBleCs_Role_Reflector)
1256 ? (tPyl)
1257 : (0);
1258
1259 /* Payload is only allowed for certain modes. */
1260 if (pStepI->mode == RCL_CmdBleCs_StepMode_1 ||
1261 pStepI->mode == RCL_CmdBleCs_StepMode_3)
1262 {
1263 /* Multiplied by two for combined effect of TX and RX */
1264 tStep += (tPyl << 1);
1265
1266 /* The duration from AA sync until the next activity scales with payload on reflector only */
1267 tAdjA += tAdj;
1268 }
1269 else
1270 {
1271 /* Enforce no payload for mode 0 & 2 */
1272 pStepI->payloadLen = 0;
1273
1274 /* No adjustment needed */
1275 tPyl = 0;
1276 }
1277 }
1278
1279 /* Configure the PBE with the scaled step duration */
1280 pStepI->tStep = (uint16_t) RCL_BLE_CS_MCE_TIMER_TO_PBE_TIMER(tStep);
1281
1282 /* Configure the MCE with the scaled intermediate timegrid parameters */
1283 pStepI->tAdjustA = tAdjA;
1284 pStepI->tAdjustB = tAdjB;
1285
1286 /* Antenna timing from beginning of step */
1287 pStepI->tAntennaA = tAntALut[pStepI->mode] + tPyl;
1288 pStepI->tAntennaB = tAntBLut[pStepI->mode];
1289
1290 /* DC compensation */
1291 if ((pCmd->precalTable) && (pCmd->precalTable->valid))
1292 {
1293 pCmd->precalTable->callback(pCmd->precalTable, pStepI->channelIdx, (uint32_t *)&pStepI->dcComp[0], (uint32_t *)&pStepI->dcComp[1]);
1294 }
1295 else
1296 {
1297 pStepI->dcComp[0].i = 0;
1298 pStepI->dcComp[0].q = 0;
1299 pStepI->dcComp[1].i = 0;
1300 pStepI->dcComp[1].q = 0;
1301 }
1302
1303 /* Antenna calculations */
1304 pStepI->antennaSequence = 0;
1305
1306 /* Decode mapping of configuration to antennas */
1307 const RCL_AntennaConfig_t *antennaEntry = &antennaConfig[pCmd->antennaConfig.select];
1308
1309 /* Avoid indexing out of range */
1310 if (pStepI->antennaPermIdx < antennaEntry->numPerm)
1311 {
1312 /* Find the proper permutation pattern based on generic config and random step related selection.
1313 The same table is used, but we index the entries differently */
1314 uint8_t apn = antennaPermutation[pStepI->antennaPermIdx * antennaEntry->increment].apn;
1315
1316 /* We reuse the same permutation table for 1:1, 1:N, N:1 and 2:2,
1317 but only use the appropriate number of entries from each row */
1318 for(uint8_t k1=0; k1<antennaEntry->numPath; k1++)
1319 {
1320 uint8_t apm = antennaEntry->ap[pCmd->mode.role & 1].apn;
1321
1322 /* Decode bitfields */
1323 uint8_t k2 = DECODE_ANTENNA(apn, k1);
1324 uint8_t k3 = DECODE_ANTENNA(apm, k2);
1325
1326 /* Antenna sequence is stored as [7:6]=A4, [5:4]=A3, [3:2]=A2, [1:0]=A1.
1327 The PBE will decode it accordingly. */
1328 pStepI->antennaSequence |= ENCODE_ANTENNA(k3, k1);
1329 }
1330 }
1331
1332 /* Look up the frequency offset actuation error (only used by mode 0) */
1333 pStepI->foffErr = (uint16_t) foffErrorLut[pStepI->channelIdx];
1334 }
1335
1336 /*
1337 * ======== Fetch the next available step result container in the list and return a pointer to it ========
1338 */
RCL_Handler_BLE_CS_fetchNextStepResult(RCL_CmdBleCs * pCmd)1339 static RCL_CmdBleCs_StepResult_Internal* RCL_Handler_BLE_CS_fetchNextStepResult(RCL_CmdBleCs *pCmd)
1340 {
1341 RCL_CmdBleCs_StepResult_Internal *pResult = NULL;
1342
1343 if ((pCmd->stats->nResultsRead < pCmd->mode.nSteps) && pCmd->results)
1344 {
1345 pResult = &pCmd->results[pCmd->stats->nResultsRead];
1346 }
1347
1348 return (pResult);
1349 }
1350
1351 /*
1352 * ======== RCL_Handler_BLE_CS_convertFreqOffset ========
1353 */
RCL_Handler_BLE_CS_convertFreqOffset(int16_t foffMeasured)1354 static int16_t RCL_Handler_BLE_CS_convertFreqOffset(int16_t foffMeasured)
1355 {
1356 /* Intermediate arithmetics on 32bit
1357 freqOffset = foff * 100 * 1e6 / 2^23
1358 = foff * (100 * 1e6 / 2^7) / 2^16
1359 = foff * 0xBEBC2 / 2^16 [0.01 ppm] */
1360 int32_t freqOffset = (int32_t)foffMeasured;
1361 freqOffset *= 0xBEBC2;
1362 freqOffset >>= 16;
1363
1364 return (int16_t)(freqOffset);
1365 }
1366
1367 /*
1368 * ======== RCL_Handler_BLE_CS_convertRtt ========
1369 */
RCL_Handler_BLE_CS_convertRtt(RCL_CmdBleCs * pCmd,uint8_t mode,int8_t channel,int16_t foff,uint8_t payloadLen,bool secondToneExtensionSlot,float toAD,uint16_t corrBefore,uint16_t corrPeak,uint16_t corrAfter)1370 static int16_t RCL_Handler_BLE_CS_convertRtt(RCL_CmdBleCs *pCmd, uint8_t mode, int8_t channel, int16_t foff, uint8_t payloadLen, bool secondToneExtensionSlot,
1371 float toAD, uint16_t corrBefore, uint16_t corrPeak, uint16_t corrAfter)
1372 {
1373 /* Raw RTT need to be compensated with:
1374 - Correlator before/after fractional approximation
1375 - Nominal tunr-around time T = TSY + TIP + TEXT + TPYL + ...
1376 - Time drift due to XTAL offset (initiator only)
1377 - CMIX (channel specific) and layout related errors */
1378
1379 /* Conversion from 4M/8M baudrate to 48M clock tick */
1380 uint8_t baud2tick = (pCmd->mode.phy) ? (6) : (12);
1381
1382 /* Quadratic interpolation with correlator values */
1383 float corr = (float)(baud2tick * (corrBefore - corrAfter))
1384 / (float)((corrAfter << 1) + (corrBefore << 1) - (corrPeak << 2));
1385
1386 /* Compensation applies with different sign */
1387 toAD += (pCmd->mode.role == RCL_CmdBleCs_Role_Initiator) ? (+corr) : (-corr);
1388
1389 /* Remove nominal turn-around time (adjust to the signal on the antenna) */
1390 toAD -= tRttAdjustLut[mode];
1391
1392 /* Adjust with optional payload duration */
1393 toAD -= (payloadLut[payloadLen] >> pCmd->mode.phy);
1394
1395 /* Adjust with optional tone-extension slot for mode-3 */
1396 if (secondToneExtensionSlot == true)
1397 {
1398 toAD -= (pCmd->timing.tPm + pCmd->timing.tSw);
1399 }
1400
1401 /* Adjust initiator */
1402 if (pCmd->mode.role == RCL_CmdBleCs_Role_Initiator)
1403 {
1404 /* XTAL offset related drift */
1405 //toAD *= 100e6/(100e6 + foff); //TODO: This takes to much execution time
1406 }
1407
1408 /* Channel specific delays */
1409 #define CHANNEL_MID (float)(40.0)
1410 #define CHANNEL_CAL (float)(0.0415)
1411 float adj = CHANNEL_CAL * (channel - CHANNEL_MID);
1412 toAD -= (pCmd->mode.role == RCL_CmdBleCs_Role_Initiator) ? (-adj) : (+adj);
1413
1414 /* Convert to 0.5ns units as per HCI spec requires */
1415 toAD *= (float)41.6666;
1416
1417 /* Round */
1418 toAD += (float)0.5;
1419
1420 /* Return the adjusted timeticks */
1421 return ((int16_t) toAD);
1422 }
1423
1424 /*
1425 * ======== RCL_Handler_BLE_CS_rotateVector ========
1426 */
RCL_Handler_BLE_CS_rotateVector(int16_t * pct_i,int16_t * pct_q,int16_t theta)1427 void RCL_Handler_BLE_CS_rotateVector(int16_t *pct_i, int16_t *pct_q, int16_t theta)
1428 {
1429 /* Quickly return if nothing to do */
1430 if (theta != 0)
1431 {
1432 /* CORDIC implementation of rotating a vector with given angle
1433 *
1434 * theta = 16bit representation of the angle in [-pi = -32768, +pi = 32767] range to rotate the PCT with
1435 * pct_i = I component of PCT
1436 * pct_q = Q component of PCT
1437 * */
1438 #define PI_div2 (1 << (16-2))
1439
1440 /* The LUT and normalization factor is generated by the following python expression:
1441 *
1442 * f = 1.0
1443 * for i in range(NBITS):
1444 * x = np.arctan(1 / 2**i) / (np.pi/2) * (2**NBITS)
1445 * atanLut += [ (np.floor)(x + 0.5) ]
1446 *
1447 * f = (f * (2**(2*i) + 1)) / 2**(2*i)
1448 *
1449 * f = 1/np.sqrt(f) * (2**NBITS)
1450 * K = (np.floor) (f + 0.5)
1451 * */
1452 #define NBITS (14)
1453 #define K14 (9949)
1454 const uint16_t atanLut[NBITS] = { 8192, 4836, 2555, 1297, 651, 326, 163, 81, 41, 20, 10, 5, 3, 1 };
1455
1456 /* Swap coordinates when angle is between [-pi,-pi/2] or [pi/2,pi] */
1457 int32_t x = ((int32_t) *pct_i);
1458 int32_t y = ((int32_t) *pct_q);
1459 if (theta > PI_div2)
1460 {
1461 theta -= PI_div2;
1462 x = -((int32_t) *pct_q);
1463 y = +((int32_t) *pct_i);
1464 }
1465 else if (theta < (-PI_div2))
1466 {
1467 theta += PI_div2;
1468 x = +((int32_t) *pct_q);
1469 y = -((int32_t) *pct_i);
1470 }
1471 x *= K14;
1472 y *= K14;
1473
1474 /* Initialize local variables */
1475 int32_t x1 = 0;
1476 int32_t y1 = 0;
1477
1478 /* The direction follows the mathematical positive direction */
1479 int32_t phi = (int32_t)(-theta);
1480
1481 /* Rotate iteratively */
1482 for (uint8_t i = 0; i < NBITS; i++)
1483 {
1484 if (phi < 0)
1485 { /* Counter-clockwise */
1486 phi += atanLut[i];
1487 y1 = y + (x >> i);
1488 x1 = x - (y >> i);
1489 }
1490 else
1491 { /* Clockwise */
1492 phi -= atanLut[i];
1493 y1 = y - (x >> i);
1494 x1 = x + (y >> i);
1495 }
1496
1497 x = x1;
1498 y = y1;
1499 }
1500
1501 /* Scale according to LUT normalization weight */
1502 *pct_i = (x >> NBITS);
1503 *pct_q = (y >> NBITS);
1504 }
1505 }
1506
1507 /*
1508 * ======== RCL_Handler_BLE_CS_convertPct ========
1509 */
RCL_Handler_BLE_CS_convertPct(int16_t pct_i,int16_t pct_q,uint8_t channelIdx)1510 static uint32_t RCL_Handler_BLE_CS_convertPct(int16_t pct_i, int16_t pct_q, uint8_t channelIdx)
1511 {
1512 /* Calibrate via the t_picosec parameter */
1513 #define t_picosec (uint64_t)(1000)
1514 #define t_picosec_ (uint64_t)(t_picosec * 3/2)
1515 #define t_const (uint64_t)((t_picosec_ << 31) / 1e6)
1516 #define t_scaler (15)
1517 #define CALC_ANGLE(ch) (-(int16_t)((((uint64_t)ch) * t_const) >> t_scaler))
1518
1519 /* Adjust the phase to the signal on the antenna (group delay and layout) */
1520 RCL_Handler_BLE_CS_rotateVector(&pct_i, &pct_q, CALC_ANGLE(channelIdx));
1521
1522 /* Compress PCTs to 24bit */
1523 uint32_t pct = (((pct_q >> 1) & 0x0FFF) << 12)
1524 | ( (pct_i >> 1) & 0x0FFF);
1525
1526 return (pct);
1527 }
1528
1529 /*
1530 * ======== RCL_Handler_BLE_CS_convertPctQuality ========
1531 */
RCL_Handler_BLE_CS_convertPctQuality(uint16_t qMin,uint16_t qMax,uint16_t qAvg,bool toneExtensionSlot,bool toneExpected)1532 static uint8_t RCL_Handler_BLE_CS_convertPctQuality(uint16_t qMin, uint16_t qMax, uint16_t qAvg, bool toneExtensionSlot, bool toneExpected)
1533 {
1534 /* Initialize with unavailable */
1535 uint8_t tnQ = RCL_CmdBleCs_ToneQuality_Unavailable;
1536
1537 /* Avoid zero-division. Metric is unavailable. Also for Initiator mode-3. */
1538 if (qAvg != 0)
1539 {
1540 /* Calculate Q3 scale */
1541 uint8_t Q3 = 100 * (qMax - qMin)/qAvg;
1542
1543 /* Classify based on thresholds */
1544 if (Q3 < BLE_CS_TONE_QUALITY_GOOD_THR) tnQ = RCL_CmdBleCs_ToneQuality_Good;
1545 else if (Q3 < BLE_CS_TONE_QUALITY_MEDIUM_THR) tnQ = RCL_CmdBleCs_ToneQuality_Medium;
1546 else tnQ = RCL_CmdBleCs_ToneQuality_Low;
1547
1548 /* Add additional flag for the tone extension slot */
1549 if (toneExtensionSlot)
1550 {
1551 /* Decode the appropriate flag */
1552 uint8_t toneExtensionFlag = (toneExpected)
1553 ? (RCL_CmdBleCs_ToneExtensionSlot_Enabled_ToneExpected)
1554 : (RCL_CmdBleCs_ToneExtensionSlot_Enabled_NoToneExpected);
1555
1556 tnQ |= (toneExtensionFlag << 4);
1557 }
1558 }
1559
1560 return (tnQ);
1561 }
1562
1563 /*
1564 * ======== RCL_Handler_BLE_CS_estimateStepResultLength ========
1565 */
RCL_Handler_BLE_CS_estimateStepResultLength(RCL_CmdBleCs * pCmd,RCL_CmdBleCs_StepResult_Internal * src)1566 static uint16_t RCL_Handler_BLE_CS_estimateStepResultLength(RCL_CmdBleCs *pCmd, RCL_CmdBleCs_StepResult_Internal* src)
1567 {
1568 /* Information extracted from registers */
1569 uint8_t role = pCmd->mode.role;
1570 uint8_t numAntennaPath = pCmd->stats->numAntennaPath + 1; // +1 = tone extension
1571
1572 /* First two fields are mandatory */
1573 uint16_t length = 0;
1574
1575 /* Dataformat varies based on the mode. Constants are known from the spec */
1576 switch (src->mode)
1577 {
1578 case (RCL_CmdBleCs_StepMode_0):
1579 /* Initiator also contains the measured frequency offset */
1580 length = (role == RCL_CmdBleCs_Role_Initiator) ? sizeof(RCL_CmdBleCs_ResultI0) : sizeof(RCL_CmdBleCs_ResultR0);
1581 break;
1582 case (RCL_CmdBleCs_StepMode_1):
1583 length = sizeof(RCL_CmdBleCs_ResultIR1);
1584 break;
1585 case (RCL_CmdBleCs_StepMode_2):
1586 length = sizeof(RCL_CmdBleCs_ResultIR2) + numAntennaPath * sizeof(RCL_CmdBleCs_Tone);
1587 break;
1588 case (RCL_CmdBleCs_StepMode_3):
1589 length = sizeof(RCL_CmdBleCs_ResultIR3) + numAntennaPath * sizeof(RCL_CmdBleCs_Tone);
1590 break;
1591 }
1592
1593 return (length);
1594 }
1595
1596 /*
1597 * ======== RCL_Handler_BLE_CS_convertStepResult ========
1598 */
RCL_Handler_BLE_CS_convertStepResult(RCL_CmdBleCs * pCmd,uint8_t * dst,RCL_CmdBleCs_StepResult_Internal * src)1599 static uint16_t RCL_Handler_BLE_CS_convertStepResult(RCL_CmdBleCs* pCmd, uint8_t *dst, RCL_CmdBleCs_StepResult_Internal* src)
1600 {
1601 #define NADM_NOT_SUPPORTED 0xFF
1602 #define PACKET_ANTENNA 1
1603
1604 uint8_t numAntennaPath = pCmd->stats->numAntennaPath + 1; // +1 = tone extension
1605
1606 /* Use register access to guarantee the latest-greatest value for the first burst */
1607 int16_t foff = RCL_Handler_BLE_CS_convertFreqOffset(HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_FOFFCOMP) << 2);
1608
1609 /* Dataformat varies based on the mode */
1610 uint8_t mode = src->mode;
1611 uint8_t channel = src->channelIdx;
1612 uint8_t payloadLen = src->payloadLen;
1613 uint8_t toneExtension = src->toneExtension;
1614
1615 /* Mode independent fields */
1616 *dst++ = mode;
1617 *dst++ = channel;
1618
1619 /* Remember the address of field 'datalength' and skip it for now, will be filled later */
1620 uint8_t *dataLength = dst++;
1621
1622 if (mode == RCL_CmdBleCs_StepMode_0)
1623 {
1624 *dst++ = src->pktResult; /* Packet_AA_Quality */
1625 *dst++ = src->pktRssi; /* Packet_RSSI */
1626 *dst++ = PACKET_ANTENNA; /* Packet_Antenna */
1627
1628 if (pCmd->mode.role == RCL_CmdBleCs_Role_Initiator)
1629 {
1630 int16_t freqOffset = RCL_Handler_BLE_CS_convertFreqOffset(src->foffMeasured);
1631 *dst++ = INT16_LSB(freqOffset);
1632 *dst++ = INT16_MSB(freqOffset);
1633 }
1634 }
1635 else if (mode == RCL_CmdBleCs_StepMode_1)
1636 {
1637 *dst++ = src->pktResult; /* Packet_AA_Quality */
1638 *dst++ = NADM_NOT_SUPPORTED; /* Packet_NADM */
1639 *dst++ = src->pktRssi; /* Packet_RSSI */
1640 *dst++ = PACKET_ANTENNA; /* Packet_Antenna */
1641
1642 /* RTT calculation using correlator values */
1643 int16_t toAD = RCL_Handler_BLE_CS_convertRtt(pCmd,
1644 RCL_CmdBleCs_StepMode_1,
1645 channel,
1646 foff,
1647 payloadLen,
1648 false, // secondToneExtensionSlot=NA, there are no tones in mode-1
1649 src->rtt, src->corr[0], src->corr[1], src->corr[2]);
1650 *dst++ = INT16_LSB(toAD);
1651 *dst++ = INT16_MSB(toAD);
1652 }
1653 else if (mode == RCL_CmdBleCs_StepMode_2)
1654 {
1655 *dst++ = src->antennaPermIdx;
1656 for (uint8_t j = 0; j < numAntennaPath; j++)
1657 {
1658 /* Compress PCT to 24bits */
1659 int16_t i = src->pct[j].i;
1660 int16_t q = src->pct[j].q;
1661
1662 uint32_t pct = RCL_Handler_BLE_CS_convertPct(i, q, channel);
1663 *dst++ = (uint8_t)((pct) & 0xFF);
1664 *dst++ = (uint8_t)((pct >> 8) & 0xFF);
1665 *dst++ = (uint8_t)((pct >> 16) & 0xFF);
1666
1667 /* Calculate PCT quality */
1668 bool toneExtensionSlot = (bool)(j == (numAntennaPath - 1));
1669 bool toneExpected = (bool)((pCmd->mode.role == RCL_CmdBleCs_Role_Reflector)
1670 ? (toneExtension & BLE_CS_TONE_EXTENSION_INITIATOR_TX)
1671 : (toneExtension & BLE_CS_TONE_EXTENSION_REFLECTOR_TX));
1672
1673 uint16_t min = src->magn[j].magnMin;
1674 uint16_t max = src->magn[j].magnMax;
1675 uint16_t avg = src->magn[j].magnAvg;
1676 *dst++ = RCL_Handler_BLE_CS_convertPctQuality(min, max, avg, toneExtensionSlot, toneExpected);
1677 }
1678 }
1679 else if (mode == RCL_CmdBleCs_StepMode_3)
1680 {
1681 /* Packet related data */
1682 *dst++ = src->pktResult; /* Packet_AA_Quality */
1683 *dst++ = NADM_NOT_SUPPORTED; /* Packet_NADM */
1684 *dst++ = src->pktRssi; /* Packet_RSSI */
1685 *dst++ = PACKET_ANTENNA; /* Packet_Antenna */
1686
1687 /* RTT calculation using correlator values */
1688 int16_t toAD = RCL_Handler_BLE_CS_convertRtt(pCmd,
1689 RCL_CmdBleCs_StepMode_3,
1690 channel,
1691 foff,
1692 payloadLen,
1693 (bool)(toneExtension & 2),
1694 src->rtt, src->corr[0], src->corr[1], src->corr[2]);
1695 *dst++ = INT16_LSB(toAD);
1696 *dst++ = INT16_MSB(toAD);
1697
1698 /* Tone related data */
1699 *dst++ = src->antennaPermIdx;
1700 for (uint8_t j = 0; j < numAntennaPath; j++)
1701 {
1702 /* Compress PCT to 24bits */
1703 int16_t i = src->pct[j].i;
1704 int16_t q = src->pct[j].q;
1705
1706 uint32_t pct = RCL_Handler_BLE_CS_convertPct(i, q, channel);
1707 *dst++ = (uint8_t)((pct) & 0xFF);
1708 *dst++ = (uint8_t)((pct >> 8) & 0xFF);
1709 *dst++ = (uint8_t)((pct >> 16) & 0xFF);
1710
1711 /* Calculate PCT quality */
1712 bool toneExtensionSlot = (bool)(j == (numAntennaPath - 1));
1713
1714 /* Mode-3 may or may not have the R->I tone extension present */
1715 bool toneExpected = (bool)((pCmd->mode.role == RCL_CmdBleCs_Role_Reflector)
1716 ? (toneExtension & BLE_CS_TONE_EXTENSION_INITIATOR_TX)
1717 : (toneExtension & BLE_CS_TONE_EXTENSION_REFLECTOR_TX));
1718
1719 uint16_t min = src->magn[j].magnMin;
1720 uint16_t max = src->magn[j].magnMax;
1721 uint16_t avg = src->magn[j].magnAvg;
1722 *dst++ = RCL_Handler_BLE_CS_convertPctQuality(min, max, avg, toneExtensionSlot, toneExpected);
1723 }
1724 }
1725
1726 /* Fill the Step_Data_Length based on difference of addresses */
1727 *dataLength = (dst - dataLength - 1);
1728
1729 /* Return the number of bytes written (+3 = mode&channel&dataLength) */
1730 return (*dataLength + 3);
1731 }
1732
1733 /*
1734 * ======== RCL_Handler_BLE_CS_findPbeErrorEndStatus ========
1735 */
RCL_Handler_BLE_CS_findPbeErrorEndStatus(uint16_t pbeEndStatus)1736 static RCL_CommandStatus RCL_Handler_BLE_CS_findPbeErrorEndStatus(uint16_t pbeEndStatus)
1737 {
1738 RCL_CommandStatus status;
1739 switch (pbeEndStatus)
1740 {
1741 case PBE_COMMON_RAM_ENDCAUSE_STAT_ERR_RXF:
1742 status = RCL_CommandStatus_Error_RxFifo;
1743 break;
1744 case PBE_COMMON_RAM_ENDCAUSE_STAT_ERR_TXF:
1745 status = RCL_CommandStatus_Error_TxFifo;
1746 break;
1747 case PBE_COMMON_RAM_ENDCAUSE_STAT_ERR_SYNTH:
1748 status = RCL_CommandStatus_Error_Synth;
1749 break;
1750 case PBE_COMMON_RAM_ENDCAUSE_STAT_ERR_STOP:
1751 status = RCL_Scheduler_findStopStatus(RCL_StopType_Hard);
1752 break;
1753 case PBE_COMMON_RAM_ENDCAUSE_STAT_ERR_PAR:
1754 status = RCL_CommandStatus_Error_Param;
1755 break;
1756 case PBE_COMMON_RAM_ENDCAUSE_STAT_ERR_BADOP:
1757 status = RCL_CommandStatus_Error_UnknownOp;
1758 break;
1759 default:
1760 Log_printf(RclCore, Log_ERROR, "Unexpected error 0x%04X from PBE", pbeEndStatus);
1761 status = RCL_CommandStatus_Error;
1762 break;
1763 }
1764 return (status);
1765 }
1766
1767 /*
1768 * ======== RCL_Handler_BLE_CS ========
1769 */
RCL_Handler_BLE_CS(RCL_Command * cmd,LRF_Events lrfEvents,RCL_Events rclEventsIn)1770 RCL_Events RCL_Handler_BLE_CS(RCL_Command *cmd, LRF_Events lrfEvents, RCL_Events rclEventsIn)
1771 {
1772 RCL_CmdBleCs *pCmd = (RCL_CmdBleCs *) cmd;
1773 RCL_Events rclEvents = {.value = 0};
1774
1775 if (rclEventsIn.setup != 0)
1776 {
1777 uint32_t earliestStartTime;
1778
1779 /* Start by enabling refsys */
1780 earliestStartTime = LRF_enableSynthRefsys();
1781
1782 /* Check if valid PLLDIV0 synth setting is used. */
1783 /* BLE CS currently supports only either 3 => FREF0=16MHz or 12 => FREF0=4MHz */
1784 uint32_t plldiv0 = (HWREG_READ_LRF(LRFDRFE_BASE + LRFDRFE_O_PRE0) & LRFDRFE_PRE0_PLLDIV0_M) >> LRFDRFE_PRE0_PLLDIV0_S;
1785 if ((plldiv0 != 3U) && (plldiv0 != 12U))
1786 {
1787 /* Override to use PLLDIV0=12, gives FREF0=4MHz */
1788 Log_printf(RclCore, Log_WARNING, "Unsupported RFE_PRE0_PLLDIV0 synth setting detected. Will override to use 12 (FREF=4MHz)");
1789 HWREG_WRITE_LRF(LRFDRFE_BASE + LRFDRFE_O_PRE0) = ((12U << LRFDRFE_PRE0_PLLDIV0_S) & LRFDRFE_PRE0_PLLDIV0_M) |
1790 ((12U << LRFDRFE_PRE0_PLLDIV1_S) & LRFDRFE_PRE0_PLLDIV1_M);
1791 }
1792
1793 /* Mark as active */
1794 cmd->status = RCL_CommandStatus_Active;
1795
1796 /* Default end status */
1797 bleCsHandlerState.common.endStatus = RCL_CommandStatus_Finished;
1798
1799 /* Enable radio */
1800 LRF_enable();
1801
1802 RCL_CommandStatus startTimeStatus = RCL_Scheduler_setStartStopTimeEarliestStart(cmd, earliestStartTime);
1803 if (startTimeStatus >= RCL_CommandStatus_Finished)
1804 {
1805 cmd->status = startTimeStatus;
1806 rclEvents.lastCmdDone = 1;
1807 }
1808 else
1809 {
1810 /* Enable interrupts (LRF_EventMdmsoft0 => "S2R samples available") */
1811 HWREG_WRITE_LRF(LRFDDBELL_BASE + LRFDDBELL_O_IMASK0) =
1812 HWREG_READ_LRF(LRFDDBELL_BASE + LRFDDBELL_O_IMASK0)
1813 | LRF_EventOpDone.value
1814 | LRF_EventOpError.value
1815 | LRF_EventMdmsoft0.value
1816 | LRF_EventRxfifo.value;
1817
1818 /* Initialize BLE CS specific registers and FIFO */
1819 RCL_Handler_BLE_CS_preprocessCommand(pCmd);
1820
1821 /* Configure TX power*/
1822 if (LRF_programTxPower(pCmd->frontend.txPower) != TxPowerResult_Ok)
1823 {
1824 cmd->status = RCL_CommandStatus_Error_Param;
1825 rclEvents.lastCmdDone = 1;
1826 }
1827 else
1828 {
1829 /* Configure the BUFRAM fifo size and interrupts */
1830 RCL_Handler_BLE_CS_configureTxRxFifo(pCmd);
1831
1832 /* Configure S2R size and mode */
1833 RCL_Handler_BLE_CS_configureS2R(pCmd);
1834
1835 /* Fill the first batch of step descriptors into the BUFRAM TX fifo */
1836 RCL_Handler_BLE_CS_preFillTxBuffer(pCmd);
1837
1838 /* Post command */
1839 LRF_waitForTopsmReady();
1840 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_COMMON_RAM_O_MSGBOX) = 0;
1841 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_API) = PBE_BLE_CS_REGDEF_API_OP_BLE_CS;
1842
1843 /* Forward fill more steps */
1844 RCL_Handler_BLE_CS_preFillTxBuffer(pCmd);
1845 }
1846 }
1847 }
1848
1849 if (cmd->status == RCL_CommandStatus_Active)
1850 {
1851 /* Read the statistics */
1852 RCL_Handler_BLE_CS_readStatistics(pCmd);
1853
1854 if (rclEventsIn.timerStart != 0)
1855 {
1856 rclEvents.cmdStarted = 1;
1857 }
1858
1859 if (lrfEvents.opDone != 0 || lrfEvents.opError != 0)
1860 {
1861 /* Drain the rx fifo of the final batch of results. Force close the buffer. */
1862 rclEvents.value |= RCL_Handler_BLE_CS_readWriteBuffer(pCmd, true).value;
1863 }
1864 else
1865 {
1866 if (lrfEvents.rxfifo != 0)
1867 {
1868 /* Read the available results, and issue event if multibuffer is exhausted */
1869 rclEvents.value |= RCL_Handler_BLE_CS_readWriteBuffer(pCmd, false).value;
1870 }
1871 }
1872
1873 if (lrfEvents.mdmsoft0 != 0)
1874 {
1875 /* Read S2R samples upon MCE event */
1876 rclEvents.value |= RCL_Handler_BLE_CS_readS2RSamples(pCmd).value;
1877 }
1878
1879 if (lrfEvents.opDone != 0)
1880 {
1881 cmd->status = bleCsHandlerState.common.endStatus;
1882 rclEvents.lastCmdDone = 1;
1883 }
1884 else if (lrfEvents.opError != 0)
1885 {
1886 RCL_CommandStatus endStatus = bleCsHandlerState.common.endStatus;
1887 if (endStatus == RCL_CommandStatus_Finished)
1888 {
1889 cmd->status = RCL_Handler_BLE_CS_findPbeErrorEndStatus(HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_COMMON_RAM_O_ENDCAUSE));
1890 }
1891 else
1892 {
1893 cmd->status = endStatus;
1894 }
1895 rclEvents.lastCmdDone = 1;
1896 }
1897 else
1898 {
1899 /* Other events need to be handled unconditionally */
1900 }
1901 }
1902
1903 if (rclEvents.lastCmdDone != 0)
1904 {
1905 LRF_disable();
1906 LRF_disableSynthRefsys();
1907 }
1908 return (rclEvents);
1909 }
1910
1911
1912
1913 /*
1914 * ======== RCL_Handler_BLE_CS_Precal ========
1915 */
RCL_Handler_BLE_CS_Precal(RCL_Command * cmd,LRF_Events lrfEvents,RCL_Events rclEventsIn)1916 RCL_Events RCL_Handler_BLE_CS_Precal(RCL_Command *cmd, LRF_Events lrfEvents, RCL_Events rclEventsIn)
1917 {
1918 RCL_CmdBleCs_Precal *pCmd = (RCL_CmdBleCs_Precal *) cmd;
1919 RCL_Events rclEvents = {.value = 0};
1920
1921 if (rclEventsIn.setup != 0)
1922 {
1923 uint32_t earliestStartTime;
1924
1925 /* Start by enabling refsys */
1926 earliestStartTime = LRF_enableSynthRefsys();
1927
1928 /* Mark as active */
1929 cmd->status = RCL_CommandStatus_Active;
1930
1931 /* Default end status */
1932 bleCsHandlerState.common.endStatus = RCL_CommandStatus_Finished;
1933
1934 /* Enable radio */
1935 LRF_enable();
1936
1937 RCL_CommandStatus startTimeStatus = RCL_Scheduler_setStartStopTimeEarliestStart(cmd, earliestStartTime);
1938 if (startTimeStatus >= RCL_CommandStatus_Finished)
1939 {
1940 cmd->status = startTimeStatus;
1941 rclEvents.lastCmdDone = 1;
1942 }
1943 else
1944 {
1945 /* Enable interrupts */
1946 HWREG_WRITE_LRF(LRFDDBELL_BASE + LRFDDBELL_O_IMASK0)
1947 = HWREG_READ_LRF(LRFDDBELL_BASE + LRFDDBELL_O_IMASK0)
1948 | LRF_EventOpDone.value
1949 | LRF_EventOpError.value;
1950
1951 RCL_Handler_BLE_CS_configureTxRxFifo(NULL);
1952
1953 /* Number of channels to calibrate on */
1954 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_BLE_CS_RAM_O_MODE) = ((pCmd->table->numEntries << PBE_BLE_CS_RAM_MODE_NUM_STEPS_S) & PBE_BLE_CS_RAM_MODE_NUM_STEPS_M);
1955 HWREG_WRITE_LRF(LRFDRFE_BASE + LRFDRFE_O_SPARE0) = pCmd->table->rxGain;
1956
1957 for (uint8_t j=0; j<pCmd->table->numEntries; j++)
1958 {
1959 /* 32bit aperture */
1960 HWREG_WRITE_LRF(LRFDTXF_BASE + LRFDTXF_O_TXD) = pCmd->table->entries[j].channel;
1961 }
1962
1963 /* Post command */
1964 LRF_waitForTopsmReady();
1965 HWREGH_WRITE_LRF(LRFD_BUFRAM_BASE + PBE_COMMON_RAM_O_MSGBOX) = 0;
1966 HWREG_WRITE_LRF(LRFDPBE_BASE + LRFDPBE_O_API) = PBE_BLE_CS_REGDEF_API_OP_PRECAL;
1967 }
1968 }
1969
1970 if (cmd->status == RCL_CommandStatus_Active)
1971 {
1972 if (lrfEvents.opDone != 0)
1973 {
1974 for (uint8_t j=0; j<pCmd->table->numEntries; j++)
1975 {
1976 /* uint32_t per read via the FIFO */
1977 uint32_t data = HWREG_READ_LRF(LRFDRXF_BASE + LRFDRXF_O_RXD);
1978 pCmd->table->entries[j].hdc.i = ((data >> 0 ) & 0xFFFF);
1979 pCmd->table->entries[j].hdc.q = ((data >> 16) & 0xFFFF);
1980
1981 data = HWREG_READ_LRF(LRFDRXF_BASE + LRFDRXF_O_RXD);
1982 pCmd->table->entries[j].ldc.i = ((data >> 0 ) & 0xFFFF);
1983 pCmd->table->entries[j].ldc.q = ((data >> 16) & 0xFFFF);
1984 }
1985
1986 /* Validate the table */
1987 pCmd->table->valid = 1;
1988
1989 cmd->status = bleCsHandlerState.common.endStatus;
1990 rclEvents.lastCmdDone = 1;
1991 }
1992 else if (lrfEvents.opError != 0)
1993 {
1994 /* Invalidate the table */
1995 pCmd->table->valid = 0;
1996
1997 RCL_CommandStatus endStatus = bleCsHandlerState.common.endStatus;
1998 if (endStatus == RCL_CommandStatus_Finished)
1999 {
2000 cmd->status = RCL_Handler_BLE_CS_findPbeErrorEndStatus(HWREGH_READ_LRF(LRFD_BUFRAM_BASE + PBE_COMMON_RAM_O_ENDCAUSE));
2001 }
2002 else
2003 {
2004 cmd->status = endStatus;
2005 }
2006 rclEvents.lastCmdDone = 1;
2007 }
2008 else
2009 {
2010 /* Other events need to be handled unconditionally */
2011 }
2012 }
2013
2014 if (rclEvents.lastCmdDone != 0)
2015 {
2016 LRF_disable();
2017 LRF_disableSynthRefsys();
2018 }
2019 return (rclEvents);
2020 }
2021
2022 /*
2023 * ======== RCL_Handler_BLE_CS_PrecalDefaultCallback ========
2024 */
RCL_Handler_BLE_CS_PrecalDefaultCallback(RCL_CmdBleCs_PrecalTable * table,uint8_t channel,uint32_t * hdc,uint32_t * ldc)2025 void RCL_Handler_BLE_CS_PrecalDefaultCallback(RCL_CmdBleCs_PrecalTable *table, uint8_t channel, uint32_t *hdc, uint32_t *ldc)
2026 {
2027 *hdc = 0;
2028 *ldc = 0;
2029
2030 if (channel <= BLE_CS_MAX_CHANNEL)
2031 {
2032 /* Default channel blocks : 10-30-50-70 MHz */
2033 #define BLE_CS_PRECAL_BIN_WIDTH 20
2034
2035 uint8_t k = channel / BLE_CS_PRECAL_BIN_WIDTH;
2036
2037 if (k < table->numEntries)
2038 {
2039 /* Populate the DC measured with high gain */
2040 uint32_t i = table->entries[k].hdc.i;
2041 uint32_t q = table->entries[k].hdc.q;
2042 *hdc = (q << 16 | i);
2043
2044 i = table->entries[k].ldc.i;
2045 q = table->entries[k].ldc.q;
2046 *ldc = (q << 16 | i);
2047 }
2048 }
2049 }
2050
