drivers/media/platform/sti/delta/delta-mjpeg-hdr.c

   1 /*
   2  * Copyright (C) STMicroelectronics SA 2013
   3  * Author: Hugues Fruchet <hugues.fruchet@st.com> for STMicroelectronics.
   4  * License terms:  GNU General Public License (GPL), version 2
   5  */
   6
   7 #include "delta.h"
   8 #include "delta-mjpeg.h"
   9
  10 #define MJPEG_SOF_0  0xc0
  11 #define MJPEG_SOF_1  0xc1
  12 #define MJPEG_SOI    0xd8
  13 #define MJPEG_MARKER 0xff
  14
  15 static char *header_str(struct mjpeg_header *header,
  16                         char *str,
  17                         unsigned int len)
  18 {
  19         char *cur = str;
  20         unsigned int left = len;
  21
  22         if (!header)
  23                 return "";
  24
  25         snprintf(cur, left, "[MJPEG header]\n"
  26                         "|- length     = %d\n"
  27                         "|- precision  = %d\n"
  28                         "|- width      = %d\n"
  29                         "|- height     = %d\n"
  30                         "|- components = %d\n",
  31                         header->length,
  32                         header->sample_precision,
  33                         header->frame_width,
  34                         header->frame_height,
  35                         header->nb_of_components);
  36
  37         return str;
  38 }
  39
  40 static int delta_mjpeg_read_sof(struct delta_ctx *pctx,
  41                                 unsigned char *data, unsigned int size,
  42                                 struct mjpeg_header *header)
  43 {
  44         struct delta_dev *delta = pctx->dev;
  45         unsigned int offset = 0;
  46
  47         if (size < 64)
  48                 goto err_no_more;
  49
  50         memset(header, 0, sizeof(*header));
  51         header->length           = be16_to_cpu(*(__be16 *)(data + offset));
  52         offset += sizeof(u16);
  53         header->sample_precision = *(u8 *)(data + offset);
  54         offset += sizeof(u8);
  55         header->frame_height     = be16_to_cpu(*(__be16 *)(data + offset));
  56         offset += sizeof(u16);
  57         header->frame_width      = be16_to_cpu(*(__be16 *)(data + offset));
  58         offset += sizeof(u16);
  59         header->nb_of_components = *(u8 *)(data + offset);
  60         offset += sizeof(u8);
  61
  62         if (header->nb_of_components >= MJPEG_MAX_COMPONENTS) {
  63                 dev_err(delta->dev,
  64                         "%s   unsupported number of components (%d > %d)\n",
  65                         pctx->name, header->nb_of_components,
  66                         MJPEG_MAX_COMPONENTS);
  67                 return -EINVAL;
  68         }
  69
  70         if ((offset + header->nb_of_components *
  71              sizeof(header->components[0])) > size)
  72                 goto err_no_more;
  73
  74         return 0;
  75
  76 err_no_more:
  77         dev_err(delta->dev,
  78                 "%s   sof: reached end of %d size input stream\n",
  79                 pctx->name, size);
  80         return -ENODATA;
  81 }
  82
  83 int delta_mjpeg_read_header(struct delta_ctx *pctx,
  84                             unsigned char *data, unsigned int size,
  85                             struct mjpeg_header *header,
  86                             unsigned int *data_offset)
  87 {
  88         struct delta_dev *delta = pctx->dev;
  89         unsigned char str[200];
  90
  91         unsigned int ret = 0;
  92         unsigned int offset = 0;
  93         unsigned int soi = 0;
  94
  95         if (size < 2)
  96                 goto err_no_more;
  97
  98         offset = 0;
  99         while (1) {
 100                 if (data[offset] == MJPEG_MARKER)
 101                         switch (data[offset + 1]) {
 102                         case MJPEG_SOI:
 103                                 soi = 1;
 104                                 *data_offset = offset;
 105                                 break;
 106
 107                         case MJPEG_SOF_0:
 108                         case MJPEG_SOF_1:
 109                                 if (!soi) {
 110                                         dev_err(delta->dev,
 111                                                 "%s   wrong sequence, got SOF while SOI not seen\n",
 112                                                 pctx->name);
 113                                         return -EINVAL;
 114                                 }
 115
 116                                 ret = delta_mjpeg_read_sof(pctx,
 117                                                            &data[offset + 2],
 118                                                            size - (offset + 2),
 119                                                            header);
 120                                 if (ret)
 121                                         goto err;
 122
 123                                 goto done;
 124
 125                         default:
 126                                 break;
 127                         }
 128
 129                 offset++;
 130                 if ((offset + 2) >= size)
 131                         goto err_no_more;
 132         }
 133
 134 done:
 135         dev_dbg(delta->dev,
 136                 "%s   found header @ offset %d:\n%s", pctx->name,
 137                 *data_offset,
 138                 header_str(header, str, sizeof(str)));
 139         return 0;
 140
 141 err_no_more:
 142         dev_err(delta->dev,
 143                 "%s   no header found within %d bytes input stream\n",
 144                 pctx->name, size);
 145         return -ENODATA;
 146
 147 err:
 148         return ret;
 149 }