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 
header_str(struct mjpeg_header * header,char * str,unsigned int len)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 
delta_mjpeg_read_sof(struct delta_ctx * pctx,unsigned char * data,unsigned int size,struct mjpeg_header * header)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 
delta_mjpeg_read_header(struct delta_ctx * pctx,unsigned char * data,unsigned int size,struct mjpeg_header * header,unsigned int * data_offset)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 }
150