1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdlib.h>
8 #include <zephyr/logging/log.h>
9 #include "feedback.h"
10
11 #include <nrfx_dppi.h>
12 #include <nrfx_gpiote.h>
13 #include <nrfx_timer.h>
14 #include <hal/nrf_gpio.h>
15 #include <helpers/nrfx_gppi.h>
16
17 LOG_MODULE_REGISTER(feedback, LOG_LEVEL_INF);
18
19 #define FEEDBACK_TIMER_USBD_SOF_CAPTURE 0
20 #define FEEDBACK_TIMER_I2S_FRAMESTART_CAPTURE 1
21
22 #if IS_ENABLED(CONFIG_SOC_COMPATIBLE_NRF5340_CPUAPP)
23
24 #include <hal/nrf_usbd.h>
25 #include <hal/nrf_i2s.h>
26
27 #define FEEDBACK_PIN NRF_GPIO_PIN_MAP(1, 9)
28 #define FEEDBACK_GPIOTE_INSTANCE_NUMBER 0
29 #define FEEDBACK_TIMER_INSTANCE_NUMBER 2
30 #define USB_SOF_EVENT_ADDRESS nrf_usbd_event_address_get(NRF_USBD, NRF_USBD_EVENT_SOF)
31 #define I2S_FRAMESTART_EVENT_ADDRESS nrf_i2s_event_address_get(NRF_I2S0, NRF_I2S_EVENT_FRAMESTART)
32
feedback_target_init(void)33 static inline void feedback_target_init(void)
34 {
35 if (IS_ENABLED(CONFIG_APP_USE_I2S_LRCLK_EDGES_COUNTER)) {
36 /* App core is using feedback pin */
37 nrf_gpio_pin_control_select(FEEDBACK_PIN, NRF_GPIO_PIN_SEL_APP);
38 }
39 }
40
41 #elif IS_ENABLED(CONFIG_SOC_SERIES_NRF54HX)
42
43 #include <hal/nrf_tdm.h>
44
45 #define FEEDBACK_PIN NRF_GPIO_PIN_MAP(0, 8)
46 #define FEEDBACK_GPIOTE_INSTANCE_NUMBER 130
47 #define FEEDBACK_TIMER_INSTANCE_NUMBER 131
48 #define USB_SOF_EVENT_ADDRESS nrf_timer_event_address_get(NRF_TIMER131, NRF_TIMER_EVENT_COMPARE5)
49 #define I2S_FRAMESTART_EVENT_ADDRESS nrf_tdm_event_address_get(NRF_TDM130, NRF_TDM_EVENT_MAXCNT)
50
feedback_target_init(void)51 static inline void feedback_target_init(void)
52 {
53 /* Enable Start-of-Frame workaround in TIMER131 */
54 *(volatile uint32_t *)0x5F9A3C04 = 0x00000002;
55 *(volatile uint32_t *)0x5F9A3C04 = 0x00000003;
56 *(volatile uint32_t *)0x5F9A3C80 = 0x00000082;
57 }
58
59 #else
60 #error "Unsupported target"
61 #endif
62
63 static const nrfx_gpiote_t gpiote =
64 NRFX_GPIOTE_INSTANCE(FEEDBACK_GPIOTE_INSTANCE_NUMBER);
65
66 static const nrfx_timer_t feedback_timer_instance =
67 NRFX_TIMER_INSTANCE(FEEDBACK_TIMER_INSTANCE_NUMBER);
68
69 /* See 5.12.4.2 Feedback in Universal Serial Bus Specification Revision 2.0 for
70 * more information about the feedback. There is a direct implementation of the
71 * specification where P=1 when @kconfig{CONFIG_APP_USE_I2S_LRCLK_EDGES_COUNTER}
72 * is enabled, because I2S LRCLK edges (and not the clock) are being counted by
73 * a timer. Otherwise, when @kconfig{CONFIG_APP_USE_I2S_LRCLK_EDGES_COUNTER} is
74 * disabled, we are faking P=5 value using indirect offset measurements and
75 * we use such estimate on PI controller updated on every SOF.
76 *
77 * While it might be possible to determine I2S FRAMESTART to USB SOF offset
78 * entirely in software, the I2S API lacks appropriate timestamping. Therefore
79 * this sample uses target-specific code to perform the measurements. Note that
80 * the use of dedicated target-specific peripheral essentially eliminates
81 * software scheduling jitter and it is likely that a pure software only
82 * solution would require additional filtering in indirect offset measurements.
83 *
84 * Full-Speed isochronous feedback is Q10.10 unsigned integer left-justified in
85 * the 24-bits so it has Q10.14 format. This sample application puts zeroes to
86 * the 4 least significant bits (does not use the bits for extra precision).
87 */
88 #define FEEDBACK_K 10
89 #if defined(CONFIG_APP_USE_I2S_LRCLK_EDGES_COUNTER)
90 #define FEEDBACK_P 1
91 #else
92 #define FEEDBACK_P 5
93 #endif
94
95 #define FEEDBACK_FS_SHIFT 4
96
97 static struct feedback_ctx {
98 uint32_t fb_value;
99 int32_t rel_sof_offset;
100 int32_t base_sof_offset;
101 union {
102 /* For edge counting */
103 struct {
104 uint32_t fb_counter;
105 uint16_t fb_periods;
106 };
107 /* For PI controller */
108 int32_t integrator;
109 };
110 } fb_ctx;
111
feedback_edge_counter_setup(void)112 static nrfx_err_t feedback_edge_counter_setup(void)
113 {
114 nrfx_err_t err;
115 uint8_t feedback_gpiote_channel;
116 uint8_t feedback_gppi_channel;
117 nrfx_gpiote_trigger_config_t trigger_config = {
118 .trigger = NRFX_GPIOTE_TRIGGER_TOGGLE,
119 .p_in_channel = &feedback_gpiote_channel,
120 };
121 nrf_gpio_pin_pull_t pull = NRF_GPIO_PIN_PULLUP;
122 nrfx_gpiote_input_pin_config_t input_pin_config = {
123 .p_pull_config = &pull,
124 .p_trigger_config = &trigger_config,
125 };
126
127 err = nrfx_gpiote_channel_alloc(&gpiote, &feedback_gpiote_channel);
128 if (err != NRFX_SUCCESS) {
129 return err;
130 }
131
132 nrfx_gpiote_input_configure(&gpiote, FEEDBACK_PIN, &input_pin_config);
133 nrfx_gpiote_trigger_enable(&gpiote, FEEDBACK_PIN, false);
134
135 /* Configure TIMER in COUNTER mode */
136 const nrfx_timer_config_t cfg = {
137 .frequency = NRFX_MHZ_TO_HZ(1UL),
138 .mode = NRF_TIMER_MODE_COUNTER,
139 .bit_width = NRF_TIMER_BIT_WIDTH_32,
140 .interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY,
141 .p_context = NULL,
142 };
143
144 err = nrfx_timer_init(&feedback_timer_instance, &cfg, NULL);
145 if (err != NRFX_SUCCESS) {
146 LOG_ERR("nrfx timer init error (sample clk feedback) - Return value: %d", err);
147 return err;
148 }
149
150 /* Subscribe TIMER COUNT task to GPIOTE IN event */
151 err = nrfx_gppi_channel_alloc(&feedback_gppi_channel);
152 if (err != NRFX_SUCCESS) {
153 LOG_ERR("gppi_channel_alloc failed with: %d\n", err);
154 return err;
155 }
156
157 nrfx_gppi_channel_endpoints_setup(feedback_gppi_channel,
158 nrfx_gpiote_in_event_address_get(&gpiote, FEEDBACK_PIN),
159 nrfx_timer_task_address_get(&feedback_timer_instance, NRF_TIMER_TASK_COUNT));
160
161 nrfx_gppi_channels_enable(BIT(feedback_gppi_channel));
162
163 return NRFX_SUCCESS;
164 }
165
feedback_relative_timer_setup(void)166 static nrfx_err_t feedback_relative_timer_setup(void)
167 {
168 nrfx_err_t err;
169 const nrfx_timer_config_t cfg = {
170 .frequency = NRFX_MHZ_TO_HZ(16UL),
171 .mode = NRF_TIMER_MODE_TIMER,
172 .bit_width = NRF_TIMER_BIT_WIDTH_32,
173 .interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY,
174 .p_context = NULL,
175 };
176
177 err = nrfx_timer_init(&feedback_timer_instance, &cfg, NULL);
178 if (err != NRFX_SUCCESS) {
179 LOG_ERR("nrfx timer init error (relative timer) - Return value: %d", err);
180 }
181
182 return err;
183 }
184
feedback_init(void)185 struct feedback_ctx *feedback_init(void)
186 {
187 nrfx_err_t err;
188 uint8_t usbd_sof_gppi_channel;
189 uint8_t i2s_framestart_gppi_channel;
190
191 feedback_target_init();
192
193 feedback_reset_ctx(&fb_ctx);
194
195 if (IS_ENABLED(CONFIG_APP_USE_I2S_LRCLK_EDGES_COUNTER)) {
196 err = feedback_edge_counter_setup();
197 } else {
198 err = feedback_relative_timer_setup();
199 }
200
201 if (err != NRFX_SUCCESS) {
202 return &fb_ctx;
203 }
204
205 /* Subscribe TIMER CAPTURE task to USBD SOF event */
206 err = nrfx_gppi_channel_alloc(&usbd_sof_gppi_channel);
207 if (err != NRFX_SUCCESS) {
208 LOG_ERR("gppi_channel_alloc failed with: %d\n", err);
209 return &fb_ctx;
210 }
211
212 nrfx_gppi_channel_endpoints_setup(usbd_sof_gppi_channel,
213 USB_SOF_EVENT_ADDRESS,
214 nrfx_timer_capture_task_address_get(&feedback_timer_instance,
215 FEEDBACK_TIMER_USBD_SOF_CAPTURE));
216 nrfx_gppi_fork_endpoint_setup(usbd_sof_gppi_channel,
217 nrfx_timer_task_address_get(&feedback_timer_instance,
218 NRF_TIMER_TASK_CLEAR));
219
220 nrfx_gppi_channels_enable(BIT(usbd_sof_gppi_channel));
221
222 /* Subscribe TIMER CAPTURE task to I2S FRAMESTART event */
223 err = nrfx_gppi_channel_alloc(&i2s_framestart_gppi_channel);
224 if (err != NRFX_SUCCESS) {
225 LOG_ERR("gppi_channel_alloc failed with: %d\n", err);
226 return &fb_ctx;
227 }
228
229 nrfx_gppi_channel_endpoints_setup(i2s_framestart_gppi_channel,
230 I2S_FRAMESTART_EVENT_ADDRESS,
231 nrfx_timer_capture_task_address_get(&feedback_timer_instance,
232 FEEDBACK_TIMER_I2S_FRAMESTART_CAPTURE));
233
234 nrfx_gppi_channels_enable(BIT(i2s_framestart_gppi_channel));
235
236 /* Enable feedback timer */
237 nrfx_timer_enable(&feedback_timer_instance);
238
239 return &fb_ctx;
240 }
241
update_sof_offset(struct feedback_ctx * ctx,uint32_t sof_cc,uint32_t framestart_cc)242 static void update_sof_offset(struct feedback_ctx *ctx, uint32_t sof_cc,
243 uint32_t framestart_cc)
244 {
245 int sof_offset;
246
247 if (!IS_ENABLED(CONFIG_APP_USE_I2S_LRCLK_EDGES_COUNTER)) {
248 uint32_t clks_per_edge;
249
250 /* Convert timer clock (independent from both Audio clock and
251 * USB host SOF clock) to fake sample clock shifted by P values.
252 * This works fine because the regulator cares only about error
253 * (SOF offset is both error and regulator input) and achieves
254 * its goal by adjusting feedback value. SOF offset is around 0
255 * when regulated and therefore the relative clock frequency
256 * discrepancies are essentially negligible.
257 */
258 clks_per_edge = sof_cc / (SAMPLES_PER_SOF << FEEDBACK_P);
259 sof_cc /= MAX(clks_per_edge, 1);
260 framestart_cc /= MAX(clks_per_edge, 1);
261 }
262
263 /* /2 because we treat the middle as a turning point from being
264 * "too late" to "too early".
265 */
266 if (framestart_cc > (SAMPLES_PER_SOF << FEEDBACK_P)/2) {
267 sof_offset = framestart_cc - (SAMPLES_PER_SOF << FEEDBACK_P);
268 } else {
269 sof_offset = framestart_cc;
270 }
271
272 /* The heuristic above is not enough when the offset gets too large.
273 * If the sign of the simple heuristic changes, check whether the offset
274 * crossed through the zero or the outer bound.
275 */
276 if ((ctx->rel_sof_offset >= 0) != (sof_offset >= 0)) {
277 uint32_t abs_diff;
278 int32_t base_change;
279
280 if (sof_offset >= 0) {
281 abs_diff = sof_offset - ctx->rel_sof_offset;
282 base_change = -(SAMPLES_PER_SOF << FEEDBACK_P);
283 } else {
284 abs_diff = ctx->rel_sof_offset - sof_offset;
285 base_change = SAMPLES_PER_SOF << FEEDBACK_P;
286 }
287
288 /* Adjust base offset only if the change happened through the
289 * outer bound. The actual changes should be significantly lower
290 * than the threshold here.
291 */
292 if (abs_diff > (SAMPLES_PER_SOF << FEEDBACK_P)/2) {
293 ctx->base_sof_offset += base_change;
294 }
295 }
296
297 ctx->rel_sof_offset = sof_offset;
298 }
299
offset_to_correction(int32_t offset)300 static inline int32_t offset_to_correction(int32_t offset)
301 {
302 return -(offset / BIT(FEEDBACK_P)) * BIT(FEEDBACK_FS_SHIFT);
303 }
304
pi_update(struct feedback_ctx * ctx)305 static int32_t pi_update(struct feedback_ctx *ctx)
306 {
307 int32_t sof_offset = ctx->rel_sof_offset + ctx->base_sof_offset;
308 /* SOF offset is measured in pow(2, -FEEDBACK_P) samples, i.e. when
309 * FEEDBACK_P is 0, offset is in samples, and for 1 -> half-samples,
310 * 2 -> quarter-samples, 3 -> eightth-samples and so on.
311 * In order to simplify the PI controller description here, normalize
312 * the offset to 1/1024 samples (alternatively it can be treated as
313 * samples in Q10 fixed point format) and use it as Process Variable.
314 */
315 int32_t PV = BIT(10 - FEEDBACK_P) * sof_offset;
316 /* The control goal is to keep I2S FRAMESTART as close as possible to
317 * USB SOF and therefore Set Point is 0.
318 */
319 int32_t SP = 0;
320 int32_t error = SP - PV;
321
322 /*
323 * With above normalization at Full-Speed, when data received during
324 * SOF n appears on I2S during SOF n+3, the Ziegler Nichols Ultimate
325 * Gain is around 1.15 and the oscillation period is around 90 SOF.
326 * (much nicer oscillations with 204.8 SOF period can be observed with
327 * gain 0.5 when the delay is not n+3, but n+33 - surprisingly the
328 * resulting PI coefficients after power of two rounding are the same).
329 *
330 * Ziegler-Nichols rule with applied stability margin of 2 results in:
331 * Kc = 0.22 * Ku = 0.22 * 1.15 = 0.253
332 * Ti = 0.83 * tu = 0.83 * 80 = 66.4
333 *
334 * Converting the rules above to parallel PI gives:
335 * Kp = Kc = 0.253
336 * Ki = Kc/Ti = 0.254/66.4 ~= 0.0038253
337 *
338 * Because we want fixed-point optimized non-tunable implementation,
339 * the parameters can be conveniently expressed with power of two:
340 * Kp ~= pow(2, -2) = 0.25 (divide by 4)
341 * Ki ~= pow(2, -8) = 0.0039 (divide by 256)
342 *
343 * This can be implemented as:
344 * ctx->integrator += error;
345 * return (error + (ctx->integrator / 64)) / 4;
346 * but unfortunately such regulator is pretty aggressive and keeps
347 * oscillating rather quickly around the setpoint (within +-1 sample).
348 *
349 * Manually tweaking the constants so the regulator output is shifted
350 * down by 4 bits (i.e. change /64 to /2048 and /4 to /128) yields
351 * really good results (the outcome is similar, even slightly better,
352 * than using I2S LRCLK edge counting directly).
353 */
354 ctx->integrator += error;
355 return (error + (ctx->integrator / 2048)) / 128;
356 }
357
feedback_process(struct feedback_ctx * ctx)358 void feedback_process(struct feedback_ctx *ctx)
359 {
360 uint32_t sof_cc;
361 uint32_t framestart_cc;
362 uint32_t fb;
363
364 sof_cc = nrfx_timer_capture_get(&feedback_timer_instance,
365 FEEDBACK_TIMER_USBD_SOF_CAPTURE);
366 framestart_cc = nrfx_timer_capture_get(&feedback_timer_instance,
367 FEEDBACK_TIMER_I2S_FRAMESTART_CAPTURE);
368
369 update_sof_offset(ctx, sof_cc, framestart_cc);
370
371 if (IS_ENABLED(CONFIG_APP_USE_I2S_LRCLK_EDGES_COUNTER)) {
372 int32_t offset = ctx->rel_sof_offset + ctx->base_sof_offset;
373
374 ctx->fb_counter += sof_cc;
375 ctx->fb_periods++;
376
377 if (ctx->fb_periods == BIT(FEEDBACK_K - FEEDBACK_P)) {
378
379 /* fb_counter holds Q10.10 value, left-justify it */
380 fb = ctx->fb_counter << FEEDBACK_FS_SHIFT;
381
382 /* Align I2S FRAMESTART to USB SOF by adjusting reported
383 * feedback value. This is endpoint specific correction
384 * mentioned but not specified in USB 2.0 Specification.
385 */
386 if (abs(offset) > BIT(FEEDBACK_P)) {
387 fb += offset_to_correction(offset);
388 }
389
390 ctx->fb_value = fb;
391 ctx->fb_counter = 0;
392 ctx->fb_periods = 0;
393 }
394 } else {
395 /* Use PI controller to generate required feedback deviation
396 * from nominal feedback value.
397 */
398 fb = SAMPLES_PER_SOF << (FEEDBACK_K + FEEDBACK_FS_SHIFT);
399 /* Clear the additional LSB bits in feedback value, i.e. do not
400 * use the optional extra resolution.
401 */
402 fb += pi_update(ctx) & ~0xF;
403 ctx->fb_value = fb;
404 }
405 }
406
feedback_reset_ctx(struct feedback_ctx * ctx)407 void feedback_reset_ctx(struct feedback_ctx *ctx)
408 {
409 /* Reset feedback to nominal value */
410 ctx->fb_value = SAMPLES_PER_SOF << (FEEDBACK_K + FEEDBACK_FS_SHIFT);
411 if (IS_ENABLED(CONFIG_APP_USE_I2S_LRCLK_EDGES_COUNTER)) {
412 ctx->fb_counter = 0;
413 ctx->fb_periods = 0;
414 } else {
415 ctx->integrator = 0;
416 }
417 }
418
feedback_start(struct feedback_ctx * ctx,int i2s_blocks_queued)419 void feedback_start(struct feedback_ctx *ctx, int i2s_blocks_queued)
420 {
421 /* I2S data was supposed to go out at SOF, but it is inevitably
422 * delayed due to triggering I2S start by software. Set relative
423 * SOF offset value in a way that ensures that values past "half
424 * frame" are treated as "too late" instead of "too early"
425 */
426 ctx->rel_sof_offset = (SAMPLES_PER_SOF << FEEDBACK_P) / 2;
427 /* If there are more than 2 I2S blocks queued, use feedback regulator
428 * to correct the situation.
429 */
430 ctx->base_sof_offset = (i2s_blocks_queued - 2) *
431 (SAMPLES_PER_SOF << FEEDBACK_P);
432 }
433
feedback_value(struct feedback_ctx * ctx)434 uint32_t feedback_value(struct feedback_ctx *ctx)
435 {
436 return ctx->fb_value;
437 }
438