[PATCH v2] uvcvideo: Unify UVC payload header parsing.

From: James Hilliard
Date: Sat Feb 18 2023 - 18:43:57 EST

From: Pawel Osciak <posciak@xxxxxxxxxxxx>

Create a separate function for parsing UVC payload headers and extract code
from other functions into it. Store the parsed values in a header struct.

Signed-off-by: Pawel Osciak <posciak@xxxxxxxxxxxx>
Signed-off-by: James Hilliard <james.hilliard1@xxxxxxxxx>
---
Changes v1 -> v2:
- rebase
---
drivers/media/usb/uvc/uvc_video.c | 263 +++++++++++++++---------------
drivers/media/usb/uvc/uvcvideo.h | 21 +++
2 files changed, 152 insertions(+), 132 deletions(-)

diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index d2eb9066e4dc..ef6ac978ef87 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -466,39 +466,14 @@ static inline ktime_t uvc_video_get_time(void)

static void
uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
- const u8 *data, int len)
+ struct uvc_payload_header *header)
{
struct uvc_clock_sample *sample;
- unsigned int header_size;
- bool has_pts = false;
- bool has_scr = false;
unsigned long flags;
ktime_t time;
u16 host_sof;
u16 dev_sof;

- switch (data[1] & (UVC_STREAM_PTS | UVC_STREAM_SCR)) {
- case UVC_STREAM_PTS | UVC_STREAM_SCR:
- header_size = 12;
- has_pts = true;
- has_scr = true;
- break;
- case UVC_STREAM_PTS:
- header_size = 6;
- has_pts = true;
- break;
- case UVC_STREAM_SCR:
- header_size = 8;
- has_scr = true;
- break;
- default:
- header_size = 2;
- break;
- }
-
- /* Check for invalid headers. */
- if (len < header_size)
- return;

/*
* Extract the timestamps:
@@ -508,17 +483,17 @@ uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
* kernel timestamps and store them with the SCR STC and SOF fields
* in the ring buffer
*/
- if (has_pts && buf != NULL)
- buf->pts = get_unaligned_le32(&data[2]);
+ if (header->has_pts && buf != NULL)
+ buf->pts = header->pts;

- if (!has_scr)
+ if (!header->has_scr)
return;

/*
* To limit the amount of data, drop SCRs with an SOF identical to the
* previous one.
*/
- dev_sof = get_unaligned_le16(&data[header_size - 2]);
+ dev_sof = header->sof;
if (dev_sof == stream->clock.last_sof)
return;

@@ -560,7 +535,7 @@ uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
spin_lock_irqsave(&stream->clock.lock, flags);

sample = &stream->clock.samples[stream->clock.head];
- sample->dev_stc = get_unaligned_le32(&data[header_size - 6]);
+ sample->dev_stc = header->stc;
sample->dev_sof = dev_sof;
sample->host_sof = host_sof;
sample->host_time = time;
@@ -804,65 +779,24 @@ void uvc_video_clock_update(struct uvc_streaming *stream,
*/

static void uvc_video_stats_decode(struct uvc_streaming *stream,
- const u8 *data, int len)
+ struct uvc_payload_header *header)
{
- unsigned int header_size;
- bool has_pts = false;
- bool has_scr = false;
- u16 scr_sof;
- u32 scr_stc;
- u32 pts;
-
if (stream->stats.stream.nb_frames == 0 &&
stream->stats.frame.nb_packets == 0)
stream->stats.stream.start_ts = ktime_get();

- switch (data[1] & (UVC_STREAM_PTS | UVC_STREAM_SCR)) {
- case UVC_STREAM_PTS | UVC_STREAM_SCR:
- header_size = 12;
- has_pts = true;
- has_scr = true;
- break;
- case UVC_STREAM_PTS:
- header_size = 6;
- has_pts = true;
- break;
- case UVC_STREAM_SCR:
- header_size = 8;
- has_scr = true;
- break;
- default:
- header_size = 2;
- break;
- }
-
- /* Check for invalid headers. */
- if (len < header_size || data[0] < header_size) {
- stream->stats.frame.nb_invalid++;
- return;
- }
-
- /* Extract the timestamps. */
- if (has_pts)
- pts = get_unaligned_le32(&data[2]);
-
- if (has_scr) {
- scr_stc = get_unaligned_le32(&data[header_size - 6]);
- scr_sof = get_unaligned_le16(&data[header_size - 2]);
- }
-
/* Is PTS constant through the whole frame ? */
- if (has_pts && stream->stats.frame.nb_pts) {
- if (stream->stats.frame.pts != pts) {
+ if (header->has_pts && stream->stats.frame.nb_pts) {
+ if (stream->stats.frame.pts != header->pts) {
stream->stats.frame.nb_pts_diffs++;
stream->stats.frame.last_pts_diff =
stream->stats.frame.nb_packets;
}
}

- if (has_pts) {
+ if (header->has_pts) {
stream->stats.frame.nb_pts++;
- stream->stats.frame.pts = pts;
+ stream->stats.frame.pts = header->pts;
}

/*
@@ -870,49 +804,49 @@ static void uvc_video_stats_decode(struct uvc_streaming *stream,
* their first empty packet ?
*/
if (stream->stats.frame.size == 0) {
- if (len > header_size)
- stream->stats.frame.has_initial_pts = has_pts;
- if (len == header_size && has_pts)
+ if (header->payload_size > 0)
+ stream->stats.frame.has_initial_pts = header->has_pts;
+ if (header->payload_size == 0 && header->has_pts)
stream->stats.frame.has_early_pts = true;
}

/* Do the SCR.STC and SCR.SOF fields vary through the frame ? */
- if (has_scr && stream->stats.frame.nb_scr) {
- if (stream->stats.frame.scr_stc != scr_stc)
+ if (header->has_scr && stream->stats.frame.nb_scr) {
+ if (stream->stats.frame.scr_stc != header->stc)
stream->stats.frame.nb_scr_diffs++;
}

- if (has_scr) {
+ if (header->has_scr) {
/* Expand the SOF counter to 32 bits and store its value. */
if (stream->stats.stream.nb_frames > 0 ||
stream->stats.frame.nb_scr > 0)
stream->stats.stream.scr_sof_count +=
- (scr_sof - stream->stats.stream.scr_sof) % 2048;
- stream->stats.stream.scr_sof = scr_sof;
+ (header->sof - stream->stats.stream.scr_sof) % 2048;
+ stream->stats.stream.scr_sof = header->sof;

stream->stats.frame.nb_scr++;
- stream->stats.frame.scr_stc = scr_stc;
- stream->stats.frame.scr_sof = scr_sof;
+ stream->stats.frame.scr_stc = header->stc;
+ stream->stats.frame.scr_sof = header->sof;

- if (scr_sof < stream->stats.stream.min_sof)
- stream->stats.stream.min_sof = scr_sof;
- if (scr_sof > stream->stats.stream.max_sof)
- stream->stats.stream.max_sof = scr_sof;
+ if (header->sof < stream->stats.stream.min_sof)
+ stream->stats.stream.min_sof = header->sof;
+ if (header->sof > stream->stats.stream.max_sof)
+ stream->stats.stream.max_sof = header->sof;
}

/* Record the first non-empty packet number. */
- if (stream->stats.frame.size == 0 && len > header_size)
+ if (stream->stats.frame.size == 0 && header->payload_size > 0)
stream->stats.frame.first_data = stream->stats.frame.nb_packets;

/* Update the frame size. */
- stream->stats.frame.size += len - header_size;
+ stream->stats.frame.size += header->payload_size;

/* Update the packets counters. */
stream->stats.frame.nb_packets++;
- if (len <= header_size)
+ if (header->payload_size == 0)
stream->stats.frame.nb_empty++;

- if (data[1] & UVC_STREAM_ERR)
+ if (header->has_err)
stream->stats.frame.nb_errors++;
}

@@ -1047,22 +981,9 @@ static void uvc_video_stats_stop(struct uvc_streaming *stream)
* uvc_video_decode_end will never be called with a NULL buffer.
*/
static int uvc_video_decode_start(struct uvc_streaming *stream,
- struct uvc_buffer *buf, const u8 *data, int len)
+ struct uvc_buffer *buf, struct uvc_payload_header *header)
{
- u8 fid;
-
- /*
- * Sanity checks:
- * - packet must be at least 2 bytes long
- * - bHeaderLength value must be at least 2 bytes (see above)
- * - bHeaderLength value can't be larger than the packet size.
- */
- if (len < 2 || data[0] < 2 || data[0] > len) {
- stream->stats.frame.nb_invalid++;
- return -EINVAL;
- }
-
- fid = data[1] & UVC_STREAM_FID;
+ u8 fid = header->fid;

/*
* Increase the sequence number regardless of any buffer states, so
@@ -1074,8 +995,8 @@ static int uvc_video_decode_start(struct uvc_streaming *stream,
uvc_video_stats_update(stream);
}

- uvc_video_clock_decode(stream, buf, data, len);
- uvc_video_stats_decode(stream, data, len);
+ uvc_video_clock_decode(stream, buf, header);
+ uvc_video_stats_decode(stream, header);

/*
* Store the payload FID bit and return immediately when the buffer is
@@ -1087,7 +1008,7 @@ static int uvc_video_decode_start(struct uvc_streaming *stream,
}

/* Mark the buffer as bad if the error bit is set. */
- if (data[1] & UVC_STREAM_ERR) {
+ if (header->has_err) {
uvc_dbg(stream->dev, FRAME,
"Marking buffer as bad (error bit set)\n");
buf->error = 1;
@@ -1107,7 +1028,7 @@ static int uvc_video_decode_start(struct uvc_streaming *stream,
uvc_dbg(stream->dev, FRAME,
"Dropping payload (out of sync)\n");
if ((stream->dev->quirks & UVC_QUIRK_STREAM_NO_FID) &&
- (data[1] & UVC_STREAM_EOF))
+ (header->has_eof))
stream->last_fid ^= UVC_STREAM_FID;
return -ENODATA;
}
@@ -1145,7 +1066,7 @@ static int uvc_video_decode_start(struct uvc_streaming *stream,

stream->last_fid = fid;

- return data[0];
+ return 0;
}

static inline enum dma_data_direction uvc_stream_dir(
@@ -1232,12 +1153,12 @@ static void uvc_video_decode_data(struct uvc_urb *uvc_urb,
}

static void uvc_video_decode_end(struct uvc_streaming *stream,
- struct uvc_buffer *buf, const u8 *data, int len)
+ struct uvc_buffer *buf, struct uvc_payload_header *header)
{
/* Mark the buffer as done if the EOF marker is set. */
- if (data[1] & UVC_STREAM_EOF && buf->bytesused != 0) {
+ if (header->has_eof && buf->bytesused != 0) {
uvc_dbg(stream->dev, FRAME, "Frame complete (EOF found)\n");
- if (data[0] == len)
+ if (header->payload_size == 0)
uvc_dbg(stream->dev, FRAME, "EOF in empty payload\n");
buf->state = UVC_BUF_STATE_READY;
if (stream->dev->quirks & UVC_QUIRK_STREAM_NO_FID)
@@ -1285,6 +1206,75 @@ static int uvc_video_encode_data(struct uvc_streaming *stream,
return nbytes;
}

+static int uvc_video_parse_header(struct uvc_streaming *stream,
+ const __u8 *data, int len, struct uvc_payload_header *header)
+{
+ unsigned int off = 2;
+
+ /* Sanity checks:
+ * - packet must be at least 2 bytes long
+ * - bHeaderLength value must be at least 2 bytes (see above)
+ */
+ if (len < 2 || len < data[0] || data[0] < 2 )
+ goto error;
+
+ header->length = 2; /* 1 byte of header length + 1 byte of BFH. */
+
+ header->has_sli = false;
+ header->has_eof = data[1] & UVC_STREAM_EOF;
+ header->has_pts = data[1] & UVC_STREAM_PTS;
+ header->has_scr = data[1] & UVC_STREAM_SCR;
+ header->has_err = data[1] & UVC_STREAM_ERR;
+
+ if (header->has_pts)
+ header->length += 4;
+
+ if (header->has_scr)
+ header->length += 6;
+
+ if (stream->cur_format->fcc == V4L2_PIX_FMT_VP8) {
+ /* VP8 payload has 2 additional bytes of BFH. */
+ header->length += 2;
+ off += 2;
+
+ /* SLI always present for VP8 simulcast (at the end of header),
+ * allowed for VP8 non-simulcast.
+ */
+ header->has_sli = data[1] & UVC_STREAM_EOH;
+ if (header->has_sli)
+ header->length += 2;
+ }
+
+ /* - bHeaderLength value can't be larger than the packet size. */
+ if (data[0] != header->length)
+ goto error;
+
+ /* PTS 4 bytes, STC 4 bytes, SOF 2 bytes. */
+ if (header->has_pts) {
+ header->pts = get_unaligned_le32(&data[off]);
+ off += 4;
+ }
+
+ if (header->has_scr) {
+ header->stc = get_unaligned_le32(&data[off]);
+ off += 4;
+ header->sof = get_unaligned_le16(&data[off]);
+ off += 2;
+ }
+
+ if (header->has_sli)
+ header->sli = get_unaligned_le16(&data[off]);
+
+ header->payload_size = len - header->length;
+ header->fid = data[1] & UVC_STREAM_FID;
+
+ return 0;
+
+error:
+ stream->stats.frame.nb_invalid++;
+ return -EINVAL;
+}
+
/* ------------------------------------------------------------------------
* Metadata
*/
@@ -1409,7 +1399,9 @@ static void uvc_video_decode_isoc(struct uvc_urb *uvc_urb,
struct uvc_buffer *buf, struct uvc_buffer *meta_buf)
{
struct urb *urb = uvc_urb->urb;
+ struct uvc_payload_header header;
struct uvc_streaming *stream = uvc_urb->stream;
+ unsigned int len;
u8 *mem;
int ret, i;

@@ -1424,11 +1416,15 @@ static void uvc_video_decode_isoc(struct uvc_urb *uvc_urb,
continue;
}

- /* Decode the payload header. */
mem = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
+ len = urb->iso_frame_desc[i].actual_length;
+
+ ret = uvc_video_parse_header(stream, mem, len, &header);
+ if (ret < 0)
+ continue;
+
do {
- ret = uvc_video_decode_start(stream, buf, mem,
- urb->iso_frame_desc[i].actual_length);
+ ret = uvc_video_decode_start(stream, buf, &header);
if (ret == -EAGAIN)
uvc_video_next_buffers(stream, &buf, &meta_buf);
} while (ret == -EAGAIN);
@@ -1439,12 +1435,11 @@ static void uvc_video_decode_isoc(struct uvc_urb *uvc_urb,
uvc_video_decode_meta(stream, meta_buf, mem, ret);

/* Decode the payload data. */
- uvc_video_decode_data(uvc_urb, buf, mem + ret,
- urb->iso_frame_desc[i].actual_length - ret);
+ uvc_video_decode_data(uvc_urb, buf, mem + header.length,
+ urb->iso_frame_desc[i].actual_length - header.length);

/* Process the header again. */
- uvc_video_decode_end(stream, buf, mem,
- urb->iso_frame_desc[i].actual_length);
+ uvc_video_decode_end(stream, buf, &header);

if (buf->state == UVC_BUF_STATE_READY)
uvc_video_next_buffers(stream, &buf, &meta_buf);
@@ -1455,6 +1450,7 @@ static void uvc_video_decode_bulk(struct uvc_urb *uvc_urb,
struct uvc_buffer *buf, struct uvc_buffer *meta_buf)
{
struct urb *urb = uvc_urb->urb;
+ struct uvc_payload_header header;
struct uvc_streaming *stream = uvc_urb->stream;
u8 *mem;
int len, ret;
@@ -1475,8 +1471,12 @@ static void uvc_video_decode_bulk(struct uvc_urb *uvc_urb,
* header.
*/
if (stream->bulk.header_size == 0 && !stream->bulk.skip_payload) {
+ ret = uvc_video_parse_header(stream, mem, len, &header);
+ if (ret < 0)
+ return;
+
do {
- ret = uvc_video_decode_start(stream, buf, mem, len);
+ ret = uvc_video_decode_start(stream, buf, &header);
if (ret == -EAGAIN)
uvc_video_next_buffers(stream, &buf, &meta_buf);
} while (ret == -EAGAIN);
@@ -1485,13 +1485,13 @@ static void uvc_video_decode_bulk(struct uvc_urb *uvc_urb,
if (ret < 0 || buf == NULL) {
stream->bulk.skip_payload = 1;
} else {
- memcpy(stream->bulk.header, mem, ret);
- stream->bulk.header_size = ret;
+ memcpy(stream->bulk.header, mem, header.length);
+ stream->bulk.header_size = header.length;

uvc_video_decode_meta(stream, meta_buf, mem, ret);

- mem += ret;
- len -= ret;
+ mem += header.length;
+ len -= header.length;
}
}

@@ -1512,8 +1512,7 @@ static void uvc_video_decode_bulk(struct uvc_urb *uvc_urb,
if (urb->actual_length < urb->transfer_buffer_length ||
stream->bulk.payload_size >= stream->bulk.max_payload_size) {
if (!stream->bulk.skip_payload && buf != NULL) {
- uvc_video_decode_end(stream, buf, stream->bulk.header,
- stream->bulk.payload_size);
+ uvc_video_decode_end(stream, buf, &header);
if (buf->state == UVC_BUF_STATE_READY)
uvc_video_next_buffers(stream, &buf, &meta_buf);
}
diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h
index df93db259312..2678c680f6bc 100644
--- a/drivers/media/usb/uvc/uvcvideo.h
+++ b/drivers/media/usb/uvc/uvcvideo.h
@@ -425,6 +425,27 @@ struct uvc_urb {
struct work_struct work;
};

+struct uvc_payload_header {
+ bool has_eof;
+
+ bool has_pts;
+ u32 pts;
+
+ bool has_scr;
+ u16 sof;
+ u32 stc;
+
+ bool has_sli;
+ u16 sli;
+
+ u8 fid;
+
+ bool has_err;
+
+ int length;
+ int payload_size;
+};
+
struct uvc_streaming {
struct list_head list;
struct uvc_device *dev;
--
2.34.1