diff --git a/class/video/usb_video.h b/class/video/usb_video.h
index facfc11b..1f045717 100644
--- a/class/video/usb_video.h
+++ b/class/video/usb_video.h
@@ -154,7 +154,7 @@
 #define VIDEO_EU_PEAK_BIT_RATE_CONTROL       0x09U
 #define VIDEO_EU_QUANTIZATION_PARAMS_CONTROL 0x0AU
 #define VIDEO_EU_SYNC_REF_FRAME_CONTROL      0x0BU
-#define VIDEO_EU_LTR_BUFFER_                 CONTROL0x0CU
+#define VIDEO_EU_LTR_BUFFER_CONTROL          0x0CU
 #define VIDEO_EU_LTR_PICTURE_CONTROL         0x0DU
 #define VIDEO_EU_LTR_VALIDATION_CONTROL      0x0EU
 #define VIDEO_EU_LEVEL_IDC_LIMIT_CONTROL     0x0FU
@@ -806,4 +806,80 @@ struct video_still_probe_and_commit_controls {
                                           receive in a single payload transfer.*/
 } __PACKED;
 
+struct video_cs_vc_if_descriptor {
+    uint8_t bLength;
+    uint8_t bDescriptorType;
+    uint8_t bDescriptorSubType;
+    uint8_t bNumFormats;
+    uint16_t wTotalLength;
+    uint8_t bEndpointAddress;
+    uint8_t bmInfo;
+    uint8_t bTerminalLink;
+    uint8_t bStillCaptureMethod;
+    uint8_t bTriggerSupport;
+    uint8_t bTriggerUsage;
+    uint8_t bControlSize;
+    uint8_t bmaControls[];
+} __PACKED;
+
+struct video_cs_vs_input_header_descriptor {
+    uint8_t bLength;
+    uint8_t bDescriptorType;
+    uint8_t bDescriptorSubType;
+    uint8_t bNumFormats;
+    uint16_t wTotalLength;
+    uint8_t bEndpointAddress;
+    uint8_t bmInfo;
+    uint8_t bTerminalLink;
+    uint8_t bStillCaptureMethod;
+    uint8_t bTriggerSupport;
+    uint8_t bTriggerUsage;
+    uint8_t bControlSize;
+    uint8_t bmaControls[];
+} __PACKED;
+
+struct video_cs_vs_output_header_descriptor {
+    uint8_t bLength;
+    uint8_t bDescriptorType;
+    uint8_t bDescriptorSubType;
+    uint8_t bNumFormats;
+    uint16_t wTotalLength;
+    uint8_t bEndpointAddress;
+    uint8_t bTerminalLink;
+    uint8_t bControlSize;
+    uint8_t bmaControls[];
+} __PACKED;
+
+struct video_cs_vs_padload_format_descriptor {
+    uint8_t bLength;
+    uint8_t bDescriptorType;
+    uint8_t bDescriptorSubType;
+    uint8_t bNumFormats;
+    uint16_t wTotalLength;
+    uint8_t bEndpointAddress;
+    uint8_t bTerminalLink;
+    uint8_t bControlSize;
+    uint8_t bmaControls[];
+} __PACKED;
+
+struct video_cs_vs_frame_descriptor {
+    uint8_t bLength;
+    uint8_t bDescriptorType;
+    uint8_t bDescriptorSubType;
+    uint8_t bFrameIndex;
+    uint8_t bmCapabilities;
+    uint16_t wWidth;
+    uint16_t wHeight;
+    uint32_t dwMinBitRate;
+    uint32_t dwMaxBitRate;
+    uint32_t dwMaxVideoFrameBufferSize;
+    uint32_t dwDefaultFrameInterval;
+    uint8_t bFrameIntervalType;
+    uint32_t dwFrameInterval[];
+} __PACKED;
+
+struct video_entity_info {
+    uint8_t bDescriptorSubtype;
+    uint8_t bEntityId;
+};
 #endif /* USB_VIDEO_H_ */
diff --git a/class/video/usbd_video.c b/class/video/usbd_video.c
index ccf30da9..c2713d7f 100644
--- a/class/video/usbd_video.c
+++ b/class/video/usbd_video.c
@@ -22,113 +22,256 @@
 #include "usbd_core.h"
 #include "usbd_video.h"
 
-extern struct video_probe_and_commit_controls probe;
-extern struct video_probe_and_commit_controls commit;
+struct usbd_video_cfg_priv {
+    struct video_entity_info *entity_info;
+    uint8_t entity_count;
+    struct video_probe_and_commit_controls *probe;
+    struct video_probe_and_commit_controls *commit;
+    uint8_t power_mode;
+    uint8_t error_code;
+    uint8_t vcintf;
+    uint8_t vsintf;
+} usbd_video_cfg = { .vcintf = 0xff, .vsintf = 0xff };
 
-int video_class_request_handler(struct usb_setup_packet *setup, uint8_t **data, uint32_t *len)
+static int usbd_video_control_request_handler(struct usb_setup_packet *setup, uint8_t **data, uint32_t *len)
 {
-    USB_LOG_DBG("Video Class request: "
-                 "bRequest 0x%02x\r\n",
-                 setup->bRequest);
+    uint8_t cs = (uint8_t)(setup->wValue >> 8); /* control_selector */
 
-    switch (setup->bRequest) {
-        case VIDEO_REQUEST_SET_CUR:
-            if (setup->wValue == 256) {
-                memcpy((uint8_t *)&probe, *data, setup->wLength);
-            } else if (setup->wValue == 512) {
-                memcpy((uint8_t *)&commit, *data, setup->wLength);
+    switch (cs) {
+        case VIDEO_VC_VIDEO_POWER_MODE_CONTROL:
+            switch (setup->bRequest) {
+                case VIDEO_REQUEST_SET_CUR:
+                    break;
+                case VIDEO_REQUEST_GET_CUR:
+                    break;
+                case VIDEO_REQUEST_GET_INFO:
+                    break;
+                default:
+                    USB_LOG_WRN("Unhandled Video Class bRequest 0x%02x\r\n", setup->bRequest);
+                    return -1;
             }
 
             break;
-
-        case VIDEO_REQUEST_GET_CUR:
-            if (setup->wValue == 256) {
-                *data = (uint8_t *)&probe;
-            } else if (setup->wValue == 512) {
-                *data = (uint8_t *)&commit;
+        case VIDEO_VC_REQUEST_ERROR_CODE_CONTROL:
+            switch (setup->bRequest) {
+                case VIDEO_REQUEST_GET_CUR:
+                    break;
+                case VIDEO_REQUEST_GET_INFO:
+                    break;
+                default:
+                    USB_LOG_WRN("Unhandled Video Class bRequest 0x%02x\r\n", setup->bRequest);
+                    return -1;
             }
 
             break;
-
-        case VIDEO_REQUEST_GET_MIN:
-            if (setup->wValue == 256) {
-                *data = (uint8_t *)&probe;
-            } else if (setup->wValue == 512) {
-                *data = (uint8_t *)&commit;
-            }
-
-            break;
-
-        case VIDEO_REQUEST_GET_MAX:
-            if (setup->wValue == 256) {
-                *data = (uint8_t *)&probe;
-            } else if (setup->wValue == 512) {
-                *data = (uint8_t *)&commit;
-            }
-
-            break;
-
-        case VIDEO_REQUEST_GET_RES:
-
-            break;
-
-        case VIDEO_REQUEST_GET_LEN:
-
-            break;
-
-        case VIDEO_REQUEST_GET_INFO:
-
-            break;
-
-        case VIDEO_REQUEST_GET_DEF:
-            if (setup->wLength == 256) {
-                *data = (uint8_t *)&probe;
-            } else if (setup->wLength == 512) {
-                *data = (uint8_t *)&commit;
-            }
-
-            break;
-
         default:
-            USB_LOG_WRN("Unhandled Video Class bRequest 0x%02x\r\n", setup->bRequest);
-            return -1;
+            break;
     }
 
     return 0;
 }
 
-void video_notify_handler(uint8_t event, void *arg)
+static int usbd_video_control_unit_terminal_request_handler(struct usb_setup_packet *setup, uint8_t **data, uint32_t *len)
+{
+    uint8_t entity_id = (uint8_t)(setup->wIndex >> 8);
+    uint8_t cs = (uint8_t)(setup->wValue >> 8); /* control_selector */
+
+    for (uint8_t i = 0; i < usbd_video_cfg.entity_count; i++) {
+        if (usbd_video_cfg.entity_info[i].bEntityId == entity_id) {
+            switch (usbd_video_cfg.entity_info[i].bDescriptorSubtype) {
+                case VIDEO_VC_INPUT_TERMINAL_DESCRIPTOR_SUBTYPE:
+                    break;
+                case VIDEO_VC_OUTPUT_TERMINAL_DESCRIPTOR_SUBTYPE:
+                    break;
+                case VIDEO_VC_PROCESSING_UNIT_DESCRIPTOR_SUBTYPE:
+                    break;
+                    switch (setup->bRequest) {
+                        case VIDEO_REQUEST_SET_CUR:
+                            if (cs == VIDEO_PU_BRIGHTNESS_CONTROL) {
+                            }
+                            break;
+                        default:
+                            USB_LOG_WRN("Unhandled Video Class bRequest 0x%02x\r\n", setup->bRequest);
+                            return -1;
+                    }
+                default:
+                    break;
+            }
+        }
+    }
+    return 0;
+}
+
+static int usbd_video_stream_request_handler(struct usb_setup_packet *setup, uint8_t **data, uint32_t *len)
+{
+    uint8_t cs = (uint8_t)(setup->wValue >> 8); /* control_selector */
+
+    switch (cs) {
+        case VIDEO_VS_PROBE_CONTROL:
+            switch (setup->bRequest) {
+                case VIDEO_REQUEST_SET_CUR:
+                    memcpy((uint8_t *)usbd_video_cfg.probe, *data, setup->wLength);
+                    break;
+                case VIDEO_REQUEST_GET_CUR:
+                    *data = (uint8_t *)usbd_video_cfg.probe;
+                    *len = sizeof(struct video_probe_and_commit_controls);
+                    break;
+
+                case VIDEO_REQUEST_GET_MIN:
+                case VIDEO_REQUEST_GET_MAX:
+                case VIDEO_REQUEST_GET_RES:
+                case VIDEO_REQUEST_GET_DEF:
+                    *data = (uint8_t *)usbd_video_cfg.probe;
+                    *len = sizeof(struct video_probe_and_commit_controls);
+                    break;
+                case VIDEO_REQUEST_GET_LEN:
+                    (*data)[0] = sizeof(struct video_probe_and_commit_controls);
+                    *len = 1;
+                    break;
+
+                case VIDEO_REQUEST_GET_INFO:
+                    (*data)[0] = 0x03;
+                    *len = 1;
+                    break;
+
+                default:
+                    USB_LOG_WRN("Unhandled Video Class bRequest 0x%02x\r\n", setup->bRequest);
+                    return -1;
+            }
+            break;
+        case VIDEO_VS_COMMIT_CONTROL:
+            switch (setup->bRequest) {
+                case VIDEO_REQUEST_SET_CUR:
+                    memcpy((uint8_t *)usbd_video_cfg.commit, *data, setup->wLength);
+                    break;
+                case VIDEO_REQUEST_GET_CUR:
+                    *data = (uint8_t *)usbd_video_cfg.commit;
+                    *len = sizeof(struct video_probe_and_commit_controls);
+                    break;
+                case VIDEO_REQUEST_GET_MIN:
+                case VIDEO_REQUEST_GET_MAX:
+                case VIDEO_REQUEST_GET_RES:
+                case VIDEO_REQUEST_GET_DEF:
+                    *data = (uint8_t *)usbd_video_cfg.commit;
+                    *len = sizeof(struct video_probe_and_commit_controls);
+                    break;
+
+                case VIDEO_REQUEST_GET_LEN:
+                    (*data)[0] = sizeof(struct video_probe_and_commit_controls);
+                    *len = 1;
+                    break;
+
+                case VIDEO_REQUEST_GET_INFO:
+                    (*data)[0] = 0x03;
+                    *len = 1;
+                    break;
+
+                default:
+                    USB_LOG_WRN("Unhandled Video Class bRequest 0x%02x\r\n", setup->bRequest);
+                    return -1;
+            }
+            break;
+        case VIDEO_VS_STREAM_ERROR_CODE_CONTROL:
+            switch (setup->bRequest) {
+                case VIDEO_REQUEST_GET_CUR:
+                    *data = &usbd_video_cfg.error_code;
+                    *len = 1;
+                    break;
+                case VIDEO_REQUEST_GET_INFO:
+                    (*data)[0] = 0x01;
+                    *len = 1;
+                    break;
+                default:
+                    USB_LOG_WRN("Unhandled Video Class bRequest 0x%02x\r\n", setup->bRequest);
+                    return -1;
+            }
+            break;
+        default:
+            break;
+    }
+
+    return 0;
+}
+
+static int video_class_request_handler(struct usb_setup_packet *setup, uint8_t **data, uint32_t *len)
+{
+    USB_LOG_DBG("Video Class request: "
+                "bRequest 0x%02x\r\n",
+                setup->bRequest);
+
+    uint8_t intf_num = (uint8_t)setup->wIndex;
+    uint8_t entity_id = (uint8_t)(setup->wIndex >> 8);
+
+    if (usbd_video_cfg.vcintf == intf_num) { /* Video Control Interface */
+        if (entity_id == 0) {
+            return usbd_video_control_request_handler(setup, data, len); /* Interface Control Requests */
+        } else {
+            return usbd_video_control_unit_terminal_request_handler(setup, data, len); /* Unit and Terminal Requests */
+        }
+    } else if (usbd_video_cfg.vsintf == intf_num) {                 /* Video Stream Inteface */
+        return usbd_video_stream_request_handler(setup, data, len); /* Interface Stream Requests */
+    }
+    return -1;
+}
+
+static void video_notify_handler(uint8_t event, void *arg)
 {
     switch (event) {
         case USBD_EVENT_RESET:
-
+            usbd_video_cfg.error_code = 0;
+            usbd_video_cfg.power_mode = 0;
             break;
 
         case USBD_EVENT_SOF:
             usbd_video_sof_callback();
             break;
 
-        case USBD_EVENT_SET_INTERFACE:
-            usbd_video_set_interface_callback(((uint8_t *)arg)[3]);
-            break;
+        case USBD_EVENT_SET_INTERFACE: {
+            struct usb_interface_descriptor *intf = (struct usb_interface_descriptor *)arg;
+            usbd_video_set_interface_callback(intf->bAlternateSetting);
+        }
 
+        break;
         default:
             break;
     }
 }
 
-void usbd_video_add_interface(usbd_class_t *devclass, usbd_interface_t *intf)
+void usbd_video_add_interface(usbd_class_t *class, usbd_interface_t *intf)
 {
     static usbd_class_t *last_class = NULL;
 
-    if (last_class != devclass) {
-        last_class = devclass;
-        usbd_class_register(devclass);
+    if (last_class != class) {
+        last_class = class;
+        usbd_class_register(class);
     }
 
     intf->class_handler = video_class_request_handler;
     intf->custom_handler = NULL;
     intf->vendor_handler = NULL;
     intf->notify_handler = video_notify_handler;
-    usbd_class_add_interface(devclass, intf);
+    usbd_class_add_interface(class, intf);
+
+    if (usbd_video_cfg.vcintf == 0xff) {
+        usbd_video_cfg.vcintf = intf->intf_num;
+    } else if (usbd_video_cfg.vsintf == 0xff) {
+        usbd_video_cfg.vsintf = intf->intf_num;
+    }
+}
+
+void usbd_video_set_probe_and_commit_controls(struct video_probe_and_commit_controls *probe,
+                                              struct video_probe_and_commit_controls *commit)
+{
+    usbd_video_cfg.probe = probe;
+    usbd_video_cfg.commit = commit;
+}
+
+void usbd_video_set_entity_info(struct video_entity_info *info, uint8_t count)
+{
+    usbd_video_cfg.entity_info = info;
+    usbd_video_cfg.entity_count = count;
+}
+
+__WEAK void usbd_video_sof_callback(void)
+{
 }
\ No newline at end of file
diff --git a/class/video/usbd_video.h b/class/video/usbd_video.h
index c36bee78..d9d0e32a 100644
--- a/class/video/usbd_video.h
+++ b/class/video/usbd_video.h
@@ -29,9 +29,13 @@
 extern "C" {
 #endif
 
+void usbd_video_add_interface(usbd_class_t *class, usbd_interface_t *intf);
+
+void usbd_video_set_probe_and_commit_controls(struct video_probe_and_commit_controls *probe,
+                                              struct video_probe_and_commit_controls *commit);
+void usbd_video_set_entity_info(struct video_entity_info *info, uint8_t count);
 void usbd_video_sof_callback(void);
 void usbd_video_set_interface_callback(uint8_t value);
-void usbd_video_add_interface(usbd_class_t *devclass, usbd_interface_t *intf);
 
 #ifdef __cplusplus
 }
diff --git a/common/usb_mem.h b/common/usb_mem.h
index f036fb97..7f518b0c 100644
--- a/common/usb_mem.h
+++ b/common/usb_mem.h
@@ -24,11 +24,18 @@
 #define _USB_MEM_H
 
 #define DCACHE_LINE_SIZE 32
-#define DCACHE_LINEMASK (DCACHE_LINE_SIZE -1)
+#define DCACHE_LINEMASK  (DCACHE_LINE_SIZE - 1)
 
 #ifdef CONFIG_USB_DCACHE_ENABLE
+#ifdef CONFIG_USB_NOCACHE_RAM
+#define USB_MEM_ALIGN32
 #define USB_NOCACHE_RAM_SECTION __attribute__((section(".nocache_ram")))
 #else
+#define USB_MEM_ALIGN32 __attribute__((aligned(DCACHE_LINE_SIZE)))
+#define USB_NOCACHE_RAM_SECTION
+#endif
+#else
+#define USB_MEM_ALIGN32
 #define USB_NOCACHE_RAM_SECTION
 #endif
 
@@ -45,7 +52,7 @@ static inline void usb_free(void *ptr)
 #ifdef CONFIG_USB_DCACHE_ENABLE
 static inline void *usb_iomalloc(size_t size)
 {
-    size  = (size + DCACHE_LINEMASK) & ~DCACHE_LINEMASK;
+    size = (size + DCACHE_LINEMASK) & ~DCACHE_LINEMASK;
     return malloc(size);
 }