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