moved ISO attributes from audio to common

4 files changed, 519 insertions, 213 deletions

diff --git a/src/class/audio/audio.h b/src/class/audio/audio.h
index f99061eae..6f9c1a6b5 100644
--- a/src/class/audio/audio.h
+++ b/src/class/audio/audio.h
@@ -494,18 +494,6 @@ typedef enum
 
 /// All remaining definitions are taken from the descriptor descriptions in the UAC2 main specification
 
-/// Isochronous End Point Attributes
-typedef enum
-{
-  TUSB_ISO_EP_ATT_NO_SYNC         = 0x00,
-  TUSB_ISO_EP_ATT_ASYNCHRONOUS    = 0x04,
-  TUSB_ISO_EP_ATT_ADAPTIVE        = 0x08,
-  TUSB_ISO_EP_ATT_SYNCHRONOUS     = 0x0C,
-  TUSB_ISO_EP_ATT_DATA            = 0x00, ///< Data End Point
-  TUSB_ISO_EP_ATT_EXPLICIT_FB     = 0x10, ///< Feedback End Point
-  TUSB_ISO_EP_ATT_IMPLICIT_FB     = 0x20, ///< Data endpoint that also serves as an implicit feedback
-} tusb_iso_ep_attribute_t;
-
 /// Audio Class-Control Values UAC2
 typedef enum
 {
diff --git a/src/class/video/video.h b/src/class/video/video.h
index f994c7fed..081860c83 100644
--- a/src/class/video/video.h
+++ b/src/class/video/video.h
@@ -29,6 +29,20 @@
 
 #include "common/tusb_common.h"
 
+/* 4.2.1.2 */
+typedef enum {
+  VIDEO_NO_ERROR = 0, /* The request succeeded. */
+  VIDEO_NOT_READY,
+  VIDEO_WRONG_STATE,
+  VIDEO_POWER,
+  VIDEO_OUT_OF_RANGE,
+  VIDEO_INVALID_UNIT,
+  VIDEO_INVALID_CONTROL,
+  VIDEO_INVALID_REQUEST,
+  VIDEO_INVALID_VALUE_WITHIN_RANGE,
+  VIDEO_UNKNOWN = 0xFF,
+} video_error_code_t;
+
 /* A.2 */
 typedef enum {
   VIDEO_SUBCLASS_UNDEFINED = 0x00,
@@ -103,14 +117,29 @@ typedef enum
   VIDEO_REQUEST_GET_DEF_ALL = 0x97
 } video_control_request_t;
 
-/* A.9 */
+/* A.9.1 */
 typedef enum
 {
-  VIDEO_CTL_SEL_UNDEF = 0x00,
-  VIDEO_CTL_SEL_VIDEO_POWER_MODE_CONTROL,
-  VIDEO_CTL_SEL_REQUEST_ERROR_CODE_CONTROL,
+  VIDEO_VC_CTL_UNDEF = 0x00,
+  VIDEO_VC_CTL_VIDEO_POWER_MODE,
+  VIDEO_VC_CTL_REQUEST_ERROR_CODE,
 } video_interface_control_selector_t;
 
+/* A.9.8 */
+typedef enum
+{
+  VIDEO_VS_CTL_UNDEF = 0x00,
+  VIDEO_VS_CTL_PROBE,
+  VIDEO_VS_CTL_COMMIT,
+  VIDEO_VS_CTL_STILL_PROBE,
+  VIDEO_VS_CTL_STILL_COMMIT,
+  VIDEO_VS_CTL_STILL_IMAGE_TRIGGER,
+  VIDEO_VS_CTL_STREAM_ERROR_CODE,
+  VIDEO_VS_CTL_GENERATE_KEY_FRAME,
+  VIDEO_VS_CTL_UPDATE_FRAME_SEGMENT,
+  VIDEO_VS_CTL_SYNCH_DELAY_CONTROL,
+} video_interface_streaming_selector_t;
+
 /* B. Terminal Types */
 typedef enum
 {
@@ -193,8 +222,37 @@ typedef struct TU_ATTR_PACKED {
       uint8_t  bControlSize;
       uint8_t  bmaControls[];
     } output;
+  };
 } tusb_desc_cs_video_stm_itf_hdr_t;
 
+//--------------------------------------------------------------------+
+// Requests
+//--------------------------------------------------------------------+
+typedef struct TU_ATTR_PACKED {
+  uint16_t bmHint;
+  uint8_t  bFormatIndex;
+  uint8_t  bFrameIndex;
+  uint32_t dwFrameInterval;
+  uint16_t wKeyFrameRate;
+  uint16_t wPFrameRate;
+  uint16_t wCompQuality;
+  uint16_t wCompWindowSize;
+  uint16_t wDelay;
+  uint32_t dwMaxVideoFrameSize;
+  uint32_t dwMaxPayloadTransferSize;
+  uint32_t dwClockFrequency;
+  uint8_t  bmFramingInfo;
+  uint8_t  bPreferedVersion;
+  uint8_t  bMinVersion;
+  uint8_t  bMaxVersion;
+  uint8_t  bUsage;
+  uint8_t  bBitDepthLum;
+  uint8_t  bmSettings;
+  uint8_t  bMaxNumberOfRefFramesPlus1;
+  uint16_t bmRateControlModes;
+  uint64_t bmLayoutPerStream;
+} video_probe_and_commit_t;
+
 #define TUD_VIDEO_DESC_IAD_LEN                    8
 #define TUD_VIDEO_DESC_STD_VC_LEN                 9
 #define TUD_VIDEO_DESC_CS_VC_LEN                  12
@@ -225,18 +283,18 @@ typedef struct TU_ATTR_PACKED {
 /* 3.7.2 */
 #define TUD_VIDEO_DESC_CS_VC(_bcdUVC, _totallen, _clkfreq, _coll, ...)	\
   TUD_VIDEO_DESC_CS_VC_LEN + _coll, TUSB_DESC_CS_INTERFACE, VIDEO_CS_VC_INTERFACE_HEADER, \
-  U16_TO_U8S_LE(_bcdUVC), U16_TO_U8S_LE(_totallen + TUD_VIDEO_DESC_CS_AC_LEN), \
+  U16_TO_U8S_LE(_bcdUVC), U16_TO_U8S_LE(_totallen + TUD_VIDEO_DESC_CS_VC_LEN), \
   U32_TO_U8S_LE(_clkfreq), _coll, __VA_ARGS__
 
 /* 3.7.2.1 */
 #define TUD_VIDEO_DESC_INPUT_TERM(_tid, _tt, _at, _stridx) \
   TUD_VIDEO_DESC_INPUT_TERM_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_VC_INTERFACE_INPUT_TERMINAL, \
-    _tid, U16_TO_U8S_LE(_tt), _at, _ti
+    _tid, U16_TO_U8S_LE(_tt), _at, _stridx
 
 /* 3.7.2.2 */
 #define TUD_VIDEO_DESC_OUTPUT_TERM(_tid, _tt, _at, _srcid, _stridx) \
   TUD_VIDEO_DESC_INPUT_TERM_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_VC_INTERFACE_OUTPUT_TERMINAL, \
-    _tid, U16_TO_U8S_LE(_tt), _at, _ti
+    _tid, U16_TO_U8S_LE(_tt), _at, _stridx
 
 /* 3.9.1 */
 #define TUD_VIDEO_DESC_STD_VS(_itfnum, _alt, _epn, _stridx)   \
@@ -258,14 +316,15 @@ typedef struct TU_ATTR_PACKED {
   _epn, _inf, _termlnk,  _trgusg, _ctlsz, __VA_ARGS__
 
 /* Uncompressed 3.1.1 */
+#define TUD_VIDEO_GUID(_g0,_g1,_g2,_g3,_g4,_g5,_g6,_g7,_g8,_g9,_g10,_g11,_g12,_g13,_g14,_g15) _g0,_g1,_g2,_g3,_g4,_g5,_g6,_g7,_g8,_g9,_g10,_g11,_g12,_g13,_g14,_g15
 #define TUD_VIDEO_DESC_CS_VS_FMT_UNCOMPR(_fmtidx, _numfrmdesc, \
-  _g0,_g1,_g2,_g3,_g4,_g5,_g6,_g7,_g8,_g9,_g10,_g11,_g12,_g13,_g14,_g15, \
-  _bitsperpix, _frmidx, _asrx, _asry, _interlace, _cp) \
+  _guid, _bitsperpix, _frmidx, _asrx, _asry, _interlace, _cp) \
   TUD_VIDEO_DESC_CS_VS_FMT_UNCOMPR_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_VS_INTERFACE_FORMAT_UNCOMPRESSED, \
-  _fmtidx, _numfrmdesc,\
-  _g0,_g1,_g2,_g3,_g4,_g5,_g6,_g7,_g8,_g9,_g10,_g11,_g12,_g13,_g14,_g15,\
+  _fmtidx, _numfrmdesc, TUD_VIDEO_GUID(_guid), \
   _bitsperpix, _frmidx, _asrx,  _asry, _interlace, _cp
 
+//  _g0,_g1,_g2,_g3,_g4,_g5,_g6,_g7,_g8,_g9,_g10,_g11,_g12,_g13,_g14,_g15,
+
 /* Uncompressed 3.1.2 Table 3-3 */
 #define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_CONT(_frmidx, _cap, _width, _height, _minbr, _maxbr, _maxfrmbufsz, _frminterval, _minfrminterval, _maxfrminterval, _frmintervalstep) \
   TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_CONT_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_VS_INTERFACE_FRAME_UNCOMPRESSED, \
@@ -281,12 +340,12 @@ typedef struct TU_ATTR_PACKED {
   U32_TO_U8S_LE(_maxfrmbufsz), U32_TO_U8S_LE(_frminterval), _numfrminterval, __VA_ARGS__
 
 /* 3.10.1.1 */
-#define TUD_VIDEO_DESC_EP_ISO(_ep, _epsize, _ep_interval)
+#define TUD_VIDEO_DESC_EP_ISO(_ep, _epsize, _ep_interval) \
   7, TUSB_DESC_ENDPOINT, _ep, TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS,\
   U16_TO_U8S_LE(_epsize), _ep_interval
 
 /* 3.10.1.2 */
-#define TUD_VIDEO_DESC_EP_BULK(_ep, _epsize, _ep_interval)
+#define TUD_VIDEO_DESC_EP_BULK(_ep, _epsize, _ep_interval) \
   7, TUSB_DESC_ENDPOINT, _ep, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), _ep_interval
 
 #endif
diff --git a/src/class/video/video_device.c b/src/class/video/video_device.c
index 637e7a127..ed703d79a 100644
--- a/src/class/video/video_device.c
+++ b/src/class/video/video_device.c
@@ -53,33 +53,32 @@ typedef struct {
   tusb_desc_cs_video_stm_itf_hdr_t stm;
 } tusb_desc_vs_itf_t;
 
+typedef union {
+  tusb_desc_cs_video_ctl_itf_hdr_t ctl;
+  tusb_desc_cs_video_stm_itf_hdr_t stm;
+} tusb_desc_video_itf_hdr_t;
+
 typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType;
   uint8_t bDescriptorSubtype;
-  union {
-    uint8_t bId;
-    uint8_t bTerminalId;
-    uint8_t bUnitId;
-  };
+  uint8_t bEntityId;
 } tusb_desc_cs_video_entity_itf_t;
 
 typedef struct
 {
-  void const *beg;
-  void const *end;
-  tusb_desc_vc_itf_t const *vc;    /* current video control interface */
-  tusb_desc_vs_itf_t const *vs[2]; /* current video streaming interfaces */
-  uint8_t ep_notif; /* notification */
-  uint8_t ep_in;    /* video IN */
-  uint8_t ep_sti;   /* still image IN */
-  uint8_t ep_out;   /* video OUT */
+  void const *beg;  /* The head of the first video control interface descriptor */
+  uint16_t    len;  /* Byte length of the descriptors */
+  uint16_t    ofs[3];  /* offsets for video control/streaming interface. 0:control 1:streaming 2:streaming */
+  uint8_t power_mode;
+  uint8_t error_code;
 
   /*------------- From this point, data is not cleared by bus reset -------------*/
 
   // Endpoint Transfer buffer
-  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_CDC_EP_BUFSIZE];
-  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_CDC_EP_BUFSIZE];
+  //  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_CDC_EP_BUFSIZE];
+  //  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_CDC_EP_BUFSIZE];
+  uint8_t ctl_buf;
 
 } videod_interface_t;
 
@@ -90,6 +89,26 @@ typedef struct
 //--------------------------------------------------------------------+
 CFG_TUSB_MEM_SECTION static videod_interface_t _videod_itf[CFG_TUD_VIDEO];
 
+static uint8_t const _cap_get     = 0x1u; /* support for GET */
+static uint8_t const _cap_get_set = 0x3u; /* support for GET and SET */
+
+static tusb_desc_vc_itf_t const* _get_desc_vc(videod_interface_t const *self)
+{
+  return (tusb_desc_vc_itf_t const *)(self->beg + self->ofs[0]);
+}
+
+static uint16_t* _get_desc_ofs(videod_interface_t *self, unsigned itfnum)
+{
+  void const *beg = self->beg;
+  uint16_t   *ofs = self->ofs;
+  for (unsigned i = 1; i < sizeof(self->ofs)/sizeof(self->ofs[0]); ++i) {
+    if (!ofs[i]) continue;
+    tusb_desc_interface_t const* itf = (tusb_desc_interface_t const*)(beg + ofs[i]);
+    if (itfnum == itf->bInterfaceNumber) return &ofs[i];
+  }
+  return NULL;
+}
+
 /** Find the first descriptor with the specified descriptor type.
  *
  * @param[in] beg     The head of descriptor byte array.
@@ -98,51 +117,47 @@ CFG_TUSB_MEM_SECTION static videod_interface_t _videod_itf[CFG_TUD_VIDEO];
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static void const* videod_find_desc(void const *beg, void const *end, uint8_t target)
+static void const* _find_desc(void const *beg, void const *end, uint8_t target)
 {
-  for (void const *cur = beg; cur < end; cur = tu_desc_next(cur)) {
-    if (target != tu_desc_type(cur))
-      return (uint8_t const*)cur;
+  void const *cur = beg;
+  while ((cur < end) && (target != tu_desc_type(cur))) {
+    cur = tu_desc_next(cur);
   }
-  return end;
+  return cur;
 }
 
-/** Find the first interface descriptor with the specified interface number and alternate setting number.
+/** Return the next interface descriptor except alternate ones.
  *
  * @param[in] beg     The head of descriptor byte array.
  * @param[in] end     The tail of descriptor byte array.
- * @param[in] itfnum  The target interface number.
- * @param[in] altnum  The target alternate setting number.
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static void const* videod_find_desc_itf(void const *beg, void const *end, unsigned itfnum, unsigned altnum)
+static void const* _next_desc_itf(void const *beg, void const *end)
 {
-  for (void const *cur = beg; cur < end; cur = videod_find_desc(cur, end, TUSB_DESC_INTERFACE)) {
-    tusb_desc_interface_t const *itf = (tusb_desc_interface_t const *)cur;
-    if (itf->bInterfaceNumber == itfnum && itf->bAlternateSettings == altnum) {
-      return itf;
-    }
-    cur = tu_desc_next(cur);
+  void const *cur = beg;
+  unsigned itfnum = ((tusb_desc_interface_t const*)cur)->bInterfaceNumber;
+  while ((cur < end) &&
+	 (itfnum == ((tusb_desc_interface_t const*)cur)->bInterfaceNumber)) {
+    cur = _find_desc(tu_desc_next(cur), end, TUSB_DESC_INTERFACE);
   }
-  return end;
+  return cur;
 }
 
-/** Find the first input or output terminal descriptor with the specified terminal id.
+/** Find the first interface descriptor with the specified interface number and alternate setting number.
  *
  * @param[in] beg     The head of descriptor byte array.
  * @param[in] end     The tail of descriptor byte array.
- * @param[in] termid  The target terminal id.
+ * @param[in] itfnum  The target interface number.
+ * @param[in] altnum  The target alternate setting number.
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static void const* videod_find_desc_term(void const *beg, void const *end, unsigned termid)
+static void const* _find_desc_itf(void const *beg, void const *end, unsigned itfnum, unsigned altnum)
 {
-  for (void const *cur = beg; cur < end; cur = videod_find_desc(cur, end, TUSB_DESC_CS_INTERFACE)) {
-    tusb_desc_cs_video_entity_itf_t const *itf = (tusb_desc_cs_video_entity_itf_t const *)cur;
-    if ((VIDEO_CS_VC_INTERFACE_INPUT_TERMINAL  == itf->bDescriptorSubtype ||
-         VIDEO_CS_VC_INTERFACE_OUTPUT_TERMINAL == itf->bDescriptorSubtype) &&
-        itf->bTerminalId == termid) {
+  for (void const *cur = beg; cur < end; cur = _find_desc(cur, end, TUSB_DESC_INTERFACE)) {
+    tusb_desc_interface_t const *itf = (tusb_desc_interface_t const *)cur;
+    if (itf->bInterfaceNumber == itfnum && itf->bAlternateSetting == altnum) {
       return itf;
     }
     cur = tu_desc_next(cur);
@@ -150,21 +165,22 @@ static void const* videod_find_desc_term(void const *beg, void const *end, unsig
   return end;
 }
 
-/** Find the first selector/processing/extension/encoding unit descriptor with the specified unit id.
+/** Find the first entity descriptor with the specified entity ID in the video control interface descriptor.
  *
- * @param[in] beg     The head of descriptor byte array.
- * @param[in] end     The tail of descriptor byte array.
- * @param[in] unitid  The target unit id.
+ * @param[in] vc        The video control interface descriptor.
+ * @param[in] entityid  The target entity id.
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static void const* videod_find_desc_unit(void const *beg, void const *end, unsigned unitid)
+static void const* _find_desc_entity(tusb_desc_vc_itf_t const *vc, unsigned entityid)
 {
-  for (void const *cur = beg; cur < end; cur = videod_find_desc(cur, end, TUSB_DESC_CS_INTERFACE)) {
+  void const *beg = (void const*)vc;
+  void const *end = beg + vc->std.bLength + vc->ctl.bLength + vc->ctl.wTotalLength;
+  for (void const *cur = beg; cur < end; cur = _find_desc(cur, end, TUSB_DESC_CS_INTERFACE)) {
     tusb_desc_cs_video_entity_itf_t const *itf = (tusb_desc_cs_video_entity_itf_t const *)cur;
-    if (VIDEO_CS_VC_INTERFACE_SELECTOR_UNIT <= itf->bDescriptorSubtype &&
-        itf->bDescriptorSubtype <= VIDEO_CS_VC_INTERFACE_ENCODING_UNIT &&
-        itf->bUnitId == unitid) {
+    if ((VIDEO_CS_VC_INTERFACE_INPUT_TERMINAL  == itf->bDescriptorSubtype ||
+         VIDEO_CS_VC_INTERFACE_OUTPUT_TERMINAL == itf->bDescriptorSubtype) &&
+        itf->bEntityId == entityid) {
       return itf;
     }
     cur = tu_desc_next(cur);
@@ -172,25 +188,72 @@ static void const* videod_find_desc_unit(void const *beg, void const *end, unsig
   return end;
 }
 
+static bool _close_vc_itf(uint8_t rhport, videod_interface_t *self)
+{
+  tusb_desc_vc_itf_t const *vc = _get_desc_vc(self);
+  /* The next descriptor after the class-specific VC interface header descriptor. */
+  void const *cur = (void const*)vc + vc->std.bLength + vc->ctl.bLength;
+  /* The end of the video control interface descriptor. */
+  void const *end = cur + vc->ctl.wTotalLength;
+  if (vc->std.bNumEndpoints) {
+    /* Find the notification endpoint descriptor. */
+    cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT);
+    TU_ASSERT(cur < end);
+    tusb_desc_endpoint_t const *notif = (tusb_desc_endpoint_t const *)cur;
+    usbd_edpt_close(rhport, notif->bEndpointAddress);
+  }
+  self->ofs[0] = 0;
+  return true;
+}
+
+static bool _open_vc_itf(uint8_t rhport, videod_interface_t *self, unsigned altnum)
+{
+  void const *beg = self->beg;
+  void const *end = beg + self->len;
+  /* The first descriptor is a video control interface descriptor. */
+  unsigned itfnum = ((tusb_desc_interface_t const *)beg)->bInterfaceNumber;
+  void const *cur = _find_desc_itf(beg, end, itfnum, altnum);
+  TU_VERIFY(cur < end);
+
+  tusb_desc_vc_itf_t const *vc = (tusb_desc_vc_itf_t const *)cur;
+  /* Support for up to 2 streaming interfaces only. */
+  TU_ASSERT(vc->ctl.bInCollection < 3);
+
+  /* Advance to the next descriptor after the class-specific VC interface header descriptor. */
+  cur += vc->std.bLength + vc->ctl.bLength;
+  /* Update to point the end of the video control interface descriptor. */
+  end  = cur + vc->ctl.wTotalLength;
+  /* Open the notification endpoint if it exist. */
+  if (vc->std.bNumEndpoints) {
+    /* Support for 1 endpoint only. */
+    TU_VERIFY(1 == vc->std.bNumEndpoints);
+    /* Find the notification endpoint descriptor. */
+    cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT);
+    TU_VERIFY(cur < end);
+    tusb_desc_endpoint_t const *notif = (tusb_desc_endpoint_t const *)cur;
+    /* Open the notification endpoint */
+    TU_ASSERT(usbd_edpt_open(rhport, notif));
+  }
+  self->ofs[0] = (void const*)vc - beg;
+  return true;
+}
+
 /** Set the specified alternate setting to own video control interface.
  *
  * @param[in,out] self     The context.
- * @param[in]     altnum   The target alternate setting number.
- *
- * @return The next descriptor after the video control interface descriptor.
- * @retval NULL   did not found interface descriptor or alternate setting */
-static void const* videod_set_vc_itf(videod_interface_t *self, unsigned altnum)
+ * @param[in]     altnum   The target alternate setting number. */
+static bool _set_vc_itf(uint8_t rhport, videod_interface_t *self, unsigned altnum)
 {
   void const *beg = self->beg;
-  void const *end = self->end;
+  void const *end = beg + self->len;
   /* The head descriptor is a video control interface descriptor. */
   unsigned itfnum = ((tusb_desc_interface_t const *)beg)->bInterfaceNumber;
-  void const *cur = videod_find_desc_itf(beg, end, itfnum, altnum);
-  TU_VERIFY(cur < end, NULL);
+  void const *cur = _find_desc_itf(beg, end, itfnum, altnum);
+  TU_VERIFY(cur < end);
 
   tusb_desc_vc_itf_t const *vc = (tusb_desc_vc_itf_t const *)cur;
   /* Support for up to 2 streaming interfaces only. */
-  TU_VERIFY(vc->ctl.bInCollection < 3, NULL);
+  TU_VERIFY(vc->ctl.bInCollection < 3);
 
   /* Close the previous notification endpoint if it is opened */
   if (self->ep_notif) {
@@ -204,42 +267,104 @@ static void const* videod_set_vc_itf(videod_interface_t *self, unsigned altnum)
   /* Open the notification endpoint if it exist. */
   if (vc->std.bNumEndpoints) {
     /* Support for 1 endpoint only. */
-    TU_VERIFY(1 == vc->std.bNumEndpoints, NULL);
+    TU_VERIFY(1 == vc->std.bNumEndpoints);
     /* Find the notification endpoint descriptor. */
-    cur = videod_find_desc(cur, end, TUSB_DESC_ENDPOINT);
-    TU_VERIFY(cur < end, NULL);
+    cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT);
+    TU_VERIFY(cur < end);
     tusb_desc_endpoint_t const *notif = (tusb_desc_endpoint_t const *)cur;
     /* Open the notification endpoint */
-    TU_ASSERT(usbd_edpt_open(rhport, notif), NULL);
+    TU_ASSERT(usbd_edpt_open(rhport, notif));
     self->ep_notif = notif->bEndpointAddress;
   }
-  self->vc = vc;
-  return end;
+  self->ofs[0] = (void const*)vc - beg;
+  return true;
+}
+
+/** Set the specified alternate setting to own video control interface.
+ *
+ * @param[in,out] self     The context.
+ * @param[in]     itfnum   The target interface number. */
+static bool _close_vs_itf(uint8_t rhport, videod_interface_t *self, unsigned itfnum)
+{
+  uint16_t *ofs = _get_desc_ofs(self, itfnum);
+  if (!ofs) return true;
+  tusb_desc_vs_itf_t const *vs = (tusb_desc_vs_itf_t const*)(self->beg + *ofs);
+  /* The next of the video streaming interface header descriptor. */
+  void const *cur = (void const*)vs + vs->std.bLength + vs->stm.bLength;
+  /* The end of the video streaming interface descriptor. */
+  void const *end = cur + vs->stm.wTotalLength;
+  if (unsigned i = 0; i < vs->std.bNumEndpoints; ++i) {
+    cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT);
+    TU_ASSERT(cur < end);
+    tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const *)cur;
+    usbd_edpt_close(rhport, ep->bEndpointAddress);
+    cur += tu_desc_len(cur);
+  }
+  *ofs = 0;
+  return true;
 }
 
 /** Set the specified alternate setting to own video control interface.
  *
  * @param[in,out] self     The context.
  * @param[in]     itfnum   The target interface number.
- * @param[in]     altnum   The target alternate setting number.
+ * @param[in]     altnum   The target alternate setting number. */
+static bool _open_vs_itf(uint8_t rhport, videod_interface_t *self, unsigned itfnum, unsigned altnum)
+{
+  uint16_t   *ofs = NULL;
+  for (unsigned i = 1; i < sizeof(self->ofs)/sizeof(self->ofs[0]); ++i) {
+    if (!self->ofs[i]) {
+      ofs = &self->ofs[i];
+      break;
+    }
+  }
+  if (!ofs) return false;
+
+  tusb_desc_vc_itf_t const *vc = _get_desc_vc(self);
+  void const *end = self->beg + self->len;
+  /* Set the end of the video control interface descriptor. */
+  void const *cur = (void const*)vc + vc->std.bLength + vc->ctl.bLength + vc->ctl.wTotalLength;
+
+  cur = _find_desc_itf(cur, end, itfnum, altnum);
+  TU_VERIFY(cur < end);
+  tusb_desc_vs_itf_t const *vs = (tusb_desc_vs_itf_t const*)cur;
+  /* Support for up to 2 endpoint only. */
+  TU_ASSERT(vs->std.bNumEndpoints < 3);
+  /* Advance to the next descriptor after the class-specific VS interface header descriptor. */
+  cur += vs->std.bLength + vs->stm.bLength;
+  /* Update to point the end of the video control interface descriptor. */
+  end  = cur + vs->stm.wTotalLength;
+  for (unsigned i = 0; i < vs->std.bNumEndpoints; ++i) {
+    cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT);
+    TU_VERIFY(cur < end);
+    tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const *)cur;
+    TU_ASSERT(usbd_edpt_open(rhport, ep));
+    cur += tu_desc_len(cur);
+  }
+  *ofs = (void const*)vs - self->beg;
+  return true;
+}
+
+/** Set the specified alternate setting to own video control interface.
  *
- * @return The next descriptor after the video control interface descriptor.
- * @retval NULL   did not found interface descriptor or alternate setting */
-static void const* videod_set_vs_itf(videod_interface_t *self, unsigned itfnum, unsigned altnum)
+ * @param[in,out] self     The context.
+ * @param[in]     itfnum   The target interface number.
+ * @param[in]     altnum   The target alternate setting number. */
+static bool _set_vs_itf(uint8_t rhport, videod_interface_t *self, unsigned itfnum, unsigned altnum)
 {
   unsigned i;
-  tusb_desc_vc_itf_t const *vc = self->vc;
-  void const *end = self->end;
+  tusb_desc_vc_itf_t const *vc = _get_desc_vc(self);
+  void const *end = self->beg + self->len;
   /* Set the end of the video control interface descriptor. */
   void const *cur = (void const*)vc + vc->std.bLength + vc->ctl.bLength + vc->ctl.wTotalLength;
 
   /* Check itfnum is valid */
-  unsigned bInCollection = self->vc->ctl.bInCollection;
+  unsigned bInCollection = vc->ctl.bInCollection;
   for (i = 0; (i < bInCollection) && (vc->ctl.baInterfaceNr[i] != itfnum); ++i) ;
-  TU_VERIFY(i < bInCollection, NULL);
-  
-  cur = videod_find_desc_itf(cur, end, itfnum, altnum);
-  TU_VERIFY(cur < end, NULL);
+  TU_VERIFY(i < bInCollection);
+
+  cur = _find_desc_itf(cur, end, itfnum, altnum);
+  TU_VERIFY(cur < end);
   tusb_desc_vs_itf_t const *vs = (tusb_desc_vs_itf_t const*)cur;
   /* Advance to the next descriptor after the class-specific VS interface header descriptor. */
   cur += vs->std.bLength + vs->stm.bLength;
@@ -247,10 +372,10 @@ static void const* videod_set_vs_itf(videod_interface_t *self, unsigned itfnum,
   end  = cur + vs->stm.wTotalLength;
 
   switch (vs->stm.bDescriptorSubType) {
-  default: return end;
+  default: return false;
   case VIDEO_CS_VS_INTERFACE_INPUT_HEADER:
     /* Support for up to 2 endpoint only. */
-    TU_VERIFY(vc->std.bNumEndpoints < 3, NULL);
+    TU_VERIFY(vc->std.bNumEndpoints < 3);
     if (self->ep_sti) {
       usbd_edpt_close(rhport, self->ep_sti);
       self->ep_sti = 0;
@@ -260,18 +385,18 @@ static void const* videod_set_vs_itf(videod_interface_t *self, unsigned itfnum,
       self->ep_in  = 0;
     }
     if (i = 0; i < vs->std.bNumEndpoints; ++i) {
-      cur = videod_find_desc(cur, end, TUSB_DESC_ENDPOINT);
-      TU_VERIFY(cur < end, NULL);
+      cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT);
+      TU_VERIFY(cur < end);
       tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const *)cur;
       if (vs->stm.bEndpointAddress == ep->bEndpointAddress) {
         /* video input endpoint */
-        TU_ASSERT(!self->ep_in, NULL);
-        TU_ASSERT(usbd_edpt_open(rhport, ep), NULL);
+        TU_ASSERT(!self->ep_in);
+        TU_ASSERT(usbd_edpt_open(rhport, ep));
         self->ep_in  = ep->bEndpointAddress;
       } else {
         /* still image input endpoint */
-        TU_ASSERT(!self->ep_sti, NULL);
-        TU_ASSERT(usbd_edpt_open(rhport, ep), NULL);
+        TU_ASSERT(!self->ep_sti);
+        TU_ASSERT(usbd_edpt_open(rhport, ep));
         self->ep_sti = ep->bEndpointAddress;
       }
       cur += tu_desc_len(cur);
@@ -279,78 +404,245 @@ static void const* videod_set_vs_itf(videod_interface_t *self, unsigned itfnum,
     break;
   case VIDEO_CS_VS_INTERFACE_OUTPUT_HEADER:
     /* Support for up to 1 endpoint only. */
-    TU_VERIFY(vc->std.bNumEndpoints < 2, NULL);
+    TU_VERIFY(vc->std.bNumEndpoints < 2);
     if (self->ep_out) {
       usbd_edpt_close(rhport, self->ep_out);
       self->ep_out = 0;
     }
     if (vs->std.bNumEndpoints) {
-      cur = videod_find_desc(cur, end, TUSB_DESC_ENDPOINT);
-      TU_VERIFY(cur < end, NULL);
+      cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT);
+      TU_VERIFY(cur < end);
       tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const *)cur;
       if (vs->stm.bEndpointAddress == ep->bEndpointAddress) {
         /* video output endpoint */
-        TU_ASSERT(usbd_edpt_open(rhport, ep), NULL);
+        TU_ASSERT(usbd_edpt_open(rhport, ep));
         self->ep_out = ep->bEndpointAddress;
       }
     }
     break;
   }
+  for (unsigned i = 1; i < sizeof(self->ofs)/sizeof(ofs[0]); ++i) {
+    if (!self->ofs[i]) {
+      return true;
+    }
+    tusb_desc_interface_t const* itf = (tusb_desc_interface_t const*)(beg + ofs[i]);
+    if (itfnum == itf->bInterfaceNumber) return itf;
+  }
+  return NULL;
   for (i = 0; i < sizeof(self->vs)/sizeof(self->vs[0]); ++i) {
     if (!self->vs[i] || self->vs[i].stm.bInterfaceNumber == vs->stm.bInterfaceNumber) {
-      self->vs[i] = vs;
-      return end;
+      self->ofs[i] = (void const*)vs - self->beg;
+      return true;
     }
   }
-  return NULL;
+  return false;
 }
 
-static bool videod_get_itf(uint8_t rhport, videod_interface_t *self, tusb_control_request_t const * request)
+static int handle_video_ctl_std_req_get_itf(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request, unsigned itf)
 {
-  unsigned altnum = tu_u16_low(p_request->wValue);
-  unsigned itfnum = tu_u16_low(p_request->wLength);
+  if (stage != CONTROL_STAGE_SETUP)
+    return VIDEO_NO_ERROR;
+  videod_interface_t *self = &_videod_itf[itf];
+  TU_VERIFY(1 == request->wLength, VIDEO_UNKNOWN);
+  tusb_desc_interface_t const *p = _get_desc_cur_itf(self, itfnum);
+  if (!p) return VIDEO_UNKNOWN;
+  if (tud_control_xfer(rhport, request, &p->bAlternateSettings, sizeof(p->bAlternateSettings)))
+    return VIDEO_NO_ERROR;
+  return VIDEO_UNKNOWN;
+}
 
-  tusb_desc_vc_itf_t const *vc = self->vc;
-  if (vc->std.bInterfaceNumber == itfnum) {
-    tud_control_xfer(rhport, request, &vc->std.bAlternateSettings, sizeof(vc->std.bAlternateSettings));
-    return true;
+/** Handle a standard request to the video control interface. */
+static int handle_video_ctl_std_req(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request, unsigned itf)
+{
+  switch (p_request->bRequest) {
+  case TUSB_REQ_GET_INTERFACE:
+    handle_video_ctl_std_req_get_itf(rhport, stage, request, itf);
+  case TUSB_REQ_SET_INTERFACE:
+    if (stage != CONTROL_STAGE_SETUP) return VIDEO_NO_ERROR;
+    if (_set_vc_itf(rhport, &_videod_itf[itf], request->wValue))
+      return VIDEO_NO_ERROR;
+    return VIDEO_UNKNOWN;
+  default: /* Unknown/Unsupported request */
+    TU_BREAKPOINT();
+    return VIDEO_INVALID_REQUEST;
   }
-  for (unsigned i = 0; i < vc->ctl.bInCollection; ++i) {
-    tusb_desc_vs_itf_t const *vs = self->vs[i];
-    if (!vs || vs->std.bInterfaceNumber == itfnum) {
-      continue;
+}
+
+static int handle_video_ctl_cs_req(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request, unsigned itf)
+{
+  videod_interface_t *self = &_videod_itf[i];
+  /* 4.2.1 Interface Control Request */
+  switch (TU_U16_HIGH(request->wValue)) {
+  case VIDEO_VC_CTL_VIDEO_POWER_MODE:
+    switch (p_request->bRequest) {
+    case VIDEO_REQUEST_SET_CUR:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Set Power Mode\r\n");
+        TU_VERIFY(1 == request->wLength, VIDEO_UNKNOWN);
+        if (!tud_control_xfer(rhport, request, &self->power_mode, sizeof(self->power_mode)))
+          return VIDEO_UNKNOWN;
+      } else if (stage == CONTROL_STAGE_ACK) {
+        if (tud_video_power_mode_cb) return tud_video_power_mode_cb(itf, &self->power_mode);
+      }
+      return VIDEO_NO_ERROR;
+    case VIDEO_REQUEST_GET_CUR:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Get Power Mode\r\n");
+        TU_VERIFY(1 == request->wLength, VIDEO_UNKNOWN);
+        if (!tud_control_xfer(rhport, request, &self->power_mode, sizeof(self->power_mode)))
+          return VIDEO_UNKNOWN;
+      }
+      return VIDEO_NO_ERROR;
+    case VIDEO_REQUEST_GET_INFO:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Get Info Power Mode\r\n");
+        TU_VERIFY(1 == request->wLength, VIDEO_UNKNOWN);
+        if (!tud_control_xfer(rhport, request, &_cap_get_set, sizeof(_cap_get_set)))
+          return VIDEO_UNKNOWN;
+      }
+      return VIDEO_NO_ERROR;
+    default: break;
     }
-    tud_control_xfer(rhport, request, &vs->std.bAlternateSettings, sizeof(vs->std.bAlternateSettings));
-    return true;
+    break;
+  case VIDEO_VC_CTL_REQUEST_ERROR_CODE:
+    switch (p_request->bRequest) {
+    case VIDEO_REQUEST_GET_CUR:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Get Error Code\r\n");
+        if (!tud_control_xfer(rhport, request, &self->error_code, sizeof(self->error_code)))
+          return VIDEO_UNKNOWN;
+      }
+      return VIDEO_NO_ERROR;
+    case VIDEO_REQUEST_GET_INFO:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Get Info Error Code\r\n");
+        if (tud_control_xfer(rhport, request, &_cap_get, sizeof(_cap_get)))
+          return VIDEO_UNKNOWN;
+      }
+      return VIDEO_NO_ERROR;
+    default: break;
+    }
+    break;
+  default: break;
   }
-  return false;
+  /* Unknown/Unsupported request */
+  TU_BREAKPOINT();
+  return VIDEO_INVALID_REQUEST;
 }
 
-static bool videod_set_itf(uint8_t rhport, videod_interface_t *self, tusb_control_request_t const * request)
+static int handle_video_ctl_req(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request, unsigned itf)
 {
-  (void)rhport;
-  unsigned altnum = tu_u16_low(p_request->wValue);
-  unsigned itfnum = tu_u16_low(p_request->wLength);
+  switch (p_request->bmRequestType_bit.type) {
+  case TUSB_REQ_TYPE_STANDARD:
+    return handle_video_ctl_std_req(rhport, stage, request, itf);
+  case TUSB_REQ_TYPE_CLASS:
+    if (!TU_U16_HIGH(request->wIndex)) {
+      return handle_video_ctl_cs_req(rhport, stage, request, itf);
+    } else {
+      /* TODO: */
+      return VIDEO_INVALID_REQUEST;
+    }
+  default:
+    return VIDEO_INVALID_REQUEST;
+  }
+}
 
-  tusb_desc_vc_itf_t const *vc = self->vc;
-  if (vc->std.bInterfaceNumber == itfnum) {
-    if (videod_set_vc_itf(self, altnum))
-      return true;
-    return false;
+static int handle_video_stm_std_req(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request, unsigned itf)
+{
+  switch (p_request->bRequest) {
+  case TUSB_REQ_GET_INTERFACE:
+    handle_video_ctl_std_req_get_itf(rhport, stage, request, itf);
+  case TUSB_REQ_SET_INTERFACE:
+    videod_interface_t *self   = &_videod_itf[itf];
+    unsigned            itfnum = tu_u16_low(p_request->wIndex);
+    if (stage != CONTROL_STAGE_SETUP) return VIDEO_NO_ERROR;
+    if (_set_vs_itf(rhport, self, itfnum, request->wValue))
+      return VIDEO_NO_ERROR;
+    return VIDEO_UNKNOWN;
+  default: /* Unknown/Unsupported request */
+    TU_BREAKPOINT();
+    return VIDEO_INVALID_REQUEST;
   }
-  for (unsigned i = 0; i < vc->ctl.bInCollection; ++i) {
-    tusb_desc_vs_itf_t const *vs = self->vs[i];
-    if (!vs || vs->std.bInterfaceNumber == itfnum) {
-      continue;
+}
+
+static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request, unsigned itf)
+{
+  videod_interface_t *self = &_videod_itf[i];
+  /* 4.2.1 Interface Control Request */
+  switch (TU_U16_HIGH(request->wValue)) {
+  case VIDEO_VS_CTL_PROBE:
+    switch (p_request->bRequest) {
+    case VIDEO_REQUEST_SET_CUR:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Set Power Mode\r\n");
+        TU_VERIFY(1 == request->wLength, VIDEO_UNKNOWN);
+        if (!tud_control_xfer(rhport, request, &self->power_mode, sizeof(self->power_mode)))
+          return VIDEO_UNKNOWN;
+      } else if (stage == CONTROL_STAGE_ACK) {
+        if (tud_video_power_mode_cb) return tud_video_power_mode_cb(itf, &self->power_mode);
+      }
+      return VIDEO_NO_ERROR;
+    case VIDEO_REQUEST_GET_CUR:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Get Power Mode\r\n");
+        TU_VERIFY(1 == request->wLength, VIDEO_UNKNOWN);
+        if (!tud_control_xfer(rhport, request, &self->power_mode, sizeof(self->power_mode)))
+          return VIDEO_UNKNOWN;
+      }
+      return VIDEO_NO_ERROR;
+    case VIDEO_REQUEST_GET_INFO:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Get Info Power Mode\r\n");
+        TU_VERIFY(1 == request->wLength, VIDEO_UNKNOWN);
+        if (!tud_control_xfer(rhport, request, &_cap_get_set, sizeof(_cap_get_set)))
+          return VIDEO_UNKNOWN;
+      }
+      return VIDEO_NO_ERROR;
+    default: break;
     }
-    if (videod_set_vs_itf(self, itfnum, altnum))
-      return true;
-    return false;
+    break;
+  case VIDEO_VS_CTL_COMMIT:
+    switch (p_request->bRequest) {
+    case VIDEO_REQUEST_GET_CUR:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Get Error Code\r\n");
+        if (!tud_control_xfer(rhport, request, &self->error_code, sizeof(self->error_code)))
+          return VIDEO_UNKNOWN;
+      }
+      return VIDEO_NO_ERROR;
+    case VIDEO_REQUEST_GET_INFO:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG2("  Get Info Error Code\r\n");
+        if (tud_control_xfer(rhport, request, &_cap_get, sizeof(_cap_get)))
+          return VIDEO_UNKNOWN;
+      }
+      return VIDEO_NO_ERROR;
+    default: break;
+    }
+    break;
+  default: break;
   }
-  return false;
+  /* Unknown/Unsupported request */
+  TU_BREAKPOINT();
+  return VIDEO_INVALID_REQUEST;
+}
+
+static int handle_video_stm_req(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request, unsigned itf)
+{
+  switch (p_request->bmRequestType_bit.type) {
+  case TUSB_REQ_TYPE_STANDARD:
+    return handle_video_stm_std_req(rhport, stage, request, itf);
+  case TUSB_REQ_TYPE_CLASS:
+    if (TU_U16_HIGH(request->wIndex))
+      return VIDEO_INVALID_REQUEST;
+    return handle_video_stm_cs_req(rhport, stage, request, itf);
+  default:
+    return VIDEO_INVALID_REQUEST;
+  }
+  return VIDEO_UNKNOWN;
 }
 
-static void _prep_out_transaction (cdcd_interface_t* p_cdc)
+static void _prep_out_transaction(cdcd_interface_t* p_cdc)
 {
   uint8_t const rhport = TUD_OPT_RHPORT;
   uint16_t available = tu_fifo_remaining(&p_cdc->rx_ff);
@@ -440,31 +732,23 @@ uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
 
   void const *end = (void const*)itf_desc + max_len;
   self->beg = itf_desc;
-  self->end = end;
+  self->len = max_len;
   /*------------- Video Control Interface -------------*/
-  void const* cur = videod_set_vc_itf(self, 0);
-  TU_VERIFY(cur, 0);
-  unsigned bInCollection = self->vc->ctl.bInCollection;
+  if (!_open_vc_itf(rhport, self, 0)) return 0;
+  tusb_desc_vc_itf_t const *vc = _get_desc_vc(self);
+  unsigned bInCollection       = vc->ctl.bInCollection;
+  /* Update end */
+  void const *cur = _next_desc_itf(itf_desc, end);
+  for (unsigned i = 0; i < bInCollection; ++i) {
+    cur = _next_desc_itf(cur, end);
+  }
+  self->len = (uintptr_t)cur - (uintptr_t)itf_desc;
   /*------------- Video Stream Interface -------------*/
   unsigned itfnum = 0;
   for (unsigned i = 0; i < bInCollection; ++i) {
     itfnum = vc->ctl.baInterfaceNr[i];
-    cur = videod_set_vs_itf(self, itfnum, 0);
-    TU_VERIFY(cur, 0);
+    if (!_open_vs_itf(rhport, self, itfnum, 0)) return 0;
   }
-
-  /* Skip alternate setting interfaces */
-  while (cur < end && TUSB_DESC_INTERFACE == tu_desc_type(cur)) {
-    tusb_desc_vs_itf_t const *vs = (tusb_desc_vs_itf_t const *)cur;
-    if (itfnum                              != vs->std.bInterfaceNumber ||
-        TUSB_DESC_CS_INTERFACE              != vs->stm.bDescriptorType  ||
-        (VIDEO_CS_VS_INTERFACE_INPUT_HEADER != vs->stm.bDescriptorSubType &&
-         VIDEO_CS_VS_INTERFACE_OUTPUT_HEADER!= vs->stm.bDescriptorSubType)) {
-      break;
-    }
-    cur += itf->std.bLength + itf->stm.bLength + itf->stm.wTotalLength;
-  }
-  self->end = cur;
   return end - cur;
 }
 
@@ -473,61 +757,33 @@ uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
 // return false to stall control endpoint (e.g unsupported request)
 bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
 {
+  int err;
   if (p_request->bmRequestType_bit.recipient != TUSB_REQ_RCPT_INTERFACE) {
     return false;
   }
   unsigned itfnum = tu_u16_low(p_request->wIndex);
   /* Identify which interface to use */
-  videod_interface_t *self = NULL;
-  for (unsigned i = 0; i < CFG_TUD_VIDEO; ++i) {
-    if (_videod_itf[i].vc->bInterfaceNumber == itfnum) {
-      self = &_videod_itf[i];
+  int itf;
+  tusb_desc_vc_itf_t const *vc = NULL;
+  for (itf = 0; itf < CFG_TUD_VIDEO; ++itf) {
+    vc = _videod_itf[itf].vc;
+    if (!vc) continue;
+    unsigned beg_itfnum = vc->bInterfaceNumber;
+    unsigned end_itfnum = vc->ctl.bInCollection;
+    if (beg_itfnum <= itfnum && itfnum < end_itfnum)
       break;
-    }
-  }
-  if (!self) return false;
-
-  /* Standard request */
-  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) {
-    if (stage != CONTROL_STAGE_SETUP) return true;
-    switch (p_request->bRequest) {
-    case TUSB_REQ_GET_INTERFACE:
-      return videod_get_itf(rhport, self, request);
-    case TUSB_REQ_SET_INTERFACE:
-      return videod_set_itf(rhport, self, request);
-    default: /* Unknown/Unsupported request */
-      TU_BREAKPOINT();
-      return false;
-    }
   }
+  if (itf == CFG_TUD_VIDEO) return false;
 
-  unsigned cs  = TU_U16_HIGH(request->wValue);
-  unsigned uid = TU_U16_HIGH(request->wIndex);
-
-  switch (request->bRequest) {
-  case VIDEO_REQUEST_GET_INFO:
-    TU_VERIFY(1 == request->wLength);
-    
-    break;
-  case VIDEO_REQUEST_SET_CUR:
-    if (stage == CONTROL_STAGE_SETUP) {
-      TU_LOG2("  Set Current Setting Attribute\r\n");
-      tud_control_xfer(rhport, request, &p_video->line_coding, sizeof(cdc_line_coding_t));
-    } else if ( stage == CONTROL_STAGE_ACK) {
-      if ( tud_cdc_line_coding_cb ) tud_cdc_line_coding_cb(itf, &p_video->line_coding);
-    }
-    break;
-  case VIDEO_REQUEST_GET_CUR:
-    if (stage == CONTROL_STAGE_SETUP) {
-      TU_LOG2("  Set Current Setting Attribute\r\n");
-      tud_control_xfer(rhport, request, &p_video->line_coding, sizeof(cdc_line_coding_t));
-    } else if ( stage == CONTROL_STAGE_ACK) {
-      if ( tud_cdc_line_coding_cb ) tud_cdc_line_coding_cb(itf, &p_video->line_coding);
-    }
-    break;
-  default: return false; // stall unsupported request
+  if (itfnum == vc->bInterfaceNumber) {
+    /* To video control interface */
+    err = handle_video_ctl_req(rhport, stage, request, itf);
+  } else {
+    /* To video streaming interface */
+    err = handle_video_stm_req(rhport, stage, request, itf);
   }
-
+  _videod_itf[itf].error_code = (uint8_t)err;
+  if (err) return false;
   return true;
 }
 
diff --git a/src/class/video/video_device.h b/src/class/video/video_device.h
index e0ff21694..cd04b7391 100644
--- a/src/class/video/video_device.h
+++ b/src/class/video/video_device.h
@@ -28,8 +28,11 @@
 #ifndef TUSB_VIDEO_DEVICE_H_
 #define TUSB_VIDEO_DEVICE_H_
 
+#include "common/tusb_common.h"
+#include "video.h"
+
 #ifdef __cplusplus
- extern "C" {
+extern "C" {
 #endif
 
 //--------------------------------------------------------------------+
@@ -41,7 +44,7 @@ TU_ATTR_WEAK bool tud_video_get_info_cb(uint8_t rhport, tusb_control_request_t c
 // Invoked when GET_INFO request received
 TU_ATTR_WEAK bool tud_video_set_cur_cb(uint8_t rhport, tusb_control_request_t const *request);
 // Invoked when GET_CUR request received
-TU_ATTR_WEAK bool tud_video_get_info_cb(uint8_t rhport, tusb_control_request_t const *request);
+TU_ATTR_WEAK bool tud_video_get_cur_cb(uint8_t rhport, tusb_control_request_t const *request);
 
 //--------------------------------------------------------------------+
 // INTERNAL USBD-CLASS DRIVER API