m2m-deinterlace.c 26 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * V4L2 deinterlacing support.
  4. *
  5. * Copyright (c) 2012 Vista Silicon S.L.
  6. * Javier Martin <javier.martin@vista-silicon.com>
  7. */
  8. #include <linux/module.h>
  9. #include <linux/slab.h>
  10. #include <linux/interrupt.h>
  11. #include <linux/dmaengine.h>
  12. #include <linux/platform_device.h>
  13. #include <media/v4l2-mem2mem.h>
  14. #include <media/v4l2-device.h>
  15. #include <media/v4l2-ioctl.h>
  16. #include <media/videobuf2-dma-contig.h>
  17. #define MEM2MEM_TEST_MODULE_NAME "mem2mem-deinterlace"
  18. MODULE_DESCRIPTION("mem2mem device which supports deinterlacing using dmaengine");
  19. MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com");
  20. MODULE_LICENSE("GPL");
  21. MODULE_VERSION("0.0.1");
  22. static bool debug;
  23. module_param(debug, bool, 0644);
  24. /* Flags that indicate a format can be used for capture/output */
  25. #define MEM2MEM_CAPTURE (1 << 0)
  26. #define MEM2MEM_OUTPUT (1 << 1)
  27. #define MEM2MEM_NAME "m2m-deinterlace"
  28. #define dprintk(dev, fmt, arg...) \
  29. v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
  30. struct deinterlace_fmt {
  31. u32 fourcc;
  32. /* Types the format can be used for */
  33. u32 types;
  34. };
  35. static struct deinterlace_fmt formats[] = {
  36. {
  37. .fourcc = V4L2_PIX_FMT_YUV420,
  38. .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
  39. },
  40. {
  41. .fourcc = V4L2_PIX_FMT_YUYV,
  42. .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
  43. },
  44. };
  45. #define NUM_FORMATS ARRAY_SIZE(formats)
  46. /* Per-queue, driver-specific private data */
  47. struct deinterlace_q_data {
  48. unsigned int width;
  49. unsigned int height;
  50. unsigned int sizeimage;
  51. struct deinterlace_fmt *fmt;
  52. enum v4l2_field field;
  53. };
  54. enum {
  55. V4L2_M2M_SRC = 0,
  56. V4L2_M2M_DST = 1,
  57. };
  58. enum {
  59. YUV420_DMA_Y_ODD,
  60. YUV420_DMA_Y_EVEN,
  61. YUV420_DMA_U_ODD,
  62. YUV420_DMA_U_EVEN,
  63. YUV420_DMA_V_ODD,
  64. YUV420_DMA_V_EVEN,
  65. YUV420_DMA_Y_ODD_DOUBLING,
  66. YUV420_DMA_U_ODD_DOUBLING,
  67. YUV420_DMA_V_ODD_DOUBLING,
  68. YUYV_DMA_ODD,
  69. YUYV_DMA_EVEN,
  70. YUYV_DMA_EVEN_DOUBLING,
  71. };
  72. /* Source and destination queue data */
  73. static struct deinterlace_q_data q_data[2];
  74. static struct deinterlace_q_data *get_q_data(enum v4l2_buf_type type)
  75. {
  76. switch (type) {
  77. case V4L2_BUF_TYPE_VIDEO_OUTPUT:
  78. return &q_data[V4L2_M2M_SRC];
  79. case V4L2_BUF_TYPE_VIDEO_CAPTURE:
  80. return &q_data[V4L2_M2M_DST];
  81. default:
  82. BUG();
  83. }
  84. return NULL;
  85. }
  86. static struct deinterlace_fmt *find_format(struct v4l2_format *f)
  87. {
  88. struct deinterlace_fmt *fmt;
  89. unsigned int k;
  90. for (k = 0; k < NUM_FORMATS; k++) {
  91. fmt = &formats[k];
  92. if ((fmt->types & f->type) &&
  93. (fmt->fourcc == f->fmt.pix.pixelformat))
  94. break;
  95. }
  96. if (k == NUM_FORMATS)
  97. return NULL;
  98. return &formats[k];
  99. }
  100. struct deinterlace_dev {
  101. struct v4l2_device v4l2_dev;
  102. struct video_device vfd;
  103. atomic_t busy;
  104. struct mutex dev_mutex;
  105. spinlock_t irqlock;
  106. struct dma_chan *dma_chan;
  107. struct v4l2_m2m_dev *m2m_dev;
  108. };
  109. struct deinterlace_ctx {
  110. struct v4l2_fh fh;
  111. struct deinterlace_dev *dev;
  112. /* Abort requested by m2m */
  113. int aborting;
  114. enum v4l2_colorspace colorspace;
  115. dma_cookie_t cookie;
  116. struct dma_interleaved_template *xt;
  117. };
  118. /*
  119. * mem2mem callbacks
  120. */
  121. static int deinterlace_job_ready(void *priv)
  122. {
  123. struct deinterlace_ctx *ctx = priv;
  124. struct deinterlace_dev *pcdev = ctx->dev;
  125. if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) > 0 &&
  126. v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) > 0 &&
  127. !atomic_read(&ctx->dev->busy)) {
  128. dprintk(pcdev, "Task ready\n");
  129. return 1;
  130. }
  131. dprintk(pcdev, "Task not ready to run\n");
  132. return 0;
  133. }
  134. static void deinterlace_job_abort(void *priv)
  135. {
  136. struct deinterlace_ctx *ctx = priv;
  137. struct deinterlace_dev *pcdev = ctx->dev;
  138. ctx->aborting = 1;
  139. dprintk(pcdev, "Aborting task\n");
  140. v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->fh.m2m_ctx);
  141. }
  142. static void dma_callback(void *data)
  143. {
  144. struct deinterlace_ctx *curr_ctx = data;
  145. struct deinterlace_dev *pcdev = curr_ctx->dev;
  146. struct vb2_v4l2_buffer *src_vb, *dst_vb;
  147. atomic_set(&pcdev->busy, 0);
  148. src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
  149. dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
  150. dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
  151. dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
  152. dst_vb->flags |=
  153. src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
  154. dst_vb->timecode = src_vb->timecode;
  155. v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
  156. v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
  157. v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->fh.m2m_ctx);
  158. dprintk(pcdev, "dma transfers completed.\n");
  159. }
  160. static void deinterlace_issue_dma(struct deinterlace_ctx *ctx, int op,
  161. int do_callback)
  162. {
  163. struct deinterlace_q_data *s_q_data;
  164. struct vb2_v4l2_buffer *src_buf, *dst_buf;
  165. struct deinterlace_dev *pcdev = ctx->dev;
  166. struct dma_chan *chan = pcdev->dma_chan;
  167. struct dma_device *dmadev = chan->device;
  168. struct dma_async_tx_descriptor *tx;
  169. unsigned int s_width, s_height;
  170. unsigned int s_size;
  171. dma_addr_t p_in, p_out;
  172. enum dma_ctrl_flags flags;
  173. src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
  174. dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
  175. s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
  176. s_width = s_q_data->width;
  177. s_height = s_q_data->height;
  178. s_size = s_width * s_height;
  179. p_in = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
  180. p_out = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf,
  181. 0);
  182. if (!p_in || !p_out) {
  183. v4l2_err(&pcdev->v4l2_dev,
  184. "Acquiring kernel pointers to buffers failed\n");
  185. return;
  186. }
  187. switch (op) {
  188. case YUV420_DMA_Y_ODD:
  189. ctx->xt->numf = s_height / 2;
  190. ctx->xt->sgl[0].size = s_width;
  191. ctx->xt->sgl[0].icg = s_width;
  192. ctx->xt->src_start = p_in;
  193. ctx->xt->dst_start = p_out;
  194. break;
  195. case YUV420_DMA_Y_EVEN:
  196. ctx->xt->numf = s_height / 2;
  197. ctx->xt->sgl[0].size = s_width;
  198. ctx->xt->sgl[0].icg = s_width;
  199. ctx->xt->src_start = p_in + s_size / 2;
  200. ctx->xt->dst_start = p_out + s_width;
  201. break;
  202. case YUV420_DMA_U_ODD:
  203. ctx->xt->numf = s_height / 4;
  204. ctx->xt->sgl[0].size = s_width / 2;
  205. ctx->xt->sgl[0].icg = s_width / 2;
  206. ctx->xt->src_start = p_in + s_size;
  207. ctx->xt->dst_start = p_out + s_size;
  208. break;
  209. case YUV420_DMA_U_EVEN:
  210. ctx->xt->numf = s_height / 4;
  211. ctx->xt->sgl[0].size = s_width / 2;
  212. ctx->xt->sgl[0].icg = s_width / 2;
  213. ctx->xt->src_start = p_in + (9 * s_size) / 8;
  214. ctx->xt->dst_start = p_out + s_size + s_width / 2;
  215. break;
  216. case YUV420_DMA_V_ODD:
  217. ctx->xt->numf = s_height / 4;
  218. ctx->xt->sgl[0].size = s_width / 2;
  219. ctx->xt->sgl[0].icg = s_width / 2;
  220. ctx->xt->src_start = p_in + (5 * s_size) / 4;
  221. ctx->xt->dst_start = p_out + (5 * s_size) / 4;
  222. break;
  223. case YUV420_DMA_V_EVEN:
  224. ctx->xt->numf = s_height / 4;
  225. ctx->xt->sgl[0].size = s_width / 2;
  226. ctx->xt->sgl[0].icg = s_width / 2;
  227. ctx->xt->src_start = p_in + (11 * s_size) / 8;
  228. ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2;
  229. break;
  230. case YUV420_DMA_Y_ODD_DOUBLING:
  231. ctx->xt->numf = s_height / 2;
  232. ctx->xt->sgl[0].size = s_width;
  233. ctx->xt->sgl[0].icg = s_width;
  234. ctx->xt->src_start = p_in;
  235. ctx->xt->dst_start = p_out + s_width;
  236. break;
  237. case YUV420_DMA_U_ODD_DOUBLING:
  238. ctx->xt->numf = s_height / 4;
  239. ctx->xt->sgl[0].size = s_width / 2;
  240. ctx->xt->sgl[0].icg = s_width / 2;
  241. ctx->xt->src_start = p_in + s_size;
  242. ctx->xt->dst_start = p_out + s_size + s_width / 2;
  243. break;
  244. case YUV420_DMA_V_ODD_DOUBLING:
  245. ctx->xt->numf = s_height / 4;
  246. ctx->xt->sgl[0].size = s_width / 2;
  247. ctx->xt->sgl[0].icg = s_width / 2;
  248. ctx->xt->src_start = p_in + (5 * s_size) / 4;
  249. ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2;
  250. break;
  251. case YUYV_DMA_ODD:
  252. ctx->xt->numf = s_height / 2;
  253. ctx->xt->sgl[0].size = s_width * 2;
  254. ctx->xt->sgl[0].icg = s_width * 2;
  255. ctx->xt->src_start = p_in;
  256. ctx->xt->dst_start = p_out;
  257. break;
  258. case YUYV_DMA_EVEN:
  259. ctx->xt->numf = s_height / 2;
  260. ctx->xt->sgl[0].size = s_width * 2;
  261. ctx->xt->sgl[0].icg = s_width * 2;
  262. ctx->xt->src_start = p_in + s_size;
  263. ctx->xt->dst_start = p_out + s_width * 2;
  264. break;
  265. case YUYV_DMA_EVEN_DOUBLING:
  266. default:
  267. ctx->xt->numf = s_height / 2;
  268. ctx->xt->sgl[0].size = s_width * 2;
  269. ctx->xt->sgl[0].icg = s_width * 2;
  270. ctx->xt->src_start = p_in;
  271. ctx->xt->dst_start = p_out + s_width * 2;
  272. break;
  273. }
  274. /* Common parameters for al transfers */
  275. ctx->xt->frame_size = 1;
  276. ctx->xt->dir = DMA_MEM_TO_MEM;
  277. ctx->xt->src_sgl = false;
  278. ctx->xt->dst_sgl = true;
  279. flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
  280. tx = dmadev->device_prep_interleaved_dma(chan, ctx->xt, flags);
  281. if (tx == NULL) {
  282. v4l2_warn(&pcdev->v4l2_dev, "DMA interleaved prep error\n");
  283. return;
  284. }
  285. if (do_callback) {
  286. tx->callback = dma_callback;
  287. tx->callback_param = ctx;
  288. }
  289. ctx->cookie = dmaengine_submit(tx);
  290. if (dma_submit_error(ctx->cookie)) {
  291. v4l2_warn(&pcdev->v4l2_dev,
  292. "DMA submit error %d with src=0x%x dst=0x%x len=0x%x\n",
  293. ctx->cookie, (unsigned)p_in, (unsigned)p_out,
  294. s_size * 3/2);
  295. return;
  296. }
  297. dma_async_issue_pending(chan);
  298. }
  299. static void deinterlace_device_run(void *priv)
  300. {
  301. struct deinterlace_ctx *ctx = priv;
  302. struct deinterlace_q_data *dst_q_data;
  303. atomic_set(&ctx->dev->busy, 1);
  304. dprintk(ctx->dev, "%s: DMA try issue.\n", __func__);
  305. dst_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
  306. /*
  307. * 4 possible field conversions are possible at the moment:
  308. * V4L2_FIELD_SEQ_TB --> V4L2_FIELD_INTERLACED_TB:
  309. * two separate fields in the same input buffer are interlaced
  310. * in the output buffer using weaving. Top field comes first.
  311. * V4L2_FIELD_SEQ_TB --> V4L2_FIELD_NONE:
  312. * top field from the input buffer is copied to the output buffer
  313. * using line doubling. Bottom field from the input buffer is discarded.
  314. * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_INTERLACED_BT:
  315. * two separate fields in the same input buffer are interlaced
  316. * in the output buffer using weaving. Bottom field comes first.
  317. * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_NONE:
  318. * bottom field from the input buffer is copied to the output buffer
  319. * using line doubling. Top field from the input buffer is discarded.
  320. */
  321. switch (dst_q_data->fmt->fourcc) {
  322. case V4L2_PIX_FMT_YUV420:
  323. switch (dst_q_data->field) {
  324. case V4L2_FIELD_INTERLACED_TB:
  325. case V4L2_FIELD_INTERLACED_BT:
  326. dprintk(ctx->dev, "%s: yuv420 interlaced tb.\n",
  327. __func__);
  328. deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
  329. deinterlace_issue_dma(ctx, YUV420_DMA_Y_EVEN, 0);
  330. deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
  331. deinterlace_issue_dma(ctx, YUV420_DMA_U_EVEN, 0);
  332. deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
  333. deinterlace_issue_dma(ctx, YUV420_DMA_V_EVEN, 1);
  334. break;
  335. case V4L2_FIELD_NONE:
  336. default:
  337. dprintk(ctx->dev, "%s: yuv420 interlaced line doubling.\n",
  338. __func__);
  339. deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
  340. deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD_DOUBLING, 0);
  341. deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
  342. deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD_DOUBLING, 0);
  343. deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
  344. deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD_DOUBLING, 1);
  345. break;
  346. }
  347. break;
  348. case V4L2_PIX_FMT_YUYV:
  349. default:
  350. switch (dst_q_data->field) {
  351. case V4L2_FIELD_INTERLACED_TB:
  352. case V4L2_FIELD_INTERLACED_BT:
  353. dprintk(ctx->dev, "%s: yuyv interlaced_tb.\n",
  354. __func__);
  355. deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
  356. deinterlace_issue_dma(ctx, YUYV_DMA_EVEN, 1);
  357. break;
  358. case V4L2_FIELD_NONE:
  359. default:
  360. dprintk(ctx->dev, "%s: yuyv interlaced line doubling.\n",
  361. __func__);
  362. deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
  363. deinterlace_issue_dma(ctx, YUYV_DMA_EVEN_DOUBLING, 1);
  364. break;
  365. }
  366. break;
  367. }
  368. dprintk(ctx->dev, "%s: DMA issue done.\n", __func__);
  369. }
  370. /*
  371. * video ioctls
  372. */
  373. static int vidioc_querycap(struct file *file, void *priv,
  374. struct v4l2_capability *cap)
  375. {
  376. strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
  377. strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
  378. strscpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->bus_info));
  379. return 0;
  380. }
  381. static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
  382. {
  383. int i, num;
  384. struct deinterlace_fmt *fmt;
  385. num = 0;
  386. for (i = 0; i < NUM_FORMATS; ++i) {
  387. if (formats[i].types & type) {
  388. /* index-th format of type type found ? */
  389. if (num == f->index)
  390. break;
  391. /* Correct type but haven't reached our index yet,
  392. * just increment per-type index */
  393. ++num;
  394. }
  395. }
  396. if (i < NUM_FORMATS) {
  397. /* Format found */
  398. fmt = &formats[i];
  399. f->pixelformat = fmt->fourcc;
  400. return 0;
  401. }
  402. /* Format not found */
  403. return -EINVAL;
  404. }
  405. static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
  406. struct v4l2_fmtdesc *f)
  407. {
  408. return enum_fmt(f, MEM2MEM_CAPTURE);
  409. }
  410. static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
  411. struct v4l2_fmtdesc *f)
  412. {
  413. return enum_fmt(f, MEM2MEM_OUTPUT);
  414. }
  415. static int vidioc_g_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
  416. {
  417. struct vb2_queue *vq;
  418. struct deinterlace_q_data *q_data;
  419. vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
  420. if (!vq)
  421. return -EINVAL;
  422. q_data = get_q_data(f->type);
  423. f->fmt.pix.width = q_data->width;
  424. f->fmt.pix.height = q_data->height;
  425. f->fmt.pix.field = q_data->field;
  426. f->fmt.pix.pixelformat = q_data->fmt->fourcc;
  427. switch (q_data->fmt->fourcc) {
  428. case V4L2_PIX_FMT_YUV420:
  429. f->fmt.pix.bytesperline = q_data->width * 3 / 2;
  430. break;
  431. case V4L2_PIX_FMT_YUYV:
  432. default:
  433. f->fmt.pix.bytesperline = q_data->width * 2;
  434. }
  435. f->fmt.pix.sizeimage = q_data->sizeimage;
  436. f->fmt.pix.colorspace = ctx->colorspace;
  437. return 0;
  438. }
  439. static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
  440. struct v4l2_format *f)
  441. {
  442. return vidioc_g_fmt(priv, f);
  443. }
  444. static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
  445. struct v4l2_format *f)
  446. {
  447. return vidioc_g_fmt(priv, f);
  448. }
  449. static int vidioc_try_fmt(struct v4l2_format *f, struct deinterlace_fmt *fmt)
  450. {
  451. switch (f->fmt.pix.pixelformat) {
  452. case V4L2_PIX_FMT_YUV420:
  453. f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
  454. break;
  455. case V4L2_PIX_FMT_YUYV:
  456. default:
  457. f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
  458. }
  459. f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
  460. return 0;
  461. }
  462. static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
  463. struct v4l2_format *f)
  464. {
  465. struct deinterlace_fmt *fmt;
  466. struct deinterlace_ctx *ctx = priv;
  467. fmt = find_format(f);
  468. if (!fmt || !(fmt->types & MEM2MEM_CAPTURE))
  469. f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
  470. f->fmt.pix.colorspace = ctx->colorspace;
  471. if (f->fmt.pix.field != V4L2_FIELD_INTERLACED_TB &&
  472. f->fmt.pix.field != V4L2_FIELD_INTERLACED_BT &&
  473. f->fmt.pix.field != V4L2_FIELD_NONE)
  474. f->fmt.pix.field = V4L2_FIELD_INTERLACED_TB;
  475. return vidioc_try_fmt(f, fmt);
  476. }
  477. static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
  478. struct v4l2_format *f)
  479. {
  480. struct deinterlace_fmt *fmt;
  481. fmt = find_format(f);
  482. if (!fmt || !(fmt->types & MEM2MEM_OUTPUT))
  483. f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
  484. if (!f->fmt.pix.colorspace)
  485. f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
  486. if (f->fmt.pix.field != V4L2_FIELD_SEQ_TB &&
  487. f->fmt.pix.field != V4L2_FIELD_SEQ_BT)
  488. f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
  489. return vidioc_try_fmt(f, fmt);
  490. }
  491. static int vidioc_s_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
  492. {
  493. struct deinterlace_q_data *q_data;
  494. struct vb2_queue *vq;
  495. vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
  496. if (!vq)
  497. return -EINVAL;
  498. q_data = get_q_data(f->type);
  499. if (!q_data)
  500. return -EINVAL;
  501. if (vb2_is_busy(vq)) {
  502. v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
  503. return -EBUSY;
  504. }
  505. q_data->fmt = find_format(f);
  506. if (!q_data->fmt) {
  507. v4l2_err(&ctx->dev->v4l2_dev,
  508. "Couldn't set format type %d, wxh: %dx%d. fmt: %d, field: %d\n",
  509. f->type, f->fmt.pix.width, f->fmt.pix.height,
  510. f->fmt.pix.pixelformat, f->fmt.pix.field);
  511. return -EINVAL;
  512. }
  513. q_data->width = f->fmt.pix.width;
  514. q_data->height = f->fmt.pix.height;
  515. q_data->field = f->fmt.pix.field;
  516. switch (f->fmt.pix.pixelformat) {
  517. case V4L2_PIX_FMT_YUV420:
  518. f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
  519. q_data->sizeimage = (q_data->width * q_data->height * 3) / 2;
  520. break;
  521. case V4L2_PIX_FMT_YUYV:
  522. default:
  523. f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
  524. q_data->sizeimage = q_data->width * q_data->height * 2;
  525. }
  526. dprintk(ctx->dev,
  527. "Setting format for type %d, wxh: %dx%d, fmt: %d, field: %d\n",
  528. f->type, q_data->width, q_data->height, q_data->fmt->fourcc,
  529. q_data->field);
  530. return 0;
  531. }
  532. static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
  533. struct v4l2_format *f)
  534. {
  535. int ret;
  536. ret = vidioc_try_fmt_vid_cap(file, priv, f);
  537. if (ret)
  538. return ret;
  539. return vidioc_s_fmt(priv, f);
  540. }
  541. static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
  542. struct v4l2_format *f)
  543. {
  544. struct deinterlace_ctx *ctx = priv;
  545. int ret;
  546. ret = vidioc_try_fmt_vid_out(file, priv, f);
  547. if (ret)
  548. return ret;
  549. ret = vidioc_s_fmt(priv, f);
  550. if (!ret)
  551. ctx->colorspace = f->fmt.pix.colorspace;
  552. return ret;
  553. }
  554. static int vidioc_streamon(struct file *file, void *priv,
  555. enum v4l2_buf_type type)
  556. {
  557. struct deinterlace_q_data *s_q_data, *d_q_data;
  558. struct deinterlace_ctx *ctx = priv;
  559. s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
  560. d_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
  561. /* Check that src and dst queues have the same pix format */
  562. if (s_q_data->fmt->fourcc != d_q_data->fmt->fourcc) {
  563. v4l2_err(&ctx->dev->v4l2_dev,
  564. "src and dst formats don't match.\n");
  565. return -EINVAL;
  566. }
  567. /* Check that input and output deinterlacing types are compatible */
  568. switch (s_q_data->field) {
  569. case V4L2_FIELD_SEQ_BT:
  570. if (d_q_data->field != V4L2_FIELD_NONE &&
  571. d_q_data->field != V4L2_FIELD_INTERLACED_BT) {
  572. v4l2_err(&ctx->dev->v4l2_dev,
  573. "src and dst field conversion [(%d)->(%d)] not supported.\n",
  574. s_q_data->field, d_q_data->field);
  575. return -EINVAL;
  576. }
  577. break;
  578. case V4L2_FIELD_SEQ_TB:
  579. if (d_q_data->field != V4L2_FIELD_NONE &&
  580. d_q_data->field != V4L2_FIELD_INTERLACED_TB) {
  581. v4l2_err(&ctx->dev->v4l2_dev,
  582. "src and dst field conversion [(%d)->(%d)] not supported.\n",
  583. s_q_data->field, d_q_data->field);
  584. return -EINVAL;
  585. }
  586. break;
  587. default:
  588. return -EINVAL;
  589. }
  590. return v4l2_m2m_streamon(file, ctx->fh.m2m_ctx, type);
  591. }
  592. static const struct v4l2_ioctl_ops deinterlace_ioctl_ops = {
  593. .vidioc_querycap = vidioc_querycap,
  594. .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
  595. .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
  596. .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
  597. .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
  598. .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
  599. .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out,
  600. .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
  601. .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,
  602. .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,
  603. .vidioc_querybuf = v4l2_m2m_ioctl_querybuf,
  604. .vidioc_qbuf = v4l2_m2m_ioctl_qbuf,
  605. .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
  606. .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
  607. .vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
  608. .vidioc_streamon = vidioc_streamon,
  609. .vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
  610. };
  611. /*
  612. * Queue operations
  613. */
  614. struct vb2_dc_conf {
  615. struct device *dev;
  616. };
  617. static int deinterlace_queue_setup(struct vb2_queue *vq,
  618. unsigned int *nbuffers, unsigned int *nplanes,
  619. unsigned int sizes[], struct device *alloc_devs[])
  620. {
  621. struct deinterlace_ctx *ctx = vb2_get_drv_priv(vq);
  622. struct deinterlace_q_data *q_data;
  623. unsigned int size, count = *nbuffers;
  624. q_data = get_q_data(vq->type);
  625. switch (q_data->fmt->fourcc) {
  626. case V4L2_PIX_FMT_YUV420:
  627. size = q_data->width * q_data->height * 3 / 2;
  628. break;
  629. case V4L2_PIX_FMT_YUYV:
  630. default:
  631. size = q_data->width * q_data->height * 2;
  632. }
  633. *nplanes = 1;
  634. *nbuffers = count;
  635. sizes[0] = size;
  636. dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
  637. return 0;
  638. }
  639. static int deinterlace_buf_prepare(struct vb2_buffer *vb)
  640. {
  641. struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
  642. struct deinterlace_q_data *q_data;
  643. dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
  644. q_data = get_q_data(vb->vb2_queue->type);
  645. if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
  646. dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
  647. __func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
  648. return -EINVAL;
  649. }
  650. vb2_set_plane_payload(vb, 0, q_data->sizeimage);
  651. return 0;
  652. }
  653. static void deinterlace_buf_queue(struct vb2_buffer *vb)
  654. {
  655. struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
  656. struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
  657. v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
  658. }
  659. static const struct vb2_ops deinterlace_qops = {
  660. .queue_setup = deinterlace_queue_setup,
  661. .buf_prepare = deinterlace_buf_prepare,
  662. .buf_queue = deinterlace_buf_queue,
  663. .wait_prepare = vb2_ops_wait_prepare,
  664. .wait_finish = vb2_ops_wait_finish,
  665. };
  666. static int queue_init(void *priv, struct vb2_queue *src_vq,
  667. struct vb2_queue *dst_vq)
  668. {
  669. struct deinterlace_ctx *ctx = priv;
  670. int ret;
  671. src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
  672. src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
  673. src_vq->drv_priv = ctx;
  674. src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
  675. src_vq->ops = &deinterlace_qops;
  676. src_vq->mem_ops = &vb2_dma_contig_memops;
  677. src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
  678. src_vq->dev = ctx->dev->v4l2_dev.dev;
  679. src_vq->lock = &ctx->dev->dev_mutex;
  680. q_data[V4L2_M2M_SRC].fmt = &formats[0];
  681. q_data[V4L2_M2M_SRC].width = 640;
  682. q_data[V4L2_M2M_SRC].height = 480;
  683. q_data[V4L2_M2M_SRC].sizeimage = (640 * 480 * 3) / 2;
  684. q_data[V4L2_M2M_SRC].field = V4L2_FIELD_SEQ_TB;
  685. ret = vb2_queue_init(src_vq);
  686. if (ret)
  687. return ret;
  688. dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  689. dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
  690. dst_vq->drv_priv = ctx;
  691. dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
  692. dst_vq->ops = &deinterlace_qops;
  693. dst_vq->mem_ops = &vb2_dma_contig_memops;
  694. dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
  695. dst_vq->dev = ctx->dev->v4l2_dev.dev;
  696. dst_vq->lock = &ctx->dev->dev_mutex;
  697. q_data[V4L2_M2M_DST].fmt = &formats[0];
  698. q_data[V4L2_M2M_DST].width = 640;
  699. q_data[V4L2_M2M_DST].height = 480;
  700. q_data[V4L2_M2M_DST].sizeimage = (640 * 480 * 3) / 2;
  701. q_data[V4L2_M2M_SRC].field = V4L2_FIELD_INTERLACED_TB;
  702. return vb2_queue_init(dst_vq);
  703. }
  704. /*
  705. * File operations
  706. */
  707. static int deinterlace_open(struct file *file)
  708. {
  709. struct deinterlace_dev *pcdev = video_drvdata(file);
  710. struct deinterlace_ctx *ctx = NULL;
  711. ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
  712. if (!ctx)
  713. return -ENOMEM;
  714. v4l2_fh_init(&ctx->fh, video_devdata(file));
  715. file->private_data = &ctx->fh;
  716. ctx->dev = pcdev;
  717. ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
  718. if (IS_ERR(ctx->fh.m2m_ctx)) {
  719. int ret = PTR_ERR(ctx->fh.m2m_ctx);
  720. kfree(ctx);
  721. return ret;
  722. }
  723. ctx->xt = kzalloc(sizeof(struct dma_interleaved_template) +
  724. sizeof(struct data_chunk), GFP_KERNEL);
  725. if (!ctx->xt) {
  726. kfree(ctx);
  727. return -ENOMEM;
  728. }
  729. ctx->colorspace = V4L2_COLORSPACE_REC709;
  730. v4l2_fh_add(&ctx->fh);
  731. dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n",
  732. ctx, ctx->fh.m2m_ctx);
  733. return 0;
  734. }
  735. static int deinterlace_release(struct file *file)
  736. {
  737. struct deinterlace_dev *pcdev = video_drvdata(file);
  738. struct deinterlace_ctx *ctx = file->private_data;
  739. dprintk(pcdev, "Releasing instance %p\n", ctx);
  740. v4l2_fh_del(&ctx->fh);
  741. v4l2_fh_exit(&ctx->fh);
  742. v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
  743. kfree(ctx->xt);
  744. kfree(ctx);
  745. return 0;
  746. }
  747. static const struct v4l2_file_operations deinterlace_fops = {
  748. .owner = THIS_MODULE,
  749. .open = deinterlace_open,
  750. .release = deinterlace_release,
  751. .poll = v4l2_m2m_fop_poll,
  752. .unlocked_ioctl = video_ioctl2,
  753. .mmap = v4l2_m2m_fop_mmap,
  754. };
  755. static const struct video_device deinterlace_videodev = {
  756. .name = MEM2MEM_NAME,
  757. .fops = &deinterlace_fops,
  758. .ioctl_ops = &deinterlace_ioctl_ops,
  759. .minor = -1,
  760. .release = video_device_release_empty,
  761. .vfl_dir = VFL_DIR_M2M,
  762. .device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
  763. };
  764. static const struct v4l2_m2m_ops m2m_ops = {
  765. .device_run = deinterlace_device_run,
  766. .job_ready = deinterlace_job_ready,
  767. .job_abort = deinterlace_job_abort,
  768. };
  769. static int deinterlace_probe(struct platform_device *pdev)
  770. {
  771. struct deinterlace_dev *pcdev;
  772. struct video_device *vfd;
  773. dma_cap_mask_t mask;
  774. int ret = 0;
  775. pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
  776. if (!pcdev)
  777. return -ENOMEM;
  778. spin_lock_init(&pcdev->irqlock);
  779. dma_cap_zero(mask);
  780. dma_cap_set(DMA_INTERLEAVE, mask);
  781. pcdev->dma_chan = dma_request_channel(mask, NULL, pcdev);
  782. if (!pcdev->dma_chan)
  783. return -ENODEV;
  784. if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) {
  785. dev_err(&pdev->dev, "DMA does not support INTERLEAVE\n");
  786. ret = -ENODEV;
  787. goto rel_dma;
  788. }
  789. ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
  790. if (ret)
  791. goto rel_dma;
  792. atomic_set(&pcdev->busy, 0);
  793. mutex_init(&pcdev->dev_mutex);
  794. vfd = &pcdev->vfd;
  795. *vfd = deinterlace_videodev;
  796. vfd->lock = &pcdev->dev_mutex;
  797. vfd->v4l2_dev = &pcdev->v4l2_dev;
  798. ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
  799. if (ret) {
  800. v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
  801. goto unreg_dev;
  802. }
  803. video_set_drvdata(vfd, pcdev);
  804. v4l2_info(&pcdev->v4l2_dev, MEM2MEM_TEST_MODULE_NAME
  805. " Device registered as /dev/video%d\n", vfd->num);
  806. platform_set_drvdata(pdev, pcdev);
  807. pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
  808. if (IS_ERR(pcdev->m2m_dev)) {
  809. v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
  810. ret = PTR_ERR(pcdev->m2m_dev);
  811. goto err_m2m;
  812. }
  813. return 0;
  814. err_m2m:
  815. video_unregister_device(&pcdev->vfd);
  816. unreg_dev:
  817. v4l2_device_unregister(&pcdev->v4l2_dev);
  818. rel_dma:
  819. dma_release_channel(pcdev->dma_chan);
  820. return ret;
  821. }
  822. static int deinterlace_remove(struct platform_device *pdev)
  823. {
  824. struct deinterlace_dev *pcdev = platform_get_drvdata(pdev);
  825. v4l2_info(&pcdev->v4l2_dev, "Removing " MEM2MEM_TEST_MODULE_NAME);
  826. v4l2_m2m_release(pcdev->m2m_dev);
  827. video_unregister_device(&pcdev->vfd);
  828. v4l2_device_unregister(&pcdev->v4l2_dev);
  829. dma_release_channel(pcdev->dma_chan);
  830. return 0;
  831. }
  832. static struct platform_driver deinterlace_pdrv = {
  833. .probe = deinterlace_probe,
  834. .remove = deinterlace_remove,
  835. .driver = {
  836. .name = MEM2MEM_NAME,
  837. },
  838. };
  839. module_platform_driver(deinterlace_pdrv);