1 /*
2 * Copyright (c) 2022, sakumisu
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 #include "usbh_core.h"
7 #include "usbh_video.h"
8
9 #define DEV_FORMAT "/dev/video%d"
10
11 /* general descriptor field offsets */
12 #define DESC_bLength 0 /** Length offset */
13 #define DESC_bDescriptorType 1 /** Descriptor type offset */
14 #define DESC_bDescriptorSubType 2 /** Descriptor subtype offset */
15 #define DESC_bNumFormats 3 /** Descriptor numformat offset */
16 #define DESC_bNumFrameDescriptors 4 /** Descriptor numframe offset */
17 #define DESC_bFormatIndex 3 /** Descriptor format index offset */
18 #define DESC_bFrameIndex 3 /** Descriptor frame index offset */
19
20 /* interface descriptor field offsets */
21 #define INTF_DESC_bInterfaceNumber 2 /** Interface number offset */
22 #define INTF_DESC_bAlternateSetting 3 /** Alternate setting offset */
23
24 USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t g_video_buf[128];
25
26 static const char *format_type[] = { "uncompressed", "mjpeg" };
27
28 static struct usbh_video g_video_class[CONFIG_USBHOST_MAX_VIDEO_CLASS];
29 static uint32_t g_devinuse = 0;
30
usbh_video_class_alloc(void)31 static struct usbh_video *usbh_video_class_alloc(void)
32 {
33 int devno;
34
35 for (devno = 0; devno < CONFIG_USBHOST_MAX_VIDEO_CLASS; devno++) {
36 if ((g_devinuse & (1 << devno)) == 0) {
37 g_devinuse |= (1 << devno);
38 memset(&g_video_class[devno], 0, sizeof(struct usbh_video));
39 g_video_class[devno].minor = devno;
40 return &g_video_class[devno];
41 }
42 }
43 return NULL;
44 }
45
usbh_video_class_free(struct usbh_video * video_class)46 static void usbh_video_class_free(struct usbh_video *video_class)
47 {
48 int devno = video_class->minor;
49
50 if (devno >= 0 && devno < 32) {
51 g_devinuse &= ~(1 << devno);
52 }
53 memset(video_class, 0, sizeof(struct usbh_video));
54 }
55
usbh_video_get(struct usbh_video * video_class,uint8_t request,uint8_t intf,uint8_t entity_id,uint8_t cs,uint8_t * buf,uint16_t len)56 int usbh_video_get(struct usbh_video *video_class, uint8_t request, uint8_t intf, uint8_t entity_id, uint8_t cs, uint8_t *buf, uint16_t len)
57 {
58 struct usb_setup_packet *setup = video_class->hport->setup;
59 int ret;
60 uint8_t retry;
61
62 setup->bmRequestType = USB_REQUEST_DIR_IN | USB_REQUEST_CLASS | USB_REQUEST_RECIPIENT_INTERFACE;
63 setup->bRequest = request;
64 setup->wValue = cs << 8;
65 setup->wIndex = (entity_id << 8) | intf;
66 setup->wLength = len;
67
68 retry = 0;
69 while (1) {
70 ret = usbh_control_transfer(video_class->hport->ep0, setup, g_video_buf);
71 if (ret > 0) {
72 break;
73 }
74 retry++;
75
76 if (retry == 3) {
77 return ret;
78 }
79 }
80
81 if (buf) {
82 memcpy(buf, g_video_buf, len);
83 }
84
85 return ret;
86 }
87
usbh_video_set(struct usbh_video * video_class,uint8_t request,uint8_t intf,uint8_t entity_id,uint8_t cs,uint8_t * buf,uint16_t len)88 int usbh_video_set(struct usbh_video *video_class, uint8_t request, uint8_t intf, uint8_t entity_id, uint8_t cs, uint8_t *buf, uint16_t len)
89 {
90 struct usb_setup_packet *setup = video_class->hport->setup;
91 int ret;
92
93 setup->bmRequestType = USB_REQUEST_DIR_OUT | USB_REQUEST_CLASS | USB_REQUEST_RECIPIENT_INTERFACE;
94 setup->bRequest = request;
95 setup->wValue = cs << 8;
96 setup->wIndex = (entity_id << 8) | intf;
97 setup->wLength = len;
98
99 memcpy(g_video_buf, buf, len);
100
101 ret = usbh_control_transfer(video_class->hport->ep0, setup, g_video_buf);
102 usb_osal_msleep(50);
103 return ret;
104 }
105
usbh_videostreaming_get_cur_probe(struct usbh_video * video_class)106 int usbh_videostreaming_get_cur_probe(struct usbh_video *video_class)
107 {
108 return usbh_video_get(video_class, VIDEO_REQUEST_GET_CUR, video_class->data_intf, 0x00, VIDEO_VS_PROBE_CONTROL, (uint8_t *)&video_class->probe, 26);
109 }
110
usbh_videostreaming_set_cur_probe(struct usbh_video * video_class,uint8_t formatindex,uint8_t frameindex)111 int usbh_videostreaming_set_cur_probe(struct usbh_video *video_class, uint8_t formatindex, uint8_t frameindex)
112 {
113 video_class->probe.bFormatIndex = formatindex;
114 video_class->probe.bFrameIndex = frameindex;
115 video_class->probe.dwMaxPayloadTransferSize = 0;
116 video_class->probe.dwFrameInterval = 333333;
117 return usbh_video_set(video_class, VIDEO_REQUEST_SET_CUR, video_class->data_intf, 0x00, VIDEO_VS_PROBE_CONTROL, (uint8_t *)&video_class->probe, 26);
118 }
119
usbh_videostreaming_set_cur_commit(struct usbh_video * video_class,uint8_t formatindex,uint8_t frameindex)120 int usbh_videostreaming_set_cur_commit(struct usbh_video *video_class, uint8_t formatindex, uint8_t frameindex)
121 {
122 memcpy(&video_class->commit, &video_class->probe, sizeof(struct video_probe_and_commit_controls));
123 video_class->commit.bFormatIndex = formatindex;
124 video_class->commit.bFrameIndex = frameindex;
125 video_class->commit.dwFrameInterval = 333333;
126 return usbh_video_set(video_class, VIDEO_REQUEST_SET_CUR, video_class->data_intf, 0x00, VIDEO_VS_COMMIT_CONTROL, (uint8_t *)&video_class->commit, 26);
127 }
128
usbh_video_open(struct usbh_video * video_class,uint8_t format_type,uint16_t wWidth,uint16_t wHeight,uint8_t altsetting)129 int usbh_video_open(struct usbh_video *video_class,
130 uint8_t format_type,
131 uint16_t wWidth,
132 uint16_t wHeight,
133 uint8_t altsetting)
134 {
135 struct usb_setup_packet *setup = video_class->hport->setup;
136 struct usb_endpoint_descriptor *ep_desc;
137 uint8_t mult;
138 uint16_t mps;
139 int ret;
140 bool found = false;
141 uint8_t formatidx = 0;
142 uint8_t frameidx = 0;
143 uint8_t step;
144
145 if (video_class->is_opened) {
146 return -EMFILE;
147 }
148
149 for (uint8_t i = 0; i < video_class->num_of_formats; i++) {
150 if (format_type == video_class->format[i].format_type) {
151 formatidx = i + 1;
152 for (uint8_t j = 0; j < video_class->format[i].num_of_frames; j++) {
153 if ((wWidth == video_class->format[i].frame[j].wWidth) &&
154 (wHeight == video_class->format[i].frame[j].wHeight)) {
155 frameidx = j + 1;
156 found = true;
157 break;
158 }
159 }
160 }
161 }
162
163 if (found == false) {
164 return -ENODEV;
165 }
166
167 if (altsetting > (video_class->num_of_intf_altsettings - 1)) {
168 return -EINVAL;
169 }
170
171 /* Open video step:
172 * Get CUR request (probe)
173 * Set CUR request (probe)
174 * Get CUR request (probe)
175 * Get MAX request (probe)
176 * Get MIN request (probe)
177 * Get CUR request (probe)
178 * Set CUR request (commit)
179 *
180 */
181 step = 0;
182 ret = usbh_videostreaming_get_cur_probe(video_class);
183 if (ret < 0) {
184 goto errout;
185 }
186
187 step = 1;
188 ret = usbh_videostreaming_set_cur_probe(video_class, formatidx, frameidx);
189 if (ret < 0) {
190 goto errout;
191 }
192
193 step = 2;
194 ret = usbh_videostreaming_get_cur_probe(video_class);
195 if (ret < 0) {
196 goto errout;
197 }
198
199 step = 3;
200 ret = usbh_video_get(video_class, VIDEO_REQUEST_GET_MAX, video_class->data_intf, 0x00, VIDEO_VS_PROBE_CONTROL, NULL, 26);
201 if (ret < 0) {
202 goto errout;
203 }
204
205 step = 4;
206 ret = usbh_video_get(video_class, VIDEO_REQUEST_GET_MIN, video_class->data_intf, 0x00, VIDEO_VS_PROBE_CONTROL, NULL, 26);
207 if (ret < 0) {
208 goto errout;
209 }
210
211 step = 5;
212 ret = usbh_videostreaming_set_cur_probe(video_class, formatidx, frameidx);
213 if (ret < 0) {
214 goto errout;
215 }
216
217 step = 6;
218 ret = usbh_videostreaming_get_cur_probe(video_class);
219 if (ret < 0) {
220 goto errout;
221 }
222
223 step = 7;
224 ret = usbh_videostreaming_set_cur_commit(video_class, formatidx, frameidx);
225 if (ret < 0) {
226 goto errout;
227 }
228
229 step = 8;
230 setup->bmRequestType = USB_REQUEST_DIR_OUT | USB_REQUEST_STANDARD | USB_REQUEST_RECIPIENT_INTERFACE;
231 setup->bRequest = USB_REQUEST_SET_INTERFACE;
232 setup->wValue = altsetting;
233 setup->wIndex = video_class->data_intf;
234 setup->wLength = 0;
235
236 ret = usbh_control_transfer(video_class->hport->ep0, setup, NULL);
237 if (ret < 0) {
238 goto errout;
239 }
240
241 ep_desc = &video_class->hport->config.intf[video_class->data_intf].altsetting[altsetting].ep[0].ep_desc;
242 mult = (ep_desc->wMaxPacketSize & USB_MAXPACKETSIZE_ADDITIONAL_TRANSCATION_MASK) >> USB_MAXPACKETSIZE_ADDITIONAL_TRANSCATION_SHIFT;
243 mps = ep_desc->wMaxPacketSize & USB_MAXPACKETSIZE_MASK;
244 if (ep_desc->bEndpointAddress & 0x80) {
245 video_class->isoin_mps = mps * (mult + 1);
246 usbh_hport_activate_epx(&video_class->isoin, video_class->hport, ep_desc);
247 } else {
248 video_class->isoout_mps = mps * (mult + 1);
249 usbh_hport_activate_epx(&video_class->isoout, video_class->hport, ep_desc);
250 }
251
252 USB_LOG_INFO("Open video and select formatidx:%u, frameidx:%u, altsetting:%u\r\n", formatidx, frameidx, altsetting);
253 video_class->is_opened = true;
254 video_class->current_format = format_type;
255 return ret;
256
257 errout:
258 USB_LOG_ERR("Fail to open video in step %u\r\n", step);
259 return ret;
260 }
261
usbh_video_close(struct usbh_video * video_class)262 int usbh_video_close(struct usbh_video *video_class)
263 {
264 struct usb_setup_packet *setup = video_class->hport->setup;
265 int ret = 0;
266
267 USB_LOG_INFO("Close video device\r\n");
268
269 video_class->is_opened = false;
270
271 if (video_class->isoin) {
272 usbh_pipe_free(video_class->isoin);
273 video_class->isoin = NULL;
274 }
275
276 if (video_class->isoout) {
277 usbh_pipe_free(video_class->isoout);
278 video_class->isoout = NULL;
279 }
280
281 setup->bmRequestType = USB_REQUEST_DIR_OUT | USB_REQUEST_STANDARD | USB_REQUEST_RECIPIENT_INTERFACE;
282 setup->bRequest = USB_REQUEST_SET_INTERFACE;
283 setup->wValue = 0;
284 setup->wIndex = video_class->data_intf;
285 setup->wLength = 0;
286
287 ret = usbh_control_transfer(video_class->hport->ep0, setup, NULL);
288 if (ret < 0) {
289 return ret;
290 }
291 return ret;
292 }
293
usbh_video_list_info(struct usbh_video * video_class)294 void usbh_video_list_info(struct usbh_video *video_class)
295 {
296 struct usb_endpoint_descriptor *ep_desc;
297 uint8_t mult;
298 uint16_t mps;
299
300 USB_LOG_INFO("============= Video device information ===================\r\n");
301 USB_LOG_INFO("bcdVDC:%04x\r\n", video_class->bcdVDC);
302 USB_LOG_INFO("Num of altsettings:%u\r\n", video_class->num_of_intf_altsettings);
303
304 for (uint8_t i = 0; i < video_class->num_of_intf_altsettings; i++) {
305 if (i == 0) {
306 USB_LOG_INFO("Ingore altsetting 0\r\n");
307 continue;
308 }
309 ep_desc = &video_class->hport->config.intf[video_class->data_intf].altsetting[i].ep[0].ep_desc;
310
311 mult = (ep_desc->wMaxPacketSize & USB_MAXPACKETSIZE_ADDITIONAL_TRANSCATION_MASK) >> USB_MAXPACKETSIZE_ADDITIONAL_TRANSCATION_SHIFT;
312 mps = ep_desc->wMaxPacketSize & USB_MAXPACKETSIZE_MASK;
313
314 USB_LOG_INFO("Altsetting:%u, Ep=%02x Attr=%02u Mps=%d Interval=%02u Mult=%02u\r\n",
315 i,
316 ep_desc->bEndpointAddress,
317 ep_desc->bmAttributes,
318 mps,
319 ep_desc->bInterval,
320 mult);
321 }
322
323 USB_LOG_INFO("bNumFormats:%u\r\n", video_class->num_of_formats);
324 for (uint8_t i = 0; i < video_class->num_of_formats; i++) {
325 USB_LOG_INFO(" FormatIndex:%u\r\n", i + 1);
326 USB_LOG_INFO(" FormatType:%s\r\n", format_type[video_class->format[i].format_type]);
327 USB_LOG_INFO(" bNumFrames:%u\r\n", video_class->format[i].num_of_frames);
328 USB_LOG_INFO(" Resolution:\r\n");
329 for (uint8_t j = 0; j < video_class->format[i].num_of_frames; j++) {
330 USB_LOG_INFO(" FrameIndex:%u\r\n", j + 1);
331 USB_LOG_INFO(" wWidth: %d, wHeight: %d\r\n",
332 video_class->format[i].frame[j].wWidth,
333 video_class->format[i].frame[j].wHeight);
334 }
335 }
336
337 USB_LOG_INFO("============= Video device information ===================\r\n");
338 }
339
usbh_video_ctrl_connect(struct usbh_hubport * hport,uint8_t intf)340 static int usbh_video_ctrl_connect(struct usbh_hubport *hport, uint8_t intf)
341 {
342 int ret;
343 uint8_t cur_iface = 0xff;
344 // uint8_t cur_alt_setting = 0xff;
345 uint8_t frame_index = 0xff;
346 uint8_t format_index = 0xff;
347 uint8_t num_of_frames = 0xff;
348 uint8_t *p;
349
350 struct usbh_video *video_class = usbh_video_class_alloc();
351 if (video_class == NULL) {
352 USB_LOG_ERR("Fail to alloc video_class\r\n");
353 return -ENOMEM;
354 }
355
356 video_class->hport = hport;
357 video_class->ctrl_intf = intf;
358 video_class->data_intf = intf + 1;
359 video_class->num_of_intf_altsettings = hport->config.intf[intf + 1].altsetting_num;
360
361 hport->config.intf[intf].priv = video_class;
362
363 ret = usbh_video_close(video_class);
364 if (ret < 0) {
365 USB_LOG_ERR("Fail to close video device\r\n");
366 return ret;
367 }
368
369 p = hport->raw_config_desc;
370 while (p[DESC_bLength]) {
371 switch (p[DESC_bDescriptorType]) {
372 case USB_DESCRIPTOR_TYPE_INTERFACE:
373 cur_iface = p[INTF_DESC_bInterfaceNumber];
374 //cur_alt_setting = p[INTF_DESC_bAlternateSetting];
375 break;
376 case USB_DESCRIPTOR_TYPE_ENDPOINT:
377 //ep_desc = (struct usb_endpoint_descriptor *)p;
378 break;
379 case VIDEO_CS_INTERFACE_DESCRIPTOR_TYPE:
380 if (cur_iface == video_class->ctrl_intf) {
381 switch (p[DESC_bDescriptorSubType]) {
382 case VIDEO_VC_HEADER_DESCRIPTOR_SUBTYPE:
383 video_class->bcdVDC = ((uint16_t)p[4] << 8) | (uint16_t)p[3];
384 break;
385 case VIDEO_VC_INPUT_TERMINAL_DESCRIPTOR_SUBTYPE:
386 break;
387 case VIDEO_VC_OUTPUT_TERMINAL_DESCRIPTOR_SUBTYPE:
388 break;
389 case VIDEO_VC_PROCESSING_UNIT_DESCRIPTOR_SUBTYPE:
390 break;
391
392 default:
393 break;
394 }
395 } else if (cur_iface == video_class->data_intf) {
396 switch (p[DESC_bDescriptorSubType]) {
397 case VIDEO_VS_INPUT_HEADER_DESCRIPTOR_SUBTYPE:
398 video_class->num_of_formats = p[DESC_bNumFormats];
399 break;
400 case VIDEO_VS_FORMAT_UNCOMPRESSED_DESCRIPTOR_SUBTYPE:
401 format_index = p[DESC_bFormatIndex];
402 num_of_frames = p[DESC_bNumFrameDescriptors];
403
404 video_class->format[format_index - 1].num_of_frames = num_of_frames;
405 video_class->format[format_index - 1].format_type = USBH_VIDEO_FORMAT_UNCOMPRESSED;
406 break;
407 case VIDEO_VS_FORMAT_MJPEG_DESCRIPTOR_SUBTYPE:
408 format_index = p[DESC_bFormatIndex];
409 num_of_frames = p[DESC_bNumFrameDescriptors];
410
411 video_class->format[format_index - 1].num_of_frames = num_of_frames;
412 video_class->format[format_index - 1].format_type = USBH_VIDEO_FORMAT_MJPEG;
413 break;
414 case VIDEO_VS_FRAME_UNCOMPRESSED_DESCRIPTOR_SUBTYPE:
415 frame_index = p[DESC_bFrameIndex];
416
417 video_class->format[format_index - 1].frame[frame_index - 1].wWidth = ((struct video_cs_if_vs_frame_uncompressed_descriptor *)p)->wWidth;
418 video_class->format[format_index - 1].frame[frame_index - 1].wHeight = ((struct video_cs_if_vs_frame_uncompressed_descriptor *)p)->wHeight;
419 break;
420 case VIDEO_VS_FRAME_MJPEG_DESCRIPTOR_SUBTYPE:
421 frame_index = p[DESC_bFrameIndex];
422
423 video_class->format[format_index - 1].frame[frame_index - 1].wWidth = ((struct video_cs_if_vs_frame_mjpeg_descriptor *)p)->wWidth;
424 video_class->format[format_index - 1].frame[frame_index - 1].wHeight = ((struct video_cs_if_vs_frame_mjpeg_descriptor *)p)->wHeight;
425 break;
426 default:
427 break;
428 }
429 }
430
431 break;
432
433 default:
434 break;
435 }
436 /* skip to next descriptor */
437 p += p[DESC_bLength];
438 }
439
440 usbh_video_list_info(video_class);
441
442 snprintf(hport->config.intf[intf].devname, CONFIG_USBHOST_DEV_NAMELEN, DEV_FORMAT, video_class->minor);
443
444 USB_LOG_INFO("Register Video Class:%s\r\n", hport->config.intf[intf].devname);
445
446 usbh_video_run(video_class);
447 return ret;
448 }
449
usbh_video_ctrl_disconnect(struct usbh_hubport * hport,uint8_t intf)450 static int usbh_video_ctrl_disconnect(struct usbh_hubport *hport, uint8_t intf)
451 {
452 int ret = 0;
453
454 struct usbh_video *video_class = (struct usbh_video *)hport->config.intf[intf].priv;
455
456 if (video_class) {
457 if (video_class->isoin) {
458 usbh_pipe_free(video_class->isoin);
459 }
460
461 if (video_class->isoout) {
462 usbh_pipe_free(video_class->isoout);
463 }
464
465 if (hport->config.intf[intf].devname[0] != '\0') {
466 USB_LOG_INFO("Unregister Video Class:%s\r\n", hport->config.intf[intf].devname);
467 usbh_video_stop(video_class);
468 }
469
470 usbh_video_class_free(video_class);
471 }
472
473 return ret;
474 }
475
usbh_video_streaming_connect(struct usbh_hubport * hport,uint8_t intf)476 static int usbh_video_streaming_connect(struct usbh_hubport *hport, uint8_t intf)
477 {
478 return 0;
479 }
480
usbh_video_streaming_disconnect(struct usbh_hubport * hport,uint8_t intf)481 static int usbh_video_streaming_disconnect(struct usbh_hubport *hport, uint8_t intf)
482 {
483 return 0;
484 }
485
usbh_video_run(struct usbh_video * video_class)486 __WEAK void usbh_video_run(struct usbh_video *video_class)
487 {
488 }
489
usbh_video_stop(struct usbh_video * video_class)490 __WEAK void usbh_video_stop(struct usbh_video *video_class)
491 {
492 }
493
494 const struct usbh_class_driver video_ctrl_class_driver = {
495 .driver_name = "video_ctrl",
496 .connect = usbh_video_ctrl_connect,
497 .disconnect = usbh_video_ctrl_disconnect
498 };
499
500 const struct usbh_class_driver video_streaming_class_driver = {
501 .driver_name = "video_streaming",
502 .connect = usbh_video_streaming_connect,
503 .disconnect = usbh_video_streaming_disconnect
504 };
505
506 CLASS_INFO_DEFINE const struct usbh_class_info video_ctrl_class_info = {
507 .match_flags = USB_CLASS_MATCH_INTF_CLASS | USB_CLASS_MATCH_INTF_SUBCLASS | USB_CLASS_MATCH_INTF_PROTOCOL,
508 .class = USB_DEVICE_CLASS_VIDEO,
509 .subclass = VIDEO_SC_VIDEOCONTROL,
510 .protocol = VIDEO_PC_PROTOCOL_UNDEFINED,
511 .vid = 0x00,
512 .pid = 0x00,
513 .class_driver = &video_ctrl_class_driver
514 };
515
516 CLASS_INFO_DEFINE const struct usbh_class_info video_streaming_class_info = {
517 .match_flags = USB_CLASS_MATCH_INTF_CLASS | USB_CLASS_MATCH_INTF_SUBCLASS | USB_CLASS_MATCH_INTF_PROTOCOL,
518 .class = USB_DEVICE_CLASS_VIDEO,
519 .subclass = VIDEO_SC_VIDEOSTREAMING,
520 .protocol = VIDEO_PC_PROTOCOL_UNDEFINED,
521 .vid = 0x00,
522 .pid = 0x00,
523 .class_driver = &video_streaming_class_driver
524 };
525