delta-mjpeg-hdr.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Copyright (C) STMicroelectronics SA 2013
  4 * 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  0xc0
 11#define MJPEG_SOF_1  0xc1
 12#define MJPEG_SOI    0xd8
 13#define MJPEG_MARKER 0xff
 14
 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}