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