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