1426 lines · c
1// SPDX-License-Identifier: GPL-2.0+2/*3 * A virtual v4l2-mem2mem example device.4 *5 * This is a virtual device driver for testing mem-to-mem vb2 framework.6 * It simulates a device that uses memory buffers for both source and7 * destination, processes the data and issues an "irq" (simulated by a delayed8 * workqueue).9 * The device is capable of multi-instance, multi-buffer-per-transaction10 * operation (via the mem2mem framework).11 *12 * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.13 * Pawel Osciak, <pawel@osciak.com>14 * Marek Szyprowski, <m.szyprowski@samsung.com>15 */16#include <linux/module.h>17#include <linux/delay.h>18#include <linux/fs.h>19#include <linux/sched.h>20#include <linux/slab.h>21 22#include <linux/platform_device.h>23#include <media/v4l2-mem2mem.h>24#include <media/v4l2-device.h>25#include <media/v4l2-ioctl.h>26#include <media/v4l2-ctrls.h>27#include <media/v4l2-event.h>28#include <media/videobuf2-vmalloc.h>29 30MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");31MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");32MODULE_LICENSE("GPL");33MODULE_VERSION("0.2");34MODULE_ALIAS("mem2mem_testdev");35 36static unsigned int debug;37module_param(debug, uint, 0644);38MODULE_PARM_DESC(debug, "debug level");39 40/* Default transaction time in msec */41static unsigned int default_transtime = 40; /* Max 25 fps */42module_param(default_transtime, uint, 0644);43MODULE_PARM_DESC(default_transtime, "default transaction time in ms");44 45#define MIN_W 3246#define MIN_H 3247#define MAX_W 64048#define MAX_H 48049 50/* Pixel alignment for non-bayer formats */51#define WIDTH_ALIGN 252#define HEIGHT_ALIGN 153 54/* Pixel alignment for bayer formats */55#define BAYER_WIDTH_ALIGN 256#define BAYER_HEIGHT_ALIGN 257 58/* Flags that indicate a format can be used for capture/output */59#define MEM2MEM_CAPTURE BIT(0)60#define MEM2MEM_OUTPUT BIT(1)61 62#define MEM2MEM_NAME "vim2m"63 64/* Per queue */65#define MEM2MEM_DEF_NUM_BUFS VIDEO_MAX_FRAME66/* In bytes, per queue */67#define MEM2MEM_VID_MEM_LIMIT (16 * 1024 * 1024)68 69/* Flags that indicate processing mode */70#define MEM2MEM_HFLIP BIT(0)71#define MEM2MEM_VFLIP BIT(1)72 73#define dprintk(dev, lvl, fmt, arg...) \74 v4l2_dbg(lvl, debug, &(dev)->v4l2_dev, "%s: " fmt, __func__, ## arg)75 76static void vim2m_dev_release(struct device *dev)77{}78 79static struct platform_device vim2m_pdev = {80 .name = MEM2MEM_NAME,81 .dev.release = vim2m_dev_release,82};83 84struct vim2m_fmt {85 u32 fourcc;86 int depth;87 /* Types the format can be used for */88 u32 types;89};90 91static struct vim2m_fmt formats[] = {92 {93 .fourcc = V4L2_PIX_FMT_RGB565, /* rrrrrggg gggbbbbb */94 .depth = 16,95 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,96 }, {97 .fourcc = V4L2_PIX_FMT_RGB565X, /* gggbbbbb rrrrrggg */98 .depth = 16,99 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,100 }, {101 .fourcc = V4L2_PIX_FMT_RGB24,102 .depth = 24,103 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,104 }, {105 .fourcc = V4L2_PIX_FMT_BGR24,106 .depth = 24,107 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,108 }, {109 .fourcc = V4L2_PIX_FMT_YUYV,110 .depth = 16,111 .types = MEM2MEM_CAPTURE,112 }, {113 .fourcc = V4L2_PIX_FMT_SBGGR8,114 .depth = 8,115 .types = MEM2MEM_CAPTURE,116 }, {117 .fourcc = V4L2_PIX_FMT_SGBRG8,118 .depth = 8,119 .types = MEM2MEM_CAPTURE,120 }, {121 .fourcc = V4L2_PIX_FMT_SGRBG8,122 .depth = 8,123 .types = MEM2MEM_CAPTURE,124 }, {125 .fourcc = V4L2_PIX_FMT_SRGGB8,126 .depth = 8,127 .types = MEM2MEM_CAPTURE,128 },129};130 131#define NUM_FORMATS ARRAY_SIZE(formats)132 133/* Per-queue, driver-specific private data */134struct vim2m_q_data {135 unsigned int width;136 unsigned int height;137 unsigned int sizeimage;138 unsigned int sequence;139 struct vim2m_fmt *fmt;140};141 142enum {143 V4L2_M2M_SRC = 0,144 V4L2_M2M_DST = 1,145};146 147#define V4L2_CID_TRANS_TIME_MSEC (V4L2_CID_USER_BASE + 0x1000)148#define V4L2_CID_TRANS_NUM_BUFS (V4L2_CID_USER_BASE + 0x1001)149 150static struct vim2m_fmt *find_format(u32 fourcc)151{152 struct vim2m_fmt *fmt;153 unsigned int k;154 155 for (k = 0; k < NUM_FORMATS; k++) {156 fmt = &formats[k];157 if (fmt->fourcc == fourcc)158 break;159 }160 161 if (k == NUM_FORMATS)162 return NULL;163 164 return &formats[k];165}166 167static void get_alignment(u32 fourcc,168 unsigned int *walign, unsigned int *halign)169{170 switch (fourcc) {171 case V4L2_PIX_FMT_SBGGR8:172 case V4L2_PIX_FMT_SGBRG8:173 case V4L2_PIX_FMT_SGRBG8:174 case V4L2_PIX_FMT_SRGGB8:175 *walign = BAYER_WIDTH_ALIGN;176 *halign = BAYER_HEIGHT_ALIGN;177 return;178 default:179 *walign = WIDTH_ALIGN;180 *halign = HEIGHT_ALIGN;181 return;182 }183}184 185struct vim2m_dev {186 struct v4l2_device v4l2_dev;187 struct video_device vfd;188#ifdef CONFIG_MEDIA_CONTROLLER189 struct media_device mdev;190#endif191 192 atomic_t num_inst;193 struct mutex dev_mutex;194 195 struct v4l2_m2m_dev *m2m_dev;196};197 198struct vim2m_ctx {199 struct v4l2_fh fh;200 struct vim2m_dev *dev;201 202 struct v4l2_ctrl_handler hdl;203 204 /* Processed buffers in this transaction */205 u8 num_processed;206 207 /* Transaction length (i.e. how many buffers per transaction) */208 u32 translen;209 /* Transaction time (i.e. simulated processing time) in milliseconds */210 u32 transtime;211 212 struct mutex vb_mutex;213 struct delayed_work work_run;214 215 /* Abort requested by m2m */216 int aborting;217 218 /* Processing mode */219 int mode;220 221 enum v4l2_colorspace colorspace;222 enum v4l2_ycbcr_encoding ycbcr_enc;223 enum v4l2_xfer_func xfer_func;224 enum v4l2_quantization quant;225 226 /* Source and destination queue data */227 struct vim2m_q_data q_data[2];228};229 230static inline struct vim2m_ctx *file2ctx(struct file *file)231{232 return container_of(file->private_data, struct vim2m_ctx, fh);233}234 235static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,236 enum v4l2_buf_type type)237{238 switch (type) {239 case V4L2_BUF_TYPE_VIDEO_OUTPUT:240 return &ctx->q_data[V4L2_M2M_SRC];241 case V4L2_BUF_TYPE_VIDEO_CAPTURE:242 return &ctx->q_data[V4L2_M2M_DST];243 default:244 return NULL;245 }246}247 248static const char *type_name(enum v4l2_buf_type type)249{250 switch (type) {251 case V4L2_BUF_TYPE_VIDEO_OUTPUT:252 return "Output";253 case V4L2_BUF_TYPE_VIDEO_CAPTURE:254 return "Capture";255 default:256 return "Invalid";257 }258}259 260#define CLIP(__color) \261 (u8)(((__color) > 0xff) ? 0xff : (((__color) < 0) ? 0 : (__color)))262 263static void copy_line(struct vim2m_q_data *q_data_out,264 u8 *src, u8 *dst, bool reverse)265{266 int x, depth = q_data_out->fmt->depth >> 3;267 268 if (!reverse) {269 memcpy(dst, src, q_data_out->width * depth);270 } else {271 for (x = 0; x < q_data_out->width >> 1; x++) {272 memcpy(dst, src, depth);273 memcpy(dst + depth, src - depth, depth);274 src -= depth << 1;275 dst += depth << 1;276 }277 return;278 }279}280 281static void copy_two_pixels(struct vim2m_q_data *q_data_in,282 struct vim2m_q_data *q_data_out,283 u8 *src[2], u8 **dst, int ypos, bool reverse)284{285 struct vim2m_fmt *out = q_data_out->fmt;286 struct vim2m_fmt *in = q_data_in->fmt;287 u8 _r[2], _g[2], _b[2], *r, *g, *b;288 int i;289 290 /* Step 1: read two consecutive pixels from src pointer */291 292 r = _r;293 g = _g;294 b = _b;295 296 switch (in->fourcc) {297 case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */298 for (i = 0; i < 2; i++) {299 u16 pix = le16_to_cpu(*(__le16 *)(src[i]));300 301 *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;302 *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;303 *b++ = (u8)((pix & 0x1f) << 3) | 0x07;304 }305 break;306 case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */307 for (i = 0; i < 2; i++) {308 u16 pix = be16_to_cpu(*(__be16 *)(src[i]));309 310 *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;311 *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;312 *b++ = (u8)((pix & 0x1f) << 3) | 0x07;313 }314 break;315 default:316 case V4L2_PIX_FMT_RGB24:317 for (i = 0; i < 2; i++) {318 *r++ = src[i][0];319 *g++ = src[i][1];320 *b++ = src[i][2];321 }322 break;323 case V4L2_PIX_FMT_BGR24:324 for (i = 0; i < 2; i++) {325 *b++ = src[i][0];326 *g++ = src[i][1];327 *r++ = src[i][2];328 }329 break;330 }331 332 /* Step 2: store two consecutive points, reversing them if needed */333 334 r = _r;335 g = _g;336 b = _b;337 338 switch (out->fourcc) {339 case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */340 for (i = 0; i < 2; i++) {341 u16 pix;342 __le16 *dst_pix = (__le16 *)*dst;343 344 pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |345 (*b >> 3);346 347 *dst_pix = cpu_to_le16(pix);348 349 *dst += 2;350 }351 return;352 case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */353 for (i = 0; i < 2; i++) {354 u16 pix;355 __be16 *dst_pix = (__be16 *)*dst;356 357 pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |358 (*b >> 3);359 360 *dst_pix = cpu_to_be16(pix);361 362 *dst += 2;363 }364 return;365 case V4L2_PIX_FMT_RGB24:366 for (i = 0; i < 2; i++) {367 *(*dst)++ = *r++;368 *(*dst)++ = *g++;369 *(*dst)++ = *b++;370 }371 return;372 case V4L2_PIX_FMT_BGR24:373 for (i = 0; i < 2; i++) {374 *(*dst)++ = *b++;375 *(*dst)++ = *g++;376 *(*dst)++ = *r++;377 }378 return;379 case V4L2_PIX_FMT_YUYV:380 default:381 {382 u8 y, y1, u, v;383 384 y = ((8453 * (*r) + 16594 * (*g) + 3223 * (*b)385 + 524288) >> 15);386 u = ((-4878 * (*r) - 9578 * (*g) + 14456 * (*b)387 + 4210688) >> 15);388 v = ((14456 * (*r++) - 12105 * (*g++) - 2351 * (*b++)389 + 4210688) >> 15);390 y1 = ((8453 * (*r) + 16594 * (*g) + 3223 * (*b)391 + 524288) >> 15);392 393 *(*dst)++ = y;394 *(*dst)++ = u;395 396 *(*dst)++ = y1;397 *(*dst)++ = v;398 return;399 }400 case V4L2_PIX_FMT_SBGGR8:401 if (!(ypos & 1)) {402 *(*dst)++ = *b;403 *(*dst)++ = *++g;404 } else {405 *(*dst)++ = *g;406 *(*dst)++ = *++r;407 }408 return;409 case V4L2_PIX_FMT_SGBRG8:410 if (!(ypos & 1)) {411 *(*dst)++ = *g;412 *(*dst)++ = *++b;413 } else {414 *(*dst)++ = *r;415 *(*dst)++ = *++g;416 }417 return;418 case V4L2_PIX_FMT_SGRBG8:419 if (!(ypos & 1)) {420 *(*dst)++ = *g;421 *(*dst)++ = *++r;422 } else {423 *(*dst)++ = *b;424 *(*dst)++ = *++g;425 }426 return;427 case V4L2_PIX_FMT_SRGGB8:428 if (!(ypos & 1)) {429 *(*dst)++ = *r;430 *(*dst)++ = *++g;431 } else {432 *(*dst)++ = *g;433 *(*dst)++ = *++b;434 }435 return;436 }437}438 439static int device_process(struct vim2m_ctx *ctx,440 struct vb2_v4l2_buffer *in_vb,441 struct vb2_v4l2_buffer *out_vb)442{443 struct vim2m_dev *dev = ctx->dev;444 struct vim2m_q_data *q_data_in, *q_data_out;445 u8 *p_in, *p_line, *p_in_x[2], *p, *p_out;446 unsigned int width, height, bytesperline, bytes_per_pixel;447 unsigned int x, y, y_in, y_out, x_int, x_fract, x_err, x_offset;448 int start, end, step;449 450 q_data_in = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);451 if (!q_data_in)452 return 0;453 bytesperline = (q_data_in->width * q_data_in->fmt->depth) >> 3;454 bytes_per_pixel = q_data_in->fmt->depth >> 3;455 456 q_data_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);457 if (!q_data_out)458 return 0;459 460 /* As we're doing scaling, use the output dimensions here */461 height = q_data_out->height;462 width = q_data_out->width;463 464 p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);465 p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);466 if (!p_in || !p_out) {467 v4l2_err(&dev->v4l2_dev,468 "Acquiring kernel pointers to buffers failed\n");469 return -EFAULT;470 }471 472 out_vb->sequence = q_data_out->sequence++;473 in_vb->sequence = q_data_in->sequence++;474 v4l2_m2m_buf_copy_metadata(in_vb, out_vb, true);475 476 if (ctx->mode & MEM2MEM_VFLIP) {477 start = height - 1;478 end = -1;479 step = -1;480 } else {481 start = 0;482 end = height;483 step = 1;484 }485 y_out = 0;486 487 /*488 * When format and resolution are identical,489 * we can use a faster copy logic490 */491 if (q_data_in->fmt->fourcc == q_data_out->fmt->fourcc &&492 q_data_in->width == q_data_out->width &&493 q_data_in->height == q_data_out->height) {494 for (y = start; y != end; y += step, y_out++) {495 p = p_in + (y * bytesperline);496 if (ctx->mode & MEM2MEM_HFLIP)497 p += bytesperline - (q_data_in->fmt->depth >> 3);498 499 copy_line(q_data_out, p, p_out,500 ctx->mode & MEM2MEM_HFLIP);501 502 p_out += bytesperline;503 }504 return 0;505 }506 507 /* Slower algorithm with format conversion, hflip, vflip and scaler */508 509 /* To speed scaler up, use Bresenham for X dimension */510 x_int = q_data_in->width / q_data_out->width;511 x_fract = q_data_in->width % q_data_out->width;512 513 for (y = start; y != end; y += step, y_out++) {514 y_in = (y * q_data_in->height) / q_data_out->height;515 x_offset = 0;516 x_err = 0;517 518 p_line = p_in + (y_in * bytesperline);519 if (ctx->mode & MEM2MEM_HFLIP)520 p_line += bytesperline - (q_data_in->fmt->depth >> 3);521 p_in_x[0] = p_line;522 523 for (x = 0; x < width >> 1; x++) {524 x_offset += x_int;525 x_err += x_fract;526 if (x_err > width) {527 x_offset++;528 x_err -= width;529 }530 531 if (ctx->mode & MEM2MEM_HFLIP)532 p_in_x[1] = p_line - x_offset * bytes_per_pixel;533 else534 p_in_x[1] = p_line + x_offset * bytes_per_pixel;535 536 copy_two_pixels(q_data_in, q_data_out,537 p_in_x, &p_out, y_out,538 ctx->mode & MEM2MEM_HFLIP);539 540 /* Calculate the next p_in_x0 */541 x_offset += x_int;542 x_err += x_fract;543 if (x_err > width) {544 x_offset++;545 x_err -= width;546 }547 548 if (ctx->mode & MEM2MEM_HFLIP)549 p_in_x[0] = p_line - x_offset * bytes_per_pixel;550 else551 p_in_x[0] = p_line + x_offset * bytes_per_pixel;552 }553 }554 555 return 0;556}557 558/*559 * mem2mem callbacks560 */561 562/*563 * job_ready() - check whether an instance is ready to be scheduled to run564 */565static int job_ready(void *priv)566{567 struct vim2m_ctx *ctx = priv;568 569 if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen570 || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) {571 dprintk(ctx->dev, 1, "Not enough buffers available\n");572 return 0;573 }574 575 return 1;576}577 578static void job_abort(void *priv)579{580 struct vim2m_ctx *ctx = priv;581 582 /* Will cancel the transaction in the next interrupt handler */583 ctx->aborting = 1;584}585 586/* device_run() - prepares and starts the device587 *588 * This simulates all the immediate preparations required before starting589 * a device. This will be called by the framework when it decides to schedule590 * a particular instance.591 */592static void device_run(void *priv)593{594 struct vim2m_ctx *ctx = priv;595 struct vb2_v4l2_buffer *src_buf, *dst_buf;596 597 src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);598 dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);599 600 /* Apply request controls if any */601 v4l2_ctrl_request_setup(src_buf->vb2_buf.req_obj.req,602 &ctx->hdl);603 604 device_process(ctx, src_buf, dst_buf);605 606 /* Complete request controls if any */607 v4l2_ctrl_request_complete(src_buf->vb2_buf.req_obj.req,608 &ctx->hdl);609 610 /* Run delayed work, which simulates a hardware irq */611 schedule_delayed_work(&ctx->work_run, msecs_to_jiffies(ctx->transtime));612}613 614static void device_work(struct work_struct *w)615{616 struct vim2m_ctx *curr_ctx;617 struct vim2m_dev *vim2m_dev;618 struct vb2_v4l2_buffer *src_vb, *dst_vb;619 620 curr_ctx = container_of(w, struct vim2m_ctx, work_run.work);621 622 vim2m_dev = curr_ctx->dev;623 624 src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);625 dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);626 627 curr_ctx->num_processed++;628 629 v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);630 v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);631 632 if (curr_ctx->num_processed == curr_ctx->translen633 || curr_ctx->aborting) {634 dprintk(curr_ctx->dev, 2, "Finishing capture buffer fill\n");635 curr_ctx->num_processed = 0;636 v4l2_m2m_job_finish(vim2m_dev->m2m_dev, curr_ctx->fh.m2m_ctx);637 } else {638 device_run(curr_ctx);639 }640}641 642/*643 * video ioctls644 */645static int vidioc_querycap(struct file *file, void *priv,646 struct v4l2_capability *cap)647{648 strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));649 strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));650 snprintf(cap->bus_info, sizeof(cap->bus_info),651 "platform:%s", MEM2MEM_NAME);652 return 0;653}654 655static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)656{657 int i, num;658 struct vim2m_fmt *fmt;659 660 num = 0;661 662 for (i = 0; i < NUM_FORMATS; ++i) {663 if (formats[i].types & type) {664 /* index-th format of type type found ? */665 if (num == f->index)666 break;667 /*668 * Correct type but haven't reached our index yet,669 * just increment per-type index670 */671 ++num;672 }673 }674 675 if (i < NUM_FORMATS) {676 /* Format found */677 fmt = &formats[i];678 f->pixelformat = fmt->fourcc;679 return 0;680 }681 682 /* Format not found */683 return -EINVAL;684}685 686static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,687 struct v4l2_fmtdesc *f)688{689 return enum_fmt(f, MEM2MEM_CAPTURE);690}691 692static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,693 struct v4l2_fmtdesc *f)694{695 return enum_fmt(f, MEM2MEM_OUTPUT);696}697 698static int vidioc_enum_framesizes(struct file *file, void *priv,699 struct v4l2_frmsizeenum *fsize)700{701 if (fsize->index != 0)702 return -EINVAL;703 704 if (!find_format(fsize->pixel_format))705 return -EINVAL;706 707 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;708 fsize->stepwise.min_width = MIN_W;709 fsize->stepwise.min_height = MIN_H;710 fsize->stepwise.max_width = MAX_W;711 fsize->stepwise.max_height = MAX_H;712 713 get_alignment(fsize->pixel_format,714 &fsize->stepwise.step_width,715 &fsize->stepwise.step_height);716 return 0;717}718 719static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)720{721 struct vb2_queue *vq;722 struct vim2m_q_data *q_data;723 724 vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);725 if (!vq)726 return -EINVAL;727 728 q_data = get_q_data(ctx, f->type);729 if (!q_data)730 return -EINVAL;731 732 f->fmt.pix.width = q_data->width;733 f->fmt.pix.height = q_data->height;734 f->fmt.pix.field = V4L2_FIELD_NONE;735 f->fmt.pix.pixelformat = q_data->fmt->fourcc;736 f->fmt.pix.bytesperline = (q_data->width * q_data->fmt->depth) >> 3;737 f->fmt.pix.sizeimage = q_data->sizeimage;738 f->fmt.pix.colorspace = ctx->colorspace;739 f->fmt.pix.xfer_func = ctx->xfer_func;740 f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;741 f->fmt.pix.quantization = ctx->quant;742 743 return 0;744}745 746static int vidioc_g_fmt_vid_out(struct file *file, void *priv,747 struct v4l2_format *f)748{749 return vidioc_g_fmt(file2ctx(file), f);750}751 752static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,753 struct v4l2_format *f)754{755 return vidioc_g_fmt(file2ctx(file), f);756}757 758static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt)759{760 int walign, halign;761 /*762 * V4L2 specification specifies the driver corrects the763 * format struct if any of the dimensions is unsupported764 */765 if (f->fmt.pix.height < MIN_H)766 f->fmt.pix.height = MIN_H;767 else if (f->fmt.pix.height > MAX_H)768 f->fmt.pix.height = MAX_H;769 770 if (f->fmt.pix.width < MIN_W)771 f->fmt.pix.width = MIN_W;772 else if (f->fmt.pix.width > MAX_W)773 f->fmt.pix.width = MAX_W;774 775 get_alignment(f->fmt.pix.pixelformat, &walign, &halign);776 f->fmt.pix.width &= ~(walign - 1);777 f->fmt.pix.height &= ~(halign - 1);778 f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;779 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;780 f->fmt.pix.field = V4L2_FIELD_NONE;781 782 return 0;783}784 785static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,786 struct v4l2_format *f)787{788 struct vim2m_fmt *fmt;789 struct vim2m_ctx *ctx = file2ctx(file);790 791 fmt = find_format(f->fmt.pix.pixelformat);792 if (!fmt) {793 f->fmt.pix.pixelformat = formats[0].fourcc;794 fmt = find_format(f->fmt.pix.pixelformat);795 }796 if (!(fmt->types & MEM2MEM_CAPTURE)) {797 v4l2_err(&ctx->dev->v4l2_dev,798 "Fourcc format (0x%08x) invalid.\n",799 f->fmt.pix.pixelformat);800 return -EINVAL;801 }802 f->fmt.pix.colorspace = ctx->colorspace;803 f->fmt.pix.xfer_func = ctx->xfer_func;804 f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;805 f->fmt.pix.quantization = ctx->quant;806 807 return vidioc_try_fmt(f, fmt);808}809 810static int vidioc_try_fmt_vid_out(struct file *file, void *priv,811 struct v4l2_format *f)812{813 struct vim2m_fmt *fmt;814 struct vim2m_ctx *ctx = file2ctx(file);815 816 fmt = find_format(f->fmt.pix.pixelformat);817 if (!fmt) {818 f->fmt.pix.pixelformat = formats[0].fourcc;819 fmt = find_format(f->fmt.pix.pixelformat);820 }821 if (!(fmt->types & MEM2MEM_OUTPUT)) {822 v4l2_err(&ctx->dev->v4l2_dev,823 "Fourcc format (0x%08x) invalid.\n",824 f->fmt.pix.pixelformat);825 return -EINVAL;826 }827 if (!f->fmt.pix.colorspace)828 f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;829 830 return vidioc_try_fmt(f, fmt);831}832 833static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)834{835 struct vim2m_q_data *q_data;836 struct vb2_queue *vq;837 838 vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);839 if (!vq)840 return -EINVAL;841 842 q_data = get_q_data(ctx, f->type);843 if (!q_data)844 return -EINVAL;845 846 if (vb2_is_busy(vq)) {847 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);848 return -EBUSY;849 }850 851 q_data->fmt = find_format(f->fmt.pix.pixelformat);852 q_data->width = f->fmt.pix.width;853 q_data->height = f->fmt.pix.height;854 q_data->sizeimage = q_data->width * q_data->height855 * q_data->fmt->depth >> 3;856 857 dprintk(ctx->dev, 1,858 "Format for type %s: %dx%d (%d bpp), fmt: %c%c%c%c\n",859 type_name(f->type), q_data->width, q_data->height,860 q_data->fmt->depth,861 (q_data->fmt->fourcc & 0xff),862 (q_data->fmt->fourcc >> 8) & 0xff,863 (q_data->fmt->fourcc >> 16) & 0xff,864 (q_data->fmt->fourcc >> 24) & 0xff);865 866 return 0;867}868 869static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,870 struct v4l2_format *f)871{872 int ret;873 874 ret = vidioc_try_fmt_vid_cap(file, priv, f);875 if (ret)876 return ret;877 878 return vidioc_s_fmt(file2ctx(file), f);879}880 881static int vidioc_s_fmt_vid_out(struct file *file, void *priv,882 struct v4l2_format *f)883{884 struct vim2m_ctx *ctx = file2ctx(file);885 int ret;886 887 ret = vidioc_try_fmt_vid_out(file, priv, f);888 if (ret)889 return ret;890 891 ret = vidioc_s_fmt(file2ctx(file), f);892 if (!ret) {893 ctx->colorspace = f->fmt.pix.colorspace;894 ctx->xfer_func = f->fmt.pix.xfer_func;895 ctx->ycbcr_enc = f->fmt.pix.ycbcr_enc;896 ctx->quant = f->fmt.pix.quantization;897 }898 return ret;899}900 901static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl)902{903 struct vim2m_ctx *ctx =904 container_of(ctrl->handler, struct vim2m_ctx, hdl);905 906 switch (ctrl->id) {907 case V4L2_CID_HFLIP:908 if (ctrl->val)909 ctx->mode |= MEM2MEM_HFLIP;910 else911 ctx->mode &= ~MEM2MEM_HFLIP;912 break;913 914 case V4L2_CID_VFLIP:915 if (ctrl->val)916 ctx->mode |= MEM2MEM_VFLIP;917 else918 ctx->mode &= ~MEM2MEM_VFLIP;919 break;920 921 case V4L2_CID_TRANS_TIME_MSEC:922 ctx->transtime = ctrl->val;923 if (ctx->transtime < 1)924 ctx->transtime = 1;925 break;926 927 case V4L2_CID_TRANS_NUM_BUFS:928 ctx->translen = ctrl->val;929 break;930 931 default:932 v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");933 return -EINVAL;934 }935 936 return 0;937}938 939static const struct v4l2_ctrl_ops vim2m_ctrl_ops = {940 .s_ctrl = vim2m_s_ctrl,941};942 943static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {944 .vidioc_querycap = vidioc_querycap,945 946 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,947 .vidioc_enum_framesizes = vidioc_enum_framesizes,948 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,949 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,950 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,951 952 .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,953 .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out,954 .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,955 .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,956 957 .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,958 .vidioc_querybuf = v4l2_m2m_ioctl_querybuf,959 .vidioc_qbuf = v4l2_m2m_ioctl_qbuf,960 .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,961 .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,962 .vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,963 .vidioc_expbuf = v4l2_m2m_ioctl_expbuf,964 965 .vidioc_streamon = v4l2_m2m_ioctl_streamon,966 .vidioc_streamoff = v4l2_m2m_ioctl_streamoff,967 968 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,969 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,970};971 972/*973 * Queue operations974 */975 976static int vim2m_queue_setup(struct vb2_queue *vq,977 unsigned int *nbuffers,978 unsigned int *nplanes,979 unsigned int sizes[],980 struct device *alloc_devs[])981{982 struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);983 struct vim2m_q_data *q_data;984 unsigned int size, count = *nbuffers;985 986 q_data = get_q_data(ctx, vq->type);987 if (!q_data)988 return -EINVAL;989 990 size = q_data->width * q_data->height * q_data->fmt->depth >> 3;991 992 while (size * count > MEM2MEM_VID_MEM_LIMIT)993 (count)--;994 *nbuffers = count;995 996 if (*nplanes)997 return sizes[0] < size ? -EINVAL : 0;998 999 *nplanes = 1;1000 sizes[0] = size;1001 1002 dprintk(ctx->dev, 1, "%s: get %d buffer(s) of size %d each.\n",1003 type_name(vq->type), count, size);1004 1005 return 0;1006}1007 1008static int vim2m_buf_out_validate(struct vb2_buffer *vb)1009{1010 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);1011 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);1012 1013 if (vbuf->field == V4L2_FIELD_ANY)1014 vbuf->field = V4L2_FIELD_NONE;1015 if (vbuf->field != V4L2_FIELD_NONE) {1016 dprintk(ctx->dev, 1, "%s field isn't supported\n", __func__);1017 return -EINVAL;1018 }1019 1020 return 0;1021}1022 1023static int vim2m_buf_prepare(struct vb2_buffer *vb)1024{1025 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);1026 struct vim2m_q_data *q_data;1027 1028 dprintk(ctx->dev, 2, "type: %s\n", type_name(vb->vb2_queue->type));1029 1030 q_data = get_q_data(ctx, vb->vb2_queue->type);1031 if (!q_data)1032 return -EINVAL;1033 if (vb2_plane_size(vb, 0) < q_data->sizeimage) {1034 dprintk(ctx->dev, 1,1035 "%s data will not fit into plane (%lu < %lu)\n",1036 __func__, vb2_plane_size(vb, 0),1037 (long)q_data->sizeimage);1038 return -EINVAL;1039 }1040 1041 vb2_set_plane_payload(vb, 0, q_data->sizeimage);1042 1043 return 0;1044}1045 1046static void vim2m_buf_queue(struct vb2_buffer *vb)1047{1048 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);1049 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);1050 1051 v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);1052}1053 1054static int vim2m_start_streaming(struct vb2_queue *q, unsigned int count)1055{1056 struct vim2m_ctx *ctx = vb2_get_drv_priv(q);1057 struct vim2m_q_data *q_data = get_q_data(ctx, q->type);1058 1059 if (!q_data)1060 return -EINVAL;1061 1062 if (V4L2_TYPE_IS_OUTPUT(q->type))1063 ctx->aborting = 0;1064 1065 q_data->sequence = 0;1066 return 0;1067}1068 1069static void vim2m_stop_streaming(struct vb2_queue *q)1070{1071 struct vim2m_ctx *ctx = vb2_get_drv_priv(q);1072 struct vb2_v4l2_buffer *vbuf;1073 1074 cancel_delayed_work_sync(&ctx->work_run);1075 1076 for (;;) {1077 if (V4L2_TYPE_IS_OUTPUT(q->type))1078 vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);1079 else1080 vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);1081 if (!vbuf)1082 return;1083 v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,1084 &ctx->hdl);1085 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);1086 }1087}1088 1089static void vim2m_buf_request_complete(struct vb2_buffer *vb)1090{1091 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);1092 1093 v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);1094}1095 1096static const struct vb2_ops vim2m_qops = {1097 .queue_setup = vim2m_queue_setup,1098 .buf_out_validate = vim2m_buf_out_validate,1099 .buf_prepare = vim2m_buf_prepare,1100 .buf_queue = vim2m_buf_queue,1101 .start_streaming = vim2m_start_streaming,1102 .stop_streaming = vim2m_stop_streaming,1103 .wait_prepare = vb2_ops_wait_prepare,1104 .wait_finish = vb2_ops_wait_finish,1105 .buf_request_complete = vim2m_buf_request_complete,1106};1107 1108static int queue_init(void *priv, struct vb2_queue *src_vq,1109 struct vb2_queue *dst_vq)1110{1111 struct vim2m_ctx *ctx = priv;1112 int ret;1113 1114 src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;1115 src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;1116 src_vq->drv_priv = ctx;1117 src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);1118 src_vq->ops = &vim2m_qops;1119 src_vq->mem_ops = &vb2_vmalloc_memops;1120 src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;1121 src_vq->lock = &ctx->vb_mutex;1122 src_vq->supports_requests = true;1123 1124 ret = vb2_queue_init(src_vq);1125 if (ret)1126 return ret;1127 1128 dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;1129 dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;1130 dst_vq->drv_priv = ctx;1131 dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);1132 dst_vq->ops = &vim2m_qops;1133 dst_vq->mem_ops = &vb2_vmalloc_memops;1134 dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;1135 dst_vq->lock = &ctx->vb_mutex;1136 1137 return vb2_queue_init(dst_vq);1138}1139 1140static struct v4l2_ctrl_config vim2m_ctrl_trans_time_msec = {1141 .ops = &vim2m_ctrl_ops,1142 .id = V4L2_CID_TRANS_TIME_MSEC,1143 .name = "Transaction Time (msec)",1144 .type = V4L2_CTRL_TYPE_INTEGER,1145 .min = 1,1146 .max = 10001,1147 .step = 1,1148};1149 1150static const struct v4l2_ctrl_config vim2m_ctrl_trans_num_bufs = {1151 .ops = &vim2m_ctrl_ops,1152 .id = V4L2_CID_TRANS_NUM_BUFS,1153 .name = "Buffers Per Transaction",1154 .type = V4L2_CTRL_TYPE_INTEGER,1155 .def = 1,1156 .min = 1,1157 .max = MEM2MEM_DEF_NUM_BUFS,1158 .step = 1,1159};1160 1161/*1162 * File operations1163 */1164static int vim2m_open(struct file *file)1165{1166 struct vim2m_dev *dev = video_drvdata(file);1167 struct vim2m_ctx *ctx = NULL;1168 struct v4l2_ctrl_handler *hdl;1169 int rc = 0;1170 1171 if (mutex_lock_interruptible(&dev->dev_mutex))1172 return -ERESTARTSYS;1173 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);1174 if (!ctx) {1175 rc = -ENOMEM;1176 goto open_unlock;1177 }1178 1179 v4l2_fh_init(&ctx->fh, video_devdata(file));1180 file->private_data = &ctx->fh;1181 ctx->dev = dev;1182 hdl = &ctx->hdl;1183 v4l2_ctrl_handler_init(hdl, 4);1184 v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);1185 v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);1186 1187 vim2m_ctrl_trans_time_msec.def = default_transtime;1188 v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_time_msec, NULL);1189 v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_num_bufs, NULL);1190 if (hdl->error) {1191 rc = hdl->error;1192 v4l2_ctrl_handler_free(hdl);1193 kfree(ctx);1194 goto open_unlock;1195 }1196 ctx->fh.ctrl_handler = hdl;1197 v4l2_ctrl_handler_setup(hdl);1198 1199 ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];1200 ctx->q_data[V4L2_M2M_SRC].width = 640;1201 ctx->q_data[V4L2_M2M_SRC].height = 480;1202 ctx->q_data[V4L2_M2M_SRC].sizeimage =1203 ctx->q_data[V4L2_M2M_SRC].width *1204 ctx->q_data[V4L2_M2M_SRC].height *1205 (ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);1206 ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];1207 ctx->colorspace = V4L2_COLORSPACE_REC709;1208 1209 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);1210 1211 mutex_init(&ctx->vb_mutex);1212 INIT_DELAYED_WORK(&ctx->work_run, device_work);1213 1214 if (IS_ERR(ctx->fh.m2m_ctx)) {1215 rc = PTR_ERR(ctx->fh.m2m_ctx);1216 1217 v4l2_ctrl_handler_free(hdl);1218 v4l2_fh_exit(&ctx->fh);1219 kfree(ctx);1220 goto open_unlock;1221 }1222 1223 v4l2_fh_add(&ctx->fh);1224 atomic_inc(&dev->num_inst);1225 1226 dprintk(dev, 1, "Created instance: %p, m2m_ctx: %p\n",1227 ctx, ctx->fh.m2m_ctx);1228 1229open_unlock:1230 mutex_unlock(&dev->dev_mutex);1231 return rc;1232}1233 1234static int vim2m_release(struct file *file)1235{1236 struct vim2m_dev *dev = video_drvdata(file);1237 struct vim2m_ctx *ctx = file2ctx(file);1238 1239 dprintk(dev, 1, "Releasing instance %p\n", ctx);1240 1241 v4l2_fh_del(&ctx->fh);1242 v4l2_fh_exit(&ctx->fh);1243 v4l2_ctrl_handler_free(&ctx->hdl);1244 mutex_lock(&dev->dev_mutex);1245 v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);1246 mutex_unlock(&dev->dev_mutex);1247 kfree(ctx);1248 1249 atomic_dec(&dev->num_inst);1250 1251 return 0;1252}1253 1254static void vim2m_device_release(struct video_device *vdev)1255{1256 struct vim2m_dev *dev = container_of(vdev, struct vim2m_dev, vfd);1257 1258 v4l2_device_unregister(&dev->v4l2_dev);1259 v4l2_m2m_release(dev->m2m_dev);1260#ifdef CONFIG_MEDIA_CONTROLLER1261 media_device_cleanup(&dev->mdev);1262#endif1263 kfree(dev);1264}1265 1266static const struct v4l2_file_operations vim2m_fops = {1267 .owner = THIS_MODULE,1268 .open = vim2m_open,1269 .release = vim2m_release,1270 .poll = v4l2_m2m_fop_poll,1271 .unlocked_ioctl = video_ioctl2,1272 .mmap = v4l2_m2m_fop_mmap,1273};1274 1275static const struct video_device vim2m_videodev = {1276 .name = MEM2MEM_NAME,1277 .vfl_dir = VFL_DIR_M2M,1278 .fops = &vim2m_fops,1279 .ioctl_ops = &vim2m_ioctl_ops,1280 .minor = -1,1281 .release = vim2m_device_release,1282 .device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,1283};1284 1285static const struct v4l2_m2m_ops m2m_ops = {1286 .device_run = device_run,1287 .job_ready = job_ready,1288 .job_abort = job_abort,1289};1290 1291static const struct media_device_ops m2m_media_ops = {1292 .req_validate = vb2_request_validate,1293 .req_queue = v4l2_m2m_request_queue,1294};1295 1296static int vim2m_probe(struct platform_device *pdev)1297{1298 struct vim2m_dev *dev;1299 struct video_device *vfd;1300 int ret;1301 1302 dev = kzalloc(sizeof(*dev), GFP_KERNEL);1303 if (!dev)1304 return -ENOMEM;1305 1306 ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);1307 if (ret)1308 goto error_free;1309 1310 atomic_set(&dev->num_inst, 0);1311 mutex_init(&dev->dev_mutex);1312 1313 dev->vfd = vim2m_videodev;1314 vfd = &dev->vfd;1315 vfd->lock = &dev->dev_mutex;1316 vfd->v4l2_dev = &dev->v4l2_dev;1317 1318 video_set_drvdata(vfd, dev);1319 v4l2_info(&dev->v4l2_dev,1320 "Device registered as /dev/video%d\n", vfd->num);1321 1322 platform_set_drvdata(pdev, dev);1323 1324 dev->m2m_dev = v4l2_m2m_init(&m2m_ops);1325 if (IS_ERR(dev->m2m_dev)) {1326 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");1327 ret = PTR_ERR(dev->m2m_dev);1328 dev->m2m_dev = NULL;1329 goto error_dev;1330 }1331 1332#ifdef CONFIG_MEDIA_CONTROLLER1333 dev->mdev.dev = &pdev->dev;1334 strscpy(dev->mdev.model, "vim2m", sizeof(dev->mdev.model));1335 strscpy(dev->mdev.bus_info, "platform:vim2m",1336 sizeof(dev->mdev.bus_info));1337 media_device_init(&dev->mdev);1338 dev->mdev.ops = &m2m_media_ops;1339 dev->v4l2_dev.mdev = &dev->mdev;1340#endif1341 1342 ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);1343 if (ret) {1344 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");1345 goto error_m2m;1346 }1347 1348#ifdef CONFIG_MEDIA_CONTROLLER1349 ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd,1350 MEDIA_ENT_F_PROC_VIDEO_SCALER);1351 if (ret) {1352 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");1353 goto error_v4l2;1354 }1355 1356 ret = media_device_register(&dev->mdev);1357 if (ret) {1358 v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");1359 goto error_m2m_mc;1360 }1361#endif1362 return 0;1363 1364#ifdef CONFIG_MEDIA_CONTROLLER1365error_m2m_mc:1366 v4l2_m2m_unregister_media_controller(dev->m2m_dev);1367#endif1368error_v4l2:1369 video_unregister_device(&dev->vfd);1370 /* vim2m_device_release called by video_unregister_device to release various objects */1371 return ret;1372error_m2m:1373 v4l2_m2m_release(dev->m2m_dev);1374error_dev:1375 v4l2_device_unregister(&dev->v4l2_dev);1376error_free:1377 kfree(dev);1378 1379 return ret;1380}1381 1382static void vim2m_remove(struct platform_device *pdev)1383{1384 struct vim2m_dev *dev = platform_get_drvdata(pdev);1385 1386 v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);1387 1388#ifdef CONFIG_MEDIA_CONTROLLER1389 media_device_unregister(&dev->mdev);1390 v4l2_m2m_unregister_media_controller(dev->m2m_dev);1391#endif1392 video_unregister_device(&dev->vfd);1393}1394 1395static struct platform_driver vim2m_pdrv = {1396 .probe = vim2m_probe,1397 .remove_new = vim2m_remove,1398 .driver = {1399 .name = MEM2MEM_NAME,1400 },1401};1402 1403static void __exit vim2m_exit(void)1404{1405 platform_driver_unregister(&vim2m_pdrv);1406 platform_device_unregister(&vim2m_pdev);1407}1408 1409static int __init vim2m_init(void)1410{1411 int ret;1412 1413 ret = platform_device_register(&vim2m_pdev);1414 if (ret)1415 return ret;1416 1417 ret = platform_driver_register(&vim2m_pdrv);1418 if (ret)1419 platform_device_unregister(&vim2m_pdev);1420 1421 return ret;1422}1423 1424module_init(vim2m_init);1425module_exit(vim2m_exit);1426