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