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