]>
Commit | Line | Data |
---|---|---|
6856ba77 HF |
1 | /* |
2 | * vimc-scaler.c Virtual Media Controller Driver | |
3 | * | |
4 | * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com> | |
5 | * | |
6 | * This program is free software; you can redistribute it and/or modify | |
7 | * it under the terms of the GNU General Public License as published by | |
8 | * the Free Software Foundation; either version 2 of the License, or | |
9 | * (at your option) any later version. | |
10 | * | |
11 | * This program is distributed in the hope that it will be useful, | |
12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
14 | * GNU General Public License for more details. | |
15 | * | |
16 | */ | |
17 | ||
18 | #include <linux/component.h> | |
19 | #include <linux/module.h> | |
20 | #include <linux/platform_device.h> | |
21 | #include <linux/vmalloc.h> | |
22 | #include <linux/v4l2-mediabus.h> | |
23 | #include <media/v4l2-subdev.h> | |
24 | ||
25 | #include "vimc-common.h" | |
26 | ||
27 | #define VIMC_SCA_DRV_NAME "vimc-scaler" | |
28 | ||
29 | static unsigned int sca_mult = 3; | |
30 | module_param(sca_mult, uint, 0000); | |
31 | MODULE_PARM_DESC(sca_mult, " the image size multiplier"); | |
32 | ||
33 | #define IS_SINK(pad) (!pad) | |
34 | #define IS_SRC(pad) (pad) | |
35 | #define MAX_ZOOM 8 | |
36 | ||
37 | struct vimc_sca_device { | |
38 | struct vimc_ent_device ved; | |
39 | struct v4l2_subdev sd; | |
40 | struct device *dev; | |
41 | /* NOTE: the source fmt is the same as the sink | |
42 | * with the width and hight multiplied by mult | |
43 | */ | |
44 | struct v4l2_mbus_framefmt sink_fmt; | |
45 | /* Values calculated when the stream starts */ | |
46 | u8 *src_frame; | |
47 | unsigned int src_line_size; | |
48 | unsigned int bpp; | |
49 | }; | |
50 | ||
51 | static const struct v4l2_mbus_framefmt sink_fmt_default = { | |
52 | .width = 640, | |
53 | .height = 480, | |
54 | .code = MEDIA_BUS_FMT_RGB888_1X24, | |
55 | .field = V4L2_FIELD_NONE, | |
56 | .colorspace = V4L2_COLORSPACE_DEFAULT, | |
57 | }; | |
58 | ||
59 | static int vimc_sca_init_cfg(struct v4l2_subdev *sd, | |
60 | struct v4l2_subdev_pad_config *cfg) | |
61 | { | |
62 | struct v4l2_mbus_framefmt *mf; | |
63 | unsigned int i; | |
64 | ||
65 | mf = v4l2_subdev_get_try_format(sd, cfg, 0); | |
66 | *mf = sink_fmt_default; | |
67 | ||
68 | for (i = 1; i < sd->entity.num_pads; i++) { | |
69 | mf = v4l2_subdev_get_try_format(sd, cfg, i); | |
70 | *mf = sink_fmt_default; | |
71 | mf->width = mf->width * sca_mult; | |
72 | mf->height = mf->height * sca_mult; | |
73 | } | |
74 | ||
75 | return 0; | |
76 | } | |
77 | ||
78 | static int vimc_sca_enum_mbus_code(struct v4l2_subdev *sd, | |
79 | struct v4l2_subdev_pad_config *cfg, | |
80 | struct v4l2_subdev_mbus_code_enum *code) | |
81 | { | |
82 | const struct vimc_pix_map *vpix = vimc_pix_map_by_index(code->index); | |
83 | ||
84 | /* We don't support bayer format */ | |
85 | if (!vpix || vpix->bayer) | |
86 | return -EINVAL; | |
87 | ||
88 | code->code = vpix->code; | |
89 | ||
90 | return 0; | |
91 | } | |
92 | ||
93 | static int vimc_sca_enum_frame_size(struct v4l2_subdev *sd, | |
94 | struct v4l2_subdev_pad_config *cfg, | |
95 | struct v4l2_subdev_frame_size_enum *fse) | |
96 | { | |
97 | const struct vimc_pix_map *vpix; | |
98 | ||
99 | if (fse->index) | |
100 | return -EINVAL; | |
101 | ||
102 | /* Only accept code in the pix map table in non bayer format */ | |
103 | vpix = vimc_pix_map_by_code(fse->code); | |
104 | if (!vpix || vpix->bayer) | |
105 | return -EINVAL; | |
106 | ||
107 | fse->min_width = VIMC_FRAME_MIN_WIDTH; | |
108 | fse->min_height = VIMC_FRAME_MIN_HEIGHT; | |
109 | ||
110 | if (IS_SINK(fse->pad)) { | |
111 | fse->max_width = VIMC_FRAME_MAX_WIDTH; | |
112 | fse->max_height = VIMC_FRAME_MAX_HEIGHT; | |
113 | } else { | |
114 | fse->max_width = VIMC_FRAME_MAX_WIDTH * MAX_ZOOM; | |
115 | fse->max_height = VIMC_FRAME_MAX_HEIGHT * MAX_ZOOM; | |
116 | } | |
117 | ||
118 | return 0; | |
119 | } | |
120 | ||
121 | static int vimc_sca_get_fmt(struct v4l2_subdev *sd, | |
122 | struct v4l2_subdev_pad_config *cfg, | |
123 | struct v4l2_subdev_format *format) | |
124 | { | |
125 | struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd); | |
126 | ||
127 | /* Get the current sink format */ | |
128 | format->format = (format->which == V4L2_SUBDEV_FORMAT_TRY) ? | |
129 | *v4l2_subdev_get_try_format(sd, cfg, 0) : | |
130 | vsca->sink_fmt; | |
131 | ||
132 | /* Scale the frame size for the source pad */ | |
133 | if (IS_SRC(format->pad)) { | |
134 | format->format.width = vsca->sink_fmt.width * sca_mult; | |
135 | format->format.height = vsca->sink_fmt.height * sca_mult; | |
136 | } | |
137 | ||
138 | return 0; | |
139 | } | |
140 | ||
141 | static void vimc_sca_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt) | |
142 | { | |
143 | const struct vimc_pix_map *vpix; | |
144 | ||
145 | /* Only accept code in the pix map table in non bayer format */ | |
146 | vpix = vimc_pix_map_by_code(fmt->code); | |
147 | if (!vpix || vpix->bayer) | |
148 | fmt->code = sink_fmt_default.code; | |
149 | ||
150 | fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH, | |
151 | VIMC_FRAME_MAX_WIDTH) & ~1; | |
152 | fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT, | |
153 | VIMC_FRAME_MAX_HEIGHT) & ~1; | |
154 | ||
155 | if (fmt->field == V4L2_FIELD_ANY) | |
156 | fmt->field = sink_fmt_default.field; | |
157 | ||
158 | vimc_colorimetry_clamp(fmt); | |
159 | } | |
160 | ||
161 | static int vimc_sca_set_fmt(struct v4l2_subdev *sd, | |
162 | struct v4l2_subdev_pad_config *cfg, | |
163 | struct v4l2_subdev_format *fmt) | |
164 | { | |
165 | struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd); | |
166 | struct v4l2_mbus_framefmt *sink_fmt; | |
167 | ||
168 | if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) { | |
169 | /* Do not change the format while stream is on */ | |
170 | if (vsca->src_frame) | |
171 | return -EBUSY; | |
172 | ||
173 | sink_fmt = &vsca->sink_fmt; | |
174 | } else { | |
175 | sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0); | |
176 | } | |
177 | ||
178 | /* | |
179 | * Do not change the format of the source pad, | |
180 | * it is propagated from the sink | |
181 | */ | |
182 | if (IS_SRC(fmt->pad)) { | |
183 | fmt->format = *sink_fmt; | |
184 | fmt->format.width = sink_fmt->width * sca_mult; | |
185 | fmt->format.height = sink_fmt->height * sca_mult; | |
186 | } else { | |
187 | /* Set the new format in the sink pad */ | |
188 | vimc_sca_adjust_sink_fmt(&fmt->format); | |
189 | ||
190 | dev_dbg(vsca->dev, "%s: sink format update: " | |
191 | "old:%dx%d (0x%x, %d, %d, %d, %d) " | |
192 | "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsca->sd.name, | |
193 | /* old */ | |
194 | sink_fmt->width, sink_fmt->height, sink_fmt->code, | |
195 | sink_fmt->colorspace, sink_fmt->quantization, | |
196 | sink_fmt->xfer_func, sink_fmt->ycbcr_enc, | |
197 | /* new */ | |
198 | fmt->format.width, fmt->format.height, fmt->format.code, | |
199 | fmt->format.colorspace, fmt->format.quantization, | |
200 | fmt->format.xfer_func, fmt->format.ycbcr_enc); | |
201 | ||
202 | *sink_fmt = fmt->format; | |
203 | } | |
204 | ||
205 | return 0; | |
206 | } | |
207 | ||
208 | static const struct v4l2_subdev_pad_ops vimc_sca_pad_ops = { | |
209 | .init_cfg = vimc_sca_init_cfg, | |
210 | .enum_mbus_code = vimc_sca_enum_mbus_code, | |
211 | .enum_frame_size = vimc_sca_enum_frame_size, | |
212 | .get_fmt = vimc_sca_get_fmt, | |
213 | .set_fmt = vimc_sca_set_fmt, | |
214 | }; | |
215 | ||
216 | static int vimc_sca_s_stream(struct v4l2_subdev *sd, int enable) | |
217 | { | |
218 | struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd); | |
219 | int ret; | |
220 | ||
221 | if (enable) { | |
222 | const struct vimc_pix_map *vpix; | |
223 | unsigned int frame_size; | |
224 | ||
225 | if (vsca->src_frame) | |
226 | return 0; | |
227 | ||
228 | /* Save the bytes per pixel of the sink */ | |
229 | vpix = vimc_pix_map_by_code(vsca->sink_fmt.code); | |
230 | vsca->bpp = vpix->bpp; | |
231 | ||
232 | /* Calculate the width in bytes of the src frame */ | |
233 | vsca->src_line_size = vsca->sink_fmt.width * | |
234 | sca_mult * vsca->bpp; | |
235 | ||
236 | /* Calculate the frame size of the source pad */ | |
237 | frame_size = vsca->src_line_size * vsca->sink_fmt.height * | |
238 | sca_mult; | |
239 | ||
240 | /* Allocate the frame buffer. Use vmalloc to be able to | |
241 | * allocate a large amount of memory | |
242 | */ | |
243 | vsca->src_frame = vmalloc(frame_size); | |
244 | if (!vsca->src_frame) | |
245 | return -ENOMEM; | |
246 | ||
247 | /* Turn the stream on in the subdevices directly connected */ | |
248 | ret = vimc_pipeline_s_stream(&vsca->sd.entity, 1); | |
249 | if (ret) { | |
250 | vfree(vsca->src_frame); | |
251 | vsca->src_frame = NULL; | |
252 | return ret; | |
253 | } | |
254 | } else { | |
255 | if (!vsca->src_frame) | |
256 | return 0; | |
257 | ||
258 | /* Disable streaming from the pipe */ | |
259 | ret = vimc_pipeline_s_stream(&vsca->sd.entity, 0); | |
260 | if (ret) | |
261 | return ret; | |
262 | ||
263 | vfree(vsca->src_frame); | |
264 | vsca->src_frame = NULL; | |
265 | } | |
266 | ||
267 | return 0; | |
268 | } | |
269 | ||
270 | static struct v4l2_subdev_video_ops vimc_sca_video_ops = { | |
271 | .s_stream = vimc_sca_s_stream, | |
272 | }; | |
273 | ||
274 | static const struct v4l2_subdev_ops vimc_sca_ops = { | |
275 | .pad = &vimc_sca_pad_ops, | |
276 | .video = &vimc_sca_video_ops, | |
277 | }; | |
278 | ||
279 | static void vimc_sca_fill_pix(u8 *const ptr, | |
280 | const u8 *const pixel, | |
281 | const unsigned int bpp) | |
282 | { | |
283 | unsigned int i; | |
284 | ||
285 | /* copy the pixel to the pointer */ | |
286 | for (i = 0; i < bpp; i++) | |
287 | ptr[i] = pixel[i]; | |
288 | } | |
289 | ||
290 | static void vimc_sca_scale_pix(const struct vimc_sca_device *const vsca, | |
291 | const unsigned int lin, const unsigned int col, | |
292 | const u8 *const sink_frame) | |
293 | { | |
294 | unsigned int i, j, index; | |
295 | const u8 *pixel; | |
296 | ||
297 | /* Point to the pixel value in position (lin, col) in the sink frame */ | |
298 | index = VIMC_FRAME_INDEX(lin, col, | |
299 | vsca->sink_fmt.width, | |
300 | vsca->bpp); | |
301 | pixel = &sink_frame[index]; | |
302 | ||
303 | dev_dbg(vsca->dev, | |
304 | "sca: %s: --- scale_pix sink pos %dx%d, index %d ---\n", | |
305 | vsca->sd.name, lin, col, index); | |
306 | ||
307 | /* point to the place we are going to put the first pixel | |
308 | * in the scaled src frame | |
309 | */ | |
310 | index = VIMC_FRAME_INDEX(lin * sca_mult, col * sca_mult, | |
311 | vsca->sink_fmt.width * sca_mult, vsca->bpp); | |
312 | ||
313 | dev_dbg(vsca->dev, "sca: %s: scale_pix src pos %dx%d, index %d\n", | |
314 | vsca->sd.name, lin * sca_mult, col * sca_mult, index); | |
315 | ||
316 | /* Repeat this pixel mult times */ | |
317 | for (i = 0; i < sca_mult; i++) { | |
318 | /* Iterate through each beginning of a | |
319 | * pixel repetition in a line | |
320 | */ | |
321 | for (j = 0; j < sca_mult * vsca->bpp; j += vsca->bpp) { | |
322 | dev_dbg(vsca->dev, | |
323 | "sca: %s: sca: scale_pix src pos %d\n", | |
324 | vsca->sd.name, index + j); | |
325 | ||
326 | /* copy the pixel to the position index + j */ | |
327 | vimc_sca_fill_pix(&vsca->src_frame[index + j], | |
328 | pixel, vsca->bpp); | |
329 | } | |
330 | ||
331 | /* move the index to the next line */ | |
332 | index += vsca->src_line_size; | |
333 | } | |
334 | } | |
335 | ||
336 | static void vimc_sca_fill_src_frame(const struct vimc_sca_device *const vsca, | |
337 | const u8 *const sink_frame) | |
338 | { | |
339 | unsigned int i, j; | |
340 | ||
341 | /* Scale each pixel from the original sink frame */ | |
342 | /* TODO: implement scale down, only scale up is supported for now */ | |
343 | for (i = 0; i < vsca->sink_fmt.height; i++) | |
344 | for (j = 0; j < vsca->sink_fmt.width; j++) | |
345 | vimc_sca_scale_pix(vsca, i, j, sink_frame); | |
346 | } | |
347 | ||
348 | static void vimc_sca_process_frame(struct vimc_ent_device *ved, | |
349 | struct media_pad *sink, | |
350 | const void *sink_frame) | |
351 | { | |
352 | struct vimc_sca_device *vsca = container_of(ved, struct vimc_sca_device, | |
353 | ved); | |
354 | unsigned int i; | |
355 | ||
356 | /* If the stream in this node is not active, just return */ | |
357 | if (!vsca->src_frame) | |
358 | return; | |
359 | ||
360 | vimc_sca_fill_src_frame(vsca, sink_frame); | |
361 | ||
362 | /* Propagate the frame through all source pads */ | |
363 | for (i = 1; i < vsca->sd.entity.num_pads; i++) { | |
364 | struct media_pad *pad = &vsca->sd.entity.pads[i]; | |
365 | ||
366 | vimc_propagate_frame(pad, vsca->src_frame); | |
367 | } | |
368 | }; | |
369 | ||
370 | static void vimc_sca_comp_unbind(struct device *comp, struct device *master, | |
371 | void *master_data) | |
372 | { | |
373 | struct vimc_ent_device *ved = dev_get_drvdata(comp); | |
374 | struct vimc_sca_device *vsca = container_of(ved, struct vimc_sca_device, | |
375 | ved); | |
376 | ||
377 | vimc_ent_sd_unregister(ved, &vsca->sd); | |
378 | kfree(vsca); | |
379 | } | |
380 | ||
381 | ||
382 | static int vimc_sca_comp_bind(struct device *comp, struct device *master, | |
383 | void *master_data) | |
384 | { | |
385 | struct v4l2_device *v4l2_dev = master_data; | |
386 | struct vimc_platform_data *pdata = comp->platform_data; | |
387 | struct vimc_sca_device *vsca; | |
388 | int ret; | |
389 | ||
390 | /* Allocate the vsca struct */ | |
391 | vsca = kzalloc(sizeof(*vsca), GFP_KERNEL); | |
392 | if (!vsca) | |
393 | return -ENOMEM; | |
394 | ||
395 | /* Initialize ved and sd */ | |
396 | ret = vimc_ent_sd_register(&vsca->ved, &vsca->sd, v4l2_dev, | |
397 | pdata->entity_name, | |
398 | MEDIA_ENT_F_ATV_DECODER, 2, | |
399 | (const unsigned long[2]) {MEDIA_PAD_FL_SINK, | |
400 | MEDIA_PAD_FL_SOURCE}, | |
401 | &vimc_sca_ops); | |
402 | if (ret) { | |
403 | kfree(vsca); | |
404 | return ret; | |
405 | } | |
406 | ||
407 | vsca->ved.process_frame = vimc_sca_process_frame; | |
408 | dev_set_drvdata(comp, &vsca->ved); | |
409 | vsca->dev = comp; | |
410 | ||
411 | /* Initialize the frame format */ | |
412 | vsca->sink_fmt = sink_fmt_default; | |
413 | ||
414 | return 0; | |
415 | } | |
416 | ||
417 | static const struct component_ops vimc_sca_comp_ops = { | |
418 | .bind = vimc_sca_comp_bind, | |
419 | .unbind = vimc_sca_comp_unbind, | |
420 | }; | |
421 | ||
422 | static int vimc_sca_probe(struct platform_device *pdev) | |
423 | { | |
424 | return component_add(&pdev->dev, &vimc_sca_comp_ops); | |
425 | } | |
426 | ||
427 | static int vimc_sca_remove(struct platform_device *pdev) | |
428 | { | |
429 | component_del(&pdev->dev, &vimc_sca_comp_ops); | |
430 | ||
431 | return 0; | |
432 | } | |
433 | ||
bf183e0f JMC |
434 | static const struct platform_device_id vimc_sca_driver_ids[] = { |
435 | { | |
436 | .name = VIMC_SCA_DRV_NAME, | |
437 | }, | |
438 | { } | |
439 | }; | |
440 | ||
6856ba77 HF |
441 | static struct platform_driver vimc_sca_pdrv = { |
442 | .probe = vimc_sca_probe, | |
443 | .remove = vimc_sca_remove, | |
bf183e0f | 444 | .id_table = vimc_sca_driver_ids, |
6856ba77 HF |
445 | .driver = { |
446 | .name = VIMC_SCA_DRV_NAME, | |
447 | }, | |
448 | }; | |
449 | ||
6856ba77 HF |
450 | module_platform_driver(vimc_sca_pdrv); |
451 | ||
452 | MODULE_DEVICE_TABLE(platform, vimc_sca_driver_ids); | |
453 | ||
454 | MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Scaler"); | |
455 | MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>"); | |
456 | MODULE_LICENSE("GPL"); |