2 * vimc-sensor.c Virtual Media Controller Driver
4 * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
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.
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.
18 #include <linux/component.h>
19 #include <linux/freezer.h>
20 #include <linux/kthread.h>
21 #include <linux/module.h>
22 #include <linux/platform_device.h>
23 #include <linux/v4l2-mediabus.h>
24 #include <linux/vmalloc.h>
25 #include <media/v4l2-subdev.h>
26 #include <media/v4l2-tpg.h>
28 #include "vimc-common.h"
30 #define VIMC_SEN_DRV_NAME "vimc-sensor"
32 struct vimc_sen_device
{
33 struct vimc_ent_device ved
;
34 struct v4l2_subdev sd
;
37 struct task_struct
*kthread_sen
;
39 /* The active format */
40 struct v4l2_mbus_framefmt mbus_format
;
43 static const struct v4l2_mbus_framefmt fmt_default
= {
46 .code
= MEDIA_BUS_FMT_RGB888_1X24
,
47 .field
= V4L2_FIELD_NONE
,
48 .colorspace
= V4L2_COLORSPACE_DEFAULT
,
51 static int vimc_sen_init_cfg(struct v4l2_subdev
*sd
,
52 struct v4l2_subdev_pad_config
*cfg
)
56 for (i
= 0; i
< sd
->entity
.num_pads
; i
++) {
57 struct v4l2_mbus_framefmt
*mf
;
59 mf
= v4l2_subdev_get_try_format(sd
, cfg
, i
);
66 static int vimc_sen_enum_mbus_code(struct v4l2_subdev
*sd
,
67 struct v4l2_subdev_pad_config
*cfg
,
68 struct v4l2_subdev_mbus_code_enum
*code
)
70 const struct vimc_pix_map
*vpix
= vimc_pix_map_by_index(code
->index
);
75 code
->code
= vpix
->code
;
80 static int vimc_sen_enum_frame_size(struct v4l2_subdev
*sd
,
81 struct v4l2_subdev_pad_config
*cfg
,
82 struct v4l2_subdev_frame_size_enum
*fse
)
84 const struct vimc_pix_map
*vpix
;
89 /* Only accept code in the pix map table */
90 vpix
= vimc_pix_map_by_code(fse
->code
);
94 fse
->min_width
= VIMC_FRAME_MIN_WIDTH
;
95 fse
->max_width
= VIMC_FRAME_MAX_WIDTH
;
96 fse
->min_height
= VIMC_FRAME_MIN_HEIGHT
;
97 fse
->max_height
= VIMC_FRAME_MAX_HEIGHT
;
102 static int vimc_sen_get_fmt(struct v4l2_subdev
*sd
,
103 struct v4l2_subdev_pad_config
*cfg
,
104 struct v4l2_subdev_format
*fmt
)
106 struct vimc_sen_device
*vsen
=
107 container_of(sd
, struct vimc_sen_device
, sd
);
109 fmt
->format
= fmt
->which
== V4L2_SUBDEV_FORMAT_TRY
?
110 *v4l2_subdev_get_try_format(sd
, cfg
, fmt
->pad
) :
116 static void vimc_sen_tpg_s_format(struct vimc_sen_device
*vsen
)
118 const struct vimc_pix_map
*vpix
=
119 vimc_pix_map_by_code(vsen
->mbus_format
.code
);
121 tpg_reset_source(&vsen
->tpg
, vsen
->mbus_format
.width
,
122 vsen
->mbus_format
.height
, vsen
->mbus_format
.field
);
123 tpg_s_bytesperline(&vsen
->tpg
, 0, vsen
->mbus_format
.width
* vpix
->bpp
);
124 tpg_s_buf_height(&vsen
->tpg
, vsen
->mbus_format
.height
);
125 tpg_s_fourcc(&vsen
->tpg
, vpix
->pixelformat
);
126 /* TODO: add support for V4L2_FIELD_ALTERNATE */
127 tpg_s_field(&vsen
->tpg
, vsen
->mbus_format
.field
, false);
128 tpg_s_colorspace(&vsen
->tpg
, vsen
->mbus_format
.colorspace
);
129 tpg_s_ycbcr_enc(&vsen
->tpg
, vsen
->mbus_format
.ycbcr_enc
);
130 tpg_s_quantization(&vsen
->tpg
, vsen
->mbus_format
.quantization
);
131 tpg_s_xfer_func(&vsen
->tpg
, vsen
->mbus_format
.xfer_func
);
134 static void vimc_sen_adjust_fmt(struct v4l2_mbus_framefmt
*fmt
)
136 const struct vimc_pix_map
*vpix
;
138 /* Only accept code in the pix map table */
139 vpix
= vimc_pix_map_by_code(fmt
->code
);
141 fmt
->code
= fmt_default
.code
;
143 fmt
->width
= clamp_t(u32
, fmt
->width
, VIMC_FRAME_MIN_WIDTH
,
144 VIMC_FRAME_MAX_WIDTH
) & ~1;
145 fmt
->height
= clamp_t(u32
, fmt
->height
, VIMC_FRAME_MIN_HEIGHT
,
146 VIMC_FRAME_MAX_HEIGHT
) & ~1;
148 /* TODO: add support for V4L2_FIELD_ALTERNATE */
149 if (fmt
->field
== V4L2_FIELD_ANY
|| fmt
->field
== V4L2_FIELD_ALTERNATE
)
150 fmt
->field
= fmt_default
.field
;
152 vimc_colorimetry_clamp(fmt
);
155 static int vimc_sen_set_fmt(struct v4l2_subdev
*sd
,
156 struct v4l2_subdev_pad_config
*cfg
,
157 struct v4l2_subdev_format
*fmt
)
159 struct vimc_sen_device
*vsen
= v4l2_get_subdevdata(sd
);
160 struct v4l2_mbus_framefmt
*mf
;
162 if (fmt
->which
== V4L2_SUBDEV_FORMAT_ACTIVE
) {
163 /* Do not change the format while stream is on */
167 mf
= &vsen
->mbus_format
;
169 mf
= v4l2_subdev_get_try_format(sd
, cfg
, fmt
->pad
);
172 /* Set the new format */
173 vimc_sen_adjust_fmt(&fmt
->format
);
175 dev_dbg(vsen
->dev
, "%s: format update: "
176 "old:%dx%d (0x%x, %d, %d, %d, %d) "
177 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsen
->sd
.name
,
179 mf
->width
, mf
->height
, mf
->code
,
180 mf
->colorspace
, mf
->quantization
,
181 mf
->xfer_func
, mf
->ycbcr_enc
,
183 fmt
->format
.width
, fmt
->format
.height
, fmt
->format
.code
,
184 fmt
->format
.colorspace
, fmt
->format
.quantization
,
185 fmt
->format
.xfer_func
, fmt
->format
.ycbcr_enc
);
192 static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops
= {
193 .init_cfg
= vimc_sen_init_cfg
,
194 .enum_mbus_code
= vimc_sen_enum_mbus_code
,
195 .enum_frame_size
= vimc_sen_enum_frame_size
,
196 .get_fmt
= vimc_sen_get_fmt
,
197 .set_fmt
= vimc_sen_set_fmt
,
200 static int vimc_sen_tpg_thread(void *data
)
202 struct vimc_sen_device
*vsen
= data
;
206 set_current_state(TASK_UNINTERRUPTIBLE
);
210 if (kthread_should_stop())
213 tpg_fill_plane_buffer(&vsen
->tpg
, 0, 0, vsen
->frame
);
215 /* Send the frame to all source pads */
216 for (i
= 0; i
< vsen
->sd
.entity
.num_pads
; i
++)
217 vimc_propagate_frame(&vsen
->sd
.entity
.pads
[i
],
220 /* 60 frames per second */
221 schedule_timeout(HZ
/60);
227 static int vimc_sen_s_stream(struct v4l2_subdev
*sd
, int enable
)
229 struct vimc_sen_device
*vsen
=
230 container_of(sd
, struct vimc_sen_device
, sd
);
234 const struct vimc_pix_map
*vpix
;
235 unsigned int frame_size
;
237 if (vsen
->kthread_sen
)
238 /* tpg is already executing */
241 /* Calculate the frame size */
242 vpix
= vimc_pix_map_by_code(vsen
->mbus_format
.code
);
243 frame_size
= vsen
->mbus_format
.width
* vpix
->bpp
*
244 vsen
->mbus_format
.height
;
247 * Allocate the frame buffer. Use vmalloc to be able to
248 * allocate a large amount of memory
250 vsen
->frame
= vmalloc(frame_size
);
254 /* configure the test pattern generator */
255 vimc_sen_tpg_s_format(vsen
);
257 /* Initialize the image generator thread */
258 vsen
->kthread_sen
= kthread_run(vimc_sen_tpg_thread
, vsen
,
259 "%s-sen", vsen
->sd
.v4l2_dev
->name
);
260 if (IS_ERR(vsen
->kthread_sen
)) {
261 dev_err(vsen
->dev
, "%s: kernel_thread() failed\n",
265 return PTR_ERR(vsen
->kthread_sen
);
268 if (!vsen
->kthread_sen
)
271 /* Stop image generator */
272 ret
= kthread_stop(vsen
->kthread_sen
);
276 vsen
->kthread_sen
= NULL
;
285 static struct v4l2_subdev_video_ops vimc_sen_video_ops
= {
286 .s_stream
= vimc_sen_s_stream
,
289 static const struct v4l2_subdev_ops vimc_sen_ops
= {
290 .pad
= &vimc_sen_pad_ops
,
291 .video
= &vimc_sen_video_ops
,
294 static void vimc_sen_comp_unbind(struct device
*comp
, struct device
*master
,
297 struct vimc_ent_device
*ved
= dev_get_drvdata(comp
);
298 struct vimc_sen_device
*vsen
=
299 container_of(ved
, struct vimc_sen_device
, ved
);
301 vimc_ent_sd_unregister(ved
, &vsen
->sd
);
302 tpg_free(&vsen
->tpg
);
306 static int vimc_sen_comp_bind(struct device
*comp
, struct device
*master
,
309 struct v4l2_device
*v4l2_dev
= master_data
;
310 struct vimc_platform_data
*pdata
= comp
->platform_data
;
311 struct vimc_sen_device
*vsen
;
314 /* Allocate the vsen struct */
315 vsen
= kzalloc(sizeof(*vsen
), GFP_KERNEL
);
319 /* Initialize ved and sd */
320 ret
= vimc_ent_sd_register(&vsen
->ved
, &vsen
->sd
, v4l2_dev
,
322 MEDIA_ENT_F_ATV_DECODER
, 1,
323 (const unsigned long[1]) {MEDIA_PAD_FL_SOURCE
},
328 dev_set_drvdata(comp
, &vsen
->ved
);
331 /* Initialize the frame format */
332 vsen
->mbus_format
= fmt_default
;
334 /* Initialize the test pattern generator */
335 tpg_init(&vsen
->tpg
, vsen
->mbus_format
.width
,
336 vsen
->mbus_format
.height
);
337 ret
= tpg_alloc(&vsen
->tpg
, VIMC_FRAME_MAX_WIDTH
);
339 goto err_unregister_ent_sd
;
343 err_unregister_ent_sd
:
344 vimc_ent_sd_unregister(&vsen
->ved
, &vsen
->sd
);
351 static const struct component_ops vimc_sen_comp_ops
= {
352 .bind
= vimc_sen_comp_bind
,
353 .unbind
= vimc_sen_comp_unbind
,
356 static int vimc_sen_probe(struct platform_device
*pdev
)
358 return component_add(&pdev
->dev
, &vimc_sen_comp_ops
);
361 static int vimc_sen_remove(struct platform_device
*pdev
)
363 component_del(&pdev
->dev
, &vimc_sen_comp_ops
);
368 static struct platform_driver vimc_sen_pdrv
= {
369 .probe
= vimc_sen_probe
,
370 .remove
= vimc_sen_remove
,
372 .name
= VIMC_SEN_DRV_NAME
,
376 static const struct platform_device_id vimc_sen_driver_ids
[] = {
378 .name
= VIMC_SEN_DRV_NAME
,
383 module_platform_driver(vimc_sen_pdrv
);
385 MODULE_DEVICE_TABLE(platform
, vimc_sen_driver_ids
);
387 MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Sensor");
388 MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
389 MODULE_LICENSE("GPL");