drivers/media/usb/s2255/s2255drv.c

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