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 struct xm2msc_q_data *get_q_data(struct xm2msc_chan_ctx *chan_ctx,
 355                                         enum v4l2_buf_type type)
 356 {
 357         switch (type) {
 358         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 359         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 360                 return &chan_ctx->q_data[XM2MSC_CHAN_OUT];
 361         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 362         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 363                 return &chan_ctx->q_data[XM2MSC_CHAN_CAP];
 364         default:
 365                 v4l2_err(&chan_ctx->xm2msc_dev->v4l2_dev,
 366                          "Not supported Q type %d\n", type);
 367         }
 368         return NULL;
 369 }
 370
 371 static u32 find_format_index(struct v4l2_format *f)
 372 {
 373         const struct xm2msc_fmt *fmt;
 374         unsigned int i;
 375
 376         for (i = 0; i < ARRAY_SIZE(formats); i++) {
 377                 fmt = &formats[i];
 378                 if (fmt->fourcc == f->fmt.pix_mp.pixelformat)
 379                         break;
 380         }
 381
 382         return i;
 383 }
 384
 385 static const struct xm2msc_fmt *find_format(struct v4l2_format *f)
 386 {
 387         const struct xm2msc_fmt *fmt;
 388         unsigned int i;
 389
 390         for (i = 0; i < ARRAY_SIZE(formats); i++) {
 391                 fmt = &formats[i];
 392                 if (fmt->fourcc == f->fmt.pix_mp.pixelformat)
 393                         break;
 394         }
 395
 396         if (i == ARRAY_SIZE(formats))
 397                 return NULL;
 398
 399         return &formats[i];
 400 }
 401
 402 static void
 403 xv_hscaler_load_ext_coeff(struct xm2m_msc_dev *xm2msc,
 404                           const short *coeff, u32 ntaps)
 405 {
 406         unsigned int i, j, pad, offset;
 407         const u32 nphases = XSCALER_MAX_PHASES;
 408
 409         /* Determine if coefficient needs padding (effective vs. max taps) */
 410         pad = XSCALER_MAX_TAPS - ntaps;
 411         offset = pad >> 1;
 412
 413         memset(xm2msc->hscaler_coeff, 0, sizeof(xm2msc->hscaler_coeff));
 414
 415         /* Load coefficients into scaler coefficient table */
 416         for (i = 0; i < nphases; i++) {
 417                 for (j = 0; j < ntaps; ++j)
 418                         xm2msc->hscaler_coeff[i][j + offset] =
 419                                                 coeff[i * ntaps + j];
 420         }
 421 }
 422
 423 static void xv_hscaler_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         int val, offset, rd_indx;
 428         unsigned int i, j;
 429         u32 ntaps = chan_ctx->taps;
 430         const u32 nphases = XSCALER_MAX_PHASES;
 431
 432         offset = (XSCALER_MAX_TAPS - ntaps) / 2;
 433         for (i = 0; i < nphases; i++) {
 434                 for (j = 0; j < ntaps / 2; j++) {
 435                         rd_indx = j * 2 + offset;
 436                         val = (xm2msc->hscaler_coeff[i][rd_indx + 1] <<
 437                                XM2MSC_BITSHIFT_16) |
 438                                (xm2msc->hscaler_coeff[i][rd_indx] &
 439                                XM2MSC_MASK_LOW_16BITS);
 440                          xm2msc_writereg((xm2msc->regs + base_addr) +
 441                                     ((i * ntaps / 2 + j) * 4), val);
 442                 }
 443         }
 444 }
 445
 446 static void
 447 xv_vscaler_load_ext_coeff(struct xm2m_msc_dev *xm2msc,
 448                           const short *coeff, const u32 ntaps)
 449 {
 450         unsigned int i, j;
 451         int pad, offset;
 452         const u32 nphases = XSCALER_MAX_PHASES;
 453
 454         /* Determine if coefficient needs padding (effective vs. max taps) */
 455         pad = XSCALER_MAX_TAPS - ntaps;
 456         offset = pad ? (pad >> 1) : 0;
 457
 458         /* Zero Entire Array */
 459         memset(xm2msc->vscaler_coeff, 0, sizeof(xm2msc->vscaler_coeff));
 460
 461         /* Load User defined coefficients into scaler coefficient table */
 462         for (i = 0; i < nphases; i++) {
 463                 for (j = 0; j < ntaps; ++j)
 464                         xm2msc->vscaler_coeff[i][j + offset] =
 465                                                 coeff[i * ntaps + j];
 466         }
 467 }
 468
 469 static void xv_vscaler_set_coeff(struct xm2msc_chan_ctx *chan_ctx,
 470                                  const u32 base_addr)
 471 {
 472         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 473         u32 val, i, j, offset, rd_indx;
 474         u32 ntaps = chan_ctx->taps;
 475         const u32 nphases = XSCALER_MAX_PHASES;
 476
 477         offset = (XSCALER_MAX_TAPS - ntaps) / 2;
 478
 479         for (i = 0; i < nphases; i++) {
 480                 for (j = 0; j < ntaps / 2; j++) {
 481                         rd_indx = j * 2 + offset;
 482                         val = (xm2msc->vscaler_coeff[i][rd_indx + 1] <<
 483                                XM2MSC_BITSHIFT_16) |
 484                                (xm2msc->vscaler_coeff[i][rd_indx] &
 485                                XM2MSC_MASK_LOW_16BITS);
 486                         xm2msc_writereg((xm2msc->regs +
 487                                    base_addr) + ((i * ntaps / 2 + j) * 4), val);
 488                 }
 489         }
 490 }
 491
 492 static void xm2mvsc_initialize_coeff_banks(struct xm2msc_chan_ctx *chan_ctx)
 493 {
 494         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 495         /* Bank 0 is init as 6 tap filter for 6, 8, 10 & 12 tap filters */
 496         xv_hscaler_load_ext_coeff(xm2msc, &xhsc_coeff_taps6[0][0],
 497                                   XSCALER_TAPS_6);
 498         xv_hscaler_set_coeff(chan_ctx, XM2MVSC_HFLTCOEFF(chan_ctx->num));
 499         xv_vscaler_load_ext_coeff(xm2msc, &xvsc_coeff_taps6[0][0],
 500                                   XSCALER_TAPS_6);
 501         xv_vscaler_set_coeff(chan_ctx, XM2MVSC_VFLTCOEFF(chan_ctx->num));
 502 }
 503
 504 static void xm2msc_set_chan_params(struct xm2msc_chan_ctx *chan_ctx,
 505                                    enum v4l2_buf_type type)
 506 {
 507         struct xm2msc_q_data *q_data = get_q_data(chan_ctx, type);
 508         const struct xm2msc_fmt *fmt = q_data->fmt;
 509         void __iomem *base = chan_ctx->regs;
 510
 511         if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 512                 xm2msc_writereg(base + XM2MSC_WIDTHIN, q_data->width);
 513                 xm2msc_writereg(base + XM2MSC_HEIGHTIN, q_data->height);
 514                 xm2msc_writereg(base + XM2MSC_INPIXELFMT, fmt->xm2msc_fmt);
 515                 xm2msc_writereg(base + XM2MSC_INSTRIDE, q_data->stride);
 516         } else {
 517                 xm2msc_writereg(base + XM2MSC_WIDTHOUT, q_data->width);
 518                 xm2msc_writereg(base + XM2MSC_HEIGHTOUT, q_data->height);
 519                 xm2msc_writereg(base + XM2MSC_OUTPIXELFMT, fmt->xm2msc_fmt);
 520                 xm2msc_writereg(base + XM2MSC_OUTSTRIDE, q_data->stride);
 521         }
 522 }
 523
 524 static void xm2msc_set_chan_com_params(struct xm2msc_chan_ctx *chan_ctx)
 525 {
 526         void __iomem *base = chan_ctx->regs;
 527         struct xm2msc_q_data *out_q_data = &chan_ctx->q_data[XM2MSC_CHAN_OUT];
 528         struct xm2msc_q_data *cap_q_data = &chan_ctx->q_data[XM2MSC_CHAN_CAP];
 529         u32 pixel_rate;
 530         u32 line_rate;
 531
 532         chan_ctx->taps = XSCALER_TAPS_6; /* Currently only 6 tabs supported */
 533         xm2mvsc_initialize_coeff_banks(chan_ctx);
 534
 535         pixel_rate = (out_q_data->width * XM2MSC_STEP_PRECISION) /
 536                 cap_q_data->width;
 537         line_rate = (out_q_data->height * XM2MSC_STEP_PRECISION) /
 538                 cap_q_data->height;
 539
 540         xm2msc_writereg(base + XM2MSC_PIXELRATE, pixel_rate);
 541         xm2msc_writereg(base + XM2MSC_LINERATE, line_rate);
 542 }
 543
 544 static void
 545 xm2msc_pr_q(struct device *dev, struct xm2msc_q_data *q, int chan,
 546             int type, const char *fun_name)
 547 {
 548         unsigned int i;
 549         const struct xm2msc_fmt *fmt = q->fmt;
 550
 551         if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 552                 dev_dbg(dev, "\n\nOUTPUT Q (%d) Context from [[ %s ]]",
 553                         chan, fun_name);
 554         else
 555                 dev_dbg(dev, "\n\nCAPTURE Q (%d) Context from [[ %s ]]",
 556                         chan, fun_name);
 557
 558         dev_dbg(dev, "width height stride clrspace field planes\n");
 559         dev_dbg(dev, "  %d  %d    %d     %d       %d    %d\n",
 560                 q->width, q->height, q->stride,
 561                 q->colorspace, q->field, q->nplanes);
 562
 563         for (i = 0; i < q->nplanes; i++) {
 564                 dev_dbg(dev, "[plane %d ] bytesperline sizeimage\n", i);
 565                 dev_dbg(dev, "                %d        %d\n",
 566                         q->bytesperline[i], q->sizeimage[i]);
 567         }
 568
 569         dev_dbg(dev, "fmt_name 4cc xlnx-fmt\n");
 570         dev_dbg(dev, "%s %d %d\n",
 571                 fmt->name, fmt->fourcc, fmt->xm2msc_fmt);
 572         dev_dbg(dev, "\n\n");
 573 }
 574
 575 static void
 576 xm2msc_pr_status(struct xm2m_msc_dev *xm2msc,
 577                  const char *fun_name)
 578 {
 579         struct device *dev = xm2msc->dev;
 580
 581         dev_dbg(dev, "Status in %s\n", fun_name);
 582         dev_dbg(dev, "opened_chan out_streamed_chan cap_streamed_chan\n");
 583         dev_dbg(dev, "0x%x           0x%x               0x%x\n",
 584                 xm2msc->opened_chan, xm2msc->out_streamed_chan,
 585                 xm2msc->cap_streamed_chan);
 586         dev_dbg(dev, "\n\n");
 587 }
 588
 589 static void
 590 xm2msc_pr_chanctx(struct xm2msc_chan_ctx *ctx, const char *fun_name)
 591 {
 592         struct device *dev = ctx->xm2msc_dev->dev;
 593
 594         dev_dbg(dev, "\n\n----- [[ %s ]]: Channel %d (0x%p) context -----\n",
 595                 fun_name, ctx->num, ctx);
 596         dev_dbg(dev, "minor = %d, taps = %d\n", ctx->minor, ctx->taps);
 597         dev_dbg(dev, "reg mapped at %p\n", ctx->regs);
 598         dev_dbg(dev, "xm2msc \tm2m_dev \tm2m_ctx\n");
 599         dev_dbg(dev, "%p \t%p \t%p\n", ctx->xm2msc_dev,
 600                 ctx->m2m_dev, ctx->m2m_ctx);
 601
 602         if (ctx->status & CHAN_OPENED)
 603                 dev_dbg(dev, "Opened ");
 604         if (ctx->status & CHAN_ATTACHED)
 605                 dev_dbg(dev, "and attached");
 606         dev_dbg(dev, "\n");
 607         dev_dbg(dev, "-----------------------------------\n");
 608         dev_dbg(dev, "\n\n");
 609 }
 610
 611 static void
 612 xm2msc_pr_screg(struct device *dev, const volatile void __iomem *base)
 613 {
 614         dev_dbg(dev, "Ctr, GIE,  IE,  IS   OUT\n");
 615         dev_dbg(dev, "0x%x  0x%x   0x%x  0x%x  0x%x\n",
 616                 xm2msc_readreg(base + XM2MSC_AP_CTRL),
 617                 xm2msc_readreg(base + XM2MSC_GIE),
 618                 xm2msc_readreg(base + XM2MSC_IER),
 619                 xm2msc_readreg(base + XM2MSC_ISR),
 620                 xm2msc_readreg(base + XM2MSC_NUM_OUTS));
 621 }
 622
 623 static void
 624 xm2msc_pr_chanreg(struct device *dev, const volatile void __iomem *base)
 625 {
 626         dev_dbg(dev, "WIN HIN INPIXELFMT INSTRIDE SRCB0L/H SRCB1L/H\n");
 627         dev_dbg(dev, "%d   %d     %d       %d      0x%x/0x%x      0x%x/0x%x\n",
 628                 xm2msc_readreg(base + XM2MSC_WIDTHIN),
 629                 xm2msc_readreg(base + XM2MSC_HEIGHTIN),
 630                 xm2msc_readreg(base + XM2MSC_INPIXELFMT),
 631                 xm2msc_readreg(base + XM2MSC_INSTRIDE),
 632                 xm2msc_readreg(base + XM2MSC_SRCIMGBUF0),
 633                 xm2msc_readreg(base + XM2MSC_SRCIMGBUF0 + 4),
 634                 xm2msc_readreg(base + XM2MSC_SRCIMGBUF1),
 635                 xm2msc_readreg(base + XM2MSC_SRCIMGBUF1 + 4));
 636         dev_dbg(dev, "WOUT HOUT OUTPIXELFMT OUTSTRIDE DBUF0L/H DBUF1L/H\n");
 637         dev_dbg(dev, "%d   %d     %d       %d      0x%x/0x%x      0x%x/0x%x\n",
 638                 xm2msc_readreg(base + XM2MSC_WIDTHOUT),
 639                 xm2msc_readreg(base + XM2MSC_HEIGHTOUT),
 640                 xm2msc_readreg(base + XM2MSC_OUTPIXELFMT),
 641                 xm2msc_readreg(base + XM2MSC_OUTSTRIDE),
 642                 xm2msc_readreg(base + XM2MSC_DSTIMGBUF0),
 643                 xm2msc_readreg(base + XM2MSC_DSTIMGBUF0 + 4),
 644                 xm2msc_readreg(base + XM2MSC_DSTIMGBUF1),
 645                 xm2msc_readreg(base + XM2MSC_DSTIMGBUF1 + 4));
 646
 647         dev_dbg(dev, "LINERATE PIXELRATE\n");
 648         dev_dbg(dev, "0x%x     0x%x\n",
 649                 xm2msc_readreg(base + XM2MSC_LINERATE),
 650                 xm2msc_readreg(base + XM2MSC_PIXELRATE));
 651 }
 652
 653 static void
 654 xm2msc_pr_allchanreg(struct xm2m_msc_dev *xm2msc)
 655 {
 656         unsigned int i;
 657         struct xm2msc_chan_ctx *chan_ctx;
 658         struct device *dev = xm2msc->dev;
 659
 660         xm2msc_pr_screg(xm2msc->dev, xm2msc->regs);
 661
 662         for (i = 0; i < xm2msc->max_chan; i++) {
 663                 chan_ctx = &xm2msc->xm2msc_chan[i];
 664                 dev_dbg(dev, "Regs val for channel %d\n", i);
 665                 dev_dbg(dev, "______________________________________________\n");
 666                 xm2msc_pr_chanreg(dev, chan_ctx->regs);
 667                 dev_dbg(dev, "______________________________________________\n");
 668         }
 669 }
 670
 671 static inline bool xm2msc_testbit(int num, u32 *addr)
 672 {
 673         return (*addr & BIT(num));
 674 }
 675
 676 static inline void xm2msc_setbit(int num, u32 *addr)
 677 {
 678         *addr |= BIT(num);
 679 }
 680
 681 static inline void xm2msc_clrbit(int num, u32 *addr)
 682 {
 683         *addr &= ~BIT(num);
 684 }
 685
 686 static void xm2msc_stop(struct xm2m_msc_dev *xm2msc)
 687 {
 688         void __iomem *base = xm2msc->regs;
 689         u32 data = xm2msc_readreg(base + XM2MSC_AP_CTRL);
 690
 691         data &= ~XM2MSC_AP_CTRL_START;
 692         xm2msc_writereg(base + XM2MSC_AP_CTRL, data);
 693 }
 694
 695 static void xm2msc_start(struct xm2m_msc_dev *xm2msc)
 696 {
 697         void __iomem *base = xm2msc->regs;
 698         u32 data = xm2msc_readreg(base + XM2MSC_AP_CTRL);
 699
 700         data |= XM2MSC_AP_CTRL_START;
 701         xm2msc_writereg(base + XM2MSC_AP_CTRL, data);
 702 }
 703
 704 static void xm2msc_set_chan(struct xm2msc_chan_ctx *ctx, bool state)
 705 {
 706         mutex_lock(&ctx->xm2msc_dev->mutex);
 707         if (state)
 708                 xm2msc_setbit(ctx->num, &ctx->xm2msc_dev->opened_chan);
 709         else
 710                 xm2msc_clrbit(ctx->num, &ctx->xm2msc_dev->opened_chan);
 711         mutex_unlock(&ctx->xm2msc_dev->mutex);
 712 }
 713
 714 static void
 715 xm2msc_set_chan_stream(struct xm2msc_chan_ctx *ctx, bool state, int type)
 716 {
 717         u32 *ptr;
 718
 719         if (type == XM2MSC_CHAN_OUT)
 720                 ptr = &ctx->xm2msc_dev->cap_streamed_chan;
 721         else
 722                 ptr = &ctx->xm2msc_dev->out_streamed_chan;
 723
 724         mutex_lock(&ctx->xm2msc_dev->mutex);
 725         if (state)
 726                 xm2msc_setbit(ctx->num, ptr);
 727         else
 728                 xm2msc_clrbit(ctx->num, ptr);
 729
 730         mutex_unlock(&ctx->xm2msc_dev->mutex);
 731 }
 732
 733 static int
 734 xm2msc_chk_chan_stream(struct xm2msc_chan_ctx *ctx, int type)
 735 {
 736         u32 *ptr;
 737         int ret;
 738
 739         if (type == XM2MSC_CHAN_OUT)
 740                 ptr = &ctx->xm2msc_dev->cap_streamed_chan;
 741         else
 742                 ptr = &ctx->xm2msc_dev->out_streamed_chan;
 743
 744         mutex_lock(&ctx->xm2msc_dev->mutex);
 745         ret = xm2msc_testbit(ctx->num, ptr);
 746         mutex_unlock(&ctx->xm2msc_dev->mutex);
 747
 748         return ret;
 749 }
 750
 751 static void xm2msc_set_fmt(struct xm2m_msc_dev *xm2msc, u32 index)
 752 {
 753         xm2msc_setbit(index, &xm2msc->supported_fmt);
 754 }
 755
 756 static int xm2msc_chk_fmt(struct xm2m_msc_dev *xm2msc, u32 index)
 757 {
 758         return xm2msc_testbit(index, &xm2msc->supported_fmt);
 759 }
 760
 761 static void xm2msc_reset(struct xm2m_msc_dev *xm2msc)
 762 {
 763         gpiod_set_value_cansleep(xm2msc->rst_gpio, XM2MSC_RESET_ASSERT);
 764         gpiod_set_value_cansleep(xm2msc->rst_gpio, XM2MSC_RESET_DEASSERT);
 765 }
 766
 767 /*
 768  * mem2mem callbacks
 769  */
 770 static int xm2msc_job_ready(void *priv)
 771 {
 772         struct xm2msc_chan_ctx *chan_ctx = priv;
 773
 774         if ((v4l2_m2m_num_src_bufs_ready(chan_ctx->m2m_ctx) > 0) &&
 775             (v4l2_m2m_num_dst_bufs_ready(chan_ctx->m2m_ctx) > 0))
 776                 return 1;
 777         return 0;
 778 }
 779
 780 static void xm2msc_chan_abort_bufs(struct xm2msc_chan_ctx *chan_ctx)
 781 {
 782         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 783         struct vb2_v4l2_buffer *dst_vb, *src_vb;
 784
 785         spin_lock(&xm2msc->lock);
 786         dev_dbg(xm2msc->dev, "aborting all buffers\n");
 787
 788         while (v4l2_m2m_num_src_bufs_ready(chan_ctx->m2m_ctx) > 0) {
 789                 src_vb = v4l2_m2m_src_buf_remove(chan_ctx->m2m_ctx);
 790                 v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_ERROR);
 791         }
 792
 793         while (v4l2_m2m_num_dst_bufs_ready(chan_ctx->m2m_ctx) > 0) {
 794                 dst_vb = v4l2_m2m_dst_buf_remove(chan_ctx->m2m_ctx);
 795                 v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_ERROR);
 796         }
 797
 798         v4l2_m2m_job_finish(chan_ctx->m2m_dev, chan_ctx->m2m_ctx);
 799         spin_unlock(&xm2msc->lock);
 800 }
 801
 802 static void xm2msc_job_abort(void *priv)
 803 {
 804         struct xm2msc_chan_ctx *chan_ctx = priv;
 805
 806         xm2msc_chan_abort_bufs(chan_ctx);
 807
 808         /*
 809          * Stream off the channel as job_abort may not always
 810          * be called after streamoff
 811          */
 812         xm2msc_set_chan_stream(chan_ctx, false, XM2MSC_CHAN_OUT);
 813         xm2msc_set_chan_stream(chan_ctx, false, XM2MSC_CHAN_CAP);
 814 }
 815
 816 static int xm2msc_set_bufaddr(struct xm2m_msc_dev *xm2msc)
 817 {
 818         unsigned int chan;
 819         struct xm2msc_chan_ctx *chan_ctx;
 820         struct vb2_v4l2_buffer *src_vb, *dst_vb;
 821         void __iomem *base;
 822         dma_addr_t src_luma, dst_luma;
 823         dma_addr_t src_croma, dst_croma;
 824
 825         for (chan = 0; chan < xm2msc->max_chan; chan++) {
 826                 chan_ctx = &xm2msc->xm2msc_chan[chan];
 827                 base = chan_ctx->regs;
 828
 829                 src_vb = v4l2_m2m_next_src_buf(chan_ctx->m2m_ctx);
 830                 dst_vb = v4l2_m2m_next_dst_buf(chan_ctx->m2m_ctx);
 831
 832                 if (!src_vb || !dst_vb) {
 833                         v4l2_err(&xm2msc->v4l2_dev, "buffer not found ");
 834                         v4l2_err(&xm2msc->v4l2_dev, "src_vb = 0x%p, dst_vb = 0x%p\n",
 835                                  src_vb, dst_vb);
 836                         return -EINVAL;
 837                 }
 838
 839                 src_luma = vb2_dma_contig_plane_dma_addr(&src_vb->vb2_buf, 0);
 840                 dst_luma = vb2_dma_contig_plane_dma_addr(&dst_vb->vb2_buf, 0);
 841
 842                 if (chan_ctx->q_data[XM2MSC_CHAN_OUT].nplanes == 2)
 843                         src_croma =
 844                             vb2_dma_contig_plane_dma_addr(&src_vb->vb2_buf, 1);
 845                 else
 846                         src_croma = 0;
 847
 848                 if (chan_ctx->q_data[XM2MSC_CHAN_CAP].nplanes == 2)
 849                         dst_croma =
 850                             vb2_dma_contig_plane_dma_addr(&dst_vb->vb2_buf, 1);
 851                 else
 852                         dst_croma = 0;
 853
 854                 if (xm2msc->dma_addr_size == 64 &&
 855                     sizeof(dma_addr_t) == sizeof(u64)) {
 856                         xm2msc_write64reg(base + XM2MSC_SRCIMGBUF0, src_luma);
 857                         xm2msc_write64reg(base + XM2MSC_SRCIMGBUF1, src_croma);
 858                         xm2msc_write64reg(base + XM2MSC_DSTIMGBUF0, dst_luma);
 859                         xm2msc_write64reg(base + XM2MSC_DSTIMGBUF1, dst_croma);
 860                 } else {
 861                         xm2msc_writereg(base + XM2MSC_SRCIMGBUF0, src_luma);
 862                         xm2msc_writereg(base + XM2MSC_SRCIMGBUF1, src_croma);
 863                         xm2msc_writereg(base + XM2MSC_DSTIMGBUF0, dst_luma);
 864                         xm2msc_writereg(base + XM2MSC_DSTIMGBUF1, dst_croma);
 865                 }
 866         }
 867         return 0;
 868 }
 869
 870 static void xm2msc_job_finish(struct xm2m_msc_dev *xm2msc)
 871 {
 872         unsigned int chan;
 873
 874         for (chan = 0; chan < xm2msc->max_chan; chan++) {
 875                 struct xm2msc_chan_ctx *chan_ctx;
 876
 877                 chan_ctx = &xm2msc->xm2msc_chan[chan];
 878                 v4l2_m2m_job_finish(chan_ctx->m2m_dev, chan_ctx->m2m_ctx);
 879         }
 880 }
 881
 882 static void xm2msc_job_done(struct xm2m_msc_dev *xm2msc)
 883 {
 884         unsigned int chan;
 885
 886         for (chan = 0; chan < xm2msc->max_chan; chan++) {
 887                 struct xm2msc_chan_ctx *chan_ctx;
 888                 struct vb2_v4l2_buffer *src_vb, *dst_vb;
 889                 unsigned long flags;
 890
 891                 chan_ctx = &xm2msc->xm2msc_chan[chan];
 892
 893                 src_vb = v4l2_m2m_src_buf_remove(chan_ctx->m2m_ctx);
 894                 dst_vb = v4l2_m2m_dst_buf_remove(chan_ctx->m2m_ctx);
 895
 896                 if (src_vb && dst_vb) {
 897                         dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 898                         dst_vb->timecode = src_vb->timecode;
 899                         dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 900                         dst_vb->flags |=
 901                             src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 902
 903                         spin_lock_irqsave(&xm2msc->lock, flags);
 904                         v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 905                         v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 906                         spin_unlock_irqrestore(&xm2msc->lock, flags);
 907                 }
 908         }
 909 }
 910
 911 static void xm2msc_device_run(void *priv)
 912 {
 913         struct xm2msc_chan_ctx *chan_ctx = priv;
 914         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
 915         void __iomem *base = xm2msc->regs;
 916         int ret;
 917
 918         /* TODO program to number of opened chan*/
 919         xm2msc_writereg(base + XM2MSC_NUM_OUTS, xm2msc->max_chan);
 920
 921         ret = xm2msc_set_bufaddr(xm2msc);
 922         if (ret) {
 923                 v4l2_err(&xm2msc->v4l2_dev, "Device can't be run\n");
 924                 return;
 925         }
 926
 927         xm2msc_writereg(base + XM2MSC_GIE, XM2MSC_GIE_EN);
 928         xm2msc_writereg(base + XM2MSC_IER, XM2MSC_ISR_DONE);
 929
 930         xm2msc_pr_status(xm2msc, __func__);
 931         xm2msc_pr_screg(xm2msc->dev, base);
 932         xm2msc_pr_allchanreg(xm2msc);
 933
 934         xm2msc_start(xm2msc);
 935 }
 936
 937 static irqreturn_t xm2msc_isr(int irq, void *data)
 938 {
 939         struct xm2m_msc_dev *xm2msc = (struct xm2m_msc_dev *)data;
 940         void __iomem *base = xm2msc->regs;
 941         u32 status;
 942
 943         status = xm2msc_readreg(base + XM2MSC_ISR);
 944         if (!(status & XM2MSC_ISR_DONE))
 945                 return IRQ_NONE;
 946
 947         xm2msc_writereg(base + XM2MSC_ISR, status & XM2MSC_ISR_DONE);
 948
 949         xm2msc_stop(xm2msc);
 950
 951         xm2msc_job_done(xm2msc);
 952
 953         if (xm2msc_job_ready(xm2msc->xm2msc_chan)) {
 954                 xm2msc_device_run(xm2msc->xm2msc_chan);
 955                 goto handled;
 956         }
 957
 958         xm2msc_job_finish(xm2msc);
 959 handled:
 960         return IRQ_HANDLED;
 961 }
 962
 963 static int xm2msc_streamon(struct file *file, void *fh,
 964                            enum v4l2_buf_type type)
 965 {
 966         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 967
 968         return v4l2_m2m_streamon(file, chan_ctx->m2m_ctx, type);
 969 }
 970
 971 static int xm2msc_streamoff(struct file *file, void *fh,
 972                             enum v4l2_buf_type type)
 973 {
 974         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 975
 976         return v4l2_m2m_streamoff(file, chan_ctx->m2m_ctx, type);
 977 }
 978
 979 static int xm2msc_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 980 {
 981         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 982
 983         return v4l2_m2m_qbuf(file, chan_ctx->m2m_ctx, buf);
 984 }
 985
 986 static int xm2msc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 987 {
 988         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 989
 990         return v4l2_m2m_dqbuf(file, chan_ctx->m2m_ctx, buf);
 991 }
 992
 993 static int xm2msc_expbuf(struct file *file, void *fh,
 994                          struct v4l2_exportbuffer *eb)
 995 {
 996         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
 997
 998         return v4l2_m2m_expbuf(file, chan_ctx->m2m_ctx, eb);
 999 }
1000
1001 static int xm2msc_createbufs(struct file *file, void *fh,
1002                              struct v4l2_create_buffers *cb)
1003 {
1004         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1005
1006         return v4l2_m2m_create_bufs(file, chan_ctx->m2m_ctx, cb);
1007 }
1008
1009 static int xm2msc_reqbufs(struct file *file, void *fh,
1010                           struct v4l2_requestbuffers *reqbufs)
1011 {
1012         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1013
1014         return v4l2_m2m_reqbufs(file, chan_ctx->m2m_ctx, reqbufs);
1015 }
1016
1017 static int xm2msc_querybuf(struct file *file, void *fh,
1018                            struct v4l2_buffer *buf)
1019 {
1020         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1021
1022         return v4l2_m2m_querybuf(file, chan_ctx->m2m_ctx, buf);
1023 }
1024
1025 static unsigned int
1026 xm2msc_cal_stride(unsigned int width, enum xm2msc_pix_fmt xfmt)
1027 {
1028         unsigned int stride;
1029
1030         /* Stride in Bytes = (Width × Bytes per Pixel); */
1031         /* TODO: The Width value must be a multiple of Pixels per Clock */
1032         switch (xfmt) {
1033         case XILINX_M2MSC_FMT_RGBX8:
1034         case XILINX_M2MSC_FMT_YUVX8:
1035         case XILINX_M2MSC_FMT_RGBX10:
1036         case XILINX_M2MSC_FMT_YUVX10:
1037         case XILINX_M2MSC_FMT_BGRX8:
1038                 stride = width * 4;
1039                 break;
1040         case XILINX_M2MSC_FMT_YUYV8:
1041         case XILINX_M2MSC_FMT_UYVY8:
1042                 stride = width * 2;
1043                 break;
1044         case XILINX_M2MSC_FMT_Y_UV8:
1045         case XILINX_M2MSC_FMT_Y_UV8_420:
1046         case XILINX_M2MSC_FMT_Y8:
1047                 stride = width * 1;
1048                 break;
1049         case XILINX_M2MSC_FMT_RGB8:
1050         case XILINX_M2MSC_FMT_YUV8:
1051         case XILINX_M2MSC_FMT_BGR8:
1052                 stride = width * 3;
1053                 break;
1054         case XILINX_M2MSC_FMT_Y_UV10:
1055         case XILINX_M2MSC_FMT_Y_UV10_420:
1056         case XILINX_M2MSC_FMT_Y10:
1057                 /* 4 bytes per 3 pixels */
1058                 stride = DIV_ROUND_UP(width * 4, 3);
1059                 break;
1060         default:
1061                 stride = 0;
1062         }
1063
1064         return stride;
1065 }
1066
1067 static int
1068 vidioc_try_fmt(struct xm2msc_chan_ctx *chan_ctx, struct v4l2_format *f)
1069 {
1070         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
1071         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
1072         struct xm2msc_q_data *q_data;
1073         struct vb2_queue *vq;
1074         int index;
1075
1076         if (pix->width < XM2MSC_MIN_WIDTH || pix->width > xm2msc->max_wd ||
1077             pix->height < XM2MSC_MIN_HEIGHT || pix->height > xm2msc->max_ht)
1078                 dev_dbg(xm2msc->dev,
1079                         "Wrong input parameters %d, wxh: %dx%d.\n",
1080                         f->type, f->fmt.pix.width, f->fmt.pix.height);
1081         /*
1082          * V4L2 specification suggests the driver corrects the
1083          * format struct if any of the dimensions is unsupported
1084          */
1085         if (pix->height < XM2MSC_MIN_HEIGHT)
1086                 pix->height = XM2MSC_MIN_HEIGHT;
1087         else if (pix->height > xm2msc->max_ht)
1088                 pix->height = xm2msc->max_ht;
1089
1090         if (pix->width < XM2MSC_MIN_WIDTH)
1091                 pix->width = XM2MSC_MIN_WIDTH;
1092         else if (pix->width > xm2msc->max_wd)
1093                 pix->width = xm2msc->max_wd;
1094
1095         vq = v4l2_m2m_get_vq(chan_ctx->m2m_ctx, f->type);
1096         if (!vq)
1097                 return -EINVAL;
1098
1099         q_data = get_q_data(chan_ctx, f->type);
1100         if (!q_data)
1101                 return -EINVAL;
1102
1103         if (vb2_is_busy(vq)) {
1104                 v4l2_err(&xm2msc->v4l2_dev,
1105                          "%s queue busy\n", __func__);
1106                 return -EBUSY;
1107         }
1108
1109         q_data->fmt = find_format(f);
1110         index = find_format_index(f);
1111         if (!q_data->fmt || index == ARRAY_SIZE(formats) ||
1112             !xm2msc_chk_fmt(xm2msc, index)) {
1113                 v4l2_err(&xm2msc->v4l2_dev,
1114                          "Couldn't set format type %d, wxh: %dx%d. ",
1115                          f->type, f->fmt.pix.width, f->fmt.pix.height);
1116                 v4l2_err(&xm2msc->v4l2_dev,
1117                          "fmt: %d, field: %d\n",
1118                          f->fmt.pix.pixelformat, f->fmt.pix.field);
1119                 return -EINVAL;
1120         }
1121
1122         return 0;
1123 }
1124
1125 static int
1126 vidioc_s_fmt(struct xm2msc_chan_ctx *chan_ctx, struct v4l2_format *f)
1127 {
1128         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
1129         struct xm2msc_q_data *q_data = get_q_data(chan_ctx, f->type);
1130         unsigned int i;
1131
1132         q_data = get_q_data(chan_ctx, f->type);
1133
1134         q_data->width = pix->width;
1135         q_data->height = pix->height;
1136         q_data->stride = xm2msc_cal_stride(pix->width,
1137                                            q_data->fmt->xm2msc_fmt);
1138         q_data->colorspace = pix->colorspace;
1139         q_data->field = pix->field;
1140         q_data->nplanes = q_data->fmt->num_planes;
1141
1142         for (i = 0; i < q_data->nplanes; i++) {
1143                 q_data->bytesperline[i] = q_data->stride;
1144                 pix->plane_fmt[i].bytesperline = q_data->bytesperline[i];
1145                 q_data->sizeimage[i] = q_data->stride * q_data->height;
1146                 pix->plane_fmt[i].sizeimage = q_data->sizeimage[i];
1147         }
1148
1149         xm2msc_pr_q(chan_ctx->xm2msc_dev->dev, q_data,
1150                     chan_ctx->num, f->type, __func__);
1151
1152         return 0;
1153 }
1154
1155 static int xm2msc_try_fmt_vid_out(struct file *file, void *fh,
1156                                   struct v4l2_format *f)
1157 {
1158         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1159
1160         return vidioc_try_fmt(chan_ctx, f);
1161 }
1162
1163 static int xm2msc_try_fmt_vid_cap(struct file *file, void *fh,
1164                                   struct v4l2_format *f)
1165 {
1166         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1167
1168         return vidioc_try_fmt(chan_ctx, f);
1169 }
1170
1171 static int xm2msc_s_fmt_vid_cap(struct file *file, void *fh,
1172                                 struct v4l2_format *f)
1173 {
1174         int ret;
1175         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1176
1177         ret = xm2msc_try_fmt_vid_cap(file, fh, f);
1178         if (ret)
1179                 return ret;
1180         return vidioc_s_fmt(chan_ctx, f);
1181 }
1182
1183 static int xm2msc_s_fmt_vid_out(struct file *file, void *fh,
1184                                 struct v4l2_format *f)
1185 {
1186         int ret;
1187         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1188
1189         ret = xm2msc_try_fmt_vid_out(file, fh, f);
1190         if (ret)
1191                 return ret;
1192
1193         return vidioc_s_fmt(chan_ctx, f);
1194 }
1195
1196 static int vidioc_g_fmt(struct xm2msc_chan_ctx *chan_ctx, struct v4l2_format *f)
1197 {
1198         struct vb2_queue *vq;
1199         struct xm2msc_q_data *q_data;
1200         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
1201         unsigned int i;
1202
1203         vq = v4l2_m2m_get_vq(chan_ctx->m2m_ctx, f->type);
1204         if (!vq)
1205                 return -EINVAL;
1206
1207         q_data = get_q_data(chan_ctx, f->type);
1208         if (!q_data)
1209                 return -EINVAL;
1210
1211         pix->width = q_data->width;
1212         pix->height = q_data->height;
1213         pix->field = V4L2_FIELD_NONE;
1214         pix->pixelformat = q_data->fmt->fourcc;
1215         pix->colorspace = q_data->colorspace;
1216         pix->num_planes = q_data->nplanes;
1217
1218         for (i = 0; i < pix->num_planes; i++) {
1219                 pix->plane_fmt[i].bytesperline = q_data->bytesperline[i];
1220                 pix->plane_fmt[i].sizeimage = q_data->sizeimage[i];
1221         }
1222
1223         return 0;
1224 }
1225
1226 static int xm2msc_g_fmt_vid_out(struct file *file, void *fh,
1227                                 struct v4l2_format *f)
1228 {
1229         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1230
1231         return vidioc_g_fmt(chan_ctx, f);
1232 }
1233
1234 static int xm2msc_g_fmt_vid_cap(struct file *file, void *fh,
1235                                 struct v4l2_format *f)
1236 {
1237         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1238
1239         return vidioc_g_fmt(chan_ctx, f);
1240 }
1241
1242 static int enum_fmt(struct xm2m_msc_dev *xm2msc, struct v4l2_fmtdesc *f)
1243 {
1244         const struct xm2msc_fmt *fmt;
1245         unsigned int i, enabled = 0;
1246
1247         for (i = 0; i < ARRAY_SIZE(formats); i++) {
1248                 if (xm2msc_chk_fmt(xm2msc, i) && enabled++ == f->index)
1249                         break;
1250         }
1251
1252         if (i == ARRAY_SIZE(formats))
1253                 /* Format not found */
1254                 return -EINVAL;
1255
1256         /* Format found */
1257         fmt = &formats[i];
1258         strlcpy(f->description, fmt->name,
1259                 sizeof(f->description));
1260         f->pixelformat = fmt->fourcc;
1261
1262         return 0;
1263 }
1264
1265 static int xm2msc_enum_fmt_vid_cap(struct file *file, void *fh,
1266                                    struct v4l2_fmtdesc *f)
1267 {
1268         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1269
1270         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1271                 return -EINVAL;
1272
1273         return enum_fmt(chan_ctx->xm2msc_dev, f);
1274 }
1275
1276 static int xm2msc_enum_fmt_vid_out(struct file *file, void *fh,
1277                                    struct v4l2_fmtdesc *f)
1278 {
1279         struct xm2msc_chan_ctx *chan_ctx = fh_to_chanctx(fh);
1280
1281         if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
1282                 return -EINVAL;
1283
1284         return enum_fmt(chan_ctx->xm2msc_dev, f);
1285 }
1286
1287 static int xm2msc_querycap(struct file *file, void *fh,
1288                            struct v4l2_capability *cap)
1289 {
1290         strncpy(cap->driver, XM2MSC_DRIVER_NAME, sizeof(cap->driver) - 1);
1291         strncpy(cap->card, XM2MSC_DRIVER_NAME, sizeof(cap->card) - 1);
1292         snprintf(cap->bus_info, sizeof(cap->bus_info),
1293                  "platform:%s", XM2MSC_DRIVER_NAME);
1294         /*
1295          * This is only a mem-to-mem video device. The STREAMING
1296          * device capability flags are left only for compatibility
1297          * and are scheduled for removal.
1298          */
1299         cap->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE;
1300         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
1301         return 0;
1302 }
1303
1304 static int xm2msc_queue_setup(struct vb2_queue *vq,
1305                               unsigned int *nbuffers, unsigned int *nplanes,
1306                               unsigned int sizes[], struct device *alloc_devs[])
1307 {
1308         unsigned int i;
1309         struct xm2msc_chan_ctx *chan_ctx = vb2_get_drv_priv(vq);
1310         struct xm2msc_q_data *q_data;
1311
1312         q_data = get_q_data(chan_ctx, vq->type);
1313         if (!q_data)
1314                 return -EINVAL;
1315
1316         *nplanes = q_data->nplanes;
1317
1318         for (i = 0; i < *nplanes; i++)
1319                 sizes[i] = q_data->sizeimage[i];
1320
1321         dev_dbg(chan_ctx->xm2msc_dev->dev, "get %d buffer(s) of size %d",
1322                 *nbuffers, sizes[0]);
1323         if (q_data->nplanes == 2)
1324                 dev_dbg(chan_ctx->xm2msc_dev->dev, " and %d\n", sizes[1]);
1325
1326         return 0;
1327 }
1328
1329 static int xm2msc_buf_prepare(struct vb2_buffer *vb)
1330 {
1331         struct xm2msc_chan_ctx *chan_ctx = vb2_get_drv_priv(vb->vb2_queue);
1332         struct xm2m_msc_dev *xm2msc = chan_ctx->xm2msc_dev;
1333         struct xm2msc_q_data *q_data;
1334         unsigned int i, num_planes;
1335
1336         q_data = get_q_data(chan_ctx, vb->vb2_queue->type);
1337         if (!q_data)
1338                 return -EINVAL;
1339         num_planes = q_data->nplanes;
1340
1341         for (i = 0; i < num_planes; i++) {
1342                 if (vb2_plane_size(vb, i) < q_data->sizeimage[i]) {
1343                         v4l2_err(&xm2msc->v4l2_dev, "data will not fit into plane ");
1344                                    v4l2_err(&xm2msc->v4l2_dev, "(%lu < %lu)\n",
1345                                             vb2_plane_size(vb, i),
1346                                             (long)q_data->sizeimage[i]);
1347                         return -EINVAL;
1348                 }
1349         }
1350
1351         for (i = 0; i < num_planes; i++)
1352                 vb2_set_plane_payload(vb, i, q_data->sizeimage[i]);
1353
1354         return 0;
1355 }
1356
1357 static void xm2msc_buf_queue(struct vb2_buffer *vb)
1358 {
1359         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1360         struct xm2msc_chan_ctx *chan_ctx = vb2_get_drv_priv(vb->vb2_queue);
1361
1362         v4l2_m2m_buf_queue(chan_ctx->m2m_ctx, vbuf);
1363 }
1364
1365 static void xm2msc_return_all_buffers(struct xm2msc_chan_ctx *chan_ctx,
1366                                       struct vb2_queue *q,
1367                                       enum vb2_buffer_state state)
1368 {
1369         struct vb2_v4l2_buffer *vb;
1370         unsigned long flags;
1371
1372         for (;;) {
1373                 if (V4L2_TYPE_IS_OUTPUT(q->type))
1374                         vb = v4l2_m2m_src_buf_remove(chan_ctx->m2m_ctx);
1375                 else
1376                         vb = v4l2_m2m_dst_buf_remove(chan_ctx->m2m_ctx);
1377                 if (!vb)
1378                         break;
1379                 spin_lock_irqsave(&chan_ctx->xm2msc_dev->lock, flags);
1380                 v4l2_m2m_buf_done(vb, state);
1381                 spin_unlock_irqrestore(&chan_ctx->xm2msc_dev->lock, flags);
1382         }
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");