drivers/media/platform/xilinx/xilinx-multi-scaler.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Xilinx Memory-to-Memory Video Multi-Scaler IP
   4  *
   5  * Copyright (C) 2018 Xilinx, Inc.
   6  *
   7  * Author: Suresh Gupta <suresh.gupta@xilinx.com>
   8  *
   9  * Based on the virtual v4l2-mem2mem example device
  10  *
  11  * This driver adds support to control the Xilinx Video Multi
  12  * Scaler Controller
  13  */
  14
  15 #include <linux/delay.h>
  16 #include <linux/fs.h>
  17 #include <linux/gpio/consumer.h>
  18 #include <linux/interrupt.h>
  19 #include <linux/io.h>
  20 #include <linux/module.h>
  21 #include <linux/of.h>
  22 #include <linux/of_irq.h>
  23 #include <linux/platform_device.h>
  24 #include <linux/sched.h>
  25 #include <linux/slab.h>
  26
  27 #include <media/v4l2-device.h>
  28 #include <media/v4l2-ioctl.h>
  29 #include <media/v4l2-mem2mem.h>
  30 #include <media/videobuf2-dma-contig.h>
  31
  32 #include "xilinx-multi-scaler-coeff.h"
  33
  34 /* 0x0000 : Control signals */
  35 #define XM2MSC_AP_CTRL                  0x0000
  36 #define XM2MSC_AP_CTRL_START            BIT(0)
  37 #define XM2MSC_AP_CTRL_DONE             BIT(1)
  38 #define XM2MSC_AP_CTRL_IDEL             BIT(2)
  39 #define XM2MSC_AP_CTRL_READY            BIT(3)
  40 #define XM2MSC_AP_CTRL_AUTO_RESTART     BIT(7)
  41
  42 /* 0x0004 : Global Interrupt Enable Register */
  43 #define XM2MSC_GIE                      0x0004
  44 #define XM2MSC_GIE_EN                   BIT(0)
  45
  46 /* 0x0008 : IP Interrupt Enable Register (Read/Write) */
  47 #define XM2MSC_IER                      0x0008
  48 #define XM2MSC_ISR                      0x000c
  49 #define XM2MSC_ISR_DONE                 BIT(0)
  50 #define XM2MSC_ISR_READY                BIT(1)
  51
  52 #define XM2MSC_NUM_OUTS                 0x0010
  53
  54 #define XM2MSC_WIDTHIN                  0x000
  55 #define XM2MSC_WIDTHOUT                 0x008
  56 #define XM2MSC_HEIGHTIN                 0x010
  57 #define XM2MSC_HEIGHTOUT                0x018
  58 #define XM2MSC_LINERATE                 0x020
  59 #define XM2MSC_PIXELRATE                0x028
  60 #define XM2MSC_INPIXELFMT               0x030
  61 #define XM2MSC_OUTPIXELFMT              0x038
  62 #define XM2MSC_INSTRIDE                 0x050
  63 #define XM2MSC_OUTSTRIDE                0x058
  64 #define XM2MSC_SRCIMGBUF0               0x060
  65 #define XM2MSC_SRCIMGBUF1               0x070
  66 #define XM2MSC_DSTIMGBUF0               0x090
  67 #define XM2MSC_DSTIMGBUF1               0x0100
  68
  69 #define XM2MVSC_VFLTCOEFF_L     0x2000
  70 #define XM2MVSC_VFLTCOEFF(x)    (XM2MVSC_VFLTCOEFF_L + 0x2000 * (x))
  71 #define XM2MVSC_HFLTCOEFF_L     0x2800
  72 #define XM2MVSC_HFLTCOEFF(x)    (XM2MVSC_HFLTCOEFF_L + 0x2000 * (x))
  73
  74 #define XM2MSC_CHAN_REGS_START(x)       (0x100 + 0x200 * x)
  75
  76 /* GPIO RESET MACROS */
  77 #define XM2MSC_RESET_ASSERT     (0x1)
  78 #define XM2MSC_RESET_DEASSERT   (0x0)
  79
  80 #define XM2MSC_MIN_CHAN         1
  81 #define XM2MSC_MAX_CHAN         8
  82
  83 #define XM2MSC_MAX_WIDTH        (3840)
  84 #define XM2MSC_MAX_HEIGHT       (2160)
  85 #define XM2MSC_MIN_WIDTH        (64)
  86 #define XM2MSC_MIN_HEIGHT       (64)
  87 #define XM2MSC_STEP_PRECISION   (65536)
  88 /* Mask definitions for Low 16 bits in a 32 bit number */
  89 #define XM2MSC_MASK_LOW_16BITS  GENMASK(15, 0)
  90 #define XM2MSC_BITSHIFT_16      (16)
  91
  92 #define XM2MSC_DRIVER_NAME      "xm2msc"
  93
  94 #define CHAN_ATTACHED           BIT(0)
  95 #define CHAN_OPENED             BIT(1)
  96
  97 #define XM2MSC_CHAN_OUT         0
  98 #define XM2MSC_CHAN_CAP         1
  99
 100 /* Xilinx Video Specific Color/Pixel Formats */
 101 enum xm2msc_pix_fmt {
 102         XILINX_M2MSC_FMT_RGBX8          = 10,
 103         XILINX_M2MSC_FMT_YUVX8          = 11,
 104         XILINX_M2MSC_FMT_YUYV8          = 12,
 105         XILINX_M2MSC_FMT_RGBX10         = 15,
 106         XILINX_M2MSC_FMT_YUVX10         = 16,
 107         XILINX_M2MSC_FMT_Y_UV8          = 18,
 108         XILINX_M2MSC_FMT_Y_UV8_420      = 19,
 109         XILINX_M2MSC_FMT_RGB8           = 20,
 110         XILINX_M2MSC_FMT_YUV8           = 21,
 111         XILINX_M2MSC_FMT_Y_UV10         = 22,
 112         XILINX_M2MSC_FMT_Y_UV10_420     = 23,
 113         XILINX_M2MSC_FMT_Y8             = 24,
 114         XILINX_M2MSC_FMT_Y10            = 25,
 115         XILINX_M2MSC_FMT_BGRX8          = 27,
 116         XILINX_M2MSC_FMT_UYVY8          = 28,
 117         XILINX_M2MSC_FMT_BGR8           = 29,
 118 };
 119
 120 /**
 121  * struct xm2msc_fmt - driver info for each of the supported video formats
 122  * @name: human-readable device tree name for this entry
 123  * @fourcc: standard format identifier
 124  * @xm2msc_fmt: Xilinx Video Specific Color/Pixel Formats
 125  * @num_planes: number of planes supported by format
 126  */
 127 struct xm2msc_fmt {
 128         char *name;
 129         u32 fourcc;
 130         enum xm2msc_pix_fmt xm2msc_fmt;
 131         u32 num_planes;
 132 };
 133
 134 static const struct xm2msc_fmt formats[] = {
 135         {
 136                 .name = "xbgr8888",
 137                 .fourcc = V4L2_PIX_FMT_BGRX32,
 138                 .xm2msc_fmt = XILINX_M2MSC_FMT_RGBX8,
 139                 .num_planes = 1,
 140         },
 141         {
 142                 .name = "xvuy8888",
 143                 .fourcc = V4L2_PIX_FMT_XVUY32,
 144                 .xm2msc_fmt = XILINX_M2MSC_FMT_YUVX8,
 145                 .num_planes = 1,
 146         },
 147         {
 148                 .name = "yuyv",
 149                 .fourcc = V4L2_PIX_FMT_YUYV,
 150                 .xm2msc_fmt = XILINX_M2MSC_FMT_YUYV8,
 151                 .num_planes = 1,
 152         },
 153         {
 154                 .name = "xbgr2101010",
 155                 .fourcc = V4L2_PIX_FMT_XBGR30,
 156                 .xm2msc_fmt = XILINX_M2MSC_FMT_RGBX10,
 157                 .num_planes = 1,
 158         },
 159         {
 160                 .name = "yuvx2101010",
 161                 .fourcc = V4L2_PIX_FMT_XVUY10,
 162                 .xm2msc_fmt = XILINX_M2MSC_FMT_YUVX10,
 163                 .num_planes = 1,
 164         },
 165         {
 166                 .name = "nv16",
 167                 .fourcc = V4L2_PIX_FMT_NV16,
 168                 .xm2msc_fmt = XILINX_M2MSC_FMT_Y_UV8,
 169                 .num_planes = 2,
 170         },
 171         {
 172                 .name = "nv12",
 173                 .fourcc = V4L2_PIX_FMT_NV12,
 174                 .xm2msc_fmt = XILINX_M2MSC_FMT_Y_UV8_420,
 175                 .num_planes = 2,
 176         },
 177         {
 178                 .name = "bgr888",
 179                 .fourcc = V4L2_PIX_FMT_RGB24,
 180                 .xm2msc_fmt = XILINX_M2MSC_FMT_RGB8,
 181                 .num_planes = 1,
 182         },
 183         {
 184                 .name = "vuy888",
 185                 .fourcc = V4L2_PIX_FMT_VUY24,
 186                 .xm2msc_fmt = XILINX_M2MSC_FMT_YUV8,
 187                 .num_planes = 1,
 188         },
 189         {
 190                 .name = "xv20",
 191                 .fourcc = V4L2_PIX_FMT_XV20,
 192                 .xm2msc_fmt = XILINX_M2MSC_FMT_Y_UV10,
 193                 .num_planes = 2,
 194         },
 195         {
 196                 .name = "xv15",
 197                 .fourcc = V4L2_PIX_FMT_XV15,
 198                 .xm2msc_fmt = XILINX_M2MSC_FMT_Y_UV10_420,
 199                 .num_planes = 2,
 200         },
 201         {
 202                 .name = "y8",
 203                 .fourcc = V4L2_PIX_FMT_GREY,
 204                 .xm2msc_fmt = XILINX_M2MSC_FMT_Y8,
 205                 .num_planes = 1,
 206         },
 207         {
 208                 .name = "y10",
 209                 .fourcc = V4L2_PIX_FMT_Y10,
 210                 .xm2msc_fmt = XILINX_M2MSC_FMT_Y10,
 211                 .num_planes = 1,
 212         },
 213         {
 214                 .name = "xrgb8888",
 215                 .fourcc = V4L2_PIX_FMT_XBGR32,
 216                 .xm2msc_fmt = XILINX_M2MSC_FMT_BGRX8,
 217                 .num_planes = 1,
 218         },
 219         {
 220                 .name = "uyvy",
 221                 .fourcc = V4L2_PIX_FMT_UYVY,
 222                 .xm2msc_fmt = XILINX_M2MSC_FMT_UYVY8,
 223                 .num_planes = 1,
 224         },
 225         {
 226                 .name = "rgb888",
 227                 .fourcc = V4L2_PIX_FMT_BGR24,
 228                 .xm2msc_fmt = XILINX_M2MSC_FMT_BGR8,
 229                 .num_planes = 1,
 230         },
 231 };
 232
 233 /**
 234  * struct xm2msc_q_data - Per-queue, driver-specific private data
 235  * There is one source queue and one destination queue for each m2m context.
 236  * @width: frame width
 237  * @height: frame height
 238  * @stride: bytes per lines
 239  * @nplanes: Current number of planes
 240  * @bytesperline: bytes per line per plane
 241  * @sizeimage: image size per plane
 242  * @colorspace: supported colorspace
 243  * @field: supported field value
 244  * @fmt: format info
 245  */
 246 struct xm2msc_q_data {
 247         unsigned int width;
 248         unsigned int height;
 249         unsigned int stride;
 250         unsigned int nplanes;
 251         unsigned int bytesperline[2];
 252         unsigned int sizeimage[2];
 253         enum v4l2_colorspace colorspace;
 254         enum v4l2_field field;
 255         const struct xm2msc_fmt *fmt;
 256 };
 257
 258 /**
 259  * struct xm2msc_chan_ctx - Scaler Channel Info, Per-Channel context
 260  * @regs: IO mapped base address of the Channel
 261  * @xm2msc_dev: Pointer to struct xm2m_msc_dev
 262  * @num: HW Scaling Channel number
 263  * @minor: Minor number of the video device
 264  * @status: channel status, CHAN_ATTACHED or CHAN_OPENED
 265  * @taps: number of hwtaps required for channel
 266  * @vfd: V4L2 device
 267  * @fh: v4l2 file handle
 268  * @m2m_dev: m2m device
 269  * @m2m_ctx: memory to memory context structure
 270  * @q_data: src & dst queue data
 271  */
 272 struct xm2msc_chan_ctx {
 273         void __iomem *regs;
 274         struct xm2m_msc_dev *xm2msc_dev;
 275         u32 num;
 276         u32 minor;
 277         u8 status;
 278         u32 taps;
 279
 280         struct video_device vfd;
 281         struct v4l2_fh fh;
 282         struct v4l2_m2m_dev *m2m_dev;
 283         struct v4l2_m2m_ctx *m2m_ctx;
 284
 285         struct xm2msc_q_data q_data[2];
 286 };
 287
 288 /**
 289  * struct xm2m_msc_dev - Xilinx M2M Multi-scaler Device
 290  * @dev: pointer to struct device instance used by the driver
 291  * @regs: IO mapped base address of the HW/IP
 292  * @irq: interrupt number
 293  * @max_chan: maximum number of Scaling Channels
 294  * @max_ht: maximum number of rows in a plane
 295  * @max_wd: maximum number of column in a plane
 296  * @supported_fmt: bitmap for all supported fmts by HW
 297  * @dma_addr_size: Size of dma address pointer in IP (either 32 or 64)
 298  * @rst_gpio: reset gpio handler
 299  * @opened_chan: bitmap for all open channel
 300  * @out_streamed_chan: bitmap for all out streamed channel
 301  * @cap_streamed_chan: bitmap for all capture streamed channel
 302  * @v4l2_dev: main struct to for V4L2 device drivers
 303  * @dev_mutex: lock for V4L2 device
 304  * @mutex: lock for channel ctx
 305  * @lock: lock used in IRQ
 306  * @xm2msc_chan: arrey of channel context
 307  * @hscaler_coeff: Array of filter coefficients for the Horizontal Scaler
 308  * @vscaler_coeff: Array of filter coefficients for the Vertical Scaler
 309  */
 310 struct xm2m_msc_dev {
 311         struct device *dev;
 312         void __iomem *regs;
 313         int irq;
 314         u32 max_chan;
 315         u32 max_ht;
 316         u32 max_wd;
 317         u32 supported_fmt;
 318         u32 dma_addr_size;
 319         struct gpio_desc *rst_gpio;
 320
 321         u32 opened_chan;
 322         u32 out_streamed_chan;
 323         u32 cap_streamed_chan;
 324
 325         struct v4l2_device v4l2_dev;
 326
 327         struct mutex dev_mutex; /*the mutex for v4l2*/
 328         struct mutex mutex; /*lock for bitmap reg*/
 329         spinlock_t lock; /*IRQ lock*/
 330
 331         struct xm2msc_chan_ctx xm2msc_chan[XM2MSC_MAX_CHAN];
 332         short hscaler_coeff[XSCALER_MAX_PHASES][XSCALER_MAX_TAPS];
 333         short vscaler_coeff[XSCALER_MAX_PHASES][XSCALER_MAX_TAPS];
 334 };
 335
 336 #define fh_to_chanctx(__fh) container_of(__fh, struct xm2msc_chan_ctx, fh)
 337
 338 static inline u32 xm2msc_readreg(const volatile void __iomem *addr)
 339 {
 340         return ioread32(addr);
 341 }
 342
 343 static inline void xm2msc_write64reg(volatile void __iomem *addr, u64 value)
 344 {
 345         iowrite32(lower_32_bits(value), addr);
 346         iowrite32(upper_32_bits(value), (void __iomem *)(addr + 4));
 347 }
 348
 349 static inline void xm2msc_writereg(volatile void __iomem *addr, u32 value)
 350 {
 351         iowrite32(value, addr);
 352 }
 353
 354 static void
 355 xv_hscaler_load_ext_coeff(struct xm2m_msc_dev *xm2msc,
 356                           const short *coeff, u32 ntaps)
 357 {
 358         unsigned int i, j, pad, offset;
 359         const u32 nphases = XSCALER_MAX_PHASES;
 360
 361         /* Determine if coefficient needs padding (effective vs. max taps) */
 362         pad = XSCALER_MAX_TAPS - ntaps;
 363         offset = pad >> 1;
 364
 365         memset(xm2msc->hscaler_coeff, 0, sizeof(xm2msc->hscaler_coeff));
 366
 367         /* Load coefficients into scaler coefficient table */
 368         for (i = 0; i < nphases; i++) {
 369                 for (j = 0; j < ntaps; ++j)
 370                         xm2msc->hscaler_coeff[i][j + offset] =
 371                                                 coeff[i * ntaps + j];
 372         }
 373 }
 374
 375 static void xv_hscaler_set_coeff(struct xm2msc_chan_ctx *chan_ctx,
 376                                  const u32 base_addr)
 377 {
 378         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 379         int val, offset, rd_indx;
 380         unsigned int i, j;
 381         u32 ntaps = chan_ctx->taps;
 382         const u32 nphases = XSCALER_MAX_PHASES;
 383
 384         offset = (XSCALER_MAX_TAPS - ntaps) / 2;
 385         for (i = 0; i < nphases; i++) {
 386                 for (j = 0; j < ntaps / 2; j++) {
 387                         rd_indx = j * 2 + offset;
 388                         val = (xm2msc->hscaler_coeff[i][rd_indx + 1] <<
 389                                XM2MSC_BITSHIFT_16) |
 390                                (xm2msc->hscaler_coeff[i][rd_indx] &
 391                                XM2MSC_MASK_LOW_16BITS);
 392                          xm2msc_writereg((xm2msc->regs + base_addr) +
 393                                     ((i * ntaps / 2 + j) * 4), val);
 394                 }
 395         }
 396 }
 397
 398 static void
 399 xv_vscaler_load_ext_coeff(struct xm2m_msc_dev *xm2msc,
 400                           const short *coeff, const u32 ntaps)
 401 {
 402         unsigned int i, j;
 403         int pad, offset;
 404         const u32 nphases = XSCALER_MAX_PHASES;
 405
 406         /* Determine if coefficient needs padding (effective vs. max taps) */
 407         pad = XSCALER_MAX_TAPS - ntaps;
 408         offset = pad ? (pad >> 1) : 0;
 409
 410         /* Zero Entire Array */
 411         memset(xm2msc->vscaler_coeff, 0, sizeof(xm2msc->vscaler_coeff));
 412
 413         /* Load User defined coefficients into scaler coefficient table */
 414         for (i = 0; i < nphases; i++) {
 415                 for (j = 0; j < ntaps; ++j)
 416                         xm2msc->vscaler_coeff[i][j + offset] =
 417                                                 coeff[i * ntaps + j];
 418         }
 419 }
 420
 421 static void xv_vscaler_set_coeff(struct xm2msc_chan_ctx *chan_ctx,
 422                                  const u32 base_addr)
 423 {
 424         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 425         u32 val, i, j, offset, rd_indx;
 426         u32 ntaps = chan_ctx->taps;
 427         const u32 nphases = XSCALER_MAX_PHASES;
 428
 429         offset = (XSCALER_MAX_TAPS - ntaps) / 2;
 430
 431         for (i = 0; i < nphases; i++) {
 432                 for (j = 0; j < ntaps / 2; j++) {
 433                         rd_indx = j * 2 + offset;
 434                         val = (xm2msc->vscaler_coeff[i][rd_indx + 1] <<
 435                                XM2MSC_BITSHIFT_16) |
 436                                (xm2msc->vscaler_coeff[i][rd_indx] &
 437                                XM2MSC_MASK_LOW_16BITS);
 438                         xm2msc_writereg((xm2msc->regs +
 439                                    base_addr) + ((i * ntaps / 2 + j) * 4), val);
 440                 }
 441         }
 442 }
 443
 444 static void xm2mvsc_initialize_coeff_banks(struct xm2msc_chan_ctx *chan_ctx)
 445 {
 446         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 447         /* Bank 0 is init as 6 tap filter for 6, 8, 10 & 12 tap filters */
 448         xv_hscaler_load_ext_coeff(xm2msc, &xhsc_coeff_taps6[0][0],
 449                                   XSCALER_TAPS_6);
 450         xv_hscaler_set_coeff(chan_ctx, XM2MVSC_HFLTCOEFF(chan_ctx->num));
 451         xv_vscaler_load_ext_coeff(xm2msc, &xvsc_coeff_taps6[0][0],
 452                                   XSCALER_TAPS_6);
 453         xv_vscaler_set_coeff(chan_ctx, XM2MVSC_VFLTCOEFF(chan_ctx->num));
 454 }
 455
 456 static void
 457 xm2msc_pr_q(struct device *dev, struct xm2msc_q_data *q, int chan,
 458             int type, const char *fun_name)
 459 {
 460         unsigned int i;
 461         const struct xm2msc_fmt *fmt = q->fmt;
 462
 463         if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 464                 dev_dbg(dev, "\n\nOUTPUT Q (%d) Context from [[ %s ]]",
 465                         chan, fun_name);
 466         else
 467                 dev_dbg(dev, "\n\nCAPTURE Q (%d) Context from [[ %s ]]",
 468                         chan, fun_name);
 469
 470         dev_dbg(dev, "width height stride clrspace field planes\n");
 471         dev_dbg(dev, "  %d  %d    %d     %d       %d    %d\n",
 472                 q->width, q->height, q->stride,
 473                 q->colorspace, q->field, q->nplanes);
 474
 475         for (i = 0; i < q->nplanes; i++) {
 476                 dev_dbg(dev, "[plane %d ] bytesperline sizeimage\n", i);
 477                 dev_dbg(dev, "                %d        %d\n",
 478                         q->bytesperline[i], q->sizeimage[i]);
 479         }
 480
 481         dev_dbg(dev, "fmt_name 4cc xlnx-fmt\n");
 482         dev_dbg(dev, "%s %d %d\n",
 483                 fmt->name, fmt->fourcc, fmt->xm2msc_fmt);
 484         dev_dbg(dev, "\n\n");
 485 }
 486
 487 static void
 488 xm2msc_pr_status(struct xm2m_msc_dev *xm2msc,
 489                  const char *fun_name)
 490 {
 491         struct device *dev = xm2msc->dev;
 492
 493         dev_dbg(dev, "Status in %s\n", fun_name);
 494         dev_dbg(dev, "opened_chan out_streamed_chan cap_streamed_chan\n");
 495         dev_dbg(dev, "0x%x           0x%x               0x%x\n",
 496                 xm2msc->opened_chan, xm2msc->out_streamed_chan,
 497                 xm2msc->cap_streamed_chan);
 498         dev_dbg(dev, "\n\n");
 499 }
 500
 501 static void
 502 xm2msc_pr_chanctx(struct xm2msc_chan_ctx *ctx, const char *fun_name)
 503 {
 504         struct device *dev = ctx->xm2msc_dev->dev;
 505
 506         dev_dbg(dev, "\n\n----- [[ %s ]]: Channel %d (0x%p) context -----\n",
 507                 fun_name, ctx->num, ctx);
 508         dev_dbg(dev, "minor = %d, taps = %d\n", ctx->minor, ctx->taps);
 509         dev_dbg(dev, "reg mapped at %p\n", ctx->regs);
 510         dev_dbg(dev, "xm2msc \tm2m_dev \tm2m_ctx\n");
 511         dev_dbg(dev, "%p \t%p \t%p\n", ctx->xm2msc_dev,
 512                 ctx->m2m_dev, ctx->m2m_ctx);
 513
 514         if (ctx->status & CHAN_OPENED)
 515                 dev_dbg(dev, "Opened ");
 516         if (ctx->status & CHAN_ATTACHED)
 517                 dev_dbg(dev, "and attached");
 518         dev_dbg(dev, "\n");
 519         dev_dbg(dev, "-----------------------------------\n");
 520         dev_dbg(dev, "\n\n");
 521 }
 522
 523 static void
 524 xm2msc_pr_screg(struct device *dev, const volatile void __iomem *base)
 525 {
 526         dev_dbg(dev, "Ctr, GIE,  IE,  IS   OUT\n");
 527         dev_dbg(dev, "0x%x  0x%x   0x%x  0x%x  0x%x\n",
 528                 xm2msc_readreg(base + XM2MSC_AP_CTRL),
 529                 xm2msc_readreg(base + XM2MSC_GIE),
 530                 xm2msc_readreg(base + XM2MSC_IER),
 531                 xm2msc_readreg(base + XM2MSC_ISR),
 532                 xm2msc_readreg(base + XM2MSC_NUM_OUTS));
 533 }
 534
 535 static void
 536 xm2msc_pr_chanreg(struct device *dev, const volatile void __iomem *base)
 537 {
 538         dev_dbg(dev, "WIN HIN INPIXELFMT INSTRIDE SRCB0L/H SRCB1L/H\n");
 539         dev_dbg(dev, "%d   %d     %d       %d      0x%x/0x%x      0x%x/0x%x\n",
 540                 xm2msc_readreg(base + XM2MSC_WIDTHIN),
 541                 xm2msc_readreg(base + XM2MSC_HEIGHTIN),
 542                 xm2msc_readreg(base + XM2MSC_INPIXELFMT),
 543                 xm2msc_readreg(base + XM2MSC_INSTRIDE),
 544                 xm2msc_readreg(base + XM2MSC_SRCIMGBUF0),
 545                 xm2msc_readreg(base + XM2MSC_SRCIMGBUF0 + 4),
 546                 xm2msc_readreg(base + XM2MSC_SRCIMGBUF1),
 547                 xm2msc_readreg(base + XM2MSC_SRCIMGBUF1 + 4));
 548         dev_dbg(dev, "WOUT HOUT OUTPIXELFMT OUTSTRIDE DBUF0L/H DBUF1L/H\n");
 549         dev_dbg(dev, "%d   %d     %d       %d      0x%x/0x%x      0x%x/0x%x\n",
 550                 xm2msc_readreg(base + XM2MSC_WIDTHOUT),
 551                 xm2msc_readreg(base + XM2MSC_HEIGHTOUT),
 552                 xm2msc_readreg(base + XM2MSC_OUTPIXELFMT),
 553                 xm2msc_readreg(base + XM2MSC_OUTSTRIDE),
 554                 xm2msc_readreg(base + XM2MSC_DSTIMGBUF0),
 555                 xm2msc_readreg(base + XM2MSC_DSTIMGBUF0 + 4),
 556                 xm2msc_readreg(base + XM2MSC_DSTIMGBUF1),
 557                 xm2msc_readreg(base + XM2MSC_DSTIMGBUF1 + 4));
 558
 559         dev_dbg(dev, "LINERATE PIXELRATE\n");
 560         dev_dbg(dev, "0x%x     0x%x\n",
 561                 xm2msc_readreg(base + XM2MSC_LINERATE),
 562                 xm2msc_readreg(base + XM2MSC_PIXELRATE));
 563 }
 564
 565 static void
 566 xm2msc_pr_allchanreg(struct xm2m_msc_dev *xm2msc)
 567 {
 568         unsigned int i;
 569         struct xm2msc_chan_ctx *chan_ctx;
 570         struct device *dev = xm2msc->dev;
 571
 572         xm2msc_pr_screg(xm2msc->dev, xm2msc->regs);
 573
 574         for (i = 0; i < xm2msc->max_chan; i++) {
 575                 chan_ctx = &xm2msc->xm2msc_chan[i];
 576                 dev_dbg(dev, "Regs val for channel %d\n", i);
 577                 dev_dbg(dev, "______________________________________________\n");
 578                 xm2msc_pr_chanreg(dev, chan_ctx->regs);
 579                 dev_dbg(dev, "______________________________________________\n");
 580         }
 581 }
 582
 583 static inline bool xm2msc_testbit(int num, u32 *addr)
 584 {
 585         return (*addr & BIT(num));
 586 }
 587
 588 static inline void xm2msc_setbit(int num, u32 *addr)
 589 {
 590         *addr |= BIT(num);
 591 }
 592
 593 static inline void xm2msc_clrbit(int num, u32 *addr)
 594 {
 595         *addr &= ~BIT(num);
 596 }
 597
 598 static void xm2msc_stop(struct xm2m_msc_dev *xm2msc)
 599 {
 600         void __iomem *base = xm2msc->regs;
 601         u32 data = xm2msc_readreg(base + XM2MSC_AP_CTRL);
 602
 603         data &= ~XM2MSC_AP_CTRL_START;
 604         xm2msc_writereg(base + XM2MSC_AP_CTRL, data);
 605 }
 606
 607 static void xm2msc_start(struct xm2m_msc_dev *xm2msc)
 608 {
 609         void __iomem *base = xm2msc->regs;
 610         u32 data = xm2msc_readreg(base + XM2MSC_AP_CTRL);
 611
 612         data |= XM2MSC_AP_CTRL_START;
 613         xm2msc_writereg(base + XM2MSC_AP_CTRL, data);
 614 }
 615
 616 static void xm2msc_set_chan(struct xm2msc_chan_ctx *ctx, bool state)
 617 {
 618         mutex_lock(&ctx->xm2msc_dev->mutex);
 619         if (state)
 620                 xm2msc_setbit(ctx->num, &ctx->xm2msc_dev->opened_chan);
 621         else
 622                 xm2msc_clrbit(ctx->num, &ctx->xm2msc_dev->opened_chan);
 623         mutex_unlock(&ctx->xm2msc_dev->mutex);
 624 }
 625
 626 static void
 627 xm2msc_set_chan_stream(struct xm2msc_chan_ctx *ctx, bool state, int type)
 628 {
 629         u32 *ptr;
 630
 631         if (type == XM2MSC_CHAN_OUT)
 632                 ptr = &ctx->xm2msc_dev->cap_streamed_chan;
 633         else
 634                 ptr = &ctx->xm2msc_dev->out_streamed_chan;
 635
 636         mutex_lock(&ctx->xm2msc_dev->mutex);
 637         if (state)
 638                 xm2msc_setbit(ctx->num, ptr);
 639         else
 640                 xm2msc_clrbit(ctx->num, ptr);
 641
 642         mutex_unlock(&ctx->xm2msc_dev->mutex);
 643 }
 644
 645 static int
 646 xm2msc_chk_chan_stream(struct xm2msc_chan_ctx *ctx, int type)
 647 {
 648         u32 *ptr;
 649         int ret;
 650
 651         if (type == XM2MSC_CHAN_OUT)
 652                 ptr = &ctx->xm2msc_dev->cap_streamed_chan;
 653         else
 654                 ptr = &ctx->xm2msc_dev->out_streamed_chan;
 655
 656         mutex_lock(&ctx->xm2msc_dev->mutex);
 657         ret = xm2msc_testbit(ctx->num, ptr);
 658         mutex_unlock(&ctx->xm2msc_dev->mutex);
 659
 660         return ret;
 661 }
 662
 663 static void xm2msc_set_fmt(struct xm2m_msc_dev *xm2msc, u32 index)
 664 {
 665         xm2msc_setbit(index, &xm2msc->supported_fmt);
 666 }
 667
 668 static int xm2msc_chk_fmt(struct xm2m_msc_dev *xm2msc, u32 index)
 669 {
 670         return xm2msc_testbit(index, &xm2msc->supported_fmt);
 671 }
 672
 673 static void xm2msc_reset(struct xm2m_msc_dev *xm2msc)
 674 {
 675         gpiod_set_value_cansleep(xm2msc->rst_gpio, XM2MSC_RESET_ASSERT);
 676         gpiod_set_value_cansleep(xm2msc->rst_gpio, XM2MSC_RESET_DEASSERT);
 677 }
 678
 679 /*
 680  * mem2mem callbacks
 681  */
 682 static int xm2msc_job_ready(void *priv)
 683 {
 684         struct xm2msc_chan_ctx *chan_ctx = priv;
 685
 686         if ((v4l2_m2m_num_src_bufs_ready(chan_ctx->m2m_ctx) > 0) &&
 687             (v4l2_m2m_num_dst_bufs_ready(chan_ctx->m2m_ctx) > 0))
 688                 return 1;
 689         return 0;
 690 }
 691
 692 static void xm2msc_chan_abort_bufs(struct xm2msc_chan_ctx *chan_ctx)
 693 {
 694         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 695         struct vb2_v4l2_buffer *dst_vb, *src_vb;
 696
 697         spin_lock(&xm2msc->lock);
 698         dev_dbg(xm2msc->dev, "aborting all buffers\n");
 699
 700         while (v4l2_m2m_num_src_bufs_ready(chan_ctx->m2m_ctx) > 0) {
 701                 src_vb = v4l2_m2m_src_buf_remove(chan_ctx->m2m_ctx);
 702                 v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_ERROR);
 703         }
 704
 705         while (v4l2_m2m_num_dst_bufs_ready(chan_ctx->m2m_ctx) > 0) {
 706                 dst_vb = v4l2_m2m_dst_buf_remove(chan_ctx->m2m_ctx);
 707                 v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_ERROR);
 708         }
 709
 710         v4l2_m2m_job_finish(chan_ctx->m2m_dev, chan_ctx->m2m_ctx);
 711         spin_unlock(&xm2msc->lock);
 712 }
 713
 714 static void xm2msc_job_abort(void *priv)
 715 {
 716         struct xm2msc_chan_ctx *chan_ctx = priv;
 717
 718         xm2msc_chan_abort_bufs(chan_ctx);
 719
 720         /*
 721          * Stream off the channel as job_abort may not always
 722          * be called after streamoff
 723          */
 724         xm2msc_set_chan_stream(chan_ctx, false, XM2MSC_CHAN_OUT);
 725         xm2msc_set_chan_stream(chan_ctx, false, XM2MSC_CHAN_CAP);
 726 }
 727
 728 static int xm2msc_set_bufaddr(struct xm2m_msc_dev *xm2msc)
 729 {
 730         unsigned int chan;
 731         struct xm2msc_chan_ctx *chan_ctx;
 732         struct vb2_v4l2_buffer *src_vb, *dst_vb;
 733         void __iomem *base;
 734         dma_addr_t src_luma, dst_luma;
 735         dma_addr_t src_croma, dst_croma;
 736
 737         for (chan = 0; chan < xm2msc->max_chan; chan++) {
 738                 chan_ctx = &xm2msc->xm2msc_chan[chan];
 739                 base = chan_ctx->regs;
 740
 741                 src_vb = v4l2_m2m_next_src_buf(chan_ctx->m2m_ctx);
 742                 dst_vb = v4l2_m2m_next_dst_buf(chan_ctx->m2m_ctx);
 743
 744                 if (!src_vb || !dst_vb) {
 745                         v4l2_err(&xm2msc->v4l2_dev, "buffer not found ");
 746                         v4l2_err(&xm2msc->v4l2_dev, "src_vb = 0x%p, dst_vb = 0x%p\n",
 747                                  src_vb, dst_vb);
 748                         return -EINVAL;
 749                 }
 750
 751                 src_luma = vb2_dma_contig_plane_dma_addr(&src_vb->vb2_buf, 0);
 752                 dst_luma = vb2_dma_contig_plane_dma_addr(&dst_vb->vb2_buf, 0);
 753
 754                 if (chan_ctx->q_data[XM2MSC_CHAN_OUT].nplanes == 2)
 755                         src_croma =
 756                             vb2_dma_contig_plane_dma_addr(&src_vb->vb2_buf, 1);
 757                 else
 758                         src_croma = 0;
 759
 760                 if (chan_ctx->q_data[XM2MSC_CHAN_CAP].nplanes == 2)
 761                         dst_croma =
 762                             vb2_dma_contig_plane_dma_addr(&dst_vb->vb2_buf, 1);
 763                 else
 764                         dst_croma = 0;
 765
 766                 if (xm2msc->dma_addr_size == 64 &&
 767                     sizeof(dma_addr_t) == sizeof(u64)) {
 768                         xm2msc_write64reg(base + XM2MSC_SRCIMGBUF0, src_luma);
 769                         xm2msc_write64reg(base + XM2MSC_SRCIMGBUF1, src_croma);
 770                         xm2msc_write64reg(base + XM2MSC_DSTIMGBUF0, dst_luma);
 771                         xm2msc_write64reg(base + XM2MSC_DSTIMGBUF1, dst_croma);
 772                 } else {
 773                         xm2msc_writereg(base + XM2MSC_SRCIMGBUF0, src_luma);
 774                         xm2msc_writereg(base + XM2MSC_SRCIMGBUF1, src_croma);
 775                         xm2msc_writereg(base + XM2MSC_DSTIMGBUF0, dst_luma);
 776                         xm2msc_writereg(base + XM2MSC_DSTIMGBUF1, dst_croma);
 777                 }
 778         }
 779         return 0;
 780 }
 781
 782 static void xm2msc_device_run(void *priv)
 783 {
 784         struct xm2msc_chan_ctx *chan_ctx = priv;
 785         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 786         void __iomem *base = xm2msc->regs;
 787         int ret;
 788
 789         /* TODO program to number of opened chan*/
 790         xm2msc_writereg(base + XM2MSC_NUM_OUTS, xm2msc->max_chan);
 791
 792         ret = xm2msc_set_bufaddr(xm2msc);
 793         if (ret) {
 794                 v4l2_err(&xm2msc->v4l2_dev, "Device can't be run\n");
 795                 return;
 796         }
 797
 798         xm2msc_writereg(base + XM2MSC_GIE, XM2MSC_GIE_EN);
 799         xm2msc_writereg(base + XM2MSC_IER, XM2MSC_ISR_DONE);
 800
 801         xm2msc_pr_status(xm2msc, __func__);
 802         xm2msc_pr_screg(xm2msc->dev, base);
 803         xm2msc_pr_allchanreg(xm2msc);
 804
 805         xm2msc_start(xm2msc);
 806 }
 807
 808 static void xm2msc_job_finish(struct xm2m_msc_dev *xm2msc)
 809 {
 810         unsigned int chan;
 811
 812         for (chan = 0; chan < xm2msc->max_chan; chan++) {
 813                 struct xm2msc_chan_ctx *chan_ctx;
 814
 815                 chan_ctx = &xm2msc->xm2msc_chan[chan];
 816                 v4l2_m2m_job_finish(chan_ctx->m2m_dev, chan_ctx->m2m_ctx);
 817         }
 818 }
 819
 820 static void xm2msc_job_done(struct xm2m_msc_dev *xm2msc)
 821 {
 822         unsigned int chan;
 823
 824         for (chan = 0; chan < xm2msc->max_chan; chan++) {
 825                 struct xm2msc_chan_ctx *chan_ctx;
 826                 struct vb2_v4l2_buffer *src_vb, *dst_vb;
 827                 unsigned long flags;
 828
 829                 chan_ctx = &xm2msc->xm2msc_chan[chan];
 830
 831                 src_vb = v4l2_m2m_src_buf_remove(chan_ctx->m2m_ctx);
 832                 dst_vb = v4l2_m2m_dst_buf_remove(chan_ctx->m2m_ctx);
 833
 834                 if (src_vb && dst_vb) {
 835                         dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 836                         dst_vb->timecode = src_vb->timecode;
 837                         dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 838                         dst_vb->flags |=
 839                             src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 840
 841                         spin_lock_irqsave(&xm2msc->lock, flags);
 842                         v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 843                         v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 844                         spin_unlock_irqrestore(&xm2msc->lock, flags);
 845                 }
 846         }
 847 }
 848
 849 static irqreturn_t xm2msc_isr(int irq, void *data)
 850 {
 851         struct xm2m_msc_dev *xm2msc = (struct xm2m_msc_dev *)data;
 852         void __iomem *base = xm2msc->regs;
 853         u32 status;
 854
 855         status = xm2msc_readreg(base + XM2MSC_ISR);
 856         if (!(status & XM2MSC_ISR_DONE))
 857                 return IRQ_NONE;
 858
 859         xm2msc_writereg(base + XM2MSC_ISR, status & XM2MSC_ISR_DONE);
 860
 861         xm2msc_stop(xm2msc);
 862
 863         xm2msc_job_done(xm2msc);
 864
 865         if (xm2msc_job_ready(xm2msc->xm2msc_chan)) {
 866                 xm2msc_device_run(xm2msc->xm2msc_chan);
 867                 goto handled;
 868         }
 869
 870         xm2msc_job_finish(xm2msc);
 871 handled:
 872         return IRQ_HANDLED;
 873 }
 874
 875 static struct xm2msc_q_data *get_q_data(struct xm2msc_chan_ctx *chan_ctx,
 876                                         enum v4l2_buf_type type)
 877 {
 878         switch (type) {
 879         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 880         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 881                 return &chan_ctx->q_data[XM2MSC_CHAN_OUT];
 882         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 883         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 884                 return &chan_ctx->q_data[XM2MSC_CHAN_CAP];
 885         default:
 886                 v4l2_err(&chan_ctx->xm2msc_dev->v4l2_dev,
 887                          "Not supported Q type %d\n", type);
 888         }
 889         return NULL;
 890 }
 891
 892 static u32 find_format_index(struct v4l2_format *f)
 893 {
 894         const struct xm2msc_fmt *fmt;
 895         unsigned int i;
 896
 897         for (i = 0; i < ARRAY_SIZE(formats); i++) {
 898                 fmt = &formats[i];
 899                 if (fmt->fourcc == f->fmt.pix_mp.pixelformat)
 900                         break;
 901         }
 902
 903         return i;
 904 }
 905
 906 static const struct xm2msc_fmt *find_format(struct v4l2_format *f)
 907 {
 908         const struct xm2msc_fmt *fmt;
 909         unsigned int i;
 910
 911         for (i = 0; i < ARRAY_SIZE(formats); i++) {
 912                 fmt = &formats[i];
 913                 if (fmt->fourcc == f->fmt.pix_mp.pixelformat)
 914                         break;
 915         }
 916
 917         if (i == ARRAY_SIZE(formats))
 918                 return NULL;
 919
 920         return &formats[i];
 921 }
 922
 923 static int xm2msc_streamon(struct file *file, void *fh,
 924                            enum v4l2_buf_type type)
 925 {
 926         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 927
 928         return v4l2_m2m_streamon(file, chan_ctx->m2m_ctx, type);
 929 }
 930
 931 static int xm2msc_streamoff(struct file *file, void *fh,
 932                             enum v4l2_buf_type type)
 933 {
 934         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 935
 936         return v4l2_m2m_streamoff(file, chan_ctx->m2m_ctx, type);
 937 }
 938
 939 static int xm2msc_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 940 {
 941         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 942
 943         return v4l2_m2m_qbuf(file, chan_ctx->m2m_ctx, buf);
 944 }
 945
 946 static int xm2msc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 947 {
 948         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 949
 950         return v4l2_m2m_dqbuf(file, chan_ctx->m2m_ctx, buf);
 951 }
 952
 953 static int xm2msc_expbuf(struct file *file, void *fh,
 954                          struct v4l2_exportbuffer *eb)
 955 {
 956         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 957
 958         return v4l2_m2m_expbuf(file, chan_ctx->m2m_ctx, eb);
 959 }
 960
 961 static int xm2msc_createbufs(struct file *file, void *fh,
 962                              struct v4l2_create_buffers *cb)
 963 {
 964         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 965
 966         return v4l2_m2m_create_bufs(file, chan_ctx->m2m_ctx, cb);
 967 }
 968
 969 static int xm2msc_reqbufs(struct file *file, void *fh,
 970                           struct v4l2_requestbuffers *reqbufs)
 971 {
 972         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 973
 974         return v4l2_m2m_reqbufs(file, chan_ctx->m2m_ctx, reqbufs);
 975 }
 976
 977 static int xm2msc_querybuf(struct file *file, void *fh,
 978                            struct v4l2_buffer *buf)
 979 {
 980         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 981
 982         return v4l2_m2m_querybuf(file, chan_ctx->m2m_ctx, buf);
 983 }
 984
 985 static unsigned int
 986 xm2msc_cal_stride(unsigned int width, enum xm2msc_pix_fmt xfmt)
 987 {
 988         unsigned int stride;
 989
 990         /* Stride in Bytes = (Width × Bytes per Pixel); */
 991         /* TODO: The Width value must be a multiple of Pixels per Clock */
 992         switch (xfmt) {
 993         case XILINX_M2MSC_FMT_RGBX8:
 994         case XILINX_M2MSC_FMT_YUVX8:
 995         case XILINX_M2MSC_FMT_RGBX10:
 996         case XILINX_M2MSC_FMT_YUVX10:
 997         case XILINX_M2MSC_FMT_BGRX8:
 998                 stride = width * 4;
 999                 break;
1000         case XILINX_M2MSC_FMT_YUYV8:
1001         case XILINX_M2MSC_FMT_UYVY8:
1002                 stride = width * 2;
1003                 break;
1004         case XILINX_M2MSC_FMT_Y_UV8:
1005         case XILINX_M2MSC_FMT_Y_UV8_420:
1006         case XILINX_M2MSC_FMT_Y8:
1007                 stride = width * 1;
1008                 break;
1009         case XILINX_M2MSC_FMT_RGB8:
1010         case XILINX_M2MSC_FMT_YUV8:
1011         case XILINX_M2MSC_FMT_BGR8:
1012                 stride = width * 3;
1013                 break;
1014         case XILINX_M2MSC_FMT_Y_UV10:
1015         case XILINX_M2MSC_FMT_Y_UV10_420:
1016         case XILINX_M2MSC_FMT_Y10:
1017                 /* 4 bytes per 3 pixels */
1018                 stride = DIV_ROUND_UP(width * 4, 3);
1019                 break;
1020         default:
1021                 stride = 0;
1022         }
1023
1024         return stride;
1025 }
1026
1027 static int
1028 vidioc_try_fmt(struct xm2msc_chan_ctx *chan_ctx, struct v4l2_format *f)
1029 {
1030         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
1031         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
1032         struct xm2msc_q_data *q_data;
1033         struct vb2_queue *vq;
1034         int index;
1035
1036         if (pix->width < XM2MSC_MIN_WIDTH || pix->width > xm2msc->max_wd ||
1037             pix->height < XM2MSC_MIN_HEIGHT || pix->height > xm2msc->max_ht)
1038                 dev_dbg(xm2msc->dev,
1039                         "Wrong input parameters %d, wxh: %dx%d.\n",
1040                         f->type, f->fmt.pix.width, f->fmt.pix.height);
1041         /*
1042          * V4L2 specification suggests the driver corrects the
1043          * format struct if any of the dimensions is unsupported
1044          */
1045         if (pix->height < XM2MSC_MIN_HEIGHT)
1046                 pix->height = XM2MSC_MIN_HEIGHT;
1047         else if (pix->height > xm2msc->max_ht)
1048                 pix->height = xm2msc->max_ht;
1049
1050         if (pix->width < XM2MSC_MIN_WIDTH)
1051                 pix->width = XM2MSC_MIN_WIDTH;
1052         else if (pix->width > xm2msc->max_wd)
1053                 pix->width = xm2msc->max_wd;
1054
1055         vq = v4l2_m2m_get_vq(chan_ctx->m2m_ctx, f->type);
1056         if (!vq)
1057                 return -EINVAL;
1058
1059         q_data = get_q_data(chan_ctx, f->type);
1060         if (!q_data)
1061                 return -EINVAL;
1062
1063         if (vb2_is_busy(vq)) {
1064                 v4l2_err(&xm2msc->v4l2_dev,
1065                          "%s queue busy\n", __func__);
1066                 return -EBUSY;
1067         }
1068
1069         q_data->fmt = find_format(f);
1070         index = find_format_index(f);
1071         if (!q_data->fmt || index == ARRAY_SIZE(formats) ||
1072             !xm2msc_chk_fmt(xm2msc, index)) {
1073                 v4l2_err(&xm2msc->v4l2_dev,
1074                          "Couldn't set format type %d, wxh: %dx%d. ",
1075                          f->type, f->fmt.pix.width, f->fmt.pix.height);
1076                 v4l2_err(&xm2msc->v4l2_dev,
1077                          "fmt: %d, field: %d\n",
1078                          f->fmt.pix.pixelformat, f->fmt.pix.field);
1079                 return -EINVAL;
1080         }
1081
1082         return 0;
1083 }
1084
1085 static int
1086 vidioc_s_fmt(struct xm2msc_chan_ctx *chan_ctx, struct v4l2_format *f)
1087 {
1088         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
1089         struct xm2msc_q_data *q_data = get_q_data(chan_ctx, f->type);
1090         unsigned int i;
1091
1092         q_data = get_q_data(chan_ctx, f->type);
1093
1094         q_data->width = pix->width;
1095         q_data->height = pix->height;
1096         q_data->stride = xm2msc_cal_stride(pix->width,
1097                                            q_data->fmt->xm2msc_fmt);
1098         q_data->colorspace = pix->colorspace;
1099         q_data->field = pix->field;
1100         q_data->nplanes = q_data->fmt->num_planes;
1101
1102         for (i = 0; i < q_data->nplanes; i++) {
1103                 q_data->bytesperline[i] = q_data->stride;
1104                 pix->plane_fmt[i].bytesperline = q_data->bytesperline[i];
1105                 q_data->sizeimage[i] = q_data->stride * q_data->height;
1106                 pix->plane_fmt[i].sizeimage = q_data->sizeimage[i];
1107         }
1108
1109         xm2msc_pr_q(chan_ctx->xm2msc_dev->dev, q_data,
1110                     chan_ctx->num, f->type, __func__);
1111
1112         return 0;
1113 }
1114
1115 static int xm2msc_try_fmt_vid_out(struct file *file, void *fh,
1116                                   struct v4l2_format *f)
1117 {
1118         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1119
1120         return vidioc_try_fmt(chan_ctx, f);
1121 }
1122
1123 static int xm2msc_try_fmt_vid_cap(struct file *file, void *fh,
1124                                   struct v4l2_format *f)
1125 {
1126         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1127
1128         return vidioc_try_fmt(chan_ctx, f);
1129 }
1130
1131 static int xm2msc_s_fmt_vid_cap(struct file *file, void *fh,
1132                                 struct v4l2_format *f)
1133 {
1134         int ret;
1135         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1136
1137         ret = xm2msc_try_fmt_vid_cap(file, fh, f);
1138         if (ret)
1139                 return ret;
1140         return vidioc_s_fmt(chan_ctx, f);
1141 }
1142
1143 static int xm2msc_s_fmt_vid_out(struct file *file, void *fh,
1144                                 struct v4l2_format *f)
1145 {
1146         int ret;
1147         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1148
1149         ret = xm2msc_try_fmt_vid_out(file, fh, f);
1150         if (ret)
1151                 return ret;
1152
1153         return vidioc_s_fmt(chan_ctx, f);
1154 }
1155
1156 static int vidioc_g_fmt(struct xm2msc_chan_ctx *chan_ctx, struct v4l2_format *f)
1157 {
1158         struct vb2_queue *vq;
1159         struct xm2msc_q_data *q_data;
1160         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
1161         unsigned int i;
1162
1163         vq = v4l2_m2m_get_vq(chan_ctx->m2m_ctx, f->type);
1164         if (!vq)
1165                 return -EINVAL;
1166
1167         q_data = get_q_data(chan_ctx, f->type);
1168         if (!q_data)
1169                 return -EINVAL;
1170
1171         pix->width = q_data->width;
1172         pix->height = q_data->height;
1173         pix->field = V4L2_FIELD_NONE;
1174         pix->pixelformat = q_data->fmt->fourcc;
1175         pix->colorspace = q_data->colorspace;
1176         pix->num_planes = q_data->nplanes;
1177
1178         for (i = 0; i < pix->num_planes; i++) {
1179                 pix->plane_fmt[i].bytesperline = q_data->bytesperline[i];
1180                 pix->plane_fmt[i].sizeimage = q_data->sizeimage[i];
1181         }
1182
1183         return 0;
1184 }
1185
1186 static int xm2msc_g_fmt_vid_out(struct file *file, void *fh,
1187                                 struct v4l2_format *f)
1188 {
1189         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1190
1191         return vidioc_g_fmt(chan_ctx, f);
1192 }
1193
1194 static int xm2msc_g_fmt_vid_cap(struct file *file, void *fh,
1195                                 struct v4l2_format *f)
1196 {
1197         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1198
1199         return vidioc_g_fmt(chan_ctx, f);
1200 }
1201
1202 static int enum_fmt(struct xm2m_msc_dev *xm2msc, struct v4l2_fmtdesc *f)
1203 {
1204         const struct xm2msc_fmt *fmt;
1205         unsigned int i, enabled = 0;
1206
1207         for (i = 0; i < ARRAY_SIZE(formats); i++) {
1208                 if (xm2msc_chk_fmt(xm2msc, i) && enabled++ == f->index)
1209                         break;
1210         }
1211
1212         if (i == ARRAY_SIZE(formats))
1213                 /* Format not found */
1214                 return -EINVAL;
1215
1216         /* Format found */
1217         fmt = &formats[i];
1218         strlcpy(f->description, fmt->name,
1219                 sizeof(f->description));
1220         f->pixelformat = fmt->fourcc;
1221
1222         return 0;
1223 }
1224
1225 static int xm2msc_enum_fmt_vid_cap(struct file *file, void *fh,
1226                                    struct v4l2_fmtdesc *f)
1227 {
1228         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1229
1230         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1231                 return -EINVAL;
1232
1233         return enum_fmt(chan_ctx->xm2msc_dev, f);
1234 }
1235
1236 static int xm2msc_enum_fmt_vid_out(struct file *file, void *fh,
1237                                    struct v4l2_fmtdesc *f)
1238 {
1239         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1240
1241         if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
1242                 return -EINVAL;
1243
1244         return enum_fmt(chan_ctx->xm2msc_dev, f);
1245 }
1246
1247 static int xm2msc_querycap(struct file *file, void *fh,
1248                            struct v4l2_capability *cap)
1249 {
1250         strncpy(cap->driver, XM2MSC_DRIVER_NAME, sizeof(cap->driver) - 1);
1251         strncpy(cap->card, XM2MSC_DRIVER_NAME, sizeof(cap->card) - 1);
1252         snprintf(cap->bus_info, sizeof(cap->bus_info),
1253                  "platform:%s", XM2MSC_DRIVER_NAME);
1254         /*
1255          * This is only a mem-to-mem video device. The STREAMING
1256          * device capability flags are left only for compatibility
1257          * and are scheduled for removal.
1258          */
1259         cap->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE;
1260         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
1261         return 0;
1262 }
1263
1264 static int xm2msc_queue_setup(struct vb2_queue *vq,
1265                               unsigned int *nbuffers, unsigned int *nplanes,
1266                               unsigned int sizes[], struct device *alloc_devs[])
1267 {
1268         unsigned int i;
1269         struct xm2msc_chan_ctx *chan_ctx = vb2_get_drv_priv(vq);
1270         struct xm2msc_q_data *q_data;
1271
1272         q_data = get_q_data(chan_ctx, vq->type);
1273         if (!q_data)
1274                 return -EINVAL;
1275
1276         *nplanes = q_data->nplanes;
1277
1278         for (i = 0; i < *nplanes; i++)
1279                 sizes[i] = q_data->sizeimage[i];
1280
1281         dev_dbg(chan_ctx->xm2msc_dev->dev, "get %d buffer(s) of size %d",
1282                 *nbuffers, sizes[0]);
1283         if (q_data->nplanes == 2)
1284                 dev_dbg(chan_ctx->xm2msc_dev->dev, " and %d\n", sizes[1]);
1285
1286         return 0;
1287 }
1288
1289 static int xm2msc_buf_prepare(struct vb2_buffer *vb)
1290 {
1291         struct xm2msc_chan_ctx *chan_ctx = vb2_get_drv_priv(vb->vb2_queue);
1292         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
1293         struct xm2msc_q_data *q_data;
1294         unsigned int i, num_planes;
1295
1296         q_data = get_q_data(chan_ctx, vb->vb2_queue->type);
1297         if (!q_data)
1298                 return -EINVAL;
1299         num_planes = q_data->nplanes;
1300
1301         for (i = 0; i < num_planes; i++) {
1302                 if (vb2_plane_size(vb, i) < q_data->sizeimage[i]) {
1303                         v4l2_err(&xm2msc->v4l2_dev, "data will not fit into plane ");
1304                                    v4l2_err(&xm2msc->v4l2_dev, "(%lu < %lu)\n",
1305                                             vb2_plane_size(vb, i),
1306                                             (long)q_data->sizeimage[i]);
1307                         return -EINVAL;
1308                 }
1309         }
1310
1311         for (i = 0; i < num_planes; i++)
1312                 vb2_set_plane_payload(vb, i, q_data->sizeimage[i]);
1313
1314         return 0;
1315 }
1316
1317 static void xm2msc_buf_queue(struct vb2_buffer *vb)
1318 {
1319         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1320         struct xm2msc_chan_ctx *chan_ctx = vb2_get_drv_priv(vb->vb2_queue);
1321
1322         v4l2_m2m_buf_queue(chan_ctx->m2m_ctx, vbuf);
1323 }
1324
1325 static void xm2msc_return_all_buffers(struct xm2msc_chan_ctx *chan_ctx,
1326                                       struct vb2_queue *q,
1327                                       enum vb2_buffer_state state)
1328 {
1329         struct vb2_v4l2_buffer *vb;
1330         unsigned long flags;
1331
1332         for (;;) {
1333                 if (V4L2_TYPE_IS_OUTPUT(q->type))
1334                         vb = v4l2_m2m_src_buf_remove(chan_ctx->m2m_ctx);
1335                 else
1336                         vb = v4l2_m2m_dst_buf_remove(chan_ctx->m2m_ctx);
1337                 if (!vb)
1338                         break;
1339                 spin_lock_irqsave(&chan_ctx->xm2msc_dev->lock, flags);
1340                 v4l2_m2m_buf_done(vb, state);
1341                 spin_unlock_irqrestore(&chan_ctx->xm2msc_dev->lock, flags);
1342         }
1343 }
1344
1345 static void xm2msc_set_chan_params(struct xm2msc_chan_ctx *chan_ctx,
1346                                    enum v4l2_buf_type type)
1347 {
1348         struct xm2msc_q_data *q_data = get_q_data(chan_ctx, type);
1349         const struct xm2msc_fmt *fmt = q_data->fmt;
1350         void __iomem *base = chan_ctx->regs;
1351
1352         if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
1353                 xm2msc_writereg(base + XM2MSC_WIDTHIN, q_data->width);
1354                 xm2msc_writereg(base + XM2MSC_HEIGHTIN, q_data->height);
1355                 xm2msc_writereg(base + XM2MSC_INPIXELFMT, fmt->xm2msc_fmt);
1356                 xm2msc_writereg(base + XM2MSC_INSTRIDE, q_data->stride);
1357         } else {
1358                 xm2msc_writereg(base + XM2MSC_WIDTHOUT, q_data->width);
1359                 xm2msc_writereg(base + XM2MSC_HEIGHTOUT, q_data->height);
1360                 xm2msc_writereg(base + XM2MSC_OUTPIXELFMT, fmt->xm2msc_fmt);
1361                 xm2msc_writereg(base + XM2MSC_OUTSTRIDE, q_data->stride);
1362         }
1363 }
1364
1365 static void xm2msc_set_chan_com_params(struct xm2msc_chan_ctx *chan_ctx)
1366 {
1367         void __iomem *base = chan_ctx->regs;
1368         struct xm2msc_q_data *out_q_data = &chan_ctx->q_data[XM2MSC_CHAN_OUT];
1369         struct xm2msc_q_data *cap_q_data = &chan_ctx->q_data[XM2MSC_CHAN_CAP];
1370         u32 pixel_rate;
1371         u32 line_rate;
1372
1373         chan_ctx->taps = XSCALER_TAPS_6; /* Currently only 6 tabs supported */
1374         xm2mvsc_initialize_coeff_banks(chan_ctx);
1375
1376         pixel_rate = (out_q_data->width * XM2MSC_STEP_PRECISION) /
1377                 cap_q_data->width;
1378         line_rate = (out_q_data->height * XM2MSC_STEP_PRECISION) /
1379                 cap_q_data->height;
1380
1381         xm2msc_writereg(base + XM2MSC_PIXELRATE, pixel_rate);
1382         xm2msc_writereg(base + XM2MSC_LINERATE, line_rate);
1383 }
1384
1385 static int xm2msc_start_streaming(struct vb2_queue *q, unsigned int count)
1386 {
1387         struct xm2msc_chan_ctx *chan_ctx = vb2_get_drv_priv(q);
1388         static struct xm2msc_q_data *q_data;
1389         int type;
1390
1391         if (V4L2_TYPE_IS_OUTPUT(q->type))
1392                 xm2msc_set_chan_stream(chan_ctx, true, XM2MSC_CHAN_OUT);
1393         else
1394                 xm2msc_set_chan_stream(chan_ctx, true, XM2MSC_CHAN_CAP);
1395
1396         xm2msc_set_chan_params(chan_ctx, q->type);
1397
1398         if (xm2msc_chk_chan_stream(chan_ctx, XM2MSC_CHAN_CAP) &&
1399             xm2msc_chk_chan_stream(chan_ctx, XM2MSC_CHAN_OUT))
1400                 xm2msc_set_chan_com_params(chan_ctx);
1401
1402         type = V4L2_TYPE_IS_OUTPUT(q->type) ?
1403                 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
1404                 V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1405         q_data = get_q_data(chan_ctx, type);
1406         xm2msc_pr_q(chan_ctx->xm2msc_dev->dev, q_data, chan_ctx->num,
1407                     type, __func__);
1408         xm2msc_pr_status(chan_ctx->xm2msc_dev, __func__);
1409
1410         return 0;
1411 }
1412
1413 static void xm2msc_stop_streaming(struct vb2_queue *q)
1414 {
1415         struct xm2msc_chan_ctx *chan_ctx = vb2_get_drv_priv(q);
1416
1417         xm2msc_return_all_buffers(chan_ctx, q, VB2_BUF_STATE_ERROR);
1418
1419         if (V4L2_TYPE_IS_OUTPUT(q->type))
1420                 xm2msc_set_chan_stream(chan_ctx, false, XM2MSC_CHAN_OUT);
1421         else
1422                 xm2msc_set_chan_stream(chan_ctx, false, XM2MSC_CHAN_CAP);
1423 }
1424
1425 static const struct vb2_ops xm2msc_qops = {
1426         .queue_setup = xm2msc_queue_setup,
1427         .buf_prepare = xm2msc_buf_prepare,
1428         .buf_queue = xm2msc_buf_queue,
1429         .start_streaming = xm2msc_start_streaming,
1430         .stop_streaming = xm2msc_stop_streaming,
1431 };
1432
1433 static int queue_init(void *priv, struct vb2_queue *src_vq,
1434                       struct vb2_queue *dst_vq)
1435 {
1436         struct xm2msc_chan_ctx *chan_ctx = priv;
1437         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
1438         int ret;
1439
1440         memset(src_vq, 0, sizeof(*src_vq));
1441         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1442         src_vq->io_modes = VB2_DMABUF | VB2_MMAP;
1443         src_vq->drv_priv = chan_ctx;
1444         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1445         src_vq->ops = &xm2msc_qops;
1446         src_vq->mem_ops = &vb2_dma_contig_memops;
1447         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1448         src_vq->lock = &xm2msc->dev_mutex;
1449         src_vq->dev = xm2msc->v4l2_dev.dev;
1450
1451         ret = vb2_queue_init(src_vq);
1452         if (ret)
1453                 return ret;
1454
1455         memset(dst_vq, 0, sizeof(*dst_vq));
1456         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1457         dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
1458         dst_vq->drv_priv = chan_ctx;
1459         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1460         dst_vq->ops = &xm2msc_qops;
1461         dst_vq->mem_ops = &vb2_dma_contig_memops;
1462         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1463         dst_vq->lock = &xm2msc->dev_mutex;
1464         dst_vq->dev = xm2msc->v4l2_dev.dev;
1465
1466         return vb2_queue_init(dst_vq);
1467 }
1468
1469 static const struct v4l2_ioctl_ops xm2msc_ioctl_ops = {
1470         .vidioc_querycap = xm2msc_querycap,
1471
1472         .vidioc_enum_fmt_vid_cap_mplane = xm2msc_enum_fmt_vid_cap,
1473         .vidioc_g_fmt_vid_cap_mplane = xm2msc_g_fmt_vid_cap,
1474         .vidioc_try_fmt_vid_cap_mplane = xm2msc_try_fmt_vid_cap,
1475         .vidioc_s_fmt_vid_cap_mplane = xm2msc_s_fmt_vid_cap,
1476
1477         .vidioc_enum_fmt_vid_out_mplane = xm2msc_enum_fmt_vid_out,
1478         .vidioc_g_fmt_vid_out_mplane = xm2msc_g_fmt_vid_out,
1479         .vidioc_try_fmt_vid_out_mplane = xm2msc_try_fmt_vid_out,
1480         .vidioc_s_fmt_vid_out_mplane = xm2msc_s_fmt_vid_out,
1481
1482         .vidioc_reqbufs = xm2msc_reqbufs,
1483         .vidioc_querybuf = xm2msc_querybuf,
1484         .vidioc_expbuf = xm2msc_expbuf,
1485         .vidioc_create_bufs = xm2msc_createbufs,
1486
1487         .vidioc_qbuf = xm2msc_qbuf,
1488         .vidioc_dqbuf = xm2msc_dqbuf,
1489
1490         .vidioc_streamon = xm2msc_streamon,
1491         .vidioc_streamoff = xm2msc_streamoff,
1492 };
1493
1494 static int xm2msc_open(struct file *file)
1495 {
1496         struct xm2m_msc_dev *xm2msc = video_drvdata(file);
1497         struct xm2msc_chan_ctx *chan_ctx = NULL;
1498         u32 minor, chan;
1499         int ret;
1500
1501         if (mutex_lock_interruptible(&xm2msc->dev_mutex))
1502                 return -ERESTARTSYS;
1503
1504         minor = iminor(file_inode(file));
1505
1506         for (chan = 0; chan < xm2msc->max_chan; chan++) {
1507                 chan_ctx = &xm2msc->xm2msc_chan[chan];
1508
1509                 if ((chan_ctx->status & CHAN_ATTACHED) &&
1510                     chan_ctx->minor == minor)
1511                         break;
1512         }
1513
1514         if (chan == xm2msc->max_chan) {
1515                 v4l2_err(&xm2msc->v4l2_dev,
1516                          "%s Chan not found with minor = %d\n",
1517                          __func__, minor);
1518                 ret = -EBADF;
1519                 goto unlock;
1520         }
1521
1522         /* Already opened, do not allow same channel
1523          * to be open more then once
1524          */
1525         if (chan_ctx->status & CHAN_OPENED) {
1526                 v4l2_warn(&xm2msc->v4l2_dev,
1527                           "%s Chan already opened for minor = %d\n",
1528                           __func__, minor);
1529                 ret = -EBUSY;
1530                 goto unlock;
1531         }
1532
1533         v4l2_fh_init(&chan_ctx->fh, &chan_ctx->vfd);
1534         file->private_data = &chan_ctx->fh;
1535         v4l2_fh_add(&chan_ctx->fh);
1536
1537         chan_ctx->m2m_ctx = v4l2_m2m_ctx_init(chan_ctx->m2m_dev,
1538                                               chan_ctx, &queue_init);
1539         if (IS_ERR(chan_ctx->m2m_ctx)) {
1540                 ret = PTR_ERR(chan_ctx->m2m_ctx);
1541                 v4l2_err(&xm2msc->v4l2_dev,
1542                          "%s Chan M2M CTX not creted for minor %d\n",
1543                          __func__, minor);
1544                 goto error_m2m;
1545         }
1546
1547         chan_ctx->fh.m2m_ctx = chan_ctx->m2m_ctx;
1548         chan_ctx->status |= CHAN_OPENED;
1549         chan_ctx->xm2msc_dev = xm2msc;
1550         xm2msc_set_chan(chan_ctx, true);
1551
1552         v4l2_info(&xm2msc->v4l2_dev, "Channel %d instance created\n", chan);
1553
1554         mutex_unlock(&xm2msc->dev_mutex);
1555         xm2msc_pr_chanctx(chan_ctx, __func__);
1556         xm2msc_pr_status(xm2msc, __func__);
1557         return 0;
1558
1559 error_m2m:
1560         v4l2_fh_del(&chan_ctx->fh);
1561         v4l2_fh_exit(&chan_ctx->fh);
1562 unlock:
1563         mutex_unlock(&xm2msc->dev_mutex);
1564         xm2msc_pr_chanctx(chan_ctx, __func__);
1565         xm2msc_pr_status(xm2msc, __func__);
1566         return ret;
1567 }
1568
1569 static int xm2msc_release(struct file *file)
1570 {
1571         struct xm2m_msc_dev *xm2msc = video_drvdata(file);
1572         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(file->private_data);
1573
1574         if (mutex_lock_interruptible(&xm2msc->dev_mutex))
1575                 return -ERESTARTSYS;
1576
1577         v4l2_m2m_ctx_release(chan_ctx->m2m_ctx);
1578         v4l2_fh_del(&chan_ctx->fh);
1579         v4l2_fh_exit(&chan_ctx->fh);
1580         chan_ctx->status &= ~CHAN_OPENED;
1581         xm2msc_set_chan(chan_ctx, false);
1582
1583         v4l2_info(&xm2msc->v4l2_dev, "Channel %d instance released\n",
1584                   chan_ctx->num);
1585
1586         mutex_unlock(&xm2msc->dev_mutex);
1587         return 0;
1588 }
1589
1590 static unsigned int xm2msc_poll(struct file *file,
1591                                 struct poll_table_struct *wait)
1592 {
1593         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(file->private_data);
1594         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
1595         int ret;
1596
1597         mutex_lock(&xm2msc->dev_mutex);
1598         ret = v4l2_m2m_poll(file, chan_ctx->m2m_ctx, wait);
1599         mutex_unlock(&xm2msc->dev_mutex);
1600
1601         return ret;
1602 }
1603
1604 static int xm2msc_mmap(struct file *file, struct vm_area_struct *vma)
1605 {
1606         struct xm2msc_chan_ctx *chan_ctx = file->private_data;
1607         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
1608         int ret;
1609
1610         mutex_lock(&xm2msc->dev_mutex);
1611         ret = v4l2_m2m_mmap(file, chan_ctx->m2m_ctx, vma);
1612
1613         mutex_unlock(&xm2msc->dev_mutex);
1614         return ret;
1615 }
1616
1617 static const struct v4l2_file_operations xm2msc_fops = {
1618         .owner = THIS_MODULE,
1619         .open = xm2msc_open,
1620         .release = xm2msc_release,
1621         .poll = xm2msc_poll,
1622         .unlocked_ioctl = video_ioctl2,
1623         .mmap = xm2msc_mmap,
1624 };
1625
1626 static const struct video_device xm2msc_videodev = {
1627         .name = XM2MSC_DRIVER_NAME,
1628         .fops = &xm2msc_fops,
1629         .ioctl_ops = &xm2msc_ioctl_ops,
1630         .minor = -1,
1631         .release = video_device_release_empty,
1632         .vfl_dir = VFL_DIR_M2M,
1633 };
1634
1635 static const struct v4l2_m2m_ops xm2msc_m2m_ops = {
1636         .device_run = xm2msc_device_run,
1637         .job_ready = xm2msc_job_ready,
1638         .job_abort = xm2msc_job_abort,
1639 };
1640
1641 static int xm2msc_parse_of(struct platform_device *pdev,
1642                            struct xm2m_msc_dev *xm2msc)
1643 {
1644         struct resource *res;
1645         struct device *dev = &pdev->dev;
1646         struct device_node *node = dev->of_node;
1647         int hw_vid_fmt_cnt;
1648         const char *vid_fmts[ARRAY_SIZE(formats)];
1649         int ret;
1650         u32 i, j;
1651
1652         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1653         xm2msc->regs = devm_ioremap_resource(dev, res);
1654         if (IS_ERR((__force void *)xm2msc->regs))
1655                 return PTR_ERR((__force const void *)xm2msc->regs);
1656
1657         dev_dbg(dev, "IO Mem 0x%llx mapped at %p\n", res->start, xm2msc->regs);
1658
1659         ret = of_property_read_u32(node, "xlnx,max-chan",
1660                                    &xm2msc->max_chan);
1661         if (ret < 0)
1662                 return ret;
1663
1664         if (xm2msc->max_chan < XM2MSC_MIN_CHAN ||
1665             xm2msc->max_chan > XM2MSC_MAX_CHAN) {
1666                 dev_err(dev,
1667                         "Invalid maximum scaler channels : %d",
1668                         xm2msc->max_chan);
1669                 return -EINVAL;
1670         }
1671
1672         ret = of_property_read_u32(node, "xlnx,max-width",
1673                                    &xm2msc->max_wd);
1674         if (ret < 0) {
1675                 dev_err(dev,
1676                         "missing xlnx,max-width prop\n");
1677                 return ret;
1678         }
1679
1680         if (xm2msc->max_wd < XM2MSC_MIN_WIDTH ||
1681             xm2msc->max_wd > XM2MSC_MAX_WIDTH) {
1682                 dev_err(dev, "Invalid width : %d",
1683                         xm2msc->max_wd);
1684                 return -EINVAL;
1685         }
1686
1687         ret = of_property_read_u32(node, "xlnx,max-height",
1688                                    &xm2msc->max_ht);
1689         if (ret < 0) {
1690                 dev_err(dev, "missing xlnx,max-height prop\n");
1691                 return ret;
1692         }
1693
1694         if (xm2msc->max_ht < XM2MSC_MIN_HEIGHT ||
1695             xm2msc->max_ht > XM2MSC_MAX_HEIGHT) {
1696                 dev_err(dev, "Invalid height : %d",
1697                         xm2msc->max_ht);
1698                 return -EINVAL;
1699         }
1700
1701         ret = of_property_read_u32(node, "xlnx,dma-addr-width",
1702                                    &xm2msc->dma_addr_size);
1703         if (ret || (xm2msc->dma_addr_size != 32 &&
1704                     xm2msc->dma_addr_size != 64)) {
1705                 dev_err(dev, "missing/invalid addr width dts prop\n");
1706                 return -EINVAL;
1707         }
1708
1709         xm2msc->irq = irq_of_parse_and_map(node, 0);
1710         if (xm2msc->irq < 0) {
1711                 dev_err(dev, "Unable to get IRQ");
1712                 return xm2msc->irq;
1713         }
1714
1715         dev_dbg(dev, "Max Channel Supported = %d\n", xm2msc->max_chan);
1716         dev_dbg(dev, "DMA Addr width Supported = %d\n", xm2msc->dma_addr_size);
1717         dev_dbg(dev, "Max col/row Supported = (%d) / (%d)\n",
1718                 xm2msc->max_wd, xm2msc->max_ht);
1719         /* read supported video formats and update internal table */
1720         hw_vid_fmt_cnt = of_property_count_strings(node, "xlnx,vid-formats");
1721
1722         ret = of_property_read_string_array(node, "xlnx,vid-formats",
1723                                             vid_fmts, hw_vid_fmt_cnt);
1724         if (ret < 0) {
1725                 dev_err(dev,
1726                         "Missing or invalid xlnx,vid-formats dts prop\n");
1727                 return ret;
1728         }
1729
1730         dev_dbg(dev, "Supported format = ");
1731         for (i = 0; i < hw_vid_fmt_cnt; i++) {
1732                 const char *vid_fmt_name = vid_fmts[i];
1733
1734                 for (j = 0; j < ARRAY_SIZE(formats); j++) {
1735                         const char *dts_name = formats[j].name;
1736
1737                         if (strcmp(vid_fmt_name, dts_name))
1738                                 continue;
1739                         dev_dbg(dev, "%s ", dts_name);
1740
1741                         xm2msc_set_fmt(xm2msc, j);
1742                 }
1743         }
1744         dev_dbg(dev, "\n");
1745         xm2msc->rst_gpio = devm_gpiod_get(dev, "reset",
1746                                           GPIOD_OUT_HIGH);
1747         if (IS_ERR(xm2msc->rst_gpio)) {
1748                 ret = PTR_ERR(xm2msc->rst_gpio);
1749                 if (ret == -EPROBE_DEFER)
1750                         dev_info(dev,
1751                                  "Probe deferred due to GPIO reset defer\n");
1752                 else
1753                         dev_err(dev,
1754                                 "Unable to locate reset property in dt\n");
1755                 return ret;
1756         }
1757
1758         return 0;
1759 }
1760
1761 static void xm2msc_unreg_video_n_m2m(struct xm2m_msc_dev *xm2msc)
1762 {
1763         struct xm2msc_chan_ctx *chan_ctx;
1764         unsigned int chan;
1765
1766         for (chan = 0; chan < xm2msc->max_chan; chan++) {
1767                 chan_ctx = &xm2msc->xm2msc_chan[chan];
1768                 if (!(chan_ctx->status & CHAN_ATTACHED))
1769                         break;  /*We register video sequentially */
1770                 video_unregister_device(&chan_ctx->vfd);
1771                 chan_ctx->status &= ~CHAN_ATTACHED;
1772
1773                 if (!IS_ERR(chan_ctx->m2m_dev))
1774                         v4l2_m2m_release(chan_ctx->m2m_dev);
1775         }
1776 }
1777
1778 static int xm2m_msc_probe(struct platform_device *pdev)
1779 {
1780         int ret;
1781         struct xm2m_msc_dev *xm2msc;
1782         struct xm2msc_chan_ctx *chan_ctx;
1783         struct video_device *vfd;
1784         unsigned int chan;
1785
1786         xm2msc = devm_kzalloc(&pdev->dev, sizeof(*xm2msc), GFP_KERNEL);
1787         if (!xm2msc)
1788                 return -ENOMEM;
1789
1790         ret = xm2msc_parse_of(pdev, xm2msc);
1791         if (ret < 0)
1792                 return ret;
1793
1794         xm2msc->dev = &pdev->dev;
1795
1796         xm2msc_reset(xm2msc);
1797
1798         spin_lock_init(&xm2msc->lock);
1799
1800         ret = v4l2_device_register(&pdev->dev, &xm2msc->v4l2_dev);
1801         if (ret)
1802                 return ret;
1803
1804         for (chan = 0; chan < xm2msc->max_chan; chan++) {
1805                 chan_ctx = &xm2msc->xm2msc_chan[chan];
1806
1807                 vfd = &chan_ctx->vfd;
1808                 *vfd = xm2msc_videodev;
1809                 vfd->lock = &xm2msc->dev_mutex;
1810                 vfd->v4l2_dev = &xm2msc->v4l2_dev;
1811
1812                 ret = video_register_device(vfd, VFL_TYPE_GRABBER, chan);
1813                 if (ret) {
1814                         v4l2_err(&xm2msc->v4l2_dev,
1815                                  "Failed to register video dev for chan %d\n",
1816                                  chan);
1817                         goto unreg_dev;
1818                 }
1819
1820                 chan_ctx->status = CHAN_ATTACHED;
1821
1822                 video_set_drvdata(vfd, xm2msc);
1823                 snprintf(vfd->name, sizeof(vfd->name),
1824                          "%s", xm2msc_videodev.name);
1825                 v4l2_info(&xm2msc->v4l2_dev,
1826                           " Device registered as /dev/video%d\n", vfd->num);
1827
1828                 dev_dbg(xm2msc->dev, "%s Device registered as /dev/video%d\n",
1829                         __func__, vfd->num);
1830
1831                 chan_ctx->m2m_dev = v4l2_m2m_init(&xm2msc_m2m_ops);
1832                 if (IS_ERR(chan_ctx->m2m_dev)) {
1833                         v4l2_err(&xm2msc->v4l2_dev,
1834                                  "Failed to init mem2mem device for chan %d\n",
1835                                  chan);
1836                         ret = PTR_ERR(chan_ctx->m2m_dev);
1837                         goto unreg_dev;
1838                 }
1839                 chan_ctx->xm2msc_dev = xm2msc;
1840                 chan_ctx->regs = xm2msc->regs + XM2MSC_CHAN_REGS_START(chan);
1841                 chan_ctx->num = chan;
1842                 chan_ctx->minor = vfd->minor;
1843                 xm2msc_pr_chanctx(chan_ctx, __func__);
1844         }
1845
1846         mutex_init(&xm2msc->dev_mutex);
1847         mutex_init(&xm2msc->mutex);
1848
1849         ret = devm_request_irq(&pdev->dev, xm2msc->irq,
1850                                xm2msc_isr, IRQF_SHARED,
1851                                XM2MSC_DRIVER_NAME, xm2msc);
1852         if (ret < 0) {
1853                 dev_err(&pdev->dev, "Unable to register IRQ\n");
1854                 goto unreg_dev;
1855         }
1856
1857         platform_set_drvdata(pdev, xm2msc);
1858
1859         return 0;
1860
1861  unreg_dev:
1862         xm2msc_unreg_video_n_m2m(xm2msc);
1863         v4l2_device_unregister(&xm2msc->v4l2_dev);
1864         return ret;
1865 }
1866
1867 static int xm2m_msc_remove(struct platform_device *pdev)
1868 {
1869         struct xm2m_msc_dev *xm2msc = platform_get_drvdata(pdev);
1870
1871         xm2msc_unreg_video_n_m2m(xm2msc);
1872         v4l2_device_unregister(&xm2msc->v4l2_dev);
1873         return 0;
1874 }
1875
1876 static const struct of_device_id xm2m_msc_of_id_table[] = {
1877         {.compatible = "xlnx,v-multi-scaler-v1.0"},
1878         {}
1879 };
1880
1881 MODULE_DEVICE_TABLE(of, xm2m_msc_of_id_table);
1882
1883 static struct platform_driver xm2m_msc_driver = {
1884         .driver = {
1885                 .name = "xilinx-multiscaler",
1886                 .of_match_table = xm2m_msc_of_id_table,
1887         },
1888         .probe = xm2m_msc_probe,
1889         .remove = xm2m_msc_remove,
1890 };
1891
1892 module_platform_driver(xm2m_msc_driver);
1893
1894 MODULE_DESCRIPTION("Xilinx M2M Multi-Scaler Driver");
1895 MODULE_LICENSE("GPL v2");
1896 MODULE_ALIAS("xlnx_m2m_multiscaler_dev");