1 /*
2 * Copyright (c) 2024 CogniPilot Foundation
3 * Copyright (c) 2024 NXP Semiconductors
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #define DT_DRV_COMPAT futaba_sbus
9
10 #include <zephyr/device.h>
11 #include <zephyr/input/input.h>
12 #include <zephyr/irq.h>
13 #include <zephyr/kernel.h>
14 #include <zephyr/logging/log.h>
15 #include <zephyr/sys/time_units.h>
16 #include <zephyr/sys_clock.h>
17 #include <zephyr/drivers/uart.h>
18
19 LOG_MODULE_REGISTER(futaba_sbus, CONFIG_INPUT_LOG_LEVEL);
20
21 /* Driver config */
22 struct sbus_input_channel {
23 uint32_t sbus_channel;
24 uint32_t type;
25 uint32_t zephyr_code;
26 };
27
28 const struct uart_config uart_cfg_sbus = {
29 .baudrate = 100000,
30 .parity = UART_CFG_PARITY_EVEN,
31 .stop_bits = UART_CFG_STOP_BITS_2,
32 .data_bits = UART_CFG_DATA_BITS_8,
33 .flow_ctrl = UART_CFG_FLOW_CTRL_NONE
34 };
35
36 struct input_sbus_config {
37 uint8_t num_channels;
38 const struct sbus_input_channel *channel_info;
39 const struct device *uart_dev;
40 uart_irq_callback_user_data_t cb;
41 };
42
43 #define SBUS_FRAME_LEN 25
44 #define SBUS_HEADER 0x0F
45 #define SBUS_FOOTER 0x00
46
47 #define SBUS_SERVO_LEN 22
48 #define SBUS_SERVO_CH_MASK 0x7FF
49
50 #define SBUS_BYTE24_IDX 23
51 #define SBUS_BYTE24_CH17 0x01
52 #define SBUS_BYTE24_CH18 0x02
53 #define SBUS_BYTE24_FRAME_LOST 0x04
54 #define SBUS_BYTE24_FAILSAFE 0x08
55
56 #define SBUS_TRANSMISSION_TIME_MS 4 /* Max transmission of a single SBUS frame */
57 #define SBUS_INTERFRAME_SPACING_MS 20 /* Max spacing between SBUS frames */
58 #define SBUS_CHANNEL_COUNT 16
59
60 #define REPORT_FILTER CONFIG_INPUT_SBUS_REPORT_FILTER
61 #define CHANNEL_VALUE_ZERO CONFIG_INPUT_SBUS_CHANNEL_VALUE_ZERO
62 #define CHANNEL_VALUE_ONE CONFIG_INPUT_SBUS_CHANNEL_VALUE_ONE
63
64 struct input_sbus_data {
65 struct k_thread thread;
66 struct k_sem report_lock;
67
68 uint16_t xfer_bytes;
69 uint8_t rd_data[SBUS_FRAME_LEN];
70 uint8_t sbus_frame[SBUS_FRAME_LEN];
71 bool partial_sync;
72 bool in_sync;
73 uint32_t last_rx_time;
74
75 uint16_t last_reported_value[SBUS_CHANNEL_COUNT];
76 int8_t channel_mapping[SBUS_CHANNEL_COUNT];
77
78 K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_INPUT_SBUS_THREAD_STACK_SIZE);
79 };
80
input_sbus_report(const struct device * dev,unsigned int sbus_channel,unsigned int value)81 static void input_sbus_report(const struct device *dev, unsigned int sbus_channel,
82 unsigned int value)
83 {
84 const struct input_sbus_config *const config = dev->config;
85 struct input_sbus_data *const data = dev->data;
86
87 int channel = data->channel_mapping[sbus_channel];
88
89 /* Not Mapped */
90 if (channel == -1) {
91 return;
92 }
93
94 if (value >= (data->last_reported_value[channel] + REPORT_FILTER) ||
95 value <= (data->last_reported_value[channel] - REPORT_FILTER)) {
96 switch (config->channel_info[channel].type) {
97 case INPUT_EV_ABS:
98 case INPUT_EV_MSC:
99 input_report(dev, config->channel_info[channel].type,
100 config->channel_info[channel].zephyr_code, value, false,
101 K_FOREVER);
102 break;
103
104 default:
105 if (value > CHANNEL_VALUE_ONE) {
106 input_report_key(dev, config->channel_info[channel].zephyr_code, 1,
107 false, K_FOREVER);
108 } else if (value < CHANNEL_VALUE_ZERO) {
109 input_report_key(dev, config->channel_info[channel].zephyr_code, 0,
110 false, K_FOREVER);
111 }
112 }
113 data->last_reported_value[channel] = value;
114 }
115 }
116
input_sbus_input_report_thread(const struct device * dev,void * dummy2,void * dummy3)117 static void input_sbus_input_report_thread(const struct device *dev, void *dummy2, void *dummy3)
118 {
119 struct input_sbus_data *const data = dev->data;
120
121 ARG_UNUSED(dummy2);
122 ARG_UNUSED(dummy3);
123
124 uint8_t i, channel;
125 uint8_t *sbus_channel_data = &data->sbus_frame[1]; /* Omit header */
126 uint16_t value;
127 int bits_read;
128 unsigned int key;
129 int ret;
130 bool connected_reported = false;
131
132 while (true) {
133 if (!data->in_sync) {
134 k_sem_take(&data->report_lock, K_FOREVER);
135 if (data->in_sync) {
136 LOG_DBG("SBUS receiver connected");
137 } else {
138 continue;
139 }
140 } else {
141 ret = k_sem_take(&data->report_lock, K_MSEC(SBUS_INTERFRAME_SPACING_MS));
142 if (ret == -EBUSY) {
143 continue;
144 } else if (ret < 0 || !data->in_sync) {
145 /* We've lost sync with the UART receiver */
146 key = irq_lock();
147
148 data->partial_sync = false;
149 data->in_sync = false;
150 data->xfer_bytes = 0;
151 irq_unlock(key);
152
153 connected_reported = false;
154 LOG_DBG("SBUS receiver connection lost");
155
156 /* Report connection lost */
157 continue;
158 }
159 }
160
161 if (connected_reported &&
162 data->sbus_frame[SBUS_BYTE24_IDX] & SBUS_BYTE24_FRAME_LOST) {
163 LOG_DBG("SBUS controller connection lost");
164 connected_reported = false;
165 } else if (!connected_reported &&
166 !(data->sbus_frame[SBUS_BYTE24_IDX] & SBUS_BYTE24_FRAME_LOST)) {
167 LOG_DBG("SBUS controller connected");
168 connected_reported = true;
169 }
170
171 /* Parse the data */
172 channel = 0;
173 value = 0;
174 bits_read = 0;
175
176 for (i = 0; i < SBUS_SERVO_LEN; i++) {
177 /* Read the next byte */
178 unsigned char byte = sbus_channel_data[i];
179
180 /* Extract bits and construct the 11-bit value */
181 value |= byte << bits_read;
182 bits_read += 8;
183
184 /* Check if we've read enough bits to form a full 11-bit value */
185 while (bits_read >= 11) {
186 input_sbus_report(dev, channel, value & SBUS_SERVO_CH_MASK);
187
188 /* Shift right to prepare for the next 11 bits */
189 value >>= 11;
190 bits_read -= 11;
191 channel++;
192 }
193 }
194
195 #ifdef CONFIG_INPUT_SBUS_SEND_SYNC
196 input_report(dev, 0, 0, 0, true, K_FOREVER);
197 #endif
198 }
199 }
200
sbus_resync(const struct device * uart_dev,struct input_sbus_data * const data)201 static void sbus_resync(const struct device *uart_dev, struct input_sbus_data *const data)
202 {
203 uint8_t *rd_data = data->rd_data;
204
205 if (data->partial_sync) {
206 data->xfer_bytes += uart_fifo_read(uart_dev, &rd_data[data->xfer_bytes],
207 SBUS_FRAME_LEN - data->xfer_bytes);
208 if (data->xfer_bytes == SBUS_FRAME_LEN) {
209 /* Transfer took longer then 4ms probably faulty */
210 if (k_uptime_get_32() - data->last_rx_time > SBUS_TRANSMISSION_TIME_MS) {
211 data->xfer_bytes = 0;
212 data->partial_sync = false;
213 } else if (rd_data[0] == SBUS_HEADER &&
214 rd_data[SBUS_FRAME_LEN - 1] == SBUS_FOOTER) {
215 data->in_sync = true;
216 } else {
217 /* Dummy read to clear fifo */
218 uart_fifo_read(uart_dev, &rd_data[0], 1);
219 data->xfer_bytes = 0;
220 data->partial_sync = false;
221 }
222 }
223 } else {
224 if (uart_fifo_read(uart_dev, &rd_data[0], 1) == 1) {
225 if (rd_data[0] == SBUS_HEADER) {
226 data->partial_sync = true;
227 data->xfer_bytes = 1;
228 data->last_rx_time = k_uptime_get_32();
229 }
230 }
231 }
232 }
233
sbus_uart_isr(const struct device * uart_dev,void * user_data)234 static void sbus_uart_isr(const struct device *uart_dev, void *user_data)
235 {
236 const struct device *dev = user_data;
237 struct input_sbus_data *const data = dev->data;
238 uint8_t *rd_data = data->rd_data;
239
240 if (uart_dev == NULL) {
241 LOG_DBG("UART device is NULL");
242 return;
243 }
244
245 if (!uart_irq_update(uart_dev)) {
246 LOG_DBG("Unable to start processing interrupts");
247 return;
248 }
249
250 while (uart_irq_rx_ready(uart_dev) && data->xfer_bytes <= SBUS_FRAME_LEN) {
251 if (data->in_sync) {
252 if (data->xfer_bytes == 0) {
253 data->last_rx_time = k_uptime_get_32();
254 }
255 data->xfer_bytes += uart_fifo_read(uart_dev, &rd_data[data->xfer_bytes],
256 SBUS_FRAME_LEN - data->xfer_bytes);
257 } else {
258 sbus_resync(uart_dev, data);
259 }
260 }
261
262 if (data->in_sync && (k_uptime_get_32() - data->last_rx_time >
263 SBUS_INTERFRAME_SPACING_MS)) {
264 data->partial_sync = false;
265 data->in_sync = false;
266 data->xfer_bytes = 0;
267 k_sem_give(&data->report_lock);
268 } else if (data->in_sync && data->xfer_bytes == SBUS_FRAME_LEN) {
269 data->xfer_bytes = 0;
270
271 if (rd_data[0] == SBUS_HEADER && rd_data[SBUS_FRAME_LEN - 1] == SBUS_FOOTER) {
272 memcpy(data->sbus_frame, rd_data, SBUS_FRAME_LEN);
273 k_sem_give(&data->report_lock);
274 } else {
275 data->partial_sync = false;
276 data->in_sync = false;
277 }
278 }
279 }
280
281 /*
282 * @brief Initialize sbus driver
283 */
input_sbus_init(const struct device * dev)284 static int input_sbus_init(const struct device *dev)
285 {
286 const struct input_sbus_config *const config = dev->config;
287 struct input_sbus_data *const data = dev->data;
288 int i, ret;
289
290 uart_irq_rx_disable(config->uart_dev);
291 uart_irq_tx_disable(config->uart_dev);
292
293 LOG_DBG("Initializing SBUS driver");
294
295 for (i = 0; i < SBUS_CHANNEL_COUNT; i++) {
296 data->last_reported_value[i] = 0;
297 data->channel_mapping[i] = -1;
298 }
299
300 data->xfer_bytes = 0;
301 data->in_sync = false;
302 data->partial_sync = false;
303 data->last_rx_time = 0;
304
305 for (i = 0; i < config->num_channels; i++) {
306 data->channel_mapping[config->channel_info[i].sbus_channel - 1] = i;
307 }
308
309 ret = uart_configure(config->uart_dev, &uart_cfg_sbus);
310 if (ret < 0) {
311 LOG_ERR("Unable to configure UART port: %d", ret);
312 return ret;
313 }
314
315 ret = uart_irq_callback_user_data_set(config->uart_dev, config->cb, (void *)dev);
316 if (ret < 0) {
317 if (ret == -ENOTSUP) {
318 LOG_ERR("Interrupt-driven UART API support not enabled");
319 } else if (ret == -ENOSYS) {
320 LOG_ERR("UART device does not support interrupt-driven API");
321 } else {
322 LOG_ERR("Error setting UART callback: %d", ret);
323 }
324 return ret;
325 }
326
327 uart_irq_rx_enable(config->uart_dev);
328
329 k_sem_init(&data->report_lock, 0, 1);
330
331 k_thread_create(&data->thread, data->thread_stack,
332 K_KERNEL_STACK_SIZEOF(data->thread_stack),
333 (k_thread_entry_t)input_sbus_input_report_thread, (void *)dev, NULL, NULL,
334 CONFIG_INPUT_SBUS_THREAD_PRIORITY, 0, K_NO_WAIT);
335
336 k_thread_name_set(&data->thread, dev->name);
337
338 return ret;
339 }
340
341 #define INPUT_CHANNEL_CHECK(input_channel_id) \
342 BUILD_ASSERT(IN_RANGE(DT_PROP(input_channel_id, channel), 1, 16), \
343 "invalid channel number"); \
344 BUILD_ASSERT(DT_PROP(input_channel_id, type) == INPUT_EV_ABS || \
345 DT_PROP(input_channel_id, type) == INPUT_EV_KEY || \
346 DT_PROP(input_channel_id, type) == INPUT_EV_MSC, \
347 "invalid channel type");
348
349 #define SBUS_INPUT_CHANNEL_INITIALIZER(input_channel_id) \
350 { \
351 .sbus_channel = DT_PROP(input_channel_id, channel), \
352 .type = DT_PROP(input_channel_id, type), \
353 .zephyr_code = DT_PROP(input_channel_id, zephyr_code), \
354 },
355
356 #define INPUT_SBUS_INIT(n) \
357 \
358 static const struct sbus_input_channel input_##n[] = { \
359 DT_INST_FOREACH_CHILD(n, SBUS_INPUT_CHANNEL_INITIALIZER) \
360 }; \
361 DT_INST_FOREACH_CHILD(n, INPUT_CHANNEL_CHECK) \
362 \
363 static struct input_sbus_data sbus_data_##n; \
364 \
365 static const struct input_sbus_config sbus_cfg_##n = { \
366 .channel_info = input_##n, \
367 .uart_dev = DEVICE_DT_GET(DT_INST_BUS(n)), \
368 .num_channels = ARRAY_SIZE(input_##n), \
369 .cb = sbus_uart_isr, \
370 }; \
371 \
372 DEVICE_DT_INST_DEFINE(n, input_sbus_init, NULL, &sbus_data_##n, &sbus_cfg_##n, \
373 POST_KERNEL, CONFIG_INPUT_INIT_PRIORITY, NULL);
374
375 DT_INST_FOREACH_STATUS_OKAY(INPUT_SBUS_INIT)
376
