drivers/media/usb/s2255/s2255drv.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *  s2255drv.c - a driver for the Sensoray 2255 USB video capture device
   4  *
   5  *   Copyright (C) 2007-2014 by Sensoray Company Inc.
   6  *                              Dean Anderson
   7  *
   8  * Some video buffer code based on vivi driver:
   9  *
  10  * Sensoray 2255 device supports 4 simultaneous channels.
  11  * The channels are not "crossbar" inputs, they are physically
  12  * attached to separate video decoders.
  13  *
  14  * Because of USB2.0 bandwidth limitations. There is only a
  15  * certain amount of data which may be transferred at one time.
  16  *
  17  * Example maximum bandwidth utilization:
  18  *
  19  * -full size, color mode YUYV or YUV422P: 2 channels at once
  20  * -full or half size Grey scale: all 4 channels at once
  21  * -half size, color mode YUYV or YUV422P: all 4 channels at once
  22  * -full size, color mode YUYV or YUV422P 1/2 frame rate: all 4 channels
  23  *  at once.
  24  */
  25
  26 #include <linux/module.h>
  27 #include <linux/firmware.h>
  28 #include <linux/kernel.h>
  29 #include <linux/mutex.h>
  30 #include <linux/slab.h>
  31 #include <linux/videodev2.h>
  32 #include <linux/mm.h>
  33 #include <linux/vmalloc.h>
  34 #include <linux/usb.h>
  35 #include <media/videobuf2-v4l2.h>
  36 #include <media/videobuf2-vmalloc.h>
  37 #include <media/v4l2-common.h>
  38 #include <media/v4l2-device.h>
  39 #include <media/v4l2-ioctl.h>
  40 #include <media/v4l2-ctrls.h>
  41 #include <media/v4l2-event.h>
  42
  43 #define S2255_VERSION           "1.25.1"
  44 #define FIRMWARE_FILE_NAME "/*(DEBLOBBED)*/"
  45
  46 /* default JPEG quality */
  47 #define S2255_DEF_JPEG_QUAL     50
  48 /* vendor request in */
  49 #define S2255_VR_IN             0
  50 /* vendor request out */
  51 #define S2255_VR_OUT            1
  52 /* firmware query */
  53 #define S2255_VR_FW             0x30
  54 /* USB endpoint number for configuring the device */
  55 #define S2255_CONFIG_EP         2
  56 /* maximum time for DSP to start responding after last FW word loaded(ms) */
  57 #define S2255_DSP_BOOTTIME      800
  58 /* maximum time to wait for firmware to load (ms) */
  59 #define S2255_LOAD_TIMEOUT      (5000 + S2255_DSP_BOOTTIME)
  60 #define S2255_MIN_BUFS          2
  61 #define S2255_SETMODE_TIMEOUT   500
  62 #define S2255_VIDSTATUS_TIMEOUT 350
  63 #define S2255_MARKER_FRAME      cpu_to_le32(0x2255DA4AL)
  64 #define S2255_MARKER_RESPONSE   cpu_to_le32(0x2255ACACL)
  65 #define S2255_RESPONSE_SETMODE  cpu_to_le32(0x01)
  66 #define S2255_RESPONSE_FW       cpu_to_le32(0x10)
  67 #define S2255_RESPONSE_STATUS   cpu_to_le32(0x20)
  68 #define S2255_USB_XFER_SIZE     (16 * 1024)
  69 #define MAX_CHANNELS            4
  70 #define SYS_FRAMES              4
  71 /* maximum size is PAL full size plus room for the marker header(s) */
  72 #define SYS_FRAMES_MAXSIZE      (720*288*2*2 + 4096)
  73 #define DEF_USB_BLOCK           S2255_USB_XFER_SIZE
  74 #define LINE_SZ_4CIFS_NTSC      640
  75 #define LINE_SZ_2CIFS_NTSC      640
  76 #define LINE_SZ_1CIFS_NTSC      320
  77 #define LINE_SZ_4CIFS_PAL       704
  78 #define LINE_SZ_2CIFS_PAL       704
  79 #define LINE_SZ_1CIFS_PAL       352
  80 #define NUM_LINES_4CIFS_NTSC    240
  81 #define NUM_LINES_2CIFS_NTSC    240
  82 #define NUM_LINES_1CIFS_NTSC    240
  83 #define NUM_LINES_4CIFS_PAL     288
  84 #define NUM_LINES_2CIFS_PAL     288
  85 #define NUM_LINES_1CIFS_PAL     288
  86 #define LINE_SZ_DEF             640
  87 #define NUM_LINES_DEF           240
  88
  89
  90 /* predefined settings */
  91 #define FORMAT_NTSC     1
  92 #define FORMAT_PAL      2
  93
  94 #define SCALE_4CIFS     1       /* 640x480(NTSC) or 704x576(PAL) */
  95 #define SCALE_2CIFS     2       /* 640x240(NTSC) or 704x288(PAL) */
  96 #define SCALE_1CIFS     3       /* 320x240(NTSC) or 352x288(PAL) */
  97 /* SCALE_4CIFSI is the 2 fields interpolated into one */
  98 #define SCALE_4CIFSI    4       /* 640x480(NTSC) or 704x576(PAL) high quality */
  99
 100 #define COLOR_YUVPL     1       /* YUV planar */
 101 #define COLOR_YUVPK     2       /* YUV packed */
 102 #define COLOR_Y8        4       /* monochrome */
 103 #define COLOR_JPG       5       /* JPEG */
 104
 105 #define MASK_COLOR       0x000000ff
 106 #define MASK_JPG_QUALITY 0x0000ff00
 107 #define MASK_INPUT_TYPE  0x000f0000
 108 /* frame decimation. */
 109 #define FDEC_1          1       /* capture every frame. default */
 110 #define FDEC_2          2       /* capture every 2nd frame */
 111 #define FDEC_3          3       /* capture every 3rd frame */
 112 #define FDEC_5          5       /* capture every 5th frame */
 113
 114 /*-------------------------------------------------------
 115  * Default mode parameters.
 116  *-------------------------------------------------------*/
 117 #define DEF_SCALE       SCALE_4CIFS
 118 #define DEF_COLOR       COLOR_YUVPL
 119 #define DEF_FDEC        FDEC_1
 120 #define DEF_BRIGHT      0
 121 #define DEF_CONTRAST    0x5c
 122 #define DEF_SATURATION  0x80
 123 #define DEF_HUE         0
 124
 125 /* usb config commands */
 126 #define IN_DATA_TOKEN   cpu_to_le32(0x2255c0de)
 127 #define CMD_2255        0xc2255000
 128 #define CMD_SET_MODE    cpu_to_le32((CMD_2255 | 0x10))
 129 #define CMD_START       cpu_to_le32((CMD_2255 | 0x20))
 130 #define CMD_STOP        cpu_to_le32((CMD_2255 | 0x30))
 131 #define CMD_STATUS      cpu_to_le32((CMD_2255 | 0x40))
 132
 133 struct s2255_mode {
 134         u32 format;     /* input video format (NTSC, PAL) */
 135         u32 scale;      /* output video scale */
 136         u32 color;      /* output video color format */
 137         u32 fdec;       /* frame decimation */
 138         u32 bright;     /* brightness */
 139         u32 contrast;   /* contrast */
 140         u32 saturation; /* saturation */
 141         u32 hue;        /* hue (NTSC only)*/
 142         u32 single;     /* capture 1 frame at a time (!=0), continuously (==0)*/
 143         u32 usb_block;  /* block size. should be 4096 of DEF_USB_BLOCK */
 144         u32 restart;    /* if DSP requires restart */
 145 };
 146
 147
 148 #define S2255_READ_IDLE         0
 149 #define S2255_READ_FRAME        1
 150
 151 /* frame structure */
 152 struct s2255_framei {
 153         unsigned long size;
 154         unsigned long ulState;  /* ulState:S2255_READ_IDLE, S2255_READ_FRAME*/
 155         void *lpvbits;          /* image data */
 156         unsigned long cur_size; /* current data copied to it */
 157 };
 158
 159 /* image buffer structure */
 160 struct s2255_bufferi {
 161         unsigned long dwFrames;                 /* number of frames in buffer */
 162         struct s2255_framei frame[SYS_FRAMES];  /* array of FRAME structures */
 163 };
 164
 165 #define DEF_MODEI_NTSC_CONT     {FORMAT_NTSC, DEF_SCALE, DEF_COLOR,     \
 166                         DEF_FDEC, DEF_BRIGHT, DEF_CONTRAST, DEF_SATURATION, \
 167                         DEF_HUE, 0, DEF_USB_BLOCK, 0}
 168
 169 /* for firmware loading, fw_state */
 170 #define S2255_FW_NOTLOADED      0
 171 #define S2255_FW_LOADED_DSPWAIT 1
 172 #define S2255_FW_SUCCESS        2
 173 #define S2255_FW_FAILED         3
 174 #define S2255_FW_DISCONNECTING  4
 175 #define S2255_FW_MARKER         cpu_to_le32(0x22552f2f)
 176 /* 2255 read states */
 177 #define S2255_READ_IDLE         0
 178 #define S2255_READ_FRAME        1
 179 struct s2255_fw {
 180         int                   fw_loaded;
 181         int                   fw_size;
 182         struct urb            *fw_urb;
 183         atomic_t              fw_state;
 184         void                  *pfw_data;
 185         wait_queue_head_t     wait_fw;
 186         const struct firmware *fw;
 187 };
 188
 189 struct s2255_pipeinfo {
 190         u32 max_transfer_size;
 191         u32 cur_transfer_size;
 192         u8 *transfer_buffer;
 193         u32 state;
 194         void *stream_urb;
 195         void *dev;      /* back pointer to s2255_dev struct*/
 196         u32 err_count;
 197         u32 idx;
 198 };
 199
 200 struct s2255_fmt; /*forward declaration */
 201 struct s2255_dev;
 202
 203 /* 2255 video channel */
 204 struct s2255_vc {
 205         struct s2255_dev        *dev;
 206         struct video_device     vdev;
 207         struct v4l2_ctrl_handler hdl;
 208         struct v4l2_ctrl        *jpegqual_ctrl;
 209         int                     resources;
 210         struct list_head        buf_list;
 211         struct s2255_bufferi    buffer;
 212         struct s2255_mode       mode;
 213         v4l2_std_id             std;
 214         /* jpeg compression */
 215         unsigned                jpegqual;
 216         /* capture parameters (for high quality mode full size) */
 217         struct v4l2_captureparm cap_parm;
 218         int                     cur_frame;
 219         int                     last_frame;
 220         /* allocated image size */
 221         unsigned long           req_image_size;
 222         /* received packet size */
 223         unsigned long           pkt_size;
 224         int                     bad_payload;
 225         unsigned long           frame_count;
 226         /* if JPEG image */
 227         int                     jpg_size;
 228         /* if channel configured to default state */
 229         int                     configured;
 230         wait_queue_head_t       wait_setmode;
 231         int                     setmode_ready;
 232         /* video status items */
 233         int                     vidstatus;
 234         wait_queue_head_t       wait_vidstatus;
 235         int                     vidstatus_ready;
 236         unsigned int            width;
 237         unsigned int            height;
 238         enum v4l2_field         field;
 239         const struct s2255_fmt  *fmt;
 240         int idx; /* channel number on device, 0-3 */
 241         struct vb2_queue vb_vidq;
 242         struct mutex vb_lock; /* streaming lock */
 243         spinlock_t qlock;
 244 };
 245
 246
 247 struct s2255_dev {
 248         struct s2255_vc         vc[MAX_CHANNELS];
 249         struct v4l2_device      v4l2_dev;
 250         atomic_t                num_channels;
 251         int                     frames;
 252         struct mutex            lock;   /* channels[].vdev.lock */
 253         struct mutex            cmdlock; /* protects cmdbuf */
 254         struct usb_device       *udev;
 255         struct usb_interface    *interface;
 256         u8                      read_endpoint;
 257         struct timer_list       timer;
 258         struct s2255_fw *fw_data;
 259         struct s2255_pipeinfo   pipe;
 260         u32                     cc;     /* current channel */
 261         int                     frame_ready;
 262         int                     chn_ready;
 263         /*(DEBLOBBED)*/
 264         int                     dsp_fw_ver;
 265         u16                     pid; /* product id */
 266 #define S2255_CMDBUF_SIZE 512
 267         __le32                  *cmdbuf;
 268 };
 269
 270 static inline struct s2255_dev *to_s2255_dev(struct v4l2_device *v4l2_dev)
 271 {
 272         return container_of(v4l2_dev, struct s2255_dev, v4l2_dev);
 273 }
 274
 275 struct s2255_fmt {
 276         char *name;
 277         u32 fourcc;
 278         int depth;
 279 };
 280
 281 /* buffer for one video frame */
 282 struct s2255_buffer {
 283         /* common v4l buffer stuff -- must be first */
 284         struct vb2_v4l2_buffer vb;
 285         struct list_head list;
 286 };
 287
 288
 289 /* current cypress EEPROM firmware version */
 290 #define S2255_CUR_USB_FWVER     ((3 << 8) | 12)
 291 /* current DSP FW version */
 292 #define S2255_CUR_DSP_FWVER     10104
 293 /* Need DSP version 5+ for video status feature */
 294 #define S2255_MIN_DSP_STATUS      5
 295 #define S2255_MIN_DSP_COLORFILTER 8
 296 #define S2255_NORMS             (V4L2_STD_ALL)
 297
 298 /* private V4L2 controls */
 299
 300 /*
 301  * The following chart displays how COLORFILTER should be set
 302  *  =========================================================
 303  *  =     fourcc              =     COLORFILTER             =
 304  *  =                         ===============================
 305  *  =                         =   0             =    1      =
 306  *  =========================================================
 307  *  =  V4L2_PIX_FMT_GREY(Y8)  = monochrome from = monochrome=
 308  *  =                         = s-video or      = composite =
 309  *  =                         = B/W camera      = input     =
 310  *  =========================================================
 311  *  =    other                = color, svideo   = color,    =
 312  *  =                         =                 = composite =
 313  *  =========================================================
 314  *
 315  * Notes:
 316  *   channels 0-3 on 2255 are composite
 317  *   channels 0-1 on 2257 are composite, 2-3 are s-video
 318  * If COLORFILTER is 0 with a composite color camera connected,
 319  * the output will appear monochrome but hatching
 320  * will occur.
 321  * COLORFILTER is different from "color killer" and "color effects"
 322  * for reasons above.
 323  */
 324 #define S2255_V4L2_YC_ON  1
 325 #define S2255_V4L2_YC_OFF 0
 326 #define V4L2_CID_S2255_COLORFILTER (V4L2_CID_USER_S2255_BASE + 0)
 327
 328 /* frame prefix size (sent once every frame) */
 329 #define PREFIX_SIZE             512
 330
 331 /* Channels on box are in reverse order */
 332 static unsigned long G_chnmap[MAX_CHANNELS] = {3, 2, 1, 0};
 333
 334 static int debug;
 335
 336 static int s2255_start_readpipe(struct s2255_dev *dev);
 337 static void s2255_stop_readpipe(struct s2255_dev *dev);
 338 static int s2255_start_acquire(struct s2255_vc *vc);
 339 static int s2255_stop_acquire(struct s2255_vc *vc);
 340 static void s2255_fillbuff(struct s2255_vc *vc, struct s2255_buffer *buf,
 341                            int jpgsize);
 342 static int s2255_set_mode(struct s2255_vc *vc, struct s2255_mode *mode);
 343 static int s2255_board_shutdown(struct s2255_dev *dev);
 344 static void s2255_fwload_start(struct s2255_dev *dev);
 345 static void s2255_destroy(struct s2255_dev *dev);
 346 static long s2255_vendor_req(struct s2255_dev *dev, unsigned char req,
 347                              u16 index, u16 value, void *buf,
 348                              s32 buf_len, int bOut);
 349
 350 /* dev_err macro with driver name */
 351 #define S2255_DRIVER_NAME "s2255"
 352 #define s2255_dev_err(dev, fmt, arg...)                                 \
 353                 dev_err(dev, S2255_DRIVER_NAME " - " fmt, ##arg)
 354
 355 #define dprintk(dev, level, fmt, arg...) \
 356         v4l2_dbg(level, debug, &dev->v4l2_dev, fmt, ## arg)
 357
 358 static struct usb_driver s2255_driver;
 359
 360 /* start video number */
 361 static int video_nr = -1;       /* /dev/videoN, -1 for autodetect */
 362
 363 /* Enable jpeg capture. */
 364 static int jpeg_enable = 1;
 365
 366 module_param(debug, int, 0644);
 367 MODULE_PARM_DESC(debug, "Debug level(0-100) default 0");
 368 module_param(video_nr, int, 0644);
 369 MODULE_PARM_DESC(video_nr, "start video minor(-1 default autodetect)");
 370 module_param(jpeg_enable, int, 0644);
 371 MODULE_PARM_DESC(jpeg_enable, "Jpeg enable(1-on 0-off) default 1");
 372
 373 /* USB device table */
 374 #define USB_SENSORAY_VID        0x1943
 375 static const struct usb_device_id s2255_table[] = {
 376         {USB_DEVICE(USB_SENSORAY_VID, 0x2255)},
 377         {USB_DEVICE(USB_SENSORAY_VID, 0x2257)}, /*same family as 2255*/
 378         { }                     /* Terminating entry */
 379 };
 380 MODULE_DEVICE_TABLE(usb, s2255_table);
 381
 382 #define BUFFER_TIMEOUT msecs_to_jiffies(400)
 383
 384 /* image formats.  */
 385 /* JPEG formats must be defined last to support jpeg_enable parameter */
 386 static const struct s2255_fmt formats[] = {
 387         {
 388                 .name = "4:2:2, packed, YUYV",
 389                 .fourcc = V4L2_PIX_FMT_YUYV,
 390                 .depth = 16
 391
 392         }, {
 393                 .name = "4:2:2, packed, UYVY",
 394                 .fourcc = V4L2_PIX_FMT_UYVY,
 395                 .depth = 16
 396         }, {
 397                 .name = "4:2:2, planar, YUV422P",
 398                 .fourcc = V4L2_PIX_FMT_YUV422P,
 399                 .depth = 16
 400
 401         }, {
 402                 .name = "8bpp GREY",
 403                 .fourcc = V4L2_PIX_FMT_GREY,
 404                 .depth = 8
 405         }, {
 406                 .name = "JPG",
 407                 .fourcc = V4L2_PIX_FMT_JPEG,
 408                 .depth = 24
 409         }, {
 410                 .name = "MJPG",
 411                 .fourcc = V4L2_PIX_FMT_MJPEG,
 412                 .depth = 24
 413         }
 414 };
 415
 416 static int norm_maxw(struct s2255_vc *vc)
 417 {
 418         return (vc->std & V4L2_STD_525_60) ?
 419             LINE_SZ_4CIFS_NTSC : LINE_SZ_4CIFS_PAL;
 420 }
 421
 422 static int norm_maxh(struct s2255_vc *vc)
 423 {
 424         return (vc->std & V4L2_STD_525_60) ?
 425             (NUM_LINES_1CIFS_NTSC * 2) : (NUM_LINES_1CIFS_PAL * 2);
 426 }
 427
 428 static int norm_minw(struct s2255_vc *vc)
 429 {
 430         return (vc->std & V4L2_STD_525_60) ?
 431             LINE_SZ_1CIFS_NTSC : LINE_SZ_1CIFS_PAL;
 432 }
 433
 434 static int norm_minh(struct s2255_vc *vc)
 435 {
 436         return (vc->std & V4L2_STD_525_60) ?
 437             (NUM_LINES_1CIFS_NTSC) : (NUM_LINES_1CIFS_PAL);
 438 }
 439
 440
 441 /*
 442  * TODO: fixme: move YUV reordering to hardware
 443  * converts 2255 planar format to yuyv or uyvy
 444  */
 445 static void planar422p_to_yuv_packed(const unsigned char *in,
 446                                      unsigned char *out,
 447                                      int width, int height,
 448                                      int fmt)
 449 {
 450         unsigned char *pY;
 451         unsigned char *pCb;
 452         unsigned char *pCr;
 453         unsigned long size = height * width;
 454         unsigned int i;
 455         pY = (unsigned char *)in;
 456         pCr = (unsigned char *)in + height * width;
 457         pCb = (unsigned char *)in + height * width + (height * width / 2);
 458         for (i = 0; i < size * 2; i += 4) {
 459                 out[i] = (fmt == V4L2_PIX_FMT_YUYV) ? *pY++ : *pCr++;
 460                 out[i + 1] = (fmt == V4L2_PIX_FMT_YUYV) ? *pCr++ : *pY++;
 461                 out[i + 2] = (fmt == V4L2_PIX_FMT_YUYV) ? *pY++ : *pCb++;
 462                 out[i + 3] = (fmt == V4L2_PIX_FMT_YUYV) ? *pCb++ : *pY++;
 463         }
 464         return;
 465 }
 466
 467 static void s2255_reset_dsppower(struct s2255_dev *dev)
 468 {
 469         s2255_vendor_req(dev, 0x40, 0x0000, 0x0001, NULL, 0, 1);
 470         msleep(50);
 471         s2255_vendor_req(dev, 0x50, 0x0000, 0x0000, NULL, 0, 1);
 472         msleep(600);
 473         s2255_vendor_req(dev, 0x10, 0x0000, 0x0000, NULL, 0, 1);
 474         return;
 475 }
 476
 477 /* kickstarts the firmware loading. from probe
 478  */
 479 static void s2255_timer(struct timer_list *t)
 480 {
 481         struct s2255_dev *dev = from_timer(dev, t, timer);
 482         struct s2255_fw *data = dev->fw_data;
 483         if (usb_submit_urb(data->fw_urb, GFP_ATOMIC) < 0) {
 484                 pr_err("s2255: can't submit urb\n");
 485                 atomic_set(&data->fw_state, S2255_FW_FAILED);
 486                 /* wake up anything waiting for the firmware */
 487                 wake_up(&data->wait_fw);
 488                 return;
 489         }
 490 }
 491
 492
 493 /* this loads the firmware asynchronously.
 494    Originally this was done synchronously in probe.
 495    But it is better to load it asynchronously here than block
 496    inside the probe function. Blocking inside probe affects boot time.
 497    FW loading is triggered by the timer in the probe function
 498 */
 499 static void s2255_fwchunk_complete(struct urb *urb)
 500 {
 501         struct s2255_fw *data = urb->context;
 502         struct usb_device *udev = urb->dev;
 503         int len;
 504         if (urb->status) {
 505                 dev_err(&udev->dev, "URB failed with status %d\n", urb->status);
 506                 atomic_set(&data->fw_state, S2255_FW_FAILED);
 507                 /* wake up anything waiting for the firmware */
 508                 wake_up(&data->wait_fw);
 509                 return;
 510         }
 511         if (data->fw_urb == NULL) {
 512                 s2255_dev_err(&udev->dev, "disconnected\n");
 513                 atomic_set(&data->fw_state, S2255_FW_FAILED);
 514                 /* wake up anything waiting for the firmware */
 515                 wake_up(&data->wait_fw);
 516                 return;
 517         }
 518 #define CHUNK_SIZE 512
 519         /* all USB transfers must be done with continuous kernel memory.
 520            can't allocate more than 128k in current linux kernel, so
 521            upload the firmware in chunks
 522          */
 523         if (data->fw_loaded < data->fw_size) {
 524                 len = (data->fw_loaded + CHUNK_SIZE) > data->fw_size ?
 525                     data->fw_size % CHUNK_SIZE : CHUNK_SIZE;
 526
 527                 if (len < CHUNK_SIZE)
 528                         memset(data->pfw_data, 0, CHUNK_SIZE);
 529
 530                 memcpy(data->pfw_data,
 531                        (char *) data->fw->data + data->fw_loaded, len);
 532
 533                 usb_fill_bulk_urb(data->fw_urb, udev, usb_sndbulkpipe(udev, 2),
 534                                   data->pfw_data, CHUNK_SIZE,
 535                                   s2255_fwchunk_complete, data);
 536                 if (usb_submit_urb(data->fw_urb, GFP_ATOMIC) < 0) {
 537                         dev_err(&udev->dev, "failed submit URB\n");
 538                         atomic_set(&data->fw_state, S2255_FW_FAILED);
 539                         /* wake up anything waiting for the firmware */
 540                         wake_up(&data->wait_fw);
 541                         return;
 542                 }
 543                 data->fw_loaded += len;
 544         } else
 545                 atomic_set(&data->fw_state, S2255_FW_LOADED_DSPWAIT);
 546         return;
 547
 548 }
 549
 550 static void s2255_got_frame(struct s2255_vc *vc, int jpgsize)
 551 {
 552         struct s2255_buffer *buf;
 553         struct s2255_dev *dev = to_s2255_dev(vc->vdev.v4l2_dev);
 554         unsigned long flags = 0;
 555
 556         spin_lock_irqsave(&vc->qlock, flags);
 557         if (list_empty(&vc->buf_list)) {
 558                 dprintk(dev, 1, "No active queue to serve\n");
 559                 spin_unlock_irqrestore(&vc->qlock, flags);
 560                 return;
 561         }
 562         buf = list_entry(vc->buf_list.next,
 563                          struct s2255_buffer, list);
 564         list_del(&buf->list);
 565         buf->vb.vb2_buf.timestamp = ktime_get_ns();
 566         buf->vb.field = vc->field;
 567         buf->vb.sequence = vc->frame_count;
 568         spin_unlock_irqrestore(&vc->qlock, flags);
 569
 570         s2255_fillbuff(vc, buf, jpgsize);
 571         /* tell v4l buffer was filled */
 572         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 573         dprintk(dev, 2, "%s: [buf] [%p]\n", __func__, buf);
 574 }
 575
 576 static const struct s2255_fmt *format_by_fourcc(int fourcc)
 577 {
 578         unsigned int i;
 579         for (i = 0; i < ARRAY_SIZE(formats); i++) {
 580                 if (-1 == formats[i].fourcc)
 581                         continue;
 582                 if (!jpeg_enable && ((formats[i].fourcc == V4L2_PIX_FMT_JPEG) ||
 583                                      (formats[i].fourcc == V4L2_PIX_FMT_MJPEG)))
 584                         continue;
 585                 if (formats[i].fourcc == fourcc)
 586                         return formats + i;
 587         }
 588         return NULL;
 589 }
 590
 591 /* video buffer vmalloc implementation based partly on VIVI driver which is
 592  *          Copyright (c) 2006 by
 593  *                  Mauro Carvalho Chehab <mchehab--a.t--infradead.org>
 594  *                  Ted Walther <ted--a.t--enumera.com>
 595  *                  John Sokol <sokol--a.t--videotechnology.com>
 596  *                  http://v4l.videotechnology.com/
 597  *
 598  */
 599 static void s2255_fillbuff(struct s2255_vc *vc,
 600                            struct s2255_buffer *buf, int jpgsize)
 601 {
 602         int pos = 0;
 603         const char *tmpbuf;
 604         char *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
 605         unsigned long last_frame;
 606         struct s2255_dev *dev = vc->dev;
 607
 608         if (!vbuf)
 609                 return;
 610         last_frame = vc->last_frame;
 611         if (last_frame != -1) {
 612                 tmpbuf =
 613                     (const char *)vc->buffer.frame[last_frame].lpvbits;
 614                 switch (vc->fmt->fourcc) {
 615                 case V4L2_PIX_FMT_YUYV:
 616                 case V4L2_PIX_FMT_UYVY:
 617                         planar422p_to_yuv_packed((const unsigned char *)tmpbuf,
 618                                                  vbuf, vc->width,
 619                                                  vc->height,
 620                                                  vc->fmt->fourcc);
 621                         break;
 622                 case V4L2_PIX_FMT_GREY:
 623                         memcpy(vbuf, tmpbuf, vc->width * vc->height);
 624                         break;
 625                 case V4L2_PIX_FMT_JPEG:
 626                 case V4L2_PIX_FMT_MJPEG:
 627                         vb2_set_plane_payload(&buf->vb.vb2_buf, 0, jpgsize);
 628                         memcpy(vbuf, tmpbuf, jpgsize);
 629                         break;
 630                 case V4L2_PIX_FMT_YUV422P:
 631                         memcpy(vbuf, tmpbuf,
 632                                vc->width * vc->height * 2);
 633                         break;
 634                 default:
 635                         pr_info("s2255: unknown format?\n");
 636                 }
 637                 vc->last_frame = -1;
 638         } else {
 639                 pr_err("s2255: =======no frame\n");
 640                 return;
 641         }
 642         dprintk(dev, 2, "s2255fill at : Buffer %p size= %d\n",
 643                 vbuf, pos);
 644 }
 645
 646
 647 /* ------------------------------------------------------------------
 648    Videobuf operations
 649    ------------------------------------------------------------------*/
 650
 651 static int queue_setup(struct vb2_queue *vq,
 652                        unsigned int *nbuffers, unsigned int *nplanes,
 653                        unsigned int sizes[], struct device *alloc_devs[])
 654 {
 655         struct s2255_vc *vc = vb2_get_drv_priv(vq);
 656         if (*nbuffers < S2255_MIN_BUFS)
 657                 *nbuffers = S2255_MIN_BUFS;
 658         *nplanes = 1;
 659         sizes[0] = vc->width * vc->height * (vc->fmt->depth >> 3);
 660         return 0;
 661 }
 662
 663 static int buffer_prepare(struct vb2_buffer *vb)
 664 {
 665         struct s2255_vc *vc = vb2_get_drv_priv(vb->vb2_queue);
 666         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 667         struct s2255_buffer *buf = container_of(vbuf, struct s2255_buffer, vb);
 668         int w = vc->width;
 669         int h = vc->height;
 670         unsigned long size;
 671
 672         dprintk(vc->dev, 4, "%s\n", __func__);
 673         if (vc->fmt == NULL)
 674                 return -EINVAL;
 675
 676         if ((w < norm_minw(vc)) ||
 677             (w > norm_maxw(vc)) ||
 678             (h < norm_minh(vc)) ||
 679             (h > norm_maxh(vc))) {
 680                 dprintk(vc->dev, 4, "invalid buffer prepare\n");
 681                 return -EINVAL;
 682         }
 683         size = w * h * (vc->fmt->depth >> 3);
 684         if (vb2_plane_size(vb, 0) < size) {
 685                 dprintk(vc->dev, 4, "invalid buffer prepare\n");
 686                 return -EINVAL;
 687         }
 688
 689         vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
 690         return 0;
 691 }
 692
 693 static void buffer_queue(struct vb2_buffer *vb)
 694 {
 695         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 696         struct s2255_buffer *buf = container_of(vbuf, struct s2255_buffer, vb);
 697         struct s2255_vc *vc = vb2_get_drv_priv(vb->vb2_queue);
 698         unsigned long flags = 0;
 699         dprintk(vc->dev, 1, "%s\n", __func__);
 700         spin_lock_irqsave(&vc->qlock, flags);
 701         list_add_tail(&buf->list, &vc->buf_list);
 702         spin_unlock_irqrestore(&vc->qlock, flags);
 703 }
 704
 705 static int start_streaming(struct vb2_queue *vq, unsigned int count);
 706 static void stop_streaming(struct vb2_queue *vq);
 707
 708 static const struct vb2_ops s2255_video_qops = {
 709         .queue_setup = queue_setup,
 710         .buf_prepare = buffer_prepare,
 711         .buf_queue = buffer_queue,
 712         .start_streaming = start_streaming,
 713         .stop_streaming = stop_streaming,
 714         .wait_prepare = vb2_ops_wait_prepare,
 715         .wait_finish = vb2_ops_wait_finish,
 716 };
 717
 718 static int vidioc_querycap(struct file *file, void *priv,
 719                            struct v4l2_capability *cap)
 720 {
 721         struct s2255_vc *vc = video_drvdata(file);
 722         struct s2255_dev *dev = vc->dev;
 723
 724         strscpy(cap->driver, "s2255", sizeof(cap->driver));
 725         strscpy(cap->card, "s2255", sizeof(cap->card));
 726         usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 727         return 0;
 728 }
 729
 730 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 731                                struct v4l2_fmtdesc *f)
 732 {
 733         int index = f->index;
 734
 735         if (index >= ARRAY_SIZE(formats))
 736                 return -EINVAL;
 737         if (!jpeg_enable && ((formats[index].fourcc == V4L2_PIX_FMT_JPEG) ||
 738                         (formats[index].fourcc == V4L2_PIX_FMT_MJPEG)))
 739                 return -EINVAL;
 740         strscpy(f->description, formats[index].name, sizeof(f->description));
 741         f->pixelformat = formats[index].fourcc;
 742         return 0;
 743 }
 744
 745 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 746                             struct v4l2_format *f)
 747 {
 748         struct s2255_vc *vc = video_drvdata(file);
 749         int is_ntsc = vc->std & V4L2_STD_525_60;
 750
 751         f->fmt.pix.width = vc->width;
 752         f->fmt.pix.height = vc->height;
 753         if (f->fmt.pix.height >=
 754             (is_ntsc ? NUM_LINES_1CIFS_NTSC : NUM_LINES_1CIFS_PAL) * 2)
 755                 f->fmt.pix.field = V4L2_FIELD_INTERLACED;
 756         else
 757                 f->fmt.pix.field = V4L2_FIELD_TOP;
 758         f->fmt.pix.pixelformat = vc->fmt->fourcc;
 759         f->fmt.pix.bytesperline = f->fmt.pix.width * (vc->fmt->depth >> 3);
 760         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 761         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 762         f->fmt.pix.priv = 0;
 763         return 0;
 764 }
 765
 766 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 767                               struct v4l2_format *f)
 768 {
 769         const struct s2255_fmt *fmt;
 770         enum v4l2_field field;
 771         struct s2255_vc *vc = video_drvdata(file);
 772         int is_ntsc = vc->std & V4L2_STD_525_60;
 773
 774         fmt = format_by_fourcc(f->fmt.pix.pixelformat);
 775
 776         if (fmt == NULL)
 777                 return -EINVAL;
 778
 779         field = f->fmt.pix.field;
 780
 781         dprintk(vc->dev, 50, "%s NTSC: %d suggested width: %d, height: %d\n",
 782                 __func__, is_ntsc, f->fmt.pix.width, f->fmt.pix.height);
 783         if (is_ntsc) {
 784                 /* NTSC */
 785                 if (f->fmt.pix.height >= NUM_LINES_1CIFS_NTSC * 2) {
 786                         f->fmt.pix.height = NUM_LINES_1CIFS_NTSC * 2;
 787                         field = V4L2_FIELD_INTERLACED;
 788                 } else {
 789                         f->fmt.pix.height = NUM_LINES_1CIFS_NTSC;
 790                         field = V4L2_FIELD_TOP;
 791                 }
 792                 if (f->fmt.pix.width >= LINE_SZ_4CIFS_NTSC)
 793                         f->fmt.pix.width = LINE_SZ_4CIFS_NTSC;
 794                 else
 795                         f->fmt.pix.width = LINE_SZ_1CIFS_NTSC;
 796         } else {
 797                 /* PAL */
 798                 if (f->fmt.pix.height >= NUM_LINES_1CIFS_PAL * 2) {
 799                         f->fmt.pix.height = NUM_LINES_1CIFS_PAL * 2;
 800                         field = V4L2_FIELD_INTERLACED;
 801                 } else {
 802                         f->fmt.pix.height = NUM_LINES_1CIFS_PAL;
 803                         field = V4L2_FIELD_TOP;
 804                 }
 805                 if (f->fmt.pix.width >= LINE_SZ_4CIFS_PAL)
 806                         f->fmt.pix.width = LINE_SZ_4CIFS_PAL;
 807                 else
 808                         f->fmt.pix.width = LINE_SZ_1CIFS_PAL;
 809         }
 810         f->fmt.pix.field = field;
 811         f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
 812         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 813         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 814         f->fmt.pix.priv = 0;
 815         dprintk(vc->dev, 50, "%s: set width %d height %d field %d\n", __func__,
 816                 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.field);
 817         return 0;
 818 }
 819
 820 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 821                             struct v4l2_format *f)
 822 {
 823         struct s2255_vc *vc = video_drvdata(file);
 824         const struct s2255_fmt *fmt;
 825         struct vb2_queue *q = &vc->vb_vidq;
 826         struct s2255_mode mode;
 827         int ret;
 828
 829         ret = vidioc_try_fmt_vid_cap(file, vc, f);
 830
 831         if (ret < 0)
 832                 return ret;
 833
 834         fmt = format_by_fourcc(f->fmt.pix.pixelformat);
 835
 836         if (fmt == NULL)
 837                 return -EINVAL;
 838
 839         if (vb2_is_busy(q)) {
 840                 dprintk(vc->dev, 1, "queue busy\n");
 841                 return -EBUSY;
 842         }
 843
 844         mode = vc->mode;
 845         vc->fmt = fmt;
 846         vc->width = f->fmt.pix.width;
 847         vc->height = f->fmt.pix.height;
 848         vc->field = f->fmt.pix.field;
 849         if (vc->width > norm_minw(vc)) {
 850                 if (vc->height > norm_minh(vc)) {
 851                         if (vc->cap_parm.capturemode &
 852                             V4L2_MODE_HIGHQUALITY)
 853                                 mode.scale = SCALE_4CIFSI;
 854                         else
 855                                 mode.scale = SCALE_4CIFS;
 856                 } else
 857                         mode.scale = SCALE_2CIFS;
 858
 859         } else {
 860                 mode.scale = SCALE_1CIFS;
 861         }
 862         /* color mode */
 863         switch (vc->fmt->fourcc) {
 864         case V4L2_PIX_FMT_GREY:
 865                 mode.color &= ~MASK_COLOR;
 866                 mode.color |= COLOR_Y8;
 867                 break;
 868         case V4L2_PIX_FMT_JPEG:
 869         case V4L2_PIX_FMT_MJPEG:
 870                 mode.color &= ~MASK_COLOR;
 871                 mode.color |= COLOR_JPG;
 872                 mode.color |= (vc->jpegqual << 8);
 873                 break;
 874         case V4L2_PIX_FMT_YUV422P:
 875                 mode.color &= ~MASK_COLOR;
 876                 mode.color |= COLOR_YUVPL;
 877                 break;
 878         case V4L2_PIX_FMT_YUYV:
 879         case V4L2_PIX_FMT_UYVY:
 880         default:
 881                 mode.color &= ~MASK_COLOR;
 882                 mode.color |= COLOR_YUVPK;
 883                 break;
 884         }
 885         if ((mode.color & MASK_COLOR) != (vc->mode.color & MASK_COLOR))
 886                 mode.restart = 1;
 887         else if (mode.scale != vc->mode.scale)
 888                 mode.restart = 1;
 889         else if (mode.format != vc->mode.format)
 890                 mode.restart = 1;
 891         vc->mode = mode;
 892         (void) s2255_set_mode(vc, &mode);
 893         return 0;
 894 }
 895
 896
 897 /* write to the configuration pipe, synchronously */
 898 static int s2255_write_config(struct usb_device *udev, unsigned char *pbuf,
 899                               int size)
 900 {
 901         int pipe;
 902         int done;
 903         long retval = -1;
 904         if (udev) {
 905                 pipe = usb_sndbulkpipe(udev, S2255_CONFIG_EP);
 906                 retval = usb_bulk_msg(udev, pipe, pbuf, size, &done, 500);
 907         }
 908         return retval;
 909 }
 910
 911 static u32 get_transfer_size(struct s2255_mode *mode)
 912 {
 913         int linesPerFrame = LINE_SZ_DEF;
 914         int pixelsPerLine = NUM_LINES_DEF;
 915         u32 outImageSize;
 916         u32 usbInSize;
 917         unsigned int mask_mult;
 918
 919         if (mode == NULL)
 920                 return 0;
 921
 922         if (mode->format == FORMAT_NTSC) {
 923                 switch (mode->scale) {
 924                 case SCALE_4CIFS:
 925                 case SCALE_4CIFSI:
 926                         linesPerFrame = NUM_LINES_4CIFS_NTSC * 2;
 927                         pixelsPerLine = LINE_SZ_4CIFS_NTSC;
 928                         break;
 929                 case SCALE_2CIFS:
 930                         linesPerFrame = NUM_LINES_2CIFS_NTSC;
 931                         pixelsPerLine = LINE_SZ_2CIFS_NTSC;
 932                         break;
 933                 case SCALE_1CIFS:
 934                         linesPerFrame = NUM_LINES_1CIFS_NTSC;
 935                         pixelsPerLine = LINE_SZ_1CIFS_NTSC;
 936                         break;
 937                 default:
 938                         break;
 939                 }
 940         } else if (mode->format == FORMAT_PAL) {
 941                 switch (mode->scale) {
 942                 case SCALE_4CIFS:
 943                 case SCALE_4CIFSI:
 944                         linesPerFrame = NUM_LINES_4CIFS_PAL * 2;
 945                         pixelsPerLine = LINE_SZ_4CIFS_PAL;
 946                         break;
 947                 case SCALE_2CIFS:
 948                         linesPerFrame = NUM_LINES_2CIFS_PAL;
 949                         pixelsPerLine = LINE_SZ_2CIFS_PAL;
 950                         break;
 951                 case SCALE_1CIFS:
 952                         linesPerFrame = NUM_LINES_1CIFS_PAL;
 953                         pixelsPerLine = LINE_SZ_1CIFS_PAL;
 954                         break;
 955                 default:
 956                         break;
 957                 }
 958         }
 959         outImageSize = linesPerFrame * pixelsPerLine;
 960         if ((mode->color & MASK_COLOR) != COLOR_Y8) {
 961                 /* 2 bytes/pixel if not monochrome */
 962                 outImageSize *= 2;
 963         }
 964
 965         /* total bytes to send including prefix and 4K padding;
 966            must be a multiple of USB_READ_SIZE */
 967         usbInSize = outImageSize + PREFIX_SIZE; /* always send prefix */
 968         mask_mult = 0xffffffffUL - DEF_USB_BLOCK + 1;
 969         /* if size not a multiple of USB_READ_SIZE */
 970         if (usbInSize & ~mask_mult)
 971                 usbInSize = (usbInSize & mask_mult) + (DEF_USB_BLOCK);
 972         return usbInSize;
 973 }
 974
 975 static void s2255_print_cfg(struct s2255_dev *sdev, struct s2255_mode *mode)
 976 {
 977         struct device *dev = &sdev->udev->dev;
 978         dev_info(dev, "------------------------------------------------\n");
 979         dev_info(dev, "format: %d\nscale %d\n", mode->format, mode->scale);
 980         dev_info(dev, "fdec: %d\ncolor %d\n", mode->fdec, mode->color);
 981         dev_info(dev, "bright: 0x%x\n", mode->bright);
 982         dev_info(dev, "------------------------------------------------\n");
 983 }
 984
 985 /*
 986  * set mode is the function which controls the DSP.
 987  * the restart parameter in struct s2255_mode should be set whenever
 988  * the image size could change via color format, video system or image
 989  * size.
 990  * When the restart parameter is set, we sleep for ONE frame to allow the
 991  * DSP time to get the new frame
 992  */
 993 static int s2255_set_mode(struct s2255_vc *vc,
 994                           struct s2255_mode *mode)
 995 {
 996         int res;
 997         unsigned long chn_rev;
 998         struct s2255_dev *dev = to_s2255_dev(vc->vdev.v4l2_dev);
 999         int i;
1000         __le32 *buffer = dev->cmdbuf;
1001
1002         mutex_lock(&dev->cmdlock);
1003         chn_rev = G_chnmap[vc->idx];
1004         dprintk(dev, 3, "%s channel: %d\n", __func__, vc->idx);
1005         /* if JPEG, set the quality */
1006         if ((mode->color & MASK_COLOR) == COLOR_JPG) {
1007                 mode->color &= ~MASK_COLOR;
1008                 mode->color |= COLOR_JPG;
1009                 mode->color &= ~MASK_JPG_QUALITY;
1010                 mode->color |= (vc->jpegqual << 8);
1011         }
1012         /* save the mode */
1013         vc->mode = *mode;
1014         vc->req_image_size = get_transfer_size(mode);
1015         dprintk(dev, 1, "%s: reqsize %ld\n", __func__, vc->req_image_size);
1016         /* set the mode */
1017         buffer[0] = IN_DATA_TOKEN;
1018         buffer[1] = (__le32) cpu_to_le32(chn_rev);
1019         buffer[2] = CMD_SET_MODE;
1020         for (i = 0; i < sizeof(struct s2255_mode) / sizeof(u32); i++)
1021                 buffer[3 + i] = cpu_to_le32(((u32 *)&vc->mode)[i]);
1022         vc->setmode_ready = 0;
1023         res = s2255_write_config(dev->udev, (unsigned char *)buffer, 512);
1024         if (debug)
1025                 s2255_print_cfg(dev, mode);
1026         /* wait at least 3 frames before continuing */
1027         if (mode->restart) {
1028                 wait_event_timeout(vc->wait_setmode,
1029                                    (vc->setmode_ready != 0),
1030                                    msecs_to_jiffies(S2255_SETMODE_TIMEOUT));
1031                 if (vc->setmode_ready != 1) {
1032                         dprintk(dev, 0, "s2255: no set mode response\n");
1033                         res = -EFAULT;
1034                 }
1035         }
1036         /* clear the restart flag */
1037         vc->mode.restart = 0;
1038         dprintk(dev, 1, "%s chn %d, result: %d\n", __func__, vc->idx, res);
1039         mutex_unlock(&dev->cmdlock);
1040         return res;
1041 }
1042
1043 static int s2255_cmd_status(struct s2255_vc *vc, u32 *pstatus)
1044 {
1045         int res;
1046         u32 chn_rev;
1047         struct s2255_dev *dev = to_s2255_dev(vc->vdev.v4l2_dev);
1048         __le32 *buffer = dev->cmdbuf;
1049
1050         mutex_lock(&dev->cmdlock);
1051         chn_rev = G_chnmap[vc->idx];
1052         dprintk(dev, 4, "%s chan %d\n", __func__, vc->idx);
1053         /* form the get vid status command */
1054         buffer[0] = IN_DATA_TOKEN;
1055         buffer[1] = (__le32) cpu_to_le32(chn_rev);
1056         buffer[2] = CMD_STATUS;
1057         *pstatus = 0;
1058         vc->vidstatus_ready = 0;
1059         res = s2255_write_config(dev->udev, (unsigned char *)buffer, 512);
1060         wait_event_timeout(vc->wait_vidstatus,
1061                            (vc->vidstatus_ready != 0),
1062                            msecs_to_jiffies(S2255_VIDSTATUS_TIMEOUT));
1063         if (vc->vidstatus_ready != 1) {
1064                 dprintk(dev, 0, "s2255: no vidstatus response\n");
1065                 res = -EFAULT;
1066         }
1067         *pstatus = vc->vidstatus;
1068         dprintk(dev, 4, "%s, vid status %d\n", __func__, *pstatus);
1069         mutex_unlock(&dev->cmdlock);
1070         return res;
1071 }
1072
1073 static int start_streaming(struct vb2_queue *vq, unsigned int count)
1074 {
1075         struct s2255_vc *vc = vb2_get_drv_priv(vq);
1076         int j;
1077
1078         vc->last_frame = -1;
1079         vc->bad_payload = 0;
1080         vc->cur_frame = 0;
1081         vc->frame_count = 0;
1082         for (j = 0; j < SYS_FRAMES; j++) {
1083                 vc->buffer.frame[j].ulState = S2255_READ_IDLE;
1084                 vc->buffer.frame[j].cur_size = 0;
1085         }
1086         return s2255_start_acquire(vc);
1087 }
1088
1089 /* abort streaming and wait for last buffer */
1090 static void stop_streaming(struct vb2_queue *vq)
1091 {
1092         struct s2255_vc *vc = vb2_get_drv_priv(vq);
1093         struct s2255_buffer *buf, *node;
1094         unsigned long flags;
1095         (void) s2255_stop_acquire(vc);
1096         spin_lock_irqsave(&vc->qlock, flags);
1097         list_for_each_entry_safe(buf, node, &vc->buf_list, list) {
1098                 list_del(&buf->list);
1099                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
1100                 dprintk(vc->dev, 2, "[%p/%d] done\n",
1101                         buf, buf->vb.vb2_buf.index);
1102         }
1103         spin_unlock_irqrestore(&vc->qlock, flags);
1104 }
1105
1106 static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id i)
1107 {
1108         struct s2255_vc *vc = video_drvdata(file);
1109         struct s2255_mode mode;
1110         struct vb2_queue *q = &vc->vb_vidq;
1111
1112         /*
1113          * Changing the standard implies a format change, which is not allowed
1114          * while buffers for use with streaming have already been allocated.
1115          */
1116         if (vb2_is_busy(q))
1117                 return -EBUSY;
1118
1119         mode = vc->mode;
1120         if (i & V4L2_STD_525_60) {
1121                 dprintk(vc->dev, 4, "%s 60 Hz\n", __func__);
1122                 /* if changing format, reset frame decimation/intervals */
1123                 if (mode.format != FORMAT_NTSC) {
1124                         mode.restart = 1;
1125                         mode.format = FORMAT_NTSC;
1126                         mode.fdec = FDEC_1;
1127                         vc->width = LINE_SZ_4CIFS_NTSC;
1128                         vc->height = NUM_LINES_4CIFS_NTSC * 2;
1129                 }
1130         } else if (i & V4L2_STD_625_50) {
1131                 dprintk(vc->dev, 4, "%s 50 Hz\n", __func__);
1132                 if (mode.format != FORMAT_PAL) {
1133                         mode.restart = 1;
1134                         mode.format = FORMAT_PAL;
1135                         mode.fdec = FDEC_1;
1136                         vc->width = LINE_SZ_4CIFS_PAL;
1137                         vc->height = NUM_LINES_4CIFS_PAL * 2;
1138                 }
1139         } else
1140                 return -EINVAL;
1141         vc->std = i;
1142         if (mode.restart)
1143                 s2255_set_mode(vc, &mode);
1144         return 0;
1145 }
1146
1147 static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *i)
1148 {
1149         struct s2255_vc *vc = video_drvdata(file);
1150
1151         *i = vc->std;
1152         return 0;
1153 }
1154
1155 /* Sensoray 2255 is a multiple channel capture device.
1156    It does not have a "crossbar" of inputs.
1157    We use one V4L device per channel. The user must
1158    be aware that certain combinations are not allowed.
1159    For instance, you cannot do full FPS on more than 2 channels(2 videodevs)
1160    at once in color(you can do full fps on 4 channels with greyscale.
1161 */
1162 static int vidioc_enum_input(struct file *file, void *priv,
1163                              struct v4l2_input *inp)
1164 {
1165         struct s2255_vc *vc = video_drvdata(file);
1166         struct s2255_dev *dev = vc->dev;
1167         u32 status = 0;
1168
1169         if (inp->index != 0)
1170                 return -EINVAL;
1171         inp->type = V4L2_INPUT_TYPE_CAMERA;
1172         inp->std = S2255_NORMS;
1173         inp->status = 0;
1174         if (dev->dsp_fw_ver >= S2255_MIN_DSP_STATUS) {
1175                 int rc;
1176                 rc = s2255_cmd_status(vc, &status);
1177                 dprintk(dev, 4, "s2255_cmd_status rc: %d status %x\n",
1178                         rc, status);
1179                 if (rc == 0)
1180                         inp->status =  (status & 0x01) ? 0
1181                                 : V4L2_IN_ST_NO_SIGNAL;
1182         }
1183         switch (dev->pid) {
1184         case 0x2255:
1185         default:
1186                 strscpy(inp->name, "Composite", sizeof(inp->name));
1187                 break;
1188         case 0x2257:
1189                 strscpy(inp->name, (vc->idx < 2) ? "Composite" : "S-Video",
1190                         sizeof(inp->name));
1191                 break;
1192         }
1193         return 0;
1194 }
1195
1196 static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
1197 {
1198         *i = 0;
1199         return 0;
1200 }
1201 static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
1202 {
1203         if (i > 0)
1204                 return -EINVAL;
1205         return 0;
1206 }
1207
1208 static int s2255_s_ctrl(struct v4l2_ctrl *ctrl)
1209 {
1210         struct s2255_vc *vc =
1211                 container_of(ctrl->handler, struct s2255_vc, hdl);
1212         struct s2255_mode mode;
1213         mode = vc->mode;
1214         /* update the mode to the corresponding value */
1215         switch (ctrl->id) {
1216         case V4L2_CID_BRIGHTNESS:
1217                 mode.bright = ctrl->val;
1218                 break;
1219         case V4L2_CID_CONTRAST:
1220                 mode.contrast = ctrl->val;
1221                 break;
1222         case V4L2_CID_HUE:
1223                 mode.hue = ctrl->val;
1224                 break;
1225         case V4L2_CID_SATURATION:
1226                 mode.saturation = ctrl->val;
1227                 break;
1228         case V4L2_CID_S2255_COLORFILTER:
1229                 mode.color &= ~MASK_INPUT_TYPE;
1230                 mode.color |= !ctrl->val << 16;
1231                 break;
1232         case V4L2_CID_JPEG_COMPRESSION_QUALITY:
1233                 vc->jpegqual = ctrl->val;
1234                 return 0;
1235         default:
1236                 return -EINVAL;
1237         }
1238         mode.restart = 0;
1239         /* set mode here.  Note: stream does not need restarted.
1240            some V4L programs restart stream unnecessarily
1241            after a s_crtl.
1242         */
1243         s2255_set_mode(vc, &mode);
1244         return 0;
1245 }
1246
1247 static int vidioc_g_jpegcomp(struct file *file, void *priv,
1248                          struct v4l2_jpegcompression *jc)
1249 {
1250         struct s2255_vc *vc = video_drvdata(file);
1251
1252         memset(jc, 0, sizeof(*jc));
1253         jc->quality = vc->jpegqual;
1254         dprintk(vc->dev, 2, "%s: quality %d\n", __func__, jc->quality);
1255         return 0;
1256 }
1257
1258 static int vidioc_s_jpegcomp(struct file *file, void *priv,
1259                          const struct v4l2_jpegcompression *jc)
1260 {
1261         struct s2255_vc *vc = video_drvdata(file);
1262
1263         if (jc->quality < 0 || jc->quality > 100)
1264                 return -EINVAL;
1265         v4l2_ctrl_s_ctrl(vc->jpegqual_ctrl, jc->quality);
1266         dprintk(vc->dev, 2, "%s: quality %d\n", __func__, jc->quality);
1267         return 0;
1268 }
1269
1270 static int vidioc_g_parm(struct file *file, void *priv,
1271                          struct v4l2_streamparm *sp)
1272 {
1273         __u32 def_num, def_dem;
1274         struct s2255_vc *vc = video_drvdata(file);
1275
1276         if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1277                 return -EINVAL;
1278         sp->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
1279         sp->parm.capture.capturemode = vc->cap_parm.capturemode;
1280         sp->parm.capture.readbuffers = S2255_MIN_BUFS;
1281         def_num = (vc->mode.format == FORMAT_NTSC) ? 1001 : 1000;
1282         def_dem = (vc->mode.format == FORMAT_NTSC) ? 30000 : 25000;
1283         sp->parm.capture.timeperframe.denominator = def_dem;
1284         switch (vc->mode.fdec) {
1285         default:
1286         case FDEC_1:
1287                 sp->parm.capture.timeperframe.numerator = def_num;
1288                 break;
1289         case FDEC_2:
1290                 sp->parm.capture.timeperframe.numerator = def_num * 2;
1291                 break;
1292         case FDEC_3:
1293                 sp->parm.capture.timeperframe.numerator = def_num * 3;
1294                 break;
1295         case FDEC_5:
1296                 sp->parm.capture.timeperframe.numerator = def_num * 5;
1297                 break;
1298         }
1299         dprintk(vc->dev, 4, "%s capture mode, %d timeperframe %d/%d\n",
1300                 __func__,
1301                 sp->parm.capture.capturemode,
1302                 sp->parm.capture.timeperframe.numerator,
1303                 sp->parm.capture.timeperframe.denominator);
1304         return 0;
1305 }
1306
1307 static int vidioc_s_parm(struct file *file, void *priv,
1308                          struct v4l2_streamparm *sp)
1309 {
1310         struct s2255_vc *vc = video_drvdata(file);
1311         struct s2255_mode mode;
1312         int fdec = FDEC_1;
1313         __u32 def_num, def_dem;
1314         if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1315                 return -EINVAL;
1316         mode = vc->mode;
1317         /* high quality capture mode requires a stream restart */
1318         if ((vc->cap_parm.capturemode != sp->parm.capture.capturemode)
1319             && vb2_is_streaming(&vc->vb_vidq))
1320                 return -EBUSY;
1321         def_num = (mode.format == FORMAT_NTSC) ? 1001 : 1000;
1322         def_dem = (mode.format == FORMAT_NTSC) ? 30000 : 25000;
1323         if (def_dem != sp->parm.capture.timeperframe.denominator)
1324                 sp->parm.capture.timeperframe.numerator = def_num;
1325         else if (sp->parm.capture.timeperframe.numerator <= def_num)
1326                 sp->parm.capture.timeperframe.numerator = def_num;
1327         else if (sp->parm.capture.timeperframe.numerator <= (def_num * 2)) {
1328                 sp->parm.capture.timeperframe.numerator = def_num * 2;
1329                 fdec = FDEC_2;
1330         } else if (sp->parm.capture.timeperframe.numerator <= (def_num * 3)) {
1331                 sp->parm.capture.timeperframe.numerator = def_num * 3;
1332                 fdec = FDEC_3;
1333         } else {
1334                 sp->parm.capture.timeperframe.numerator = def_num * 5;
1335                 fdec = FDEC_5;
1336         }
1337         mode.fdec = fdec;
1338         sp->parm.capture.timeperframe.denominator = def_dem;
1339         sp->parm.capture.readbuffers = S2255_MIN_BUFS;
1340         s2255_set_mode(vc, &mode);
1341         dprintk(vc->dev, 4, "%s capture mode, %d timeperframe %d/%d, fdec %d\n",
1342                 __func__,
1343                 sp->parm.capture.capturemode,
1344                 sp->parm.capture.timeperframe.numerator,
1345                 sp->parm.capture.timeperframe.denominator, fdec);
1346         return 0;
1347 }
1348
1349 #define NUM_SIZE_ENUMS 3
1350 static const struct v4l2_frmsize_discrete ntsc_sizes[] = {
1351         { 640, 480 },
1352         { 640, 240 },
1353         { 320, 240 },
1354 };
1355 static const struct v4l2_frmsize_discrete pal_sizes[] = {
1356         { 704, 576 },
1357         { 704, 288 },
1358         { 352, 288 },
1359 };
1360
1361 static int vidioc_enum_framesizes(struct file *file, void *priv,
1362                             struct v4l2_frmsizeenum *fe)
1363 {
1364         struct s2255_vc *vc = video_drvdata(file);
1365         int is_ntsc = vc->std & V4L2_STD_525_60;
1366         const struct s2255_fmt *fmt;
1367
1368         if (fe->index >= NUM_SIZE_ENUMS)
1369                 return -EINVAL;
1370
1371         fmt = format_by_fourcc(fe->pixel_format);
1372         if (fmt == NULL)
1373                 return -EINVAL;
1374         fe->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1375         fe->discrete = is_ntsc ?  ntsc_sizes[fe->index] : pal_sizes[fe->index];
1376         return 0;
1377 }
1378
1379 static int vidioc_enum_frameintervals(struct file *file, void *priv,
1380                             struct v4l2_frmivalenum *fe)
1381 {
1382         struct s2255_vc *vc = video_drvdata(file);
1383         const struct s2255_fmt *fmt;
1384         const struct v4l2_frmsize_discrete *sizes;
1385         int is_ntsc = vc->std & V4L2_STD_525_60;
1386 #define NUM_FRAME_ENUMS 4
1387         int frm_dec[NUM_FRAME_ENUMS] = {1, 2, 3, 5};
1388         int i;
1389
1390         if (fe->index >= NUM_FRAME_ENUMS)
1391                 return -EINVAL;
1392
1393         fmt = format_by_fourcc(fe->pixel_format);
1394         if (fmt == NULL)
1395                 return -EINVAL;
1396
1397         sizes = is_ntsc ? ntsc_sizes : pal_sizes;
1398         for (i = 0; i < NUM_SIZE_ENUMS; i++, sizes++)
1399                 if (fe->width == sizes->width &&
1400                     fe->height == sizes->height)
1401                         break;
1402         if (i == NUM_SIZE_ENUMS)
1403                 return -EINVAL;
1404
1405         fe->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1406         fe->discrete.denominator = is_ntsc ? 30000 : 25000;
1407         fe->discrete.numerator = (is_ntsc ? 1001 : 1000) * frm_dec[fe->index];
1408         dprintk(vc->dev, 4, "%s discrete %d/%d\n", __func__,
1409                 fe->discrete.numerator,
1410                 fe->discrete.denominator);
1411         return 0;
1412 }
1413
1414 static int s2255_open(struct file *file)
1415 {
1416         struct s2255_vc *vc = video_drvdata(file);
1417         struct s2255_dev *dev = vc->dev;
1418         int state;
1419         int rc = 0;
1420
1421         rc = v4l2_fh_open(file);
1422         if (rc != 0)
1423                 return rc;
1424
1425         dprintk(dev, 1, "s2255: %s\n", __func__);
1426         state = atomic_read(&dev->fw_data->fw_state);
1427         switch (state) {
1428         case S2255_FW_DISCONNECTING:
1429                 return -ENODEV;
1430         case S2255_FW_FAILED:
1431                 s2255_dev_err(&dev->udev->dev,
1432                         "firmware load failed. retrying.\n");
1433                 s2255_fwload_start(dev);
1434                 wait_event_timeout(dev->fw_data->wait_fw,
1435                                    ((atomic_read(&dev->fw_data->fw_state)
1436                                      == S2255_FW_SUCCESS) ||
1437                                     (atomic_read(&dev->fw_data->fw_state)
1438                                      == S2255_FW_DISCONNECTING)),
1439                                    msecs_to_jiffies(S2255_LOAD_TIMEOUT));
1440                 /* state may have changed, re-read */
1441                 state = atomic_read(&dev->fw_data->fw_state);
1442                 break;
1443         case S2255_FW_NOTLOADED:
1444         case S2255_FW_LOADED_DSPWAIT:
1445                 /* give S2255_LOAD_TIMEOUT time for firmware to load in case
1446                    driver loaded and then device immediately opened */
1447                 pr_info("%s waiting for firmware load\n", __func__);
1448                 wait_event_timeout(dev->fw_data->wait_fw,
1449                                    ((atomic_read(&dev->fw_data->fw_state)
1450                                      == S2255_FW_SUCCESS) ||
1451                                     (atomic_read(&dev->fw_data->fw_state)
1452                                      == S2255_FW_DISCONNECTING)),
1453                                    msecs_to_jiffies(S2255_LOAD_TIMEOUT));
1454                 /* state may have changed, re-read */
1455                 state = atomic_read(&dev->fw_data->fw_state);
1456                 break;
1457         case S2255_FW_SUCCESS:
1458         default:
1459                 break;
1460         }
1461         /* state may have changed in above switch statement */
1462         switch (state) {
1463         case S2255_FW_SUCCESS:
1464                 break;
1465         case S2255_FW_FAILED:
1466                 pr_info("2255 firmware load failed.\n");
1467                 return -ENODEV;
1468         case S2255_FW_DISCONNECTING:
1469                 pr_info("%s: disconnecting\n", __func__);
1470                 return -ENODEV;
1471         case S2255_FW_LOADED_DSPWAIT:
1472         case S2255_FW_NOTLOADED:
1473                 pr_info("%s: firmware not loaded, please retry\n",
1474                         __func__);
1475                 /*
1476                  * Timeout on firmware load means device unusable.
1477                  * Set firmware failure state.
1478                  * On next s2255_open the firmware will be reloaded.
1479                  */
1480                 atomic_set(&dev->fw_data->fw_state,
1481                            S2255_FW_FAILED);
1482                 return -EAGAIN;
1483         default:
1484                 pr_info("%s: unknown state\n", __func__);
1485                 return -EFAULT;
1486         }
1487         if (!vc->configured) {
1488                 /* configure channel to default state */
1489                 vc->fmt = &formats[0];
1490                 s2255_set_mode(vc, &vc->mode);
1491                 vc->configured = 1;
1492         }
1493         return 0;
1494 }
1495
1496 static void s2255_destroy(struct s2255_dev *dev)
1497 {
1498         dprintk(dev, 1, "%s", __func__);
1499         /* board shutdown stops the read pipe if it is running */
1500         s2255_board_shutdown(dev);
1501         /* make sure firmware still not trying to load */
1502         del_timer_sync(&dev->timer);  /* only started in .probe and .open */
1503         if (dev->fw_data->fw_urb) {
1504                 usb_kill_urb(dev->fw_data->fw_urb);
1505                 usb_free_urb(dev->fw_data->fw_urb);
1506                 dev->fw_data->fw_urb = NULL;
1507         }
1508         release_firmware(dev->fw_data->fw);
1509         kfree(dev->fw_data->pfw_data);
1510         kfree(dev->fw_data);
1511         /* reset the DSP so firmware can be reloaded next time */
1512         s2255_reset_dsppower(dev);
1513         mutex_destroy(&dev->lock);
1514         usb_put_dev(dev->udev);
1515         v4l2_device_unregister(&dev->v4l2_dev);
1516         kfree(dev->cmdbuf);
1517         kfree(dev);
1518 }
1519
1520 static const struct v4l2_file_operations s2255_fops_v4l = {
1521         .owner = THIS_MODULE,
1522         .open = s2255_open,
1523         .release = vb2_fop_release,
1524         .poll = vb2_fop_poll,
1525         .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
1526         .mmap = vb2_fop_mmap,
1527         .read = vb2_fop_read,
1528 };
1529
1530 static const struct v4l2_ioctl_ops s2255_ioctl_ops = {
1531         .vidioc_querycap = vidioc_querycap,
1532         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
1533         .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
1534         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
1535         .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
1536         .vidioc_reqbufs = vb2_ioctl_reqbufs,
1537         .vidioc_querybuf = vb2_ioctl_querybuf,
1538         .vidioc_qbuf = vb2_ioctl_qbuf,
1539         .vidioc_dqbuf = vb2_ioctl_dqbuf,
1540         .vidioc_s_std = vidioc_s_std,
1541         .vidioc_g_std = vidioc_g_std,
1542         .vidioc_enum_input = vidioc_enum_input,
1543         .vidioc_g_input = vidioc_g_input,
1544         .vidioc_s_input = vidioc_s_input,
1545         .vidioc_streamon = vb2_ioctl_streamon,
1546         .vidioc_streamoff = vb2_ioctl_streamoff,
1547         .vidioc_s_jpegcomp = vidioc_s_jpegcomp,
1548         .vidioc_g_jpegcomp = vidioc_g_jpegcomp,
1549         .vidioc_s_parm = vidioc_s_parm,
1550         .vidioc_g_parm = vidioc_g_parm,
1551         .vidioc_enum_framesizes = vidioc_enum_framesizes,
1552         .vidioc_enum_frameintervals = vidioc_enum_frameintervals,
1553         .vidioc_log_status  = v4l2_ctrl_log_status,
1554         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1555         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1556 };
1557
1558 static void s2255_video_device_release(struct video_device *vdev)
1559 {
1560         struct s2255_dev *dev = to_s2255_dev(vdev->v4l2_dev);
1561         struct s2255_vc *vc =
1562                 container_of(vdev, struct s2255_vc, vdev);
1563
1564         dprintk(dev, 4, "%s, chnls: %d\n", __func__,
1565                 atomic_read(&dev->num_channels));
1566
1567         v4l2_ctrl_handler_free(&vc->hdl);
1568
1569         if (atomic_dec_and_test(&dev->num_channels))
1570                 s2255_destroy(dev);
1571         return;
1572 }
1573
1574 static const struct video_device template = {
1575         .name = "s2255v",
1576         .fops = &s2255_fops_v4l,
1577         .ioctl_ops = &s2255_ioctl_ops,
1578         .release = s2255_video_device_release,
1579         .tvnorms = S2255_NORMS,
1580 };
1581
1582 static const struct v4l2_ctrl_ops s2255_ctrl_ops = {
1583         .s_ctrl = s2255_s_ctrl,
1584 };
1585
1586 static const struct v4l2_ctrl_config color_filter_ctrl = {
1587         .ops = &s2255_ctrl_ops,
1588         .name = "Color Filter",
1589         .id = V4L2_CID_S2255_COLORFILTER,
1590         .type = V4L2_CTRL_TYPE_BOOLEAN,
1591         .max = 1,
1592         .step = 1,
1593         .def = 1,
1594 };
1595
1596 static int s2255_probe_v4l(struct s2255_dev *dev)
1597 {
1598         int ret;
1599         int i;
1600         int cur_nr = video_nr;
1601         struct s2255_vc *vc;
1602         struct vb2_queue *q;
1603
1604         ret = v4l2_device_register(&dev->interface->dev, &dev->v4l2_dev);
1605         if (ret)
1606                 return ret;
1607         /* initialize all video 4 linux */
1608         /* register 4 video devices */
1609         for (i = 0; i < MAX_CHANNELS; i++) {
1610                 vc = &dev->vc[i];
1611                 INIT_LIST_HEAD(&vc->buf_list);
1612
1613                 v4l2_ctrl_handler_init(&vc->hdl, 6);
1614                 v4l2_ctrl_new_std(&vc->hdl, &s2255_ctrl_ops,
1615                                 V4L2_CID_BRIGHTNESS, -127, 127, 1, DEF_BRIGHT);
1616                 v4l2_ctrl_new_std(&vc->hdl, &s2255_ctrl_ops,
1617                                 V4L2_CID_CONTRAST, 0, 255, 1, DEF_CONTRAST);
1618                 v4l2_ctrl_new_std(&vc->hdl, &s2255_ctrl_ops,
1619                                 V4L2_CID_SATURATION, 0, 255, 1, DEF_SATURATION);
1620                 v4l2_ctrl_new_std(&vc->hdl, &s2255_ctrl_ops,
1621                                 V4L2_CID_HUE, 0, 255, 1, DEF_HUE);
1622                 vc->jpegqual_ctrl = v4l2_ctrl_new_std(&vc->hdl,
1623                                 &s2255_ctrl_ops,
1624                                 V4L2_CID_JPEG_COMPRESSION_QUALITY,
1625                                 0, 100, 1, S2255_DEF_JPEG_QUAL);
1626                 if (dev->dsp_fw_ver >= S2255_MIN_DSP_COLORFILTER &&
1627                     (dev->pid != 0x2257 || vc->idx <= 1))
1628                         v4l2_ctrl_new_custom(&vc->hdl, &color_filter_ctrl,
1629                                              NULL);
1630                 if (vc->hdl.error) {
1631                         ret = vc->hdl.error;
1632                         v4l2_ctrl_handler_free(&vc->hdl);
1633                         dev_err(&dev->udev->dev, "couldn't register control\n");
1634                         break;
1635                 }
1636                 q = &vc->vb_vidq;
1637                 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1638                 q->io_modes = VB2_MMAP | VB2_READ | VB2_USERPTR;
1639                 q->drv_priv = vc;
1640                 q->lock = &vc->vb_lock;
1641                 q->buf_struct_size = sizeof(struct s2255_buffer);
1642                 q->mem_ops = &vb2_vmalloc_memops;
1643                 q->ops = &s2255_video_qops;
1644                 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1645                 ret = vb2_queue_init(q);
1646                 if (ret != 0) {
1647                         dev_err(&dev->udev->dev,
1648                                 "%s vb2_queue_init 0x%x\n", __func__, ret);
1649                         break;
1650                 }
1651                 /* register video devices */
1652                 vc->vdev = template;
1653                 vc->vdev.queue = q;
1654                 vc->vdev.ctrl_handler = &vc->hdl;
1655                 vc->vdev.lock = &dev->lock;
1656                 vc->vdev.v4l2_dev = &dev->v4l2_dev;
1657                 vc->vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE |
1658                                        V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
1659                 video_set_drvdata(&vc->vdev, vc);
1660                 if (video_nr == -1)
1661                         ret = video_register_device(&vc->vdev,
1662                                                     VFL_TYPE_GRABBER,
1663                                                     video_nr);
1664                 else
1665                         ret = video_register_device(&vc->vdev,
1666                                                     VFL_TYPE_GRABBER,
1667                                                     cur_nr + i);
1668
1669                 if (ret) {
1670                         dev_err(&dev->udev->dev,
1671                                 "failed to register video device!\n");
1672                         break;
1673                 }
1674                 atomic_inc(&dev->num_channels);
1675                 v4l2_info(&dev->v4l2_dev, "V4L2 device registered as %s\n",
1676                           video_device_node_name(&vc->vdev));
1677
1678         }
1679         pr_info("Sensoray 2255 V4L driver Revision: %s\n",
1680                 S2255_VERSION);
1681         /* if no channels registered, return error and probe will fail*/
1682         if (atomic_read(&dev->num_channels) == 0) {
1683                 v4l2_device_unregister(&dev->v4l2_dev);
1684                 return ret;
1685         }
1686         if (atomic_read(&dev->num_channels) != MAX_CHANNELS)
1687                 pr_warn("s2255: Not all channels available.\n");
1688         return 0;
1689 }
1690
1691 /* this function moves the usb stream read pipe data
1692  * into the system buffers.
1693  * returns 0 on success, EAGAIN if more data to process( call this
1694  * function again).
1695  *
1696  * Received frame structure:
1697  * bytes 0-3:  marker : 0x2255DA4AL (S2255_MARKER_FRAME)
1698  * bytes 4-7:  channel: 0-3
1699  * bytes 8-11: payload size:  size of the frame
1700  * bytes 12-payloadsize+12:  frame data
1701  */
1702 static int save_frame(struct s2255_dev *dev, struct s2255_pipeinfo *pipe_info)
1703 {
1704         char *pdest;
1705         u32 offset = 0;
1706         int bframe = 0;
1707         char *psrc;
1708         unsigned long copy_size;
1709         unsigned long size;
1710         s32 idx = -1;
1711         struct s2255_framei *frm;
1712         unsigned char *pdata;
1713         struct s2255_vc *vc;
1714         dprintk(dev, 100, "buffer to user\n");
1715         vc = &dev->vc[dev->cc];
1716         idx = vc->cur_frame;
1717         frm = &vc->buffer.frame[idx];
1718         if (frm->ulState == S2255_READ_IDLE) {
1719                 int jj;
1720                 unsigned int cc;
1721                 __le32 *pdword; /*data from dsp is little endian */
1722                 int payload;
1723                 /* search for marker codes */
1724                 pdata = (unsigned char *)pipe_info->transfer_buffer;
1725                 pdword = (__le32 *)pdata;
1726                 for (jj = 0; jj < (pipe_info->cur_transfer_size - 12); jj++) {
1727                         switch (*pdword) {
1728                         case S2255_MARKER_FRAME:
1729                                 dprintk(dev, 4, "marker @ offset: %d [%x %x]\n",
1730                                         jj, pdata[0], pdata[1]);
1731                                 offset = jj + PREFIX_SIZE;
1732                                 bframe = 1;
1733                                 cc = le32_to_cpu(pdword[1]);
1734                                 if (cc >= MAX_CHANNELS) {
1735                                         dprintk(dev, 0,
1736                                                 "bad channel\n");
1737                                         return -EINVAL;
1738                                 }
1739                                 /* reverse it */
1740                                 dev->cc = G_chnmap[cc];
1741                                 vc = &dev->vc[dev->cc];
1742                                 payload =  le32_to_cpu(pdword[3]);
1743                                 if (payload > vc->req_image_size) {
1744                                         vc->bad_payload++;
1745                                         /* discard the bad frame */
1746                                         return -EINVAL;
1747                                 }
1748                                 vc->pkt_size = payload;
1749                                 vc->jpg_size = le32_to_cpu(pdword[4]);
1750                                 break;
1751                         case S2255_MARKER_RESPONSE:
1752
1753                                 pdata += DEF_USB_BLOCK;
1754                                 jj += DEF_USB_BLOCK;
1755                                 if (le32_to_cpu(pdword[1]) >= MAX_CHANNELS)
1756                                         break;
1757                                 cc = G_chnmap[le32_to_cpu(pdword[1])];
1758                                 if (cc >= MAX_CHANNELS)
1759                                         break;
1760                                 vc = &dev->vc[cc];
1761                                 switch (pdword[2]) {
1762                                 case S2255_RESPONSE_SETMODE:
1763                                         /* check if channel valid */
1764                                         /* set mode ready */
1765                                         vc->setmode_ready = 1;
1766                                         wake_up(&vc->wait_setmode);
1767                                         dprintk(dev, 5, "setmode rdy %d\n", cc);
1768                                         break;
1769                                 case S2255_RESPONSE_FW:
1770                                         dev->chn_ready |= (1 << cc);
1771                                         if ((dev->chn_ready & 0x0f) != 0x0f)
1772                                                 break;
1773                                         /* all channels ready */
1774                                         pr_info("s2255: fw loaded\n");
1775                                         atomic_set(&dev->fw_data->fw_state,
1776                                                    S2255_FW_SUCCESS);
1777                                         wake_up(&dev->fw_data->wait_fw);
1778                                         break;
1779                                 case S2255_RESPONSE_STATUS:
1780                                         vc->vidstatus = le32_to_cpu(pdword[3]);
1781                                         vc->vidstatus_ready = 1;
1782                                         wake_up(&vc->wait_vidstatus);
1783                                         dprintk(dev, 5, "vstat %x chan %d\n",
1784                                                 le32_to_cpu(pdword[3]), cc);
1785                                         break;
1786                                 default:
1787                                         pr_info("s2255 unknown resp\n");
1788                                 }
1789                                 pdata++;
1790                                 break;
1791                         default:
1792                                 pdata++;
1793                                 break;
1794                         }
1795                         if (bframe)
1796                                 break;
1797                 } /* for */
1798                 if (!bframe)
1799                         return -EINVAL;
1800         }
1801         vc = &dev->vc[dev->cc];
1802         idx = vc->cur_frame;
1803         frm = &vc->buffer.frame[idx];
1804         /* search done.  now find out if should be acquiring on this channel */
1805         if (!vb2_is_streaming(&vc->vb_vidq)) {
1806                 /* we found a frame, but this channel is turned off */
1807                 frm->ulState = S2255_READ_IDLE;
1808                 return -EINVAL;
1809         }
1810
1811         if (frm->ulState == S2255_READ_IDLE) {
1812                 frm->ulState = S2255_READ_FRAME;
1813                 frm->cur_size = 0;
1814         }
1815
1816         /* skip the marker 512 bytes (and offset if out of sync) */
1817         psrc = (u8 *)pipe_info->transfer_buffer + offset;
1818
1819
1820         if (frm->lpvbits == NULL) {
1821                 dprintk(dev, 1, "s2255 frame buffer == NULL.%p %p %d %d",
1822                         frm, dev, dev->cc, idx);
1823                 return -ENOMEM;
1824         }
1825
1826         pdest = frm->lpvbits + frm->cur_size;
1827
1828         copy_size = (pipe_info->cur_transfer_size - offset);
1829
1830         size = vc->pkt_size - PREFIX_SIZE;
1831
1832         /* sanity check on pdest */
1833         if ((copy_size + frm->cur_size) < vc->req_image_size)
1834                 memcpy(pdest, psrc, copy_size);
1835
1836         frm->cur_size += copy_size;
1837         dprintk(dev, 4, "cur_size: %lu, size: %lu\n", frm->cur_size, size);
1838
1839         if (frm->cur_size >= size) {
1840                 dprintk(dev, 2, "******[%d]Buffer[%d]full*******\n",
1841                         dev->cc, idx);
1842                 vc->last_frame = vc->cur_frame;
1843                 vc->cur_frame++;
1844                 /* end of system frame ring buffer, start at zero */
1845                 if ((vc->cur_frame == SYS_FRAMES) ||
1846                     (vc->cur_frame == vc->buffer.dwFrames))
1847                         vc->cur_frame = 0;
1848                 /* frame ready */
1849                 if (vb2_is_streaming(&vc->vb_vidq))
1850                         s2255_got_frame(vc, vc->jpg_size);
1851                 vc->frame_count++;
1852                 frm->ulState = S2255_READ_IDLE;
1853                 frm->cur_size = 0;
1854
1855         }
1856         /* done successfully */
1857         return 0;
1858 }
1859
1860 static void s2255_read_video_callback(struct s2255_dev *dev,
1861                                       struct s2255_pipeinfo *pipe_info)
1862 {
1863         int res;
1864         dprintk(dev, 50, "callback read video\n");
1865
1866         if (dev->cc >= MAX_CHANNELS) {
1867                 dev->cc = 0;
1868                 dev_err(&dev->udev->dev, "invalid channel\n");
1869                 return;
1870         }
1871         /* otherwise copy to the system buffers */
1872         res = save_frame(dev, pipe_info);
1873         if (res != 0)
1874                 dprintk(dev, 4, "s2255: read callback failed\n");
1875
1876         dprintk(dev, 50, "callback read video done\n");
1877         return;
1878 }
1879
1880 static long s2255_vendor_req(struct s2255_dev *dev, unsigned char Request,
1881                              u16 Index, u16 Value, void *TransferBuffer,
1882                              s32 TransferBufferLength, int bOut)
1883 {
1884         int r;
1885         unsigned char *buf;
1886
1887         buf = kmalloc(TransferBufferLength, GFP_KERNEL);
1888         if (!buf)
1889                 return -ENOMEM;
1890
1891         if (!bOut) {
1892                 r = usb_control_msg(dev->udev, usb_rcvctrlpipe(dev->udev, 0),
1893                                     Request,
1894                                     USB_TYPE_VENDOR | USB_RECIP_DEVICE |
1895                                     USB_DIR_IN,
1896                                     Value, Index, buf,
1897                                     TransferBufferLength, HZ * 5);
1898
1899                 if (r >= 0)
1900                         memcpy(TransferBuffer, buf, TransferBufferLength);
1901         } else {
1902                 memcpy(buf, TransferBuffer, TransferBufferLength);
1903                 r = usb_control_msg(dev->udev, usb_sndctrlpipe(dev->udev, 0),
1904                                     Request, USB_TYPE_VENDOR | USB_RECIP_DEVICE,
1905                                     Value, Index, buf,
1906                                     TransferBufferLength, HZ * 5);
1907         }
1908         kfree(buf);
1909         return r;
1910 }
1911
1912 /*
1913  * retrieve FX2 firmware version. future use.
1914  * @param dev pointer to device extension
1915  * @return -1 for fail, else returns firmware version as an int(16 bits)
1916  */
1917 static int s2255_get_fx2fw(struct s2255_dev *dev)
1918 {
1919         int fw;
1920         int ret;
1921         unsigned char transBuffer[64];
1922         ret = s2255_vendor_req(dev, S2255_VR_FW, 0, 0, transBuffer, 2,
1923                                S2255_VR_IN);
1924         if (ret < 0)
1925                 dprintk(dev, 2, "get fw error: %x\n", ret);
1926         fw = transBuffer[0] + (transBuffer[1] << 8);
1927         dprintk(dev, 2, "Get FW %x %x\n", transBuffer[0], transBuffer[1]);
1928         return fw;
1929 }
1930
1931 /*
1932  * Create the system ring buffer to copy frames into from the
1933  * usb read pipe.
1934  */
1935 static int s2255_create_sys_buffers(struct s2255_vc *vc)
1936 {
1937         unsigned long i;
1938         unsigned long reqsize;
1939         vc->buffer.dwFrames = SYS_FRAMES;
1940         /* always allocate maximum size(PAL) for system buffers */
1941         reqsize = SYS_FRAMES_MAXSIZE;
1942
1943         if (reqsize > SYS_FRAMES_MAXSIZE)
1944                 reqsize = SYS_FRAMES_MAXSIZE;
1945
1946         for (i = 0; i < SYS_FRAMES; i++) {
1947                 /* allocate the frames */
1948                 vc->buffer.frame[i].lpvbits = vmalloc(reqsize);
1949                 vc->buffer.frame[i].size = reqsize;
1950                 if (vc->buffer.frame[i].lpvbits == NULL) {
1951                         pr_info("out of memory.  using less frames\n");
1952                         vc->buffer.dwFrames = i;
1953                         break;
1954                 }
1955         }
1956
1957         /* make sure internal states are set */
1958         for (i = 0; i < SYS_FRAMES; i++) {
1959                 vc->buffer.frame[i].ulState = 0;
1960                 vc->buffer.frame[i].cur_size = 0;
1961         }
1962
1963         vc->cur_frame = 0;
1964         vc->last_frame = -1;
1965         return 0;
1966 }
1967
1968 static int s2255_release_sys_buffers(struct s2255_vc *vc)
1969 {
1970         unsigned long i;
1971         for (i = 0; i < SYS_FRAMES; i++) {
1972                 vfree(vc->buffer.frame[i].lpvbits);
1973                 vc->buffer.frame[i].lpvbits = NULL;
1974         }
1975         return 0;
1976 }
1977
1978 static int s2255_board_init(struct s2255_dev *dev)
1979 {
1980         struct s2255_mode mode_def = DEF_MODEI_NTSC_CONT;
1981         int fw_ver;
1982         int j;
1983         struct s2255_pipeinfo *pipe = &dev->pipe;
1984         dprintk(dev, 4, "board init: %p", dev);
1985         memset(pipe, 0, sizeof(*pipe));
1986         pipe->dev = dev;
1987         pipe->cur_transfer_size = S2255_USB_XFER_SIZE;
1988         pipe->max_transfer_size = S2255_USB_XFER_SIZE;
1989
1990         pipe->transfer_buffer = kzalloc(pipe->max_transfer_size,
1991                                         GFP_KERNEL);
1992         if (pipe->transfer_buffer == NULL) {
1993                 dprintk(dev, 1, "out of memory!\n");
1994                 return -ENOMEM;
1995         }
1996         /* query the firmware */
1997         fw_ver = s2255_get_fx2fw(dev);
1998
1999         pr_info("s2255: usb firmware version %d.%d\n",
2000                 (fw_ver >> 8) & 0xff,
2001                 fw_ver & 0xff);
2002
2003         if (fw_ver < S2255_CUR_USB_FWVER)
2004                 pr_info("s2255: newer USB firmware available\n");
2005
2006         for (j = 0; j < MAX_CHANNELS; j++) {
2007                 struct s2255_vc *vc = &dev->vc[j];
2008                 vc->mode = mode_def;
2009                 if (dev->pid == 0x2257 && j > 1)
2010                         vc->mode.color |= (1 << 16);
2011                 vc->jpegqual = S2255_DEF_JPEG_QUAL;
2012                 vc->width = LINE_SZ_4CIFS_NTSC;
2013                 vc->height = NUM_LINES_4CIFS_NTSC * 2;
2014                 vc->std = V4L2_STD_NTSC_M;
2015                 vc->fmt = &formats[0];
2016                 vc->mode.restart = 1;
2017                 vc->req_image_size = get_transfer_size(&mode_def);
2018                 vc->frame_count = 0;
2019                 /* create the system buffers */
2020                 s2255_create_sys_buffers(vc);
2021         }
2022         /* start read pipe */
2023         s2255_start_readpipe(dev);
2024         dprintk(dev, 1, "%s: success\n", __func__);
2025         return 0;
2026 }
2027
2028 static int s2255_board_shutdown(struct s2255_dev *dev)
2029 {
2030         u32 i;
2031         dprintk(dev, 1, "%s: dev: %p", __func__,  dev);
2032
2033         for (i = 0; i < MAX_CHANNELS; i++) {
2034                 if (vb2_is_streaming(&dev->vc[i].vb_vidq))
2035                         s2255_stop_acquire(&dev->vc[i]);
2036         }
2037         s2255_stop_readpipe(dev);
2038         for (i = 0; i < MAX_CHANNELS; i++)
2039                 s2255_release_sys_buffers(&dev->vc[i]);
2040         /* release transfer buffer */
2041         kfree(dev->pipe.transfer_buffer);
2042         return 0;
2043 }
2044
2045 static void read_pipe_completion(struct urb *purb)
2046 {
2047         struct s2255_pipeinfo *pipe_info;
2048         struct s2255_dev *dev;
2049         int status;
2050         int pipe;
2051         pipe_info = purb->context;
2052         if (pipe_info == NULL) {
2053                 dev_err(&purb->dev->dev, "no context!\n");
2054                 return;
2055         }
2056         dev = pipe_info->dev;
2057         if (dev == NULL) {
2058                 dev_err(&purb->dev->dev, "no context!\n");
2059                 return;
2060         }
2061         status = purb->status;
2062         /* if shutting down, do not resubmit, exit immediately */
2063         if (status == -ESHUTDOWN) {
2064                 dprintk(dev, 2, "%s: err shutdown\n", __func__);
2065                 pipe_info->err_count++;
2066                 return;
2067         }
2068
2069         if (pipe_info->state == 0) {
2070                 dprintk(dev, 2, "%s: exiting USB pipe", __func__);
2071                 return;
2072         }
2073
2074         if (status == 0)
2075                 s2255_read_video_callback(dev, pipe_info);
2076         else {
2077                 pipe_info->err_count++;
2078                 dprintk(dev, 1, "%s: failed URB %d\n", __func__, status);
2079         }
2080
2081         pipe = usb_rcvbulkpipe(dev->udev, dev->read_endpoint);
2082         /* reuse urb */
2083         usb_fill_bulk_urb(pipe_info->stream_urb, dev->udev,
2084                           pipe,
2085                           pipe_info->transfer_buffer,
2086                           pipe_info->cur_transfer_size,
2087                           read_pipe_completion, pipe_info);
2088
2089         if (pipe_info->state != 0) {
2090                 if (usb_submit_urb(pipe_info->stream_urb, GFP_ATOMIC))
2091                         dev_err(&dev->udev->dev, "error submitting urb\n");
2092         } else {
2093                 dprintk(dev, 2, "%s :complete state 0\n", __func__);
2094         }
2095         return;
2096 }
2097
2098 static int s2255_start_readpipe(struct s2255_dev *dev)
2099 {
2100         int pipe;
2101         int retval;
2102         struct s2255_pipeinfo *pipe_info = &dev->pipe;
2103         pipe = usb_rcvbulkpipe(dev->udev, dev->read_endpoint);
2104         dprintk(dev, 2, "%s: IN %d\n", __func__, dev->read_endpoint);
2105         pipe_info->state = 1;
2106         pipe_info->err_count = 0;
2107         pipe_info->stream_urb = usb_alloc_urb(0, GFP_KERNEL);
2108         if (!pipe_info->stream_urb)
2109                 return -ENOMEM;
2110         /* transfer buffer allocated in board_init */
2111         usb_fill_bulk_urb(pipe_info->stream_urb, dev->udev,
2112                           pipe,
2113                           pipe_info->transfer_buffer,
2114                           pipe_info->cur_transfer_size,
2115                           read_pipe_completion, pipe_info);
2116         retval = usb_submit_urb(pipe_info->stream_urb, GFP_KERNEL);
2117         if (retval) {
2118                 pr_err("s2255: start read pipe failed\n");
2119                 return retval;
2120         }
2121         return 0;
2122 }
2123
2124 /* starts acquisition process */
2125 static int s2255_start_acquire(struct s2255_vc *vc)
2126 {
2127         int res;
2128         unsigned long chn_rev;
2129         int j;
2130         struct s2255_dev *dev = to_s2255_dev(vc->vdev.v4l2_dev);
2131         __le32 *buffer = dev->cmdbuf;
2132
2133         mutex_lock(&dev->cmdlock);
2134         chn_rev = G_chnmap[vc->idx];
2135         vc->last_frame = -1;
2136         vc->bad_payload = 0;
2137         vc->cur_frame = 0;
2138         for (j = 0; j < SYS_FRAMES; j++) {
2139                 vc->buffer.frame[j].ulState = 0;
2140                 vc->buffer.frame[j].cur_size = 0;
2141         }
2142
2143         /* send the start command */
2144         buffer[0] = IN_DATA_TOKEN;
2145         buffer[1] = (__le32) cpu_to_le32(chn_rev);
2146         buffer[2] = CMD_START;
2147         res = s2255_write_config(dev->udev, (unsigned char *)buffer, 512);
2148         if (res != 0)
2149                 dev_err(&dev->udev->dev, "CMD_START error\n");
2150
2151         dprintk(dev, 2, "start acquire exit[%d] %d\n", vc->idx, res);
2152         mutex_unlock(&dev->cmdlock);
2153         return res;
2154 }
2155
2156 static int s2255_stop_acquire(struct s2255_vc *vc)
2157 {
2158         int res;
2159         unsigned long chn_rev;
2160         struct s2255_dev *dev = to_s2255_dev(vc->vdev.v4l2_dev);
2161         __le32 *buffer = dev->cmdbuf;
2162
2163         mutex_lock(&dev->cmdlock);
2164         chn_rev = G_chnmap[vc->idx];
2165         /* send the stop command */
2166         buffer[0] = IN_DATA_TOKEN;
2167         buffer[1] = (__le32) cpu_to_le32(chn_rev);
2168         buffer[2] = CMD_STOP;
2169
2170         res = s2255_write_config(dev->udev, (unsigned char *)buffer, 512);
2171         if (res != 0)
2172                 dev_err(&dev->udev->dev, "CMD_STOP error\n");
2173
2174         dprintk(dev, 4, "%s: chn %d, res %d\n", __func__, vc->idx, res);
2175         mutex_unlock(&dev->cmdlock);
2176         return res;
2177 }
2178
2179 static void s2255_stop_readpipe(struct s2255_dev *dev)
2180 {
2181         struct s2255_pipeinfo *pipe = &dev->pipe;
2182
2183         pipe->state = 0;
2184         if (pipe->stream_urb) {
2185                 /* cancel urb */
2186                 usb_kill_urb(pipe->stream_urb);
2187                 usb_free_urb(pipe->stream_urb);
2188                 pipe->stream_urb = NULL;
2189         }
2190         dprintk(dev, 4, "%s", __func__);
2191         return;
2192 }
2193
2194 static void s2255_fwload_start(struct s2255_dev *dev)
2195 {
2196         s2255_reset_dsppower(dev);
2197         dev->fw_data->fw_size = dev->fw_data->fw->size;
2198         atomic_set(&dev->fw_data->fw_state, S2255_FW_NOTLOADED);
2199         memcpy(dev->fw_data->pfw_data,
2200                dev->fw_data->fw->data, CHUNK_SIZE);
2201         dev->fw_data->fw_loaded = CHUNK_SIZE;
2202         usb_fill_bulk_urb(dev->fw_data->fw_urb, dev->udev,
2203                           usb_sndbulkpipe(dev->udev, 2),
2204                           dev->fw_data->pfw_data,
2205                           CHUNK_SIZE, s2255_fwchunk_complete,
2206                           dev->fw_data);
2207         mod_timer(&dev->timer, jiffies + HZ);
2208 }
2209
2210 /* standard usb probe function */
2211 static int s2255_probe(struct usb_interface *interface,
2212                        const struct usb_device_id *id)
2213 {
2214         struct s2255_dev *dev = NULL;
2215         struct usb_host_interface *iface_desc;
2216         struct usb_endpoint_descriptor *endpoint;
2217         int i;
2218         int retval = -ENOMEM;
2219         __le32 *pdata;
2220         int fw_size;
2221
2222         /* allocate memory for our device state and initialize it to zero */
2223         dev = kzalloc(sizeof(struct s2255_dev), GFP_KERNEL);
2224         if (dev == NULL) {
2225                 s2255_dev_err(&interface->dev, "out of memory\n");
2226                 return -ENOMEM;
2227         }
2228
2229         dev->cmdbuf = kzalloc(S2255_CMDBUF_SIZE, GFP_KERNEL);
2230         if (dev->cmdbuf == NULL) {
2231                 s2255_dev_err(&interface->dev, "out of memory\n");
2232                 goto errorFWDATA1;
2233         }
2234
2235         atomic_set(&dev->num_channels, 0);
2236         dev->pid = id->idProduct;
2237         dev->fw_data = kzalloc(sizeof(struct s2255_fw), GFP_KERNEL);
2238         if (!dev->fw_data)
2239                 goto errorFWDATA1;
2240         mutex_init(&dev->lock);
2241         mutex_init(&dev->cmdlock);
2242         /* grab usb_device and save it */
2243         dev->udev = usb_get_dev(interface_to_usbdev(interface));
2244         if (dev->udev == NULL) {
2245                 dev_err(&interface->dev, "null usb device\n");
2246                 retval = -ENODEV;
2247                 goto errorUDEV;
2248         }
2249         dev_dbg(&interface->dev, "dev: %p, udev %p interface %p\n",
2250                 dev, dev->udev, interface);
2251         dev->interface = interface;
2252         /* set up the endpoint information  */
2253         iface_desc = interface->cur_altsetting;
2254         dev_dbg(&interface->dev, "num EP: %d\n",
2255                 iface_desc->desc.bNumEndpoints);
2256         for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
2257                 endpoint = &iface_desc->endpoint[i].desc;
2258                 if (!dev->read_endpoint && usb_endpoint_is_bulk_in(endpoint)) {
2259                         /* we found the bulk in endpoint */
2260                         dev->read_endpoint = endpoint->bEndpointAddress;
2261                 }
2262         }
2263
2264         if (!dev->read_endpoint) {
2265                 dev_err(&interface->dev, "Could not find bulk-in endpoint\n");
2266                 goto errorEP;
2267         }
2268         timer_setup(&dev->timer, s2255_timer, 0);
2269         init_waitqueue_head(&dev->fw_data->wait_fw);
2270         for (i = 0; i < MAX_CHANNELS; i++) {
2271                 struct s2255_vc *vc = &dev->vc[i];
2272                 vc->idx = i;
2273                 vc->dev = dev;
2274                 init_waitqueue_head(&vc->wait_setmode);
2275                 init_waitqueue_head(&vc->wait_vidstatus);
2276                 spin_lock_init(&vc->qlock);
2277                 mutex_init(&vc->vb_lock);
2278         }
2279
2280         dev->fw_data->fw_urb = usb_alloc_urb(0, GFP_KERNEL);
2281         if (!dev->fw_data->fw_urb)
2282                 goto errorFWURB;
2283
2284         dev->fw_data->pfw_data = kzalloc(CHUNK_SIZE, GFP_KERNEL);
2285         if (!dev->fw_data->pfw_data) {
2286                 dev_err(&interface->dev, "out of memory!\n");
2287                 goto errorFWDATA2;
2288         }
2289         /* load the first chunk */
2290         if (reject_firmware(&dev->fw_data->fw,
2291                              FIRMWARE_FILE_NAME, &dev->udev->dev)) {
2292                 dev_err(&interface->dev, "sensoray 2255 failed to get firmware\n");
2293                 goto errorREQFW;
2294         }
2295         /* check the firmware is valid */
2296         fw_size = dev->fw_data->fw->size;
2297         pdata = (__le32 *) &dev->fw_data->fw->data[fw_size - 8];
2298
2299         if (*pdata != S2255_FW_MARKER) {
2300                 dev_err(&interface->dev, "Firmware invalid.\n");
2301                 retval = -ENODEV;
2302                 goto errorFWMARKER;
2303         } else {
2304                 /* make sure firmware is the latest */
2305                 __le32 *pRel;
2306                 pRel = (__le32 *) &dev->fw_data->fw->data[fw_size - 4];
2307                 pr_info("s2255 dsp fw version %x\n", le32_to_cpu(*pRel));
2308                 dev->dsp_fw_ver = le32_to_cpu(*pRel);
2309                 if (dev->dsp_fw_ver < S2255_CUR_DSP_FWVER)
2310                         pr_info("s2255: /*(DEBLOBBED)*/ out of date.\n");
2311                 if (dev->pid == 0x2257 &&
2312                                 dev->dsp_fw_ver < S2255_MIN_DSP_COLORFILTER)
2313                         pr_warn("2257 needs firmware %d or above.\n",
2314                                 S2255_MIN_DSP_COLORFILTER);
2315         }
2316         usb_reset_device(dev->udev);
2317         /* load 2255 board specific */
2318         retval = s2255_board_init(dev);
2319         if (retval)
2320                 goto errorBOARDINIT;
2321         s2255_fwload_start(dev);
2322         /* loads v4l specific */
2323         retval = s2255_probe_v4l(dev);
2324         if (retval)
2325                 goto errorBOARDINIT;
2326         dev_info(&interface->dev, "Sensoray 2255 detected\n");
2327         return 0;
2328 errorBOARDINIT:
2329         s2255_board_shutdown(dev);
2330 errorFWMARKER:
2331         release_firmware(dev->fw_data->fw);
2332 errorREQFW:
2333         kfree(dev->fw_data->pfw_data);
2334 errorFWDATA2:
2335         usb_free_urb(dev->fw_data->fw_urb);
2336 errorFWURB:
2337         del_timer_sync(&dev->timer);
2338 errorEP:
2339         usb_put_dev(dev->udev);
2340 errorUDEV:
2341         kfree(dev->fw_data);
2342         mutex_destroy(&dev->lock);
2343 errorFWDATA1:
2344         kfree(dev->cmdbuf);
2345         kfree(dev);
2346         pr_warn("Sensoray 2255 driver load failed: 0x%x\n", retval);
2347         return retval;
2348 }
2349
2350 /* disconnect routine. when board is removed physically or with rmmod */
2351 static void s2255_disconnect(struct usb_interface *interface)
2352 {
2353         struct s2255_dev *dev = to_s2255_dev(usb_get_intfdata(interface));
2354         int i;
2355         int channels = atomic_read(&dev->num_channels);
2356         mutex_lock(&dev->lock);
2357         v4l2_device_disconnect(&dev->v4l2_dev);
2358         mutex_unlock(&dev->lock);
2359         /*see comments in the uvc_driver.c usb disconnect function */
2360         atomic_inc(&dev->num_channels);
2361         /* unregister each video device. */
2362         for (i = 0; i < channels; i++)
2363                 video_unregister_device(&dev->vc[i].vdev);
2364         /* wake up any of our timers */
2365         atomic_set(&dev->fw_data->fw_state, S2255_FW_DISCONNECTING);
2366         wake_up(&dev->fw_data->wait_fw);
2367         for (i = 0; i < MAX_CHANNELS; i++) {
2368                 dev->vc[i].setmode_ready = 1;
2369                 wake_up(&dev->vc[i].wait_setmode);
2370                 dev->vc[i].vidstatus_ready = 1;
2371                 wake_up(&dev->vc[i].wait_vidstatus);
2372         }
2373         if (atomic_dec_and_test(&dev->num_channels))
2374                 s2255_destroy(dev);
2375         dev_info(&interface->dev, "%s\n", __func__);
2376 }
2377
2378 static struct usb_driver s2255_driver = {
2379         .name = S2255_DRIVER_NAME,
2380         .probe = s2255_probe,
2381         .disconnect = s2255_disconnect,
2382         .id_table = s2255_table,
2383 };
2384
2385 module_usb_driver(s2255_driver);
2386
2387 MODULE_DESCRIPTION("Sensoray 2255 Video for Linux driver");
2388 MODULE_AUTHOR("Dean Anderson (Sensoray Company Inc.)");
2389 MODULE_LICENSE("GPL");
2390 MODULE_VERSION(S2255_VERSION);
2391 /*(DEBLOBBED)*/