fix video payload head toggle

1 files changed, 5 insertions, 2 deletions

diff --git a/class/video/usbd_video.c b/class/video/usbd_video.c
index b04816c6..a21f324c 100644
--- a/class/video/usbd_video.c
+++ b/class/video/usbd_video.c
@@ -24,6 +24,8 @@ struct usbd_video_cfg_priv {
     .info[2] = { .bDescriptorSubtype = VIDEO_VC_PROCESSING_UNIT_DESCRIPTOR_SUBTYPE, .bEntityId = 0x02, .wTerminalType = 0x00 },
 };
 
+static bool head_toggle = false;
+
 static int usbd_video_control_request_handler(struct usb_setup_packet *setup, uint8_t **data, uint32_t *len)
 {
     uint8_t control_selector = (uint8_t)(setup->wValue >> 8);
@@ -694,6 +696,7 @@ static void video_notify_handler(uint8_t event, void *arg)
                 usbd_video_open(intf->bInterfaceNumber);
             } else {
                 usbd_video_close(intf->bInterfaceNumber);
+                head_toggle = false;
             }
         }
 
@@ -770,7 +773,7 @@ uint32_t usbd_video_mjpeg_payload_fill(uint8_t *input, uint32_t input_len, uint8
 
     for (size_t i = 0; i < packets; i++) {
         output[usbd_video_cfg.probe.dwMaxPayloadTransferSize * i] = 0x02;
-        output[usbd_video_cfg.probe.dwMaxPayloadTransferSize * i + 1] ^= 0x01;
+        output[usbd_video_cfg.probe.dwMaxPayloadTransferSize * i + 1] = head_toggle;
         if (i == (packets - 1)) {
             memcpy(&output[2 + usbd_video_cfg.probe.dwMaxPayloadTransferSize * i], &input[picture_pos], last_packet_size - 2);
         } else {
@@ -778,7 +781,7 @@ uint32_t usbd_video_mjpeg_payload_fill(uint8_t *input, uint32_t input_len, uint8
             picture_pos += usbd_video_cfg.probe.dwMaxPayloadTransferSize - 2;
         }
     }
-
+    head_toggle ^= 1;
     *out_len = (input_len + 2 * packets);
     return packets;
 }