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}
150