vimc-sensor.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423
  1. /*
  2. * vimc-sensor.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. #include <linux/component.h>
  18. #include <linux/module.h>
  19. #include <linux/mod_devicetable.h>
  20. #include <linux/platform_device.h>
  21. #include <linux/v4l2-mediabus.h>
  22. #include <linux/vmalloc.h>
  23. #include <media/v4l2-ctrls.h>
  24. #include <media/v4l2-event.h>
  25. #include <media/v4l2-subdev.h>
  26. #include <media/tpg/v4l2-tpg.h>
  27. #include "vimc-common.h"
  28. #define VIMC_SEN_DRV_NAME "vimc-sensor"
  29. struct vimc_sen_device {
  30. struct vimc_ent_device ved;
  31. struct v4l2_subdev sd;
  32. struct device *dev;
  33. struct tpg_data tpg;
  34. struct task_struct *kthread_sen;
  35. u8 *frame;
  36. /* The active format */
  37. struct v4l2_mbus_framefmt mbus_format;
  38. struct v4l2_ctrl_handler hdl;
  39. };
  40. static const struct v4l2_mbus_framefmt fmt_default = {
  41. .width = 640,
  42. .height = 480,
  43. .code = MEDIA_BUS_FMT_RGB888_1X24,
  44. .field = V4L2_FIELD_NONE,
  45. .colorspace = V4L2_COLORSPACE_DEFAULT,
  46. };
  47. static int vimc_sen_init_cfg(struct v4l2_subdev *sd,
  48. struct v4l2_subdev_pad_config *cfg)
  49. {
  50. unsigned int i;
  51. for (i = 0; i < sd->entity.num_pads; i++) {
  52. struct v4l2_mbus_framefmt *mf;
  53. mf = v4l2_subdev_get_try_format(sd, cfg, i);
  54. *mf = fmt_default;
  55. }
  56. return 0;
  57. }
  58. static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
  59. struct v4l2_subdev_pad_config *cfg,
  60. struct v4l2_subdev_mbus_code_enum *code)
  61. {
  62. const struct vimc_pix_map *vpix = vimc_pix_map_by_index(code->index);
  63. if (!vpix)
  64. return -EINVAL;
  65. code->code = vpix->code;
  66. return 0;
  67. }
  68. static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
  69. struct v4l2_subdev_pad_config *cfg,
  70. struct v4l2_subdev_frame_size_enum *fse)
  71. {
  72. const struct vimc_pix_map *vpix;
  73. if (fse->index)
  74. return -EINVAL;
  75. /* Only accept code in the pix map table */
  76. vpix = vimc_pix_map_by_code(fse->code);
  77. if (!vpix)
  78. return -EINVAL;
  79. fse->min_width = VIMC_FRAME_MIN_WIDTH;
  80. fse->max_width = VIMC_FRAME_MAX_WIDTH;
  81. fse->min_height = VIMC_FRAME_MIN_HEIGHT;
  82. fse->max_height = VIMC_FRAME_MAX_HEIGHT;
  83. return 0;
  84. }
  85. static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
  86. struct v4l2_subdev_pad_config *cfg,
  87. struct v4l2_subdev_format *fmt)
  88. {
  89. struct vimc_sen_device *vsen =
  90. container_of(sd, struct vimc_sen_device, sd);
  91. fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
  92. *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) :
  93. vsen->mbus_format;
  94. return 0;
  95. }
  96. static void vimc_sen_tpg_s_format(struct vimc_sen_device *vsen)
  97. {
  98. const struct vimc_pix_map *vpix =
  99. vimc_pix_map_by_code(vsen->mbus_format.code);
  100. tpg_reset_source(&vsen->tpg, vsen->mbus_format.width,
  101. vsen->mbus_format.height, vsen->mbus_format.field);
  102. tpg_s_bytesperline(&vsen->tpg, 0, vsen->mbus_format.width * vpix->bpp);
  103. tpg_s_buf_height(&vsen->tpg, vsen->mbus_format.height);
  104. tpg_s_fourcc(&vsen->tpg, vpix->pixelformat);
  105. /* TODO: add support for V4L2_FIELD_ALTERNATE */
  106. tpg_s_field(&vsen->tpg, vsen->mbus_format.field, false);
  107. tpg_s_colorspace(&vsen->tpg, vsen->mbus_format.colorspace);
  108. tpg_s_ycbcr_enc(&vsen->tpg, vsen->mbus_format.ycbcr_enc);
  109. tpg_s_quantization(&vsen->tpg, vsen->mbus_format.quantization);
  110. tpg_s_xfer_func(&vsen->tpg, vsen->mbus_format.xfer_func);
  111. }
  112. static void vimc_sen_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
  113. {
  114. const struct vimc_pix_map *vpix;
  115. /* Only accept code in the pix map table */
  116. vpix = vimc_pix_map_by_code(fmt->code);
  117. if (!vpix)
  118. fmt->code = fmt_default.code;
  119. fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
  120. VIMC_FRAME_MAX_WIDTH) & ~1;
  121. fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
  122. VIMC_FRAME_MAX_HEIGHT) & ~1;
  123. /* TODO: add support for V4L2_FIELD_ALTERNATE */
  124. if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
  125. fmt->field = fmt_default.field;
  126. vimc_colorimetry_clamp(fmt);
  127. }
  128. static int vimc_sen_set_fmt(struct v4l2_subdev *sd,
  129. struct v4l2_subdev_pad_config *cfg,
  130. struct v4l2_subdev_format *fmt)
  131. {
  132. struct vimc_sen_device *vsen = v4l2_get_subdevdata(sd);
  133. struct v4l2_mbus_framefmt *mf;
  134. if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
  135. /* Do not change the format while stream is on */
  136. if (vsen->frame)
  137. return -EBUSY;
  138. mf = &vsen->mbus_format;
  139. } else {
  140. mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
  141. }
  142. /* Set the new format */
  143. vimc_sen_adjust_fmt(&fmt->format);
  144. dev_dbg(vsen->dev, "%s: format update: "
  145. "old:%dx%d (0x%x, %d, %d, %d, %d) "
  146. "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsen->sd.name,
  147. /* old */
  148. mf->width, mf->height, mf->code,
  149. mf->colorspace, mf->quantization,
  150. mf->xfer_func, mf->ycbcr_enc,
  151. /* new */
  152. fmt->format.width, fmt->format.height, fmt->format.code,
  153. fmt->format.colorspace, fmt->format.quantization,
  154. fmt->format.xfer_func, fmt->format.ycbcr_enc);
  155. *mf = fmt->format;
  156. return 0;
  157. }
  158. static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
  159. .init_cfg = vimc_sen_init_cfg,
  160. .enum_mbus_code = vimc_sen_enum_mbus_code,
  161. .enum_frame_size = vimc_sen_enum_frame_size,
  162. .get_fmt = vimc_sen_get_fmt,
  163. .set_fmt = vimc_sen_set_fmt,
  164. };
  165. static void *vimc_sen_process_frame(struct vimc_ent_device *ved,
  166. const void *sink_frame)
  167. {
  168. struct vimc_sen_device *vsen = container_of(ved, struct vimc_sen_device,
  169. ved);
  170. tpg_fill_plane_buffer(&vsen->tpg, 0, 0, vsen->frame);
  171. return vsen->frame;
  172. }
  173. static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
  174. {
  175. struct vimc_sen_device *vsen =
  176. container_of(sd, struct vimc_sen_device, sd);
  177. if (enable) {
  178. const struct vimc_pix_map *vpix;
  179. unsigned int frame_size;
  180. if (vsen->kthread_sen)
  181. /* tpg is already executing */
  182. return 0;
  183. /* Calculate the frame size */
  184. vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
  185. frame_size = vsen->mbus_format.width * vpix->bpp *
  186. vsen->mbus_format.height;
  187. /*
  188. * Allocate the frame buffer. Use vmalloc to be able to
  189. * allocate a large amount of memory
  190. */
  191. vsen->frame = vmalloc(frame_size);
  192. if (!vsen->frame)
  193. return -ENOMEM;
  194. /* configure the test pattern generator */
  195. vimc_sen_tpg_s_format(vsen);
  196. } else {
  197. vfree(vsen->frame);
  198. vsen->frame = NULL;
  199. return 0;
  200. }
  201. return 0;
  202. }
  203. static struct v4l2_subdev_core_ops vimc_sen_core_ops = {
  204. .log_status = v4l2_ctrl_subdev_log_status,
  205. .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
  206. .unsubscribe_event = v4l2_event_subdev_unsubscribe,
  207. };
  208. static const struct v4l2_subdev_video_ops vimc_sen_video_ops = {
  209. .s_stream = vimc_sen_s_stream,
  210. };
  211. static const struct v4l2_subdev_ops vimc_sen_ops = {
  212. .core = &vimc_sen_core_ops,
  213. .pad = &vimc_sen_pad_ops,
  214. .video = &vimc_sen_video_ops,
  215. };
  216. static int vimc_sen_s_ctrl(struct v4l2_ctrl *ctrl)
  217. {
  218. struct vimc_sen_device *vsen =
  219. container_of(ctrl->handler, struct vimc_sen_device, hdl);
  220. switch (ctrl->id) {
  221. case VIMC_CID_TEST_PATTERN:
  222. tpg_s_pattern(&vsen->tpg, ctrl->val);
  223. break;
  224. case V4L2_CID_HFLIP:
  225. tpg_s_hflip(&vsen->tpg, ctrl->val);
  226. break;
  227. case V4L2_CID_VFLIP:
  228. tpg_s_vflip(&vsen->tpg, ctrl->val);
  229. break;
  230. default:
  231. return -EINVAL;
  232. }
  233. return 0;
  234. }
  235. static const struct v4l2_ctrl_ops vimc_sen_ctrl_ops = {
  236. .s_ctrl = vimc_sen_s_ctrl,
  237. };
  238. static void vimc_sen_comp_unbind(struct device *comp, struct device *master,
  239. void *master_data)
  240. {
  241. struct vimc_ent_device *ved = dev_get_drvdata(comp);
  242. struct vimc_sen_device *vsen =
  243. container_of(ved, struct vimc_sen_device, ved);
  244. vimc_ent_sd_unregister(ved, &vsen->sd);
  245. v4l2_ctrl_handler_free(&vsen->hdl);
  246. tpg_free(&vsen->tpg);
  247. kfree(vsen);
  248. }
  249. /* Image Processing Controls */
  250. static const struct v4l2_ctrl_config vimc_sen_ctrl_class = {
  251. .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
  252. .id = VIMC_CID_VIMC_CLASS,
  253. .name = "VIMC Controls",
  254. .type = V4L2_CTRL_TYPE_CTRL_CLASS,
  255. };
  256. static const struct v4l2_ctrl_config vimc_sen_ctrl_test_pattern = {
  257. .ops = &vimc_sen_ctrl_ops,
  258. .id = VIMC_CID_TEST_PATTERN,
  259. .name = "Test Pattern",
  260. .type = V4L2_CTRL_TYPE_MENU,
  261. .max = TPG_PAT_NOISE,
  262. .qmenu = tpg_pattern_strings,
  263. };
  264. static int vimc_sen_comp_bind(struct device *comp, struct device *master,
  265. void *master_data)
  266. {
  267. struct v4l2_device *v4l2_dev = master_data;
  268. struct vimc_platform_data *pdata = comp->platform_data;
  269. struct vimc_sen_device *vsen;
  270. int ret;
  271. /* Allocate the vsen struct */
  272. vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
  273. if (!vsen)
  274. return -ENOMEM;
  275. v4l2_ctrl_handler_init(&vsen->hdl, 4);
  276. v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_class, NULL);
  277. v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_test_pattern, NULL);
  278. v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
  279. V4L2_CID_VFLIP, 0, 1, 1, 0);
  280. v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
  281. V4L2_CID_HFLIP, 0, 1, 1, 0);
  282. vsen->sd.ctrl_handler = &vsen->hdl;
  283. if (vsen->hdl.error) {
  284. ret = vsen->hdl.error;
  285. goto err_free_vsen;
  286. }
  287. /* Initialize ved and sd */
  288. ret = vimc_ent_sd_register(&vsen->ved, &vsen->sd, v4l2_dev,
  289. pdata->entity_name,
  290. MEDIA_ENT_F_CAM_SENSOR, 1,
  291. (const unsigned long[1]) {MEDIA_PAD_FL_SOURCE},
  292. &vimc_sen_ops);
  293. if (ret)
  294. goto err_free_hdl;
  295. vsen->ved.process_frame = vimc_sen_process_frame;
  296. dev_set_drvdata(comp, &vsen->ved);
  297. vsen->dev = comp;
  298. /* Initialize the frame format */
  299. vsen->mbus_format = fmt_default;
  300. /* Initialize the test pattern generator */
  301. tpg_init(&vsen->tpg, vsen->mbus_format.width,
  302. vsen->mbus_format.height);
  303. ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH);
  304. if (ret)
  305. goto err_unregister_ent_sd;
  306. return 0;
  307. err_unregister_ent_sd:
  308. vimc_ent_sd_unregister(&vsen->ved, &vsen->sd);
  309. err_free_hdl:
  310. v4l2_ctrl_handler_free(&vsen->hdl);
  311. err_free_vsen:
  312. kfree(vsen);
  313. return ret;
  314. }
  315. static const struct component_ops vimc_sen_comp_ops = {
  316. .bind = vimc_sen_comp_bind,
  317. .unbind = vimc_sen_comp_unbind,
  318. };
  319. static int vimc_sen_probe(struct platform_device *pdev)
  320. {
  321. return component_add(&pdev->dev, &vimc_sen_comp_ops);
  322. }
  323. static int vimc_sen_remove(struct platform_device *pdev)
  324. {
  325. component_del(&pdev->dev, &vimc_sen_comp_ops);
  326. return 0;
  327. }
  328. static const struct platform_device_id vimc_sen_driver_ids[] = {
  329. {
  330. .name = VIMC_SEN_DRV_NAME,
  331. },
  332. { }
  333. };
  334. static struct platform_driver vimc_sen_pdrv = {
  335. .probe = vimc_sen_probe,
  336. .remove = vimc_sen_remove,
  337. .id_table = vimc_sen_driver_ids,
  338. .driver = {
  339. .name = VIMC_SEN_DRV_NAME,
  340. },
  341. };
  342. module_platform_driver(vimc_sen_pdrv);
  343. MODULE_DEVICE_TABLE(platform, vimc_sen_driver_ids);
  344. MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Sensor");
  345. MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
  346. MODULE_LICENSE("GPL");