150 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (C) STMicroelectronics SA 20134 * Author: Hugues Fruchet <hugues.fruchet@st.com> for STMicroelectronics.5 */6 7#include "delta.h"8#include "delta-mjpeg.h"9 10#define MJPEG_SOF_0 0xc011#define MJPEG_SOF_1 0xc112#define MJPEG_SOI 0xd813#define MJPEG_MARKER 0xff14 15static 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 40static 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 76err_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 83int 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 134done: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 141err_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 147err:148 return ret;149}150