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