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_timer.h>
13 #include <hal/nrf_usbd.h>
14 #include <hal/nrf_i2s.h>
15 #include <helpers/nrfx_gppi.h>
16
17 LOG_MODULE_REGISTER(feedback, LOG_LEVEL_INF);
18
19 #define FEEDBACK_TIMER_INSTANCE_NUMBER 2
20 #define FEEDBACK_TIMER_USBD_SOF_CAPTURE 0
21 #define FEEDBACK_TIMER_I2S_FRAMESTART_CAPTURE 1
22
23 static const nrfx_timer_t feedback_timer_instance =
24 NRFX_TIMER_INSTANCE(FEEDBACK_TIMER_INSTANCE_NUMBER);
25
26 /* While it might be possible to determine I2S FRAMESTART to USB SOF offset
27 * entirely in software, the I2S API lacks appropriate timestamping. Therefore
28 * this sample uses target-specific code to perform the measurements. Note that
29 * the use of dedicated target-specific peripheral essentially eliminates
30 * software scheduling jitter and it is likely that a pure software only
31 * solution would require additional filtering in indirect offset measurements.
32 *
33 * Use timer clock (independent from both Audio clock and USB host SOF clock)
34 * values directly to determine samples offset. This works fine because the
35 * regulator cares only about error (SOF offset is both error and regulator
36 * input) and achieves its goal by sending nominal + 1 or nominal - 1 samples.
37 * SOF offset is around 0 when regulated and therefore the relative clock
38 * frequency discrepancies are essentially negligible.
39 */
40 #define CLKS_PER_SAMPLE (16000000 / (SAMPLES_PER_SOF * 1000))
41
42 static struct feedback_ctx {
43 int32_t rel_sof_offset;
44 int32_t base_sof_offset;
45 } fb_ctx;
46
feedback_init(void)47 struct feedback_ctx *feedback_init(void)
48 {
49 nrfx_err_t err;
50 uint8_t usbd_sof_gppi_channel;
51 uint8_t i2s_framestart_gppi_channel;
52 const nrfx_timer_config_t cfg = {
53 .frequency = NRFX_MHZ_TO_HZ(16UL),
54 .mode = NRF_TIMER_MODE_TIMER,
55 .bit_width = NRF_TIMER_BIT_WIDTH_32,
56 .interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY,
57 .p_context = NULL,
58 };
59
60 feedback_reset_ctx(&fb_ctx);
61
62 err = nrfx_timer_init(&feedback_timer_instance, &cfg, NULL);
63 if (err != NRFX_SUCCESS) {
64 LOG_ERR("nrfx timer init error - Return value: %d", err);
65 return &fb_ctx;
66 }
67
68 /* Subscribe TIMER CAPTURE task to USBD SOF event */
69 err = nrfx_gppi_channel_alloc(&usbd_sof_gppi_channel);
70 if (err != NRFX_SUCCESS) {
71 LOG_ERR("gppi_channel_alloc failed with: %d\n", err);
72 return &fb_ctx;
73 }
74
75 nrfx_gppi_channel_endpoints_setup(usbd_sof_gppi_channel,
76 nrf_usbd_event_address_get(NRF_USBD, NRF_USBD_EVENT_SOF),
77 nrfx_timer_capture_task_address_get(&feedback_timer_instance,
78 FEEDBACK_TIMER_USBD_SOF_CAPTURE));
79 nrfx_gppi_fork_endpoint_setup(usbd_sof_gppi_channel,
80 nrfx_timer_task_address_get(&feedback_timer_instance,
81 NRF_TIMER_TASK_CLEAR));
82
83 nrfx_gppi_channels_enable(BIT(usbd_sof_gppi_channel));
84
85 /* Subscribe TIMER CAPTURE task to I2S FRAMESTART event */
86 err = nrfx_gppi_channel_alloc(&i2s_framestart_gppi_channel);
87 if (err != NRFX_SUCCESS) {
88 LOG_ERR("gppi_channel_alloc failed with: %d\n", err);
89 return &fb_ctx;
90 }
91
92 nrfx_gppi_channel_endpoints_setup(i2s_framestart_gppi_channel,
93 nrf_i2s_event_address_get(NRF_I2S0, NRF_I2S_EVENT_FRAMESTART),
94 nrfx_timer_capture_task_address_get(&feedback_timer_instance,
95 FEEDBACK_TIMER_I2S_FRAMESTART_CAPTURE));
96
97 nrfx_gppi_channels_enable(BIT(i2s_framestart_gppi_channel));
98
99 /* Enable feedback timer */
100 nrfx_timer_enable(&feedback_timer_instance);
101
102 return &fb_ctx;
103 }
104
update_sof_offset(struct feedback_ctx * ctx,uint32_t sof_cc,uint32_t framestart_cc)105 static void update_sof_offset(struct feedback_ctx *ctx, uint32_t sof_cc,
106 uint32_t framestart_cc)
107 {
108 int sof_offset;
109
110 /* /2 because we treat the middle as a turning point from being
111 * "too late" to "too early".
112 */
113 if (framestart_cc > (SAMPLES_PER_SOF * CLKS_PER_SAMPLE)/2) {
114 sof_offset = framestart_cc - SAMPLES_PER_SOF * CLKS_PER_SAMPLE;
115 } else {
116 sof_offset = framestart_cc;
117 }
118
119 /* The heuristic above is not enough when the offset gets too large.
120 * If the sign of the simple heuristic changes, check whether the offset
121 * crossed through the zero or the outer bound.
122 */
123 if ((ctx->rel_sof_offset >= 0) != (sof_offset >= 0)) {
124 uint32_t abs_diff;
125 int32_t base_change;
126
127 if (sof_offset >= 0) {
128 abs_diff = sof_offset - ctx->rel_sof_offset;
129 base_change = -(SAMPLES_PER_SOF * CLKS_PER_SAMPLE);
130 } else {
131 abs_diff = ctx->rel_sof_offset - sof_offset;
132 base_change = SAMPLES_PER_SOF * CLKS_PER_SAMPLE;
133 }
134
135 /* Adjust base offset only if the change happened through the
136 * outer bound. The actual changes should be significantly lower
137 * than the threshold here.
138 */
139 if (abs_diff > (SAMPLES_PER_SOF * CLKS_PER_SAMPLE)/2) {
140 ctx->base_sof_offset += base_change;
141 }
142 }
143
144 ctx->rel_sof_offset = sof_offset;
145 }
146
feedback_process(struct feedback_ctx * ctx)147 void feedback_process(struct feedback_ctx *ctx)
148 {
149 uint32_t sof_cc;
150 uint32_t framestart_cc;
151
152 sof_cc = nrfx_timer_capture_get(&feedback_timer_instance,
153 FEEDBACK_TIMER_USBD_SOF_CAPTURE);
154 framestart_cc = nrfx_timer_capture_get(&feedback_timer_instance,
155 FEEDBACK_TIMER_I2S_FRAMESTART_CAPTURE);
156
157 update_sof_offset(ctx, sof_cc, framestart_cc);
158 }
159
feedback_reset_ctx(struct feedback_ctx * ctx)160 void feedback_reset_ctx(struct feedback_ctx *ctx)
161 {
162 ARG_UNUSED(ctx);
163 }
164
feedback_start(struct feedback_ctx * ctx,int i2s_blocks_queued)165 void feedback_start(struct feedback_ctx *ctx, int i2s_blocks_queued)
166 {
167 /* I2S data was supposed to go out at SOF, but it is inevitably
168 * delayed due to triggering I2S start by software. Set relative
169 * SOF offset value in a way that ensures that values past "half
170 * frame" are treated as "too late" instead of "too early"
171 */
172 ctx->rel_sof_offset = (SAMPLES_PER_SOF * CLKS_PER_SAMPLE) / 2;
173 /* If there are more than 2 I2S TX blocks queued, use feedback regulator
174 * to correct the situation.
175 */
176 ctx->base_sof_offset = (i2s_blocks_queued - 2) *
177 (SAMPLES_PER_SOF * CLKS_PER_SAMPLE);
178 }
179
feedback_samples_offset(struct feedback_ctx * ctx)180 int feedback_samples_offset(struct feedback_ctx *ctx)
181 {
182 int32_t offset = ctx->rel_sof_offset + ctx->base_sof_offset;
183
184 return offset / CLKS_PER_SAMPLE;
185 }
186
