1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * vimc-common.c Virtual Media Controller Driver
4  *
5  * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
6  */
7 
8 #include <linux/init.h>
9 #include <linux/module.h>
10 
11 #include "vimc-common.h"
12 
13 /*
14  * NOTE: non-bayer formats need to come first (necessary for enum_mbus_code
15  * in the scaler)
16  */
17 static const struct vimc_pix_map vimc_pix_map_list[] = {
18 	/* TODO: add all missing formats */
19 
20 	/* RGB formats */
21 	{
22 		.code = {
23 			MEDIA_BUS_FMT_BGR888_1X24,
24 			MEDIA_BUS_FMT_BGR888_3X8
25 		},
26 		.pixelformat = V4L2_PIX_FMT_BGR24,
27 		.bpp = 3,
28 		.bayer = false,
29 	},
30 	{
31 		.code = {
32 			MEDIA_BUS_FMT_RGB888_1X24,
33 			MEDIA_BUS_FMT_RGB888_2X12_BE,
34 			MEDIA_BUS_FMT_RGB888_2X12_LE,
35 			MEDIA_BUS_FMT_RGB888_3X8,
36 			MEDIA_BUS_FMT_RGB888_1X7X4_SPWG,
37 			MEDIA_BUS_FMT_RGB888_1X7X4_JEIDA,
38 			MEDIA_BUS_FMT_RGB888_1X32_PADHI,
39 			MEDIA_BUS_FMT_GBR888_1X24
40 		},
41 		.pixelformat = V4L2_PIX_FMT_RGB24,
42 		.bpp = 3,
43 		.bayer = false,
44 	},
45 	{
46 		.code = { MEDIA_BUS_FMT_ARGB8888_1X32 },
47 		.pixelformat = V4L2_PIX_FMT_ARGB32,
48 		.bpp = 4,
49 		.bayer = false,
50 	},
51 
52 	/* Bayer formats */
53 	{
54 		.code = { MEDIA_BUS_FMT_SBGGR8_1X8 },
55 		.pixelformat = V4L2_PIX_FMT_SBGGR8,
56 		.bpp = 1,
57 		.bayer = true,
58 	},
59 	{
60 		.code = { MEDIA_BUS_FMT_SGBRG8_1X8 },
61 		.pixelformat = V4L2_PIX_FMT_SGBRG8,
62 		.bpp = 1,
63 		.bayer = true,
64 	},
65 	{
66 		.code = { MEDIA_BUS_FMT_SGRBG8_1X8 },
67 		.pixelformat = V4L2_PIX_FMT_SGRBG8,
68 		.bpp = 1,
69 		.bayer = true,
70 	},
71 	{
72 		.code = { MEDIA_BUS_FMT_SRGGB8_1X8 },
73 		.pixelformat = V4L2_PIX_FMT_SRGGB8,
74 		.bpp = 1,
75 		.bayer = true,
76 	},
77 	{
78 		.code = { MEDIA_BUS_FMT_SBGGR10_1X10 },
79 		.pixelformat = V4L2_PIX_FMT_SBGGR10,
80 		.bpp = 2,
81 		.bayer = true,
82 	},
83 	{
84 		.code = { MEDIA_BUS_FMT_SGBRG10_1X10 },
85 		.pixelformat = V4L2_PIX_FMT_SGBRG10,
86 		.bpp = 2,
87 		.bayer = true,
88 	},
89 	{
90 		.code = { MEDIA_BUS_FMT_SGRBG10_1X10 },
91 		.pixelformat = V4L2_PIX_FMT_SGRBG10,
92 		.bpp = 2,
93 		.bayer = true,
94 	},
95 	{
96 		.code = { MEDIA_BUS_FMT_SRGGB10_1X10 },
97 		.pixelformat = V4L2_PIX_FMT_SRGGB10,
98 		.bpp = 2,
99 		.bayer = true,
100 	},
101 
102 	/* 10bit raw bayer a-law compressed to 8 bits */
103 	{
104 		.code = { MEDIA_BUS_FMT_SBGGR10_ALAW8_1X8 },
105 		.pixelformat = V4L2_PIX_FMT_SBGGR10ALAW8,
106 		.bpp = 1,
107 		.bayer = true,
108 	},
109 	{
110 		.code = { MEDIA_BUS_FMT_SGBRG10_ALAW8_1X8 },
111 		.pixelformat = V4L2_PIX_FMT_SGBRG10ALAW8,
112 		.bpp = 1,
113 		.bayer = true,
114 	},
115 	{
116 		.code = { MEDIA_BUS_FMT_SGRBG10_ALAW8_1X8 },
117 		.pixelformat = V4L2_PIX_FMT_SGRBG10ALAW8,
118 		.bpp = 1,
119 		.bayer = true,
120 	},
121 	{
122 		.code = { MEDIA_BUS_FMT_SRGGB10_ALAW8_1X8 },
123 		.pixelformat = V4L2_PIX_FMT_SRGGB10ALAW8,
124 		.bpp = 1,
125 		.bayer = true,
126 	},
127 
128 	/* 10bit raw bayer DPCM compressed to 8 bits */
129 	{
130 		.code = { MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8 },
131 		.pixelformat = V4L2_PIX_FMT_SBGGR10DPCM8,
132 		.bpp = 1,
133 		.bayer = true,
134 	},
135 	{
136 		.code = { MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8 },
137 		.pixelformat = V4L2_PIX_FMT_SGBRG10DPCM8,
138 		.bpp = 1,
139 		.bayer = true,
140 	},
141 	{
142 		.code = { MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8 },
143 		.pixelformat = V4L2_PIX_FMT_SGRBG10DPCM8,
144 		.bpp = 1,
145 		.bayer = true,
146 	},
147 	{
148 		.code = { MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8 },
149 		.pixelformat = V4L2_PIX_FMT_SRGGB10DPCM8,
150 		.bpp = 1,
151 		.bayer = true,
152 	},
153 	{
154 		.code = { MEDIA_BUS_FMT_SBGGR12_1X12 },
155 		.pixelformat = V4L2_PIX_FMT_SBGGR12,
156 		.bpp = 2,
157 		.bayer = true,
158 	},
159 	{
160 		.code = { MEDIA_BUS_FMT_SGBRG12_1X12 },
161 		.pixelformat = V4L2_PIX_FMT_SGBRG12,
162 		.bpp = 2,
163 		.bayer = true,
164 	},
165 	{
166 		.code = { MEDIA_BUS_FMT_SGRBG12_1X12 },
167 		.pixelformat = V4L2_PIX_FMT_SGRBG12,
168 		.bpp = 2,
169 		.bayer = true,
170 	},
171 	{
172 		.code = { MEDIA_BUS_FMT_SRGGB12_1X12 },
173 		.pixelformat = V4L2_PIX_FMT_SRGGB12,
174 		.bpp = 2,
175 		.bayer = true,
176 	},
177 };
178 
vimc_is_source(struct media_entity * ent)179 bool vimc_is_source(struct media_entity *ent)
180 {
181 	unsigned int i;
182 
183 	for (i = 0; i < ent->num_pads; i++)
184 		if (ent->pads[i].flags & MEDIA_PAD_FL_SINK)
185 			return false;
186 	return true;
187 }
188 
vimc_pix_map_by_index(unsigned int i)189 const struct vimc_pix_map *vimc_pix_map_by_index(unsigned int i)
190 {
191 	if (i >= ARRAY_SIZE(vimc_pix_map_list))
192 		return NULL;
193 
194 	return &vimc_pix_map_list[i];
195 }
196 
vimc_mbus_code_by_index(unsigned int index)197 u32 vimc_mbus_code_by_index(unsigned int index)
198 {
199 	unsigned int i, j;
200 
201 	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
202 		for (j = 0; j < ARRAY_SIZE(vimc_pix_map_list[i].code); j++) {
203 			if (!vimc_pix_map_list[i].code[j])
204 				break;
205 
206 			if (!index)
207 				return vimc_pix_map_list[i].code[j];
208 			index--;
209 		}
210 	}
211 	return 0;
212 }
213 
vimc_pix_map_by_code(u32 code)214 const struct vimc_pix_map *vimc_pix_map_by_code(u32 code)
215 {
216 	unsigned int i, j;
217 
218 	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
219 		for (j = 0; j < ARRAY_SIZE(vimc_pix_map_list[i].code); j++) {
220 			if (vimc_pix_map_list[i].code[j] == code)
221 				return &vimc_pix_map_list[i];
222 		}
223 	}
224 	return NULL;
225 }
226 
vimc_pix_map_by_pixelformat(u32 pixelformat)227 const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat)
228 {
229 	unsigned int i;
230 
231 	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
232 		if (vimc_pix_map_list[i].pixelformat == pixelformat)
233 			return &vimc_pix_map_list[i];
234 	}
235 	return NULL;
236 }
237 
vimc_get_pix_format(struct media_pad * pad,struct v4l2_pix_format * fmt)238 static int vimc_get_pix_format(struct media_pad *pad,
239 			       struct v4l2_pix_format *fmt)
240 {
241 	if (is_media_entity_v4l2_subdev(pad->entity)) {
242 		struct v4l2_subdev *sd =
243 			media_entity_to_v4l2_subdev(pad->entity);
244 		struct v4l2_subdev_format sd_fmt;
245 		const struct vimc_pix_map *pix_map;
246 		int ret;
247 
248 		sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
249 		sd_fmt.pad = pad->index;
250 
251 		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt);
252 		if (ret)
253 			return ret;
254 
255 		v4l2_fill_pix_format(fmt, &sd_fmt.format);
256 		pix_map = vimc_pix_map_by_code(sd_fmt.format.code);
257 		fmt->pixelformat = pix_map->pixelformat;
258 	} else if (is_media_entity_v4l2_video_device(pad->entity)) {
259 		struct video_device *vdev = container_of(pad->entity,
260 							 struct video_device,
261 							 entity);
262 		struct vimc_ent_device *ved = video_get_drvdata(vdev);
263 
264 		if (!ved->vdev_get_format)
265 			return -ENOIOCTLCMD;
266 
267 		ved->vdev_get_format(ved, fmt);
268 	} else {
269 		return -EINVAL;
270 	}
271 
272 	return 0;
273 }
274 
vimc_vdev_link_validate(struct media_link * link)275 int vimc_vdev_link_validate(struct media_link *link)
276 {
277 	struct v4l2_pix_format source_fmt, sink_fmt;
278 	int ret;
279 
280 	ret = vimc_get_pix_format(link->source, &source_fmt);
281 	if (ret)
282 		return ret;
283 
284 	ret = vimc_get_pix_format(link->sink, &sink_fmt);
285 	if (ret)
286 		return ret;
287 
288 	pr_info("vimc link validate: "
289 		"%s:src:%dx%d (0x%x, %d, %d, %d, %d) "
290 		"%s:snk:%dx%d (0x%x, %d, %d, %d, %d)\n",
291 		/* src */
292 		link->source->entity->name,
293 		source_fmt.width, source_fmt.height,
294 		source_fmt.pixelformat, source_fmt.colorspace,
295 		source_fmt.quantization, source_fmt.xfer_func,
296 		source_fmt.ycbcr_enc,
297 		/* sink */
298 		link->sink->entity->name,
299 		sink_fmt.width, sink_fmt.height,
300 		sink_fmt.pixelformat, sink_fmt.colorspace,
301 		sink_fmt.quantization, sink_fmt.xfer_func,
302 		sink_fmt.ycbcr_enc);
303 
304 	/* The width, height and pixelformat must match. */
305 	if (source_fmt.width != sink_fmt.width ||
306 	    source_fmt.height != sink_fmt.height ||
307 	    source_fmt.pixelformat != sink_fmt.pixelformat)
308 		return -EPIPE;
309 
310 	/*
311 	 * The field order must match, or the sink field order must be NONE
312 	 * to support interlaced hardware connected to bridges that support
313 	 * progressive formats only.
314 	 */
315 	if (source_fmt.field != sink_fmt.field &&
316 	    sink_fmt.field != V4L2_FIELD_NONE)
317 		return -EPIPE;
318 
319 	/*
320 	 * If colorspace is DEFAULT, then assume all the colorimetry is also
321 	 * DEFAULT, return 0 to skip comparing the other colorimetry parameters
322 	 */
323 	if (source_fmt.colorspace == V4L2_COLORSPACE_DEFAULT ||
324 	    sink_fmt.colorspace == V4L2_COLORSPACE_DEFAULT)
325 		return 0;
326 
327 	/* Colorspace must match. */
328 	if (source_fmt.colorspace != sink_fmt.colorspace)
329 		return -EPIPE;
330 
331 	/* Colorimetry must match if they are not set to DEFAULT */
332 	if (source_fmt.ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT &&
333 	    sink_fmt.ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT &&
334 	    source_fmt.ycbcr_enc != sink_fmt.ycbcr_enc)
335 		return -EPIPE;
336 
337 	if (source_fmt.quantization != V4L2_QUANTIZATION_DEFAULT &&
338 	    sink_fmt.quantization != V4L2_QUANTIZATION_DEFAULT &&
339 	    source_fmt.quantization != sink_fmt.quantization)
340 		return -EPIPE;
341 
342 	if (source_fmt.xfer_func != V4L2_XFER_FUNC_DEFAULT &&
343 	    sink_fmt.xfer_func != V4L2_XFER_FUNC_DEFAULT &&
344 	    source_fmt.xfer_func != sink_fmt.xfer_func)
345 		return -EPIPE;
346 
347 	return 0;
348 }
349 
350 static const struct media_entity_operations vimc_ent_sd_mops = {
351 	.link_validate = v4l2_subdev_link_validate,
352 };
353 
vimc_ent_sd_register(struct vimc_ent_device * ved,struct v4l2_subdev * sd,struct v4l2_device * v4l2_dev,const char * const name,u32 function,u16 num_pads,struct media_pad * pads,const struct v4l2_subdev_ops * sd_ops)354 int vimc_ent_sd_register(struct vimc_ent_device *ved,
355 			 struct v4l2_subdev *sd,
356 			 struct v4l2_device *v4l2_dev,
357 			 const char *const name,
358 			 u32 function,
359 			 u16 num_pads,
360 			 struct media_pad *pads,
361 			 const struct v4l2_subdev_ops *sd_ops)
362 {
363 	int ret;
364 
365 	/* Fill the vimc_ent_device struct */
366 	ved->ent = &sd->entity;
367 
368 	/* Initialize the subdev */
369 	v4l2_subdev_init(sd, sd_ops);
370 	sd->entity.function = function;
371 	sd->entity.ops = &vimc_ent_sd_mops;
372 	sd->owner = THIS_MODULE;
373 	strscpy(sd->name, name, sizeof(sd->name));
374 	v4l2_set_subdevdata(sd, ved);
375 
376 	/* Expose this subdev to user space */
377 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
378 	if (sd->ctrl_handler)
379 		sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS;
380 
381 	/* Initialize the media entity */
382 	ret = media_entity_pads_init(&sd->entity, num_pads, pads);
383 	if (ret)
384 		return ret;
385 
386 	/* Register the subdev with the v4l2 and the media framework */
387 	ret = v4l2_device_register_subdev(v4l2_dev, sd);
388 	if (ret) {
389 		dev_err(v4l2_dev->dev,
390 			"%s: subdev register failed (err=%d)\n",
391 			name, ret);
392 		goto err_clean_m_ent;
393 	}
394 
395 	return 0;
396 
397 err_clean_m_ent:
398 	media_entity_cleanup(&sd->entity);
399 	return ret;
400 }
401