1 /*
2 * Copyright (c) 2025, sakumisu
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 #include "usbd_core.h"
7 #include "usbd_cdc_acm.h"
8 #include "chry_ringbuffer.h"
9 #include <mavlink.h>
10
11 /*!< endpoint address */
12 #define CDC_IN_EP 0x81
13 #define CDC_OUT_EP 0x02
14 #define CDC_INT_EP 0x83
15
16 #define USBD_VID 0xFFFF
17 #define USBD_PID 0xFFFF
18 #define USBD_MAX_POWER 100
19 #define USBD_LANGID_STRING 1033
20
21 /*!< config descriptor size */
22 #define USB_CONFIG_SIZE (9 + CDC_ACM_DESCRIPTOR_LEN)
23
24 #ifdef CONFIG_USB_HS
25 #define CDC_MAX_MPS 512
26 #else
27 #define CDC_MAX_MPS 64
28 #endif
29
30 #ifdef CONFIG_USBDEV_ADVANCE_DESC
31 static const uint8_t device_descriptor[] = {
32 USB_DEVICE_DESCRIPTOR_INIT(USB_2_0, 0xEF, 0x02, 0x01, USBD_VID, USBD_PID, 0x0100, 0x01)
33 };
34
35 static const uint8_t config_descriptor[] = {
36 USB_CONFIG_DESCRIPTOR_INIT(USB_CONFIG_SIZE, 0x02, 0x01, USB_CONFIG_BUS_POWERED, USBD_MAX_POWER),
37 CDC_ACM_DESCRIPTOR_INIT(0x00, CDC_INT_EP, CDC_OUT_EP, CDC_IN_EP, CDC_MAX_MPS, 0x02)
38 };
39
40 static const uint8_t device_quality_descriptor[] = {
41 ///////////////////////////////////////
42 /// device qualifier descriptor
43 ///////////////////////////////////////
44 0x0a,
45 USB_DESCRIPTOR_TYPE_DEVICE_QUALIFIER,
46 0x00,
47 0x02,
48 0x00,
49 0x00,
50 0x00,
51 0x40,
52 0x00,
53 0x00,
54 };
55
56 static const char *string_descriptors[] = {
57 (const char[]){ 0x09, 0x04 }, /* Langid */
58 "CherryUSB", /* Manufacturer */
59 "CherryUSB CDC DEMO", /* Product */
60 "2022123456", /* Serial Number */
61 };
62
device_descriptor_callback(uint8_t speed)63 static const uint8_t *device_descriptor_callback(uint8_t speed)
64 {
65 return device_descriptor;
66 }
67
config_descriptor_callback(uint8_t speed)68 static const uint8_t *config_descriptor_callback(uint8_t speed)
69 {
70 return config_descriptor;
71 }
72
device_quality_descriptor_callback(uint8_t speed)73 static const uint8_t *device_quality_descriptor_callback(uint8_t speed)
74 {
75 return device_quality_descriptor;
76 }
77
string_descriptor_callback(uint8_t speed,uint8_t index)78 static const char *string_descriptor_callback(uint8_t speed, uint8_t index)
79 {
80 if (index > 3) {
81 return NULL;
82 }
83 return string_descriptors[index];
84 }
85
86 const struct usb_descriptor cdc_descriptor = {
87 .device_descriptor_callback = device_descriptor_callback,
88 .config_descriptor_callback = config_descriptor_callback,
89 .device_quality_descriptor_callback = device_quality_descriptor_callback,
90 .string_descriptor_callback = string_descriptor_callback
91 };
92 #else
93 /*!< global descriptor */
94 static const uint8_t cdc_descriptor[] = {
95 USB_DEVICE_DESCRIPTOR_INIT(USB_2_0, 0xEF, 0x02, 0x01, USBD_VID, USBD_PID, 0x0100, 0x01),
96 USB_CONFIG_DESCRIPTOR_INIT(USB_CONFIG_SIZE, 0x02, 0x01, USB_CONFIG_BUS_POWERED, USBD_MAX_POWER),
97 CDC_ACM_DESCRIPTOR_INIT(0x00, CDC_INT_EP, CDC_OUT_EP, CDC_IN_EP, CDC_MAX_MPS, 0x02),
98 ///////////////////////////////////////
99 /// string0 descriptor
100 ///////////////////////////////////////
101 USB_LANGID_INIT(USBD_LANGID_STRING),
102 ///////////////////////////////////////
103 /// string1 descriptor
104 ///////////////////////////////////////
105 0x14, /* bLength */
106 USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
107 'C', 0x00, /* wcChar0 */
108 'h', 0x00, /* wcChar1 */
109 'e', 0x00, /* wcChar2 */
110 'r', 0x00, /* wcChar3 */
111 'r', 0x00, /* wcChar4 */
112 'y', 0x00, /* wcChar5 */
113 'U', 0x00, /* wcChar6 */
114 'S', 0x00, /* wcChar7 */
115 'B', 0x00, /* wcChar8 */
116 ///////////////////////////////////////
117 /// string2 descriptor
118 ///////////////////////////////////////
119 0x26, /* bLength */
120 USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
121 'C', 0x00, /* wcChar0 */
122 'h', 0x00, /* wcChar1 */
123 'e', 0x00, /* wcChar2 */
124 'r', 0x00, /* wcChar3 */
125 'r', 0x00, /* wcChar4 */
126 'y', 0x00, /* wcChar5 */
127 'U', 0x00, /* wcChar6 */
128 'S', 0x00, /* wcChar7 */
129 'B', 0x00, /* wcChar8 */
130 ' ', 0x00, /* wcChar9 */
131 'C', 0x00, /* wcChar10 */
132 'D', 0x00, /* wcChar11 */
133 'C', 0x00, /* wcChar12 */
134 ' ', 0x00, /* wcChar13 */
135 'D', 0x00, /* wcChar14 */
136 'E', 0x00, /* wcChar15 */
137 'M', 0x00, /* wcChar16 */
138 'O', 0x00, /* wcChar17 */
139 ///////////////////////////////////////
140 /// string3 descriptor
141 ///////////////////////////////////////
142 0x16, /* bLength */
143 USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
144 '2', 0x00, /* wcChar0 */
145 '0', 0x00, /* wcChar1 */
146 '2', 0x00, /* wcChar2 */
147 '2', 0x00, /* wcChar3 */
148 '1', 0x00, /* wcChar4 */
149 '2', 0x00, /* wcChar5 */
150 '3', 0x00, /* wcChar6 */
151 '4', 0x00, /* wcChar7 */
152 '5', 0x00, /* wcChar8 */
153 '6', 0x00, /* wcChar9 */
154 #ifdef CONFIG_USB_HS
155 ///////////////////////////////////////
156 /// device qualifier descriptor
157 ///////////////////////////////////////
158 0x0a,
159 USB_DESCRIPTOR_TYPE_DEVICE_QUALIFIER,
160 0x00,
161 0x02,
162 0x00,
163 0x00,
164 0x00,
165 0x40,
166 0x00,
167 0x00,
168 #endif
169 0x00
170 };
171 #endif
172
173 chry_ringbuffer_t usb_rx_rb;
174 uint8_t usb_rx_buffer[2048];
175
176 USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t temp_rx_buffer[512];
177 USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t usb_tx_buffer[MAVLINK_MAX_PACKET_LEN];
178
179 volatile bool ep_tx_busy_flag = false;
180
usbd_event_handler(uint8_t busid,uint8_t event)181 static void usbd_event_handler(uint8_t busid, uint8_t event)
182 {
183 switch (event) {
184 case USBD_EVENT_RESET:
185 break;
186 case USBD_EVENT_CONNECTED:
187 break;
188 case USBD_EVENT_DISCONNECTED:
189 break;
190 case USBD_EVENT_RESUME:
191 break;
192 case USBD_EVENT_SUSPEND:
193 break;
194 case USBD_EVENT_CONFIGURED:
195 ep_tx_busy_flag = false;
196 /* setup first out ep read transfer */
197 usbd_ep_start_read(busid, CDC_OUT_EP, temp_rx_buffer, usbd_get_ep_mps(busid, CDC_OUT_EP));
198 break;
199 case USBD_EVENT_SET_REMOTE_WAKEUP:
200 break;
201 case USBD_EVENT_CLR_REMOTE_WAKEUP:
202 break;
203
204 default:
205 break;
206 }
207 }
208
usbd_cdc_acm_bulk_out(uint8_t busid,uint8_t ep,uint32_t nbytes)209 void usbd_cdc_acm_bulk_out(uint8_t busid, uint8_t ep, uint32_t nbytes)
210 {
211 USB_LOG_RAW("actual out len:%d\r\n", (unsigned int)nbytes);
212
213 chry_ringbuffer_write(&usb_rx_rb, temp_rx_buffer, nbytes);
214 usbd_ep_start_read(busid, CDC_OUT_EP, temp_rx_buffer, usbd_get_ep_mps(busid, CDC_OUT_EP));
215 }
216
usbd_cdc_acm_bulk_in(uint8_t busid,uint8_t ep,uint32_t nbytes)217 void usbd_cdc_acm_bulk_in(uint8_t busid, uint8_t ep, uint32_t nbytes)
218 {
219 USB_LOG_RAW("actual in len:%d\r\n", (unsigned int)nbytes);
220
221 if ((nbytes % usbd_get_ep_mps(busid, ep)) == 0 && nbytes) {
222 /* send zlp */
223 usbd_ep_start_write(busid, CDC_IN_EP, NULL, 0);
224 } else {
225 ep_tx_busy_flag = false;
226 }
227 }
228
229 /*!< endpoint call back */
230 struct usbd_endpoint cdc_out_ep = {
231 .ep_addr = CDC_OUT_EP,
232 .ep_cb = usbd_cdc_acm_bulk_out
233 };
234
235 struct usbd_endpoint cdc_in_ep = {
236 .ep_addr = CDC_IN_EP,
237 .ep_cb = usbd_cdc_acm_bulk_in
238 };
239
240 static struct usbd_interface intf0;
241 static struct usbd_interface intf1;
242
cdc_acm_mavlink_init(uint8_t busid,uintptr_t reg_base)243 void cdc_acm_mavlink_init(uint8_t busid, uintptr_t reg_base)
244 {
245 chry_ringbuffer_init(&usb_rx_rb, usb_rx_buffer, sizeof(usb_rx_buffer));
246 #ifdef CONFIG_USBDEV_ADVANCE_DESC
247 usbd_desc_register(busid, &cdc_descriptor);
248 #else
249 usbd_desc_register(busid, cdc_descriptor);
250 #endif
251 usbd_add_interface(busid, usbd_cdc_acm_init_intf(busid, &intf0));
252 usbd_add_interface(busid, usbd_cdc_acm_init_intf(busid, &intf1));
253 usbd_add_endpoint(busid, &cdc_out_ep);
254 usbd_add_endpoint(busid, &cdc_in_ep);
255 usbd_initialize(busid, reg_base, usbd_event_handler);
256 }
257
cdc_acm_mavlink_write(uint8_t * data,uint32_t len)258 void cdc_acm_mavlink_write(uint8_t *data, uint32_t len)
259 {
260 if (!usb_device_is_configured(0)) {
261 return;
262 }
263 ep_tx_busy_flag = true;
264 usbd_ep_start_write(0, CDC_IN_EP, data, len);
265 while (ep_tx_busy_flag) {
266 }
267 }
268
send_heartbeat(void)269 void send_heartbeat(void)
270 {
271 mavlink_message_t message;
272
273 const uint8_t system_id = 42;
274 const uint8_t base_mode = 0;
275 const uint8_t custom_mode = 0;
276 mavlink_msg_heartbeat_pack_chan(
277 system_id,
278 MAV_COMP_ID_PERIPHERAL,
279 MAVLINK_COMM_0,
280 &message,
281 MAV_TYPE_GENERIC,
282 MAV_AUTOPILOT_GENERIC,
283 base_mode,
284 custom_mode,
285 MAV_STATE_STANDBY);
286
287 const int len = mavlink_msg_to_send_buffer(usb_tx_buffer, &message);
288 cdc_acm_mavlink_write(usb_tx_buffer, len);
289 }
290
handle_heartbeat(const mavlink_message_t * message)291 void handle_heartbeat(const mavlink_message_t *message)
292 {
293 mavlink_heartbeat_t heartbeat;
294 mavlink_msg_heartbeat_decode(message, &heartbeat);
295
296 USB_LOG_RAW("Got heartbeat from ");
297 switch (heartbeat.autopilot) {
298 case MAV_AUTOPILOT_GENERIC:
299 USB_LOG_RAW("generic");
300 break;
301 case MAV_AUTOPILOT_ARDUPILOTMEGA:
302 USB_LOG_RAW("ArduPilot");
303 break;
304 case MAV_AUTOPILOT_PX4:
305 USB_LOG_RAW("PX4");
306 break;
307 default:
308 USB_LOG_RAW("other");
309 break;
310 }
311 USB_LOG_RAW(" autopilot\n");
312
313 send_heartbeat();
314 }
315
mavlink_polling(void)316 void mavlink_polling(void)
317 {
318 uint8_t ch;
319 bool ret;
320 mavlink_message_t message;
321 mavlink_status_t status;
322
323 ret = chry_ringbuffer_read_byte(&usb_rx_rb, &ch);
324 if (ret) {
325 if (mavlink_parse_char(MAVLINK_COMM_0, ch, &message, &status) == 1) {
326 USB_LOG_INFO(
327 "Received message %d from %d/%d\n",
328 message.msgid, message.sysid, message.compid);
329
330 switch (message.msgid) {
331 case MAVLINK_MSG_ID_HEARTBEAT:
332 handle_heartbeat(&message);
333 break;
334 }
335 }
336 }
337 }