Quelle  delta-mjpeg-hdr.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (C) STMicroelectronics SA 2013
 * Author: Hugues Fruchet <hugues.fruchet@st.com> for STMicroelectronics.
 */

#include "delta.h"
#include "delta-mjpeg.h"

#define MJPEG_SOF_0  0xc0
#define MJPEG_SOF_1  0xc1
#define MJPEG_SOI    0xd8
#define MJPEG_MARKER 0xff

static char *header_str(struct mjpeg_header *header,
   char *str,
   unsigned int len)
{
 char *cur = str;
 unsigned int left = len;

 if (!header)
  return "";

 snprintf(cur, left, "[MJPEG header]\n"
   "|- length = %d\n"
   "|- precision = %d\n"
   "|- width = %d\n"
   "|- height = %d\n"
   "|- components = %d\n",
   header->length,
   header->sample_precision,
   header->frame_width,
   header->frame_height,
   header->nb_of_components);

 return str;
}

static int delta_mjpeg_read_sof(struct delta_ctx *pctx,
    unsigned char *data, unsigned int size,
    struct mjpeg_header *header)
{
 struct delta_dev *delta = pctx->dev;
 unsigned int offset = 0;

 if (size < 64)
  goto err_no_more;

 memset(header, 0, sizeof(*header));
 header->length           = be16_to_cpu(*(__be16 *)(data + offset));
 offset += sizeof(u16);
 header->sample_precision = *(u8 *)(data + offset);
 offset += sizeof(u8);
 header->frame_height     = be16_to_cpu(*(__be16 *)(data + offset));
 offset += sizeof(u16);
 header->frame_width      = be16_to_cpu(*(__be16 *)(data + offset));
 offset += sizeof(u16);
 header->nb_of_components = *(u8 *)(data + offset);
 offset += sizeof(u8);

 if (header->nb_of_components >= MJPEG_MAX_COMPONENTS) {
  dev_err(delta->dev,
   "%s unsupported number of components (%d > %d)\n",
   pctx->name, header->nb_of_components,
   MJPEG_MAX_COMPONENTS);
  return -EINVAL;
 }

 if ((offset + header->nb_of_components *
      sizeof(header->components[0])) > size)
  goto err_no_more;

 return 0;

err_no_more:
 dev_err(delta->dev,
  "%s sof: reached end of %d size input stream\n",
  pctx->name, size);
 return -ENODATA;
}

int delta_mjpeg_read_header(struct delta_ctx *pctx,
       unsigned char *data, unsigned int size,
       struct mjpeg_header *header,
       unsigned int *data_offset)
{
 struct delta_dev *delta = pctx->dev;
 unsigned char str[200];

 unsigned int ret = 0;
 unsigned int offset = 0;
 unsigned int soi = 0;

 if (size < 2)
  goto err_no_more;

 offset = 0;
 while (1) {
  if (data[offset] == MJPEG_MARKER)
   switch (data[offset + 1]) {
   case MJPEG_SOI:
    soi = 1;
    *data_offset = offset;
    break;

   case MJPEG_SOF_0:
   case MJPEG_SOF_1:
    if (!soi) {
     dev_err(delta->dev,
      "%s wrong sequence, got SOF while SOI not seen\n",
      pctx->name);
     return -EINVAL;
    }

    ret = delta_mjpeg_read_sof(pctx,
          &data[offset + 2],
          size - (offset + 2),
          header);
    if (ret)
     goto err;

    goto done;

   default:
    break;
   }

  offset++;
  if ((offset + 2) >= size)
   goto err_no_more;
 }

done:
 dev_dbg(delta->dev,
  "%s found header @ offset %d:\n%s", pctx->name,
  *data_offset,
  header_str(header, str, sizeof(str)));
 return 0;

err_no_more:
 dev_err(delta->dev,
  "%s no header found within %d bytes input stream\n",
  pctx->name, size);
 return -ENODATA;

err:
 return ret;
}