/*
 * OmniVision OV511 Camera-to-USB Bridge Driver
 *
 * Copyright (c) 1999-2003 Mark W. McClelland
 * Original decompression code Copyright 1998-2000 OmniVision Technologies
 * Many improvements by Bret Wallach <bwallac1@san.rr.com>
 * Color fixes by by Orion Sky Lawlor <olawlor@acm.org> (2/26/2000)
 * Snapshot code by Kevin Moore
 * OV7620 fixes by Charl P. Botha <cpbotha@ieee.org>
 * Changes by Claudio Matsuoka <claudio@conectiva.com>
 * Original SAA7111A code by Dave Perks <dperks@ibm.net>
 * URB error messages from pwc driver by Nemosoft
 * generic_ioctl() code from videodev.c by Gerd Knorr and Alan Cox
 * Memory management (rvmalloc) code from bttv driver, by Gerd Knorr and others
 *
 * Based on the Linux CPiA driver written by Peter Pregler,
 * Scott J. Bertin and Johannes Erdfelt.
 * 
 * Please see the file: linux/Documentation/usb/ov511.txt 
 * and the website at:  http://alpha.dyndns.org/ov511
 * for more info.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <linux/config.h>
#include <linux/version.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/fs.h>
#include <linux/vmalloc.h>
#include <linux/slab.h>
#include <linux/proc_fs.h>
#include <linux/ctype.h>
#include <linux/pagemap.h>
#include <asm/io.h>
#include <asm/semaphore.h>
#include <asm/processor.h>
#include <linux/wrapper.h>
#include <linux/mm.h>

#if defined (__i386__)
        #include <asm/cpufeature.h>
#endif

#include "ov511.h"

/*
 * Version Information
 */
#define DRIVER_VERSION "v1.63 for Linux 2.4"
#define EMAIL "mark@alpha.dyndns.org"
#define DRIVER_AUTHOR "Mark McClelland <mark@alpha.dyndns.org> & Bret Wallach \
        & Orion Sky Lawlor <olawlor@acm.org> & Kevin Moore & Charl P. Botha \
        <cpbotha@ieee.org> & Claudio Matsuoka <claudio@conectiva.com>"
#define DRIVER_DESC "ov511 USB Camera Driver"

#define OV511_I2C_RETRIES 3
#define ENABLE_Y_QUANTABLE 1
#define ENABLE_UV_QUANTABLE 1

#define OV511_MAX_UNIT_VIDEO 16

/* Pixel count * 3 bytes for RGB */
#define MAX_FRAME_SIZE(w, h) ((w) * (h) * 3)

#define MAX_DATA_SIZE(w, h) (MAX_FRAME_SIZE(w, h) + sizeof(struct timeval))

/* Max size * bytes per YUV420 pixel (1.5) + one extra isoc frame for safety */
#define MAX_RAW_DATA_SIZE(w, h) ((w) * (h) * 3 / 2 + 1024)

#define FATAL_ERROR(rc) ((rc) < 0 && (rc) != -EPERM)

/**********************************************************************
 * Module Parameters
 * (See ov511.txt for detailed descriptions of these)
 **********************************************************************/

/* These variables (and all static globals) default to zero */
static int autobright           = 1;
static int autogain             = 1;
static int autoexp              = 1;
static int debug;
static int snapshot;
static int fix_rgb_offset;
static int force_rgb;
static int cams                 = 1;
static int compress;
static int testpat;
static int dumppix;
static int led                  = 1;
static int dump_bridge;
static int dump_sensor;
static int printph;
static int phy                  = 0x1f;
static int phuv                 = 0x05;
static int pvy                  = 0x06;
static int pvuv                 = 0x06;
static int qhy                  = 0x14;
static int qhuv                 = 0x03;
static int qvy                  = 0x04;
static int qvuv                 = 0x04;
static int lightfreq;
static int bandingfilter;
static int clockdiv             = -1;
static int packetsize           = -1;
static int framedrop            = -1;
static int fastset;
static int force_palette;
static int backlight;
static int unit_video[OV511_MAX_UNIT_VIDEO];
static int remove_zeros;
static int mirror;
static int ov518_color;

MODULE_PARM(autobright, "i");
MODULE_PARM_DESC(autobright, "Sensor automatically changes brightness");
MODULE_PARM(autogain, "i");
MODULE_PARM_DESC(autogain, "Sensor automatically changes gain");
MODULE_PARM(autoexp, "i");
MODULE_PARM_DESC(autoexp, "Sensor automatically changes exposure");
MODULE_PARM(debug, "i");
MODULE_PARM_DESC(debug,
  "Debug level: 0=none, 1=inits, 2=warning, 3=config, 4=functions, 5=max");
MODULE_PARM(snapshot, "i");
MODULE_PARM_DESC(snapshot, "Enable snapshot mode");
MODULE_PARM(fix_rgb_offset, "i");
MODULE_PARM_DESC(fix_rgb_offset,
  "Fix vertical misalignment of red and blue at 640x480");
MODULE_PARM(force_rgb, "i");
MODULE_PARM_DESC(force_rgb, "Read RGB instead of BGR");
MODULE_PARM(cams, "i");
MODULE_PARM_DESC(cams, "Number of simultaneous cameras");
MODULE_PARM(compress, "i");
MODULE_PARM_DESC(compress, "Turn on compression (not reliable yet)");
MODULE_PARM(testpat, "i");
MODULE_PARM_DESC(testpat,
  "Replace image with vertical bar testpattern (only partially working)");
MODULE_PARM(dumppix, "i");
MODULE_PARM_DESC(dumppix, "Dump raw pixel data");
MODULE_PARM(led, "i");
MODULE_PARM_DESC(led,
  "LED policy (OV511+ or later). 0=off, 1=on (default), 2=auto (on when open)");
MODULE_PARM(dump_bridge, "i");
MODULE_PARM_DESC(dump_bridge, "Dump the bridge registers");
MODULE_PARM(dump_sensor, "i");
MODULE_PARM_DESC(dump_sensor, "Dump the sensor registers");
MODULE_PARM(printph, "i");
MODULE_PARM_DESC(printph, "Print frame start/end headers");
MODULE_PARM(phy, "i");
MODULE_PARM_DESC(phy, "Prediction range (horiz. Y)");
MODULE_PARM(phuv, "i");
MODULE_PARM_DESC(phuv, "Prediction range (horiz. UV)");
MODULE_PARM(pvy, "i");
MODULE_PARM_DESC(pvy, "Prediction range (vert. Y)");
MODULE_PARM(pvuv, "i");
MODULE_PARM_DESC(pvuv, "Prediction range (vert. UV)");
MODULE_PARM(qhy, "i");
MODULE_PARM_DESC(qhy, "Quantization threshold (horiz. Y)");
MODULE_PARM(qhuv, "i");
MODULE_PARM_DESC(qhuv, "Quantization threshold (horiz. UV)");
MODULE_PARM(qvy, "i");
MODULE_PARM_DESC(qvy, "Quantization threshold (vert. Y)");
MODULE_PARM(qvuv, "i");
MODULE_PARM_DESC(qvuv, "Quantization threshold (vert. UV)");
MODULE_PARM(lightfreq, "i");
MODULE_PARM_DESC(lightfreq,
  "Light frequency. Set to 50 or 60 Hz, or zero for default settings");
MODULE_PARM(bandingfilter, "i");
MODULE_PARM_DESC(bandingfilter,
  "Enable banding filter (to reduce effects of fluorescent lighting)");
MODULE_PARM(clockdiv, "i");
MODULE_PARM_DESC(clockdiv, "Force pixel clock divisor to a specific value");
MODULE_PARM(packetsize, "i");
MODULE_PARM_DESC(packetsize, "Force a specific isoc packet size");
MODULE_PARM(framedrop, "i");
MODULE_PARM_DESC(framedrop, "Force a specific frame drop register setting");
MODULE_PARM(fastset, "i");
MODULE_PARM_DESC(fastset, "Allows picture settings to take effect immediately");
MODULE_PARM(force_palette, "i");
MODULE_PARM_DESC(force_palette, "Force the palette to a specific value");
MODULE_PARM(backlight, "i");
MODULE_PARM_DESC(backlight, "For objects that are lit from behind");
MODULE_PARM(unit_video, "1-" __MODULE_STRING(OV511_MAX_UNIT_VIDEO) "i");
MODULE_PARM_DESC(unit_video,
  "Force use of specific minor number(s). 0 is not allowed.");
MODULE_PARM(remove_zeros, "i");
MODULE_PARM_DESC(remove_zeros,
  "Remove zero-padding from uncompressed incoming data");
MODULE_PARM(mirror, "i");
MODULE_PARM_DESC(mirror, "Reverse image horizontally");
MODULE_PARM(ov518_color, "i");
MODULE_PARM_DESC(ov518_color, "Enable OV518 color (experimental)");

MODULE_AUTHOR(DRIVER_AUTHOR);
MODULE_DESCRIPTION(DRIVER_DESC);
MODULE_LICENSE("GPL");

/**********************************************************************
 * Miscellaneous Globals
 **********************************************************************/

static struct usb_driver ov511_driver;

static struct ov51x_decomp_ops *ov511_decomp_ops;
static struct ov51x_decomp_ops *ov511_mmx_decomp_ops;
static struct ov51x_decomp_ops *ov518_decomp_ops;
static struct ov51x_decomp_ops *ov518_mmx_decomp_ops;

/* Number of times to retry a failed I2C transaction. Increase this if you
 * are getting "Failed to read sensor ID..." */
static int i2c_detect_tries = 5;

/* MMX support is present in kernel and CPU. Checked upon decomp module load. */
static int ov51x_mmx_available;

static struct usb_device_id device_table [] = {
        { USB_DEVICE(VEND_OMNIVISION, PROD_OV511) },
        { USB_DEVICE(VEND_OMNIVISION, PROD_OV511PLUS) },
        { USB_DEVICE(VEND_OMNIVISION, PROD_OV518) },
        { USB_DEVICE(VEND_OMNIVISION, PROD_OV518PLUS) },
        { USB_DEVICE(VEND_MATTEL, PROD_ME2CAM) },
        { }  /* Terminating entry */
};

MODULE_DEVICE_TABLE (usb, device_table);

static unsigned char yQuanTable511[] = OV511_YQUANTABLE;
static unsigned char uvQuanTable511[] = OV511_UVQUANTABLE;
static unsigned char yQuanTable518[] = OV518_YQUANTABLE;
static unsigned char uvQuanTable518[] = OV518_UVQUANTABLE;

/**********************************************************************
 * Symbolic Names
 **********************************************************************/

/* Known OV511-based cameras */
static struct symbolic_list camlist[] = {
        {   0, "Generic Camera (no ID)" },
        {   1, "Mustek WCam 3X" },
        {   3, "D-Link DSB-C300" },
        {   4, "Generic OV511/OV7610" },
        {   5, "Puretek PT-6007" },
        {   6, "Lifeview USB Life TV (NTSC)" },
        {  21, "Creative Labs WebCam 3" },
        {  22, "Lifeview USB Life TV (PAL D/K+B/G)" },
        {  36, "Koala-Cam" },
        {  38, "Lifeview USB Life TV (PAL)" },
        {  41, "Samsung Anycam MPC-M10" },
        {  43, "Mtekvision Zeca MV402" },
        {  46, "Suma eON" },
        {  70, "Lifeview USB Life TV (PAL/SECAM)" },
        { 100, "Lifeview RoboCam" },
        { 102, "AverMedia InterCam Elite" },
        { 112, "MediaForte MV300" },    /* or OV7110 evaluation kit */
        { 134, "Ezonics EZCam II" },
        { 192, "Webeye 2000B" },
        { 253, "Alpha Vision Tech. AlphaCam SE" },
        {  -1, NULL }
};

/* Video4Linux1 Palettes */
static struct symbolic_list v4l1_plist[] = {
        { VIDEO_PALETTE_GREY,   "GREY" },
        { VIDEO_PALETTE_HI240,  "HI240" },
        { VIDEO_PALETTE_RGB565, "RGB565" },
        { VIDEO_PALETTE_RGB24,  "RGB24" },
        { VIDEO_PALETTE_RGB32,  "RGB32" },
        { VIDEO_PALETTE_RGB555, "RGB555" },
        { VIDEO_PALETTE_YUV422, "YUV422" },
        { VIDEO_PALETTE_YUYV,   "YUYV" },
        { VIDEO_PALETTE_UYVY,   "UYVY" },
        { VIDEO_PALETTE_YUV420, "YUV420" },
        { VIDEO_PALETTE_YUV411, "YUV411" },
        { VIDEO_PALETTE_RAW,    "RAW" },
        { VIDEO_PALETTE_YUV422P,"YUV422P" },
        { VIDEO_PALETTE_YUV411P,"YUV411P" },
        { VIDEO_PALETTE_YUV420P,"YUV420P" },
        { VIDEO_PALETTE_YUV410P,"YUV410P" },
        { -1, NULL }
};

static struct symbolic_list brglist[] = {
        { BRG_OV511,            "OV511" },
        { BRG_OV511PLUS,        "OV511+" },
        { BRG_OV518,            "OV518" },
        { BRG_OV518PLUS,        "OV518+" },
        { -1, NULL }
};

#if defined(CONFIG_VIDEO_PROC_FS)
static struct symbolic_list senlist[] = {
        { SEN_OV76BE,   "OV76BE" },
        { SEN_OV7610,   "OV7610" },
        { SEN_OV7620,   "OV7620" },
        { SEN_OV7620AE, "OV7620AE" },
        { SEN_OV6620,   "OV6620" },
        { SEN_OV6630,   "OV6630" },
        { SEN_OV6630AE, "OV6630AE" },
        { SEN_OV6630AF, "OV6630AF" },
        { SEN_OV8600,   "OV8600" },
        { SEN_KS0127,   "KS0127" },
        { SEN_KS0127B,  "KS0127B" },
        { SEN_SAA7111A, "SAA7111A" },
        { -1, NULL }
};
#endif

/* URB error codes: */
static struct symbolic_list urb_errlist[] = {
        { -ENOSR,       "Buffer error (overrun)" },
        { -EPIPE,       "Stalled (device not responding)" },
        { -EOVERFLOW,   "Babble (bad cable?)" },
        { -EPROTO,      "Bit-stuff error (bad cable?)" },
        { -EILSEQ,      "CRC/Timeout" },
        { -ETIMEDOUT,   "NAK (device does not respond)" },
        { -1, NULL }
};

/**********************************************************************
 * Prototypes
 **********************************************************************/

static void ov51x_clear_snapshot(struct usb_ov511 *);
static int sensor_get_picture(struct usb_ov511 *,
                              struct video_picture *);
#if defined(CONFIG_PROC_FS) && defined(CONFIG_VIDEO_PROC_FS)
static int sensor_get_exposure(struct usb_ov511 *, unsigned char *);
static int ov51x_control_ioctl(struct inode *, struct file *, unsigned int,
                               unsigned long);
static int ov51x_check_snapshot(struct usb_ov511 *);
#endif

/**********************************************************************
 * Memory management
 **********************************************************************/

/* Here we want the physical address of the memory.
 * This is used when initializing the contents of the area.
 */
static inline unsigned long
kvirt_to_pa(unsigned long adr)
{
        unsigned long kva, ret;

        kva = (unsigned long) page_address(vmalloc_to_page((void *)adr));
        kva |= adr & (PAGE_SIZE-1); /* restore the offset */
        ret = __pa(kva);
        return ret;
}

static void *
rvmalloc(unsigned long size)
{
        void *mem;
        unsigned long adr;

        size = PAGE_ALIGN(size);
        mem = vmalloc_32(size);
        if (!mem)
                return NULL;

        memset(mem, 0, size); /* Clear the ram out, no junk to the user */
        adr = (unsigned long) mem;
        while (size > 0) {
                mem_map_reserve(vmalloc_to_page((void *)adr));
                adr += PAGE_SIZE;
                size -= PAGE_SIZE;
        }

        return mem;
}

static void
rvfree(void *mem, unsigned long size)
{
        unsigned long adr;

        if (!mem)
                return;

        adr = (unsigned long) mem;
        while ((long) size > 0) {
                mem_map_unreserve(vmalloc_to_page((void *)adr));
                adr += PAGE_SIZE;
                size -= PAGE_SIZE;
        }
        vfree(mem);
}

/**********************************************************************
 * /proc interface
 * Based on the CPiA driver version 0.7.4 -claudio
 **********************************************************************/

#if defined(CONFIG_PROC_FS) && defined(CONFIG_VIDEO_PROC_FS)

static struct proc_dir_entry *ov511_proc_entry = NULL;
extern struct proc_dir_entry *video_proc_entry;

static struct file_operations ov511_control_fops = {
        .ioctl =        ov51x_control_ioctl,
};

#define YES_NO(x) ((x) ? "yes" : "no")

/* /proc/video/ov511/<minor#>/info */
static int
ov511_read_proc_info(char *page, char **start, off_t off, int count, int *eof,
                     void *data)
{
        char *out = page;
        int i, len;
        struct usb_ov511 *ov = data;
        struct video_picture p;
        unsigned char exp;

        if (!ov || !ov->dev)
                return -ENODEV;

        sensor_get_picture(ov, &p);
        sensor_get_exposure(ov, &exp);

        /* IMPORTANT: This output MUST be kept under PAGE_SIZE
         *            or we need to get more sophisticated. */

        out += sprintf(out, "driver_version  : %s\n", DRIVER_VERSION);
        out += sprintf(out, "custom_id       : %d\n", ov->customid);
        out += sprintf(out, "model           : %s\n", ov->desc);
        out += sprintf(out, "streaming       : %s\n", YES_NO(ov->streaming));
        out += sprintf(out, "grabbing        : %s\n", YES_NO(ov->grabbing));
        out += sprintf(out, "compress        : %s\n", YES_NO(ov->compress));
        out += sprintf(out, "subcapture      : %s\n", YES_NO(ov->sub_flag));
        out += sprintf(out, "sub_size        : %d %d %d %d\n",
                       ov->subx, ov->suby, ov->subw, ov->subh);
        out += sprintf(out, "data_format     : %s\n",
                       force_rgb ? "RGB" : "BGR");
        out += sprintf(out, "brightness      : %d\n", p.brightness >> 8);
        out += sprintf(out, "colour          : %d\n", p.colour >> 8);
        out += sprintf(out, "contrast        : %d\n", p.contrast >> 8);
        out += sprintf(out, "hue             : %d\n", p.hue >> 8);
        out += sprintf(out, "exposure        : %d\n", exp);
        out += sprintf(out, "num_frames      : %d\n", OV511_NUMFRAMES);
        for (i = 0; i < OV511_NUMFRAMES; i++) {
                out += sprintf(out, "frame           : %d\n", i);
                out += sprintf(out, "  depth         : %d\n",
                               ov->frame[i].depth);
                out += sprintf(out, "  size          : %d %d\n",
                               ov->frame[i].width, ov->frame[i].height);
                out += sprintf(out, "  format        : %s\n",
                               symbolic(v4l1_plist, ov->frame[i].format));
                out += sprintf(out, "  data_buffer   : 0x%p\n",
                               ov->frame[i].data);
        }
        out += sprintf(out, "snap_enabled    : %s\n", YES_NO(ov->snap_enabled));
        out += sprintf(out, "bridge          : %s\n",
                       symbolic(brglist, ov->bridge));
        out += sprintf(out, "sensor          : %s\n",
                       symbolic(senlist, ov->sensor));
        out += sprintf(out, "packet_size     : %d\n", ov->packet_size);
        out += sprintf(out, "framebuffer     : 0x%p\n", ov->fbuf);
        out += sprintf(out, "packet_numbering: %d\n", ov->packet_numbering);
        out += sprintf(out, "topology        : %s\n", ov->usb_path);

        len = out - page;
        len -= off;
        if (len < count) {
                *eof = 1;
                if (len <= 0)
                        return 0;
        } else
                len = count;

        *start = page + off;

        return len;
}

/* /proc/video/ov511/<minor#>/button
 *
 * When the camera's button is pressed, the output of this will change from a
 * 0 to a 1 (ASCII). It will retain this value until it is read, after which
 * it will reset to zero.
 *
 * SECURITY NOTE: Since reading this file can change the state of the snapshot
 * status, it is important for applications that open it to keep it locked
 * against access by other processes, using flock() or a similar mechanism. No
 * locking is provided by this driver.
 */
static int
ov511_read_proc_button(char *page, char **start, off_t off, int count, int *eof,
                       void *data)
{
        char *out = page;
        int len, status;
        struct usb_ov511 *ov = data;

        if (!ov || !ov->dev)
                return -ENODEV;

        status = ov51x_check_snapshot(ov);
        out += sprintf(out, "%d", status);

        if (status)
                ov51x_clear_snapshot(ov);

        len = out - page;
        len -= off;
        if (len < count) {
                *eof = 1;
                if (len <= 0)
                        return 0;
        } else {
                len = count;
        }

        *start = page + off;

        return len;
}

static void
create_proc_ov511_cam(struct usb_ov511 *ov)
{
        char dirname[10];

        if (!ov511_proc_entry || !ov)
                return;

        /* Create per-device directory */
        snprintf(dirname, 10, "%d", ov->vdev.minor);
        PDEBUG(4, "creating /proc/video/ov511/%s/", dirname);
        ov->proc_devdir = create_proc_entry(dirname, S_IFDIR, ov511_proc_entry);
        if (!ov->proc_devdir)
                return;
        ov->proc_devdir->owner = THIS_MODULE;

        /* Create "info" entry (human readable device information) */
        PDEBUG(4, "creating /proc/video/ov511/%s/info", dirname);
        ov->proc_info = create_proc_read_entry("info", S_IFREG|S_IRUGO|S_IWUSR,
                ov->proc_devdir, ov511_read_proc_info, ov);
        if (!ov->proc_info)
                return;
        ov->proc_info->owner = THIS_MODULE;

        /* Don't create it if old snapshot mode on (would cause race cond.) */
        if (!snapshot) {
                /* Create "button" entry (snapshot button status) */
                PDEBUG(4, "creating /proc/video/ov511/%s/button", dirname);
                ov->proc_button = create_proc_read_entry("button",
                        S_IFREG|S_IRUGO|S_IWUSR, ov->proc_devdir,
                        ov511_read_proc_button, ov);
                if (!ov->proc_button)
                        return;
                ov->proc_button->owner = THIS_MODULE;
        }

        /* Create "control" entry (ioctl() interface) */
        PDEBUG(4, "creating /proc/video/ov511/%s/control", dirname);
        lock_kernel();
        ov->proc_control = create_proc_entry("control", S_IFREG|S_IRUGO|S_IWUSR,
                ov->proc_devdir);
        if (!ov->proc_control) {
                unlock_kernel();
                return;
        }
        ov->proc_control->owner = THIS_MODULE;
        ov->proc_control->data = ov;
        ov->proc_control->proc_fops = &ov511_control_fops;
        unlock_kernel();
}

static void
destroy_proc_ov511_cam(struct usb_ov511 *ov)
{
        char dirname[10];

        if (!ov || !ov->proc_devdir)
                return;

        snprintf(dirname, 10, "%d", ov->vdev.minor);

        /* Destroy "control" entry */
        if (ov->proc_control) {
                PDEBUG(4, "destroying /proc/video/ov511/%s/control", dirname);
                remove_proc_entry("control", ov->proc_devdir);
                ov->proc_control = NULL;
        }

        /* Destroy "button" entry */
        if (ov->proc_button) {
                PDEBUG(4, "destroying /proc/video/ov511/%s/button", dirname);
                remove_proc_entry("button", ov->proc_devdir);
                ov->proc_button = NULL;
        }

        /* Destroy "info" entry */
        if (ov->proc_info) {
                PDEBUG(4, "destroying /proc/video/ov511/%s/info", dirname);
                remove_proc_entry("info", ov->proc_devdir);
                ov->proc_info = NULL;
        }

        /* Destroy per-device directory */
        PDEBUG(4, "destroying /proc/video/ov511/%s/", dirname);
        remove_proc_entry(dirname, ov511_proc_entry);
        ov->proc_devdir = NULL;
}

static void
proc_ov511_create(void)
{
        /* No current standard here. Alan prefers /proc/video/ as it keeps
         * /proc "less cluttered than /proc/randomcardifoundintheshed/"
         * -claudio
         */
        if (video_proc_entry == NULL) {
                err("Error: /proc/video/ does not exist");
                return;
        }

        ov511_proc_entry = create_proc_entry("ov511", S_IFDIR,
                                             video_proc_entry);

        if (ov511_proc_entry)
                ov511_proc_entry->owner = THIS_MODULE;
        else
                err("Unable to create /proc/video/ov511");
}

static void
proc_ov511_destroy(void)
{
        PDEBUG(3, "removing /proc/video/ov511");

        if (ov511_proc_entry == NULL)
                return;

        remove_proc_entry("ov511", video_proc_entry);
}
#endif /* CONFIG_PROC_FS && CONFIG_VIDEO_PROC_FS */

/**********************************************************************
 *
 * Register I/O
 *
 **********************************************************************/

/* Write an OV51x register */
static int
reg_w(struct usb_ov511 *ov, unsigned char reg, unsigned char value)
{
        int rc;

        PDEBUG(5, "0x%02X:0x%02X", reg, value);

        down(&ov->cbuf_lock);
        ov->cbuf[0] = value;
        rc = usb_control_msg(ov->dev,
                             usb_sndctrlpipe(ov->dev, 0),
                             (ov->bclass == BCL_OV518)?1:2 /* REG_IO */,
                             USB_TYPE_VENDOR | USB_RECIP_DEVICE,
                             0, (__u16)reg, &ov->cbuf[0], 1, HZ);
        up(&ov->cbuf_lock);

        if (rc < 0)
                err("reg write: error %d: %s", rc, symbolic(urb_errlist, rc));

        return rc;
}

/* Read from an OV51x register */
/* returns: negative is error, pos or zero is data */
static int
reg_r(struct usb_ov511 *ov, unsigned char reg)
{
        int rc;

        down(&ov->cbuf_lock);
        rc = usb_control_msg(ov->dev,
                             usb_rcvctrlpipe(ov->dev, 0),
                             (ov->bclass == BCL_OV518)?1:3 /* REG_IO */,
                             USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
                             0, (__u16)reg, &ov->cbuf[0], 1, HZ);

        if (rc < 0) {
                err("reg read: error %d: %s", rc, symbolic(urb_errlist, rc));
        } else {
                rc = ov->cbuf[0];
                PDEBUG(5, "0x%02X:0x%02X", reg, ov->cbuf[0]);
        }

        up(&ov->cbuf_lock);

        return rc;
}

/*
 * Writes bits at positions specified by mask to an OV51x reg. Bits that are in
 * the same position as 1's in "mask" are cleared and set to "value". Bits
 * that are in the same position as 0's in "mask" are preserved, regardless
 * of their respective state in "value".
 */
static int
reg_w_mask(struct usb_ov511 *ov,
           unsigned char reg,
           unsigned char value,
           unsigned char mask)
{
        int ret;
        unsigned char oldval, newval;

        ret = reg_r(ov, reg);
        if (ret < 0)
                return ret;

        oldval = (unsigned char) ret;
        oldval &= (~mask);              /* Clear the masked bits */
        value &= mask;                  /* Enforce mask on value */
        newval = oldval | value;        /* Set the desired bits */

        return (reg_w(ov, reg, newval));
}

/* 
 * Writes multiple (n) byte value to a single register. Only valid with certain
 * registers (0x30 and 0xc4 - 0xce).
 */
static int
ov518_reg_w32(struct usb_ov511 *ov, unsigned char reg, u32 val, int n)
{
        int rc;

        PDEBUG(5, "0x%02X:%7d, n=%d", reg, val, n);

        down(&ov->cbuf_lock);

        *((u32 *)ov->cbuf) = __cpu_to_le32(val);

        rc = usb_control_msg(ov->dev,
                             usb_sndctrlpipe(ov->dev, 0),
                             1 /* REG_IO */,
                             USB_TYPE_VENDOR | USB_RECIP_DEVICE,
                             0, (__u16)reg, ov->cbuf, n, HZ);
        up(&ov->cbuf_lock);

        if (rc < 0)
                err("reg write multiple: error %d: %s", rc,
                    symbolic(urb_errlist, rc));

        return rc;
}

static int
ov511_upload_quan_tables(struct usb_ov511 *ov)
{
        unsigned char *pYTable = yQuanTable511;
        unsigned char *pUVTable = uvQuanTable511;
        unsigned char val0, val1;
        int i, rc, reg = R511_COMP_LUT_BEGIN;

        PDEBUG(4, "Uploading quantization tables");

        for (i = 0; i < OV511_QUANTABLESIZE / 2; i++) {
                if (ENABLE_Y_QUANTABLE) {
                        val0 = *pYTable++;
                        val1 = *pYTable++;
                        val0 &= 0x0f;
                        val1 &= 0x0f;
                        val0 |= val1 << 4;
                        rc = reg_w(ov, reg, val0);
                        if (rc < 0)
                                return rc;
                }

                if (ENABLE_UV_QUANTABLE) {
                        val0 = *pUVTable++;
                        val1 = *pUVTable++;
                        val0 &= 0x0f;
                        val1 &= 0x0f;
                        val0 |= val1 << 4;
                        rc = reg_w(ov, reg + OV511_QUANTABLESIZE/2, val0);
                        if (rc < 0)
                                return rc;
                }

                reg++;
        }

        return 0;
}

/* OV518 quantization tables are 8x4 (instead of 8x8) */
static int
ov518_upload_quan_tables(struct usb_ov511 *ov)
{
        unsigned char *pYTable = yQuanTable518;
        unsigned char *pUVTable = uvQuanTable518;
        unsigned char val0, val1;
        int i, rc, reg = R511_COMP_LUT_BEGIN;

        PDEBUG(4, "Uploading quantization tables");

        for (i = 0; i < OV518_QUANTABLESIZE / 2; i++) {
                if (ENABLE_Y_QUANTABLE) {
                        val0 = *pYTable++;
                        val1 = *pYTable++;
                        val0 &= 0x0f;
                        val1 &= 0x0f;
                        val0 |= val1 << 4;
                        rc = reg_w(ov, reg, val0);
                        if (rc < 0)
                                return rc;
                }

                if (ENABLE_UV_QUANTABLE) {
                        val0 = *pUVTable++;
                        val1 = *pUVTable++;
                        val0 &= 0x0f;
                        val1 &= 0x0f;
                        val0 |= val1 << 4;
                        rc = reg_w(ov, reg + OV518_QUANTABLESIZE/2, val0);
                        if (rc < 0)
                                return rc;
                }

                reg++;
        }

        return 0;
}

static int
ov51x_reset(struct usb_ov511 *ov, unsigned char reset_type)
{
        int rc;

        /* Setting bit 0 not allowed on 518/518Plus */
        if (ov->bclass == BCL_OV518)
                reset_type &= 0xfe;

        PDEBUG(4, "Reset: type=0x%02X", reset_type);

        rc = reg_w(ov, R51x_SYS_RESET, reset_type);
        rc = reg_w(ov, R51x_SYS_RESET, 0);

        if (rc < 0)
                err("reset: command failed");

        return rc;
}

/**********************************************************************
 *
 * Low-level I2C I/O functions
 *
 **********************************************************************/

/* NOTE: Do not call this function directly!
 * The OV518 I2C I/O procedure is different, hence, this function.
 * This is normally only called from i2c_w(). Note that this function
 * always succeeds regardless of whether the sensor is present and working.
 */
static int
ov518_i2c_write_internal(struct usb_ov511 *ov,
                         unsigned char reg,
                         unsigned char value)
{
        int rc;

        PDEBUG(5, "0x%02X:0x%02X", reg, value);

        /* Select camera register */
        rc = reg_w(ov, R51x_I2C_SADDR_3, reg);
        if (rc < 0) return rc;

        /* Write "value" to I2C data port of OV511 */
        rc = reg_w(ov, R51x_I2C_DATA, value);
        if (rc < 0) return rc;

        /* Initiate 3-byte write cycle */
        rc = reg_w(ov, R518_I2C_CTL, 0x01);
        if (rc < 0) return rc;

        return 0;
}

/* NOTE: Do not call this function directly! */
static int
ov511_i2c_write_internal(struct usb_ov511 *ov,
                         unsigned char reg,
                         unsigned char value)
{
        int rc, retries;

        PDEBUG(5, "0x%02X:0x%02X", reg, value);

        /* Three byte write cycle */
        for (retries = OV511_I2C_RETRIES; ; ) {
                /* Select camera register */
                rc = reg_w(ov, R51x_I2C_SADDR_3, reg);
                if (rc < 0) return rc;

                /* Write "value" to I2C data port of OV511 */
                rc = reg_w(ov, R51x_I2C_DATA, value);
                if (rc < 0) return rc;

                /* Initiate 3-byte write cycle */
                rc = reg_w(ov, R511_I2C_CTL, 0x01);
                if (rc < 0) return rc;

                do rc = reg_r(ov, R511_I2C_CTL);
                while (rc > 0 && ((rc&1) == 0)); /* Retry until idle */
                if (rc < 0) return rc;

                if ((rc&2) == 0) /* Ack? */
                        break;
#if 0
                /* I2C abort */
                reg_w(ov, R511_I2C_CTL, 0x10);
#endif
                if (--retries < 0) {
                        err("i2c write retries exhausted");
                        return -1;
                }
        }

        return 0;
}

/* NOTE: Do not call this function directly!
 * The OV518 I2C I/O procedure is different, hence, this function.
 * This is normally only called from i2c_r(). Note that this function
 * always succeeds regardless of whether the sensor is present and working.
 */
static int
ov518_i2c_read_internal(struct usb_ov511 *ov, unsigned char reg)
{
        int rc, value;

        /* Select camera register */
        rc = reg_w(ov, R51x_I2C_SADDR_2, reg);
        if (rc < 0) return rc;

        /* Initiate 2-byte write cycle */
        rc = reg_w(ov, R518_I2C_CTL, 0x03);
        if (rc < 0) return rc;

        /* Initiate 2-byte read cycle */
        rc = reg_w(ov, R518_I2C_CTL, 0x05);
        if (rc < 0) return rc;

        value = reg_r(ov, R51x_I2C_DATA);

        PDEBUG(5, "0x%02X:0x%02X", reg, value);

        return value;
}

/* NOTE: Do not call this function directly!
 * returns: negative is error, pos or zero is data */
static int
ov511_i2c_read_internal(struct usb_ov511 *ov, unsigned char reg)
{
        int rc, value, retries;

        /* Two byte write cycle */
        for (retries = OV511_I2C_RETRIES; ; ) {
                /* Select camera register */
                rc = reg_w(ov, R51x_I2C_SADDR_2, reg);
                if (rc < 0) return rc;

                /* Initiate 2-byte write cycle */
                rc = reg_w(ov, R511_I2C_CTL, 0x03);
                if (rc < 0) return rc;

                do rc = reg_r(ov, R511_I2C_CTL);
                while (rc > 0 && ((rc&1) == 0)); /* Retry until idle */
                if (rc < 0) return rc;

                if ((rc&2) == 0) /* Ack? */
                        break;

                /* I2C abort */
                reg_w(ov, R511_I2C_CTL, 0x10);

                if (--retries < 0) {
                        err("i2c write retries exhausted");

                        return -1;
                }
        }

        /* Two byte read cycle */
        for (retries = OV511_I2C_RETRIES; ; ) {
                /* Initiate 2-byte read cycle */
                rc = reg_w(ov, R511_I2C_CTL, 0x05);
                if (rc < 0) return rc;

                do rc = reg_r(ov, R511_I2C_CTL);
                while (rc > 0 && ((rc&1) == 0)); /* Retry until idle */
                if (rc < 0) return rc;

                if ((rc&2) == 0) /* Ack? */
                        break;

                /* I2C abort */
                rc = reg_w(ov, R511_I2C_CTL, 0x10);
                if (rc < 0) return rc;

                if (--retries < 0) {
                        err("i2c read retries exhausted");
                        return -1;
                }
        }

        value = reg_r(ov, R51x_I2C_DATA);

        PDEBUG(5, "0x%02X:0x%02X", reg, value);

        /* This is needed to make i2c_w() work */
        rc = reg_w(ov, R511_I2C_CTL, 0x05);
        if (rc < 0)
                return rc;

        return value;
}

/* returns: negative is error, pos or zero is data */
static int
i2c_r(struct usb_ov511 *ov, unsigned char reg)
{
        int rc;

        down(&ov->i2c_lock);

        if (ov->bclass == BCL_OV518)
                rc = ov518_i2c_read_internal(ov, reg);
        else
                rc = ov511_i2c_read_internal(ov, reg);

        up(&ov->i2c_lock);

        return rc;
}

static int
i2c_w(struct usb_ov511 *ov, unsigned char reg, unsigned char value)
{
        int rc;

        down(&ov->i2c_lock);

        if (ov->bclass == BCL_OV518)
                rc = ov518_i2c_write_internal(ov, reg, value);
        else
                rc = ov511_i2c_write_internal(ov, reg, value);

        up(&ov->i2c_lock);

        return rc;
}

/* Do not call this function directly! */
static int
ov51x_i2c_write_mask_internal(struct usb_ov511 *ov,
                              unsigned char reg,
                              unsigned char value,
                              unsigned char mask)
{
        int rc;
        unsigned char oldval, newval;

        if (mask == 0xff) {
                newval = value;
        } else {
                if (ov->bclass == BCL_OV518)
                        rc = ov518_i2c_read_internal(ov, reg);
                else
                        rc = ov511_i2c_read_internal(ov, reg);
                if (rc < 0)
                        return rc;

                oldval = (unsigned char) rc;
                oldval &= (~mask);              /* Clear the masked bits */
                value &= mask;                  /* Enforce mask on value */
                newval = oldval | value;        /* Set the desired bits */
        }

        if (ov->bclass == BCL_OV518)
                return (ov518_i2c_write_internal(ov, reg, newval));
        else
                return (ov511_i2c_write_internal(ov, reg, newval));
}

/* Writes bits at positions specified by mask to an I2C reg. Bits that are in
 * the same position as 1's in "mask" are cleared and set to "value". Bits
 * that are in the same position as 0's in "mask" are preserved, regardless
 * of their respective state in "value".
 */
static int
i2c_w_mask(struct usb_ov511 *ov,
           unsigned char reg,
           unsigned char value,
           unsigned char mask)
{
        int rc;

        down(&ov->i2c_lock);
        rc = ov51x_i2c_write_mask_internal(ov, reg, value, mask);
        up(&ov->i2c_lock);

        return rc;
}

/* Set the read and write slave IDs. The "slave" argument is the write slave,
 * and the read slave will be set to (slave + 1). ov->i2c_lock should be held
 * when calling this. This should not be called from outside the i2c I/O
 * functions.
 */
static inline int
i2c_set_slave_internal(struct usb_ov511 *ov, unsigned char slave)
{
        int rc;

        rc = reg_w(ov, R51x_I2C_W_SID, slave);
        if (rc < 0) return rc;

        rc = reg_w(ov, R51x_I2C_R_SID, slave + 1);
        if (rc < 0) return rc;

        return 0;
}

/* Write to a specific I2C slave ID and register, using the specified mask */
static int
i2c_w_slave(struct usb_ov511 *ov,
            unsigned char slave,
            unsigned char reg,
            unsigned char value,
            unsigned char mask)
{
        int rc = 0;

        down(&ov->i2c_lock);

        /* Set new slave IDs */
        rc = i2c_set_slave_internal(ov, slave);
        if (rc < 0) goto out;

        rc = ov51x_i2c_write_mask_internal(ov, reg, value, mask);

out:
        /* Restore primary IDs */
        if (i2c_set_slave_internal(ov, ov->primary_i2c_slave) < 0)
                err("Couldn't restore primary I2C slave");

        up(&ov->i2c_lock);
        return rc;
}

/* Read from a specific I2C slave ID and register */
static int
i2c_r_slave(struct usb_ov511 *ov,
            unsigned char slave,
            unsigned char reg)
{
        int rc;

        down(&ov->i2c_lock);

        /* Set new slave IDs */
        rc = i2c_set_slave_internal(ov, slave);
        if (rc < 0) goto out;

        if (ov->bclass == BCL_OV518)
                rc = ov518_i2c_read_internal(ov, reg);
        else
                rc = ov511_i2c_read_internal(ov, reg);

out:
        /* Restore primary IDs */
        if (i2c_set_slave_internal(ov, ov->primary_i2c_slave) < 0)
                err("Couldn't restore primary I2C slave");

        up(&ov->i2c_lock);
        return rc;
}

/* Sets I2C read and write slave IDs. Returns <0 for error */
static int
ov51x_set_slave_ids(struct usb_ov511 *ov, unsigned char sid)
{
        int rc;

        down(&ov->i2c_lock);

        rc = i2c_set_slave_internal(ov, sid);
        if (rc < 0) goto out;

        // FIXME: Is this actually necessary?
        rc = ov51x_reset(ov, OV511_RESET_NOREGS);
        if (rc < 0) goto out;

out:
        up(&ov->i2c_lock);
        return rc;
}

static int
write_regvals(struct usb_ov511 *ov, struct ov511_regvals * pRegvals)
{
        int rc;

        while (pRegvals->bus != OV511_DONE_BUS) {
                if (pRegvals->bus == OV511_REG_BUS) {
                        if ((rc = reg_w(ov, pRegvals->reg, pRegvals->val)) < 0)
                                return rc;
                } else if (pRegvals->bus == OV511_I2C_BUS) {
                        if ((rc = i2c_w(ov, pRegvals->reg, pRegvals->val)) < 0)
                                return rc;
                } else {
                        err("Bad regval array");
                        return -1;
                }
                pRegvals++;
        }
        return 0;
}

#ifdef OV511_DEBUG
static void
dump_i2c_range(struct usb_ov511 *ov, int reg1, int regn)
{
        int i, rc;

        for (i = reg1; i <= regn; i++) {
                rc = i2c_r(ov, i);
                info("Sensor[0x%02X] = 0x%02X", i, rc);
        }
}

static void
dump_i2c_regs(struct usb_ov511 *ov)
{
        info("I2C REGS");
        dump_i2c_range(ov, 0x00, 0x7C);
}

static void
dump_reg_range(struct usb_ov511 *ov, int reg1, int regn)
{
        int i, rc;

        for (i = reg1; i <= regn; i++) {
                rc = reg_r(ov, i);
                info("OV511[0x%02X] = 0x%02X", i, rc);
        }
}

static void
ov511_dump_regs(struct usb_ov511 *ov)
{
        info("CAMERA INTERFACE REGS");
        dump_reg_range(ov, 0x10, 0x1f);
        info("DRAM INTERFACE REGS");
        dump_reg_range(ov, 0x20, 0x23);
        info("ISO FIFO REGS");
        dump_reg_range(ov, 0x30, 0x31);
        info("PIO REGS");
        dump_reg_range(ov, 0x38, 0x39);
        dump_reg_range(ov, 0x3e, 0x3e);
        info("I2C REGS");
        dump_reg_range(ov, 0x40, 0x49);
        info("SYSTEM CONTROL REGS");
        dump_reg_range(ov, 0x50, 0x55);
        dump_reg_range(ov, 0x5e, 0x5f);
        info("OmniCE REGS");
        dump_reg_range(ov, 0x70, 0x79);
        /* NOTE: Quantization tables are not readable. You will get the value
         * in reg. 0x79 for every table register */
        dump_reg_range(ov, 0x80, 0x9f);
        dump_reg_range(ov, 0xa0, 0xbf);

}

static void
ov518_dump_regs(struct usb_ov511 *ov)
{
        info("VIDEO MODE REGS");
        dump_reg_range(ov, 0x20, 0x2f);
        info("DATA PUMP AND SNAPSHOT REGS");
        dump_reg_range(ov, 0x30, 0x3f);
        info("I2C REGS");
        dump_reg_range(ov, 0x40, 0x4f);
        info("SYSTEM CONTROL AND VENDOR REGS");
        dump_reg_range(ov, 0x50, 0x5f);
        info("60 - 6F");
        dump_reg_range(ov, 0x60, 0x6f);
        info("70 - 7F");
        dump_reg_range(ov, 0x70, 0x7f);
        info("Y QUANTIZATION TABLE");
        dump_reg_range(ov, 0x80, 0x8f);
        info("UV QUANTIZATION TABLE");
        dump_reg_range(ov, 0x90, 0x9f);
        info("A0 - BF");
        dump_reg_range(ov, 0xa0, 0xbf);
        info("CBR");
        dump_reg_range(ov, 0xc0, 0xcf);
}
#endif

/*****************************************************************************/

/* Temporarily stops OV511 from functioning. Must do this before changing
 * registers while the camera is streaming */
static inline int
ov51x_stop(struct usb_ov511 *ov)
{
        PDEBUG(4, "stopping");
        ov->stopped = 1;
        if (ov->bclass == BCL_OV518)
                return (reg_w_mask(ov, R51x_SYS_RESET, 0x3a, 0x3a));
        else
                return (reg_w(ov, R51x_SYS_RESET, 0x3d));
}

/* Restarts OV511 after ov511_stop() is called. Has no effect if it is not
 * actually stopped (for performance). */
static inline int
ov51x_restart(struct usb_ov511 *ov)
{
        if (ov->stopped) {
                PDEBUG(4, "restarting");
                ov->stopped = 0;

                /* Reinitialize the stream */
                if (ov->bclass == BCL_OV518)
                        reg_w(ov, 0x2f, 0x80);

                return (reg_w(ov, R51x_SYS_RESET, 0x00));
        }

        return 0;
}

/* Resets the hardware snapshot button */
static void
ov51x_clear_snapshot(struct usb_ov511 *ov)
{
        if (ov->bclass == BCL_OV511) {
                reg_w(ov, R51x_SYS_SNAP, 0x00);
                reg_w(ov, R51x_SYS_SNAP, 0x02);
                reg_w(ov, R51x_SYS_SNAP, 0x00);
        } else if (ov->bclass == BCL_OV518) {
                warn("snapshot reset not supported yet on OV518(+)");
        } else {
                err("clear snap: invalid bridge type");
        }
}

#if defined(CONFIG_PROC_FS) && defined(CONFIG_VIDEO_PROC_FS)
/* Checks the status of the snapshot button. Returns 1 if it was pressed since
 * it was last cleared, and zero in all other cases (including errors) */
static int
ov51x_check_snapshot(struct usb_ov511 *ov)
{
        int ret, status = 0;

        if (ov->bclass == BCL_OV511) {
                ret = reg_r(ov, R51x_SYS_SNAP);
                if (ret < 0) {
                        err("Error checking snspshot status (%d)", ret);
                } else if (ret & 0x08) {
                        status = 1;
                }
        } else if (ov->bclass == BCL_OV518) {
                warn("snapshot check not supported yet on OV518(+)");
        } else {
                err("check snap: invalid bridge type");
        }

        return status;
}
#endif

/* This does an initial reset of an OmniVision sensor and ensures that I2C
 * is synchronized. Returns <0 for failure.
 */
static int
init_ov_sensor(struct usb_ov511 *ov)
{
        int i, success;

        /* Reset the sensor */
        if (i2c_w(ov, 0x12, 0x80) < 0) return -EIO;

        /* Wait for it to initialize */
        schedule_timeout(1 + 150 * HZ / 1000);

        for (i = 0, success = 0; i < i2c_detect_tries && !success; i++) {
                if ((i2c_r(ov, OV7610_REG_ID_HIGH) == 0x7F) &&
                    (i2c_r(ov, OV7610_REG_ID_LOW) == 0xA2)) {
                        success = 1;
                        continue;
                }

                /* Reset the sensor */
                if (i2c_w(ov, 0x12, 0x80) < 0) return -EIO;
                /* Wait for it to initialize */
                schedule_timeout(1 + 150 * HZ / 1000);
                /* Dummy read to sync I2C */
                if (i2c_r(ov, 0x00) < 0) return -EIO;
        }

        if (!success)
                return -EIO;

        PDEBUG(1, "I2C synced in %d attempt(s)", i);

        return 0;
}

static int
ov511_set_packet_size(struct usb_ov511 *ov, int size)
{
        int alt, mult;

        if (ov51x_stop(ov) < 0)
                return -EIO;

        mult = size >> 5;

        if (ov->bridge == BRG_OV511) {
                if (size == 0) alt = OV511_ALT_SIZE_0;
                else if (size == 257) alt = OV511_ALT_SIZE_257;
                else if (size == 513) alt = OV511_ALT_SIZE_513;
                else if (size == 769) alt = OV511_ALT_SIZE_769;
                else if (size == 993) alt = OV511_ALT_SIZE_993;
                else {
                        err("Set packet size: invalid size (%d)", size);
                        return -EINVAL;
                }
        } else if (ov->bridge == BRG_OV511PLUS) {
                if (size == 0) alt = OV511PLUS_ALT_SIZE_0;
                else if (size == 33) alt = OV511PLUS_ALT_SIZE_33;
                else if (size == 129) alt = OV511PLUS_ALT_SIZE_129;
                else if (size == 257) alt = OV511PLUS_ALT_SIZE_257;
                else if (size == 385) alt = OV511PLUS_ALT_SIZE_385;
                else if (size == 513) alt = OV511PLUS_ALT_SIZE_513;
                else if (size == 769) alt = OV511PLUS_ALT_SIZE_769;
                else if (size == 961) alt = OV511PLUS_ALT_SIZE_961;
                else {
                        err("Set packet size: invalid size (%d)", size);
                        return -EINVAL;
                }
        } else {
                err("Set packet size: Invalid bridge type");
                return -EINVAL;
        }

        PDEBUG(3, "%d, mult=%d, alt=%d", size, mult, alt);

        if (reg_w(ov, R51x_FIFO_PSIZE, mult) < 0)
                return -EIO;

        if (usb_set_interface(ov->dev, ov->iface, alt) < 0) {
                err("Set packet size: set interface error");
                return -EBUSY;
        }

        if (ov51x_reset(ov, OV511_RESET_NOREGS) < 0)
                return -EIO;

        ov->packet_size = size;

        if (ov51x_restart(ov) < 0)
                return -EIO;

        return 0;
}

/* Note: Unlike the OV511/OV511+, the size argument does NOT include the
 * optional packet number byte. The actual size *is* stored in ov->packet_size,
 * though. */
static int
ov518_set_packet_size(struct usb_ov511 *ov, int size)
{
        int alt;

        if (ov51x_stop(ov) < 0)
                return -EIO;

        if (ov->bclass == BCL_OV518) {
                if (size == 0) alt = OV518_ALT_SIZE_0;
                else if (size == 128) alt = OV518_ALT_SIZE_128;
                else if (size == 256) alt = OV518_ALT_SIZE_256;
                else if (size == 384) alt = OV518_ALT_SIZE_384;
                else if (size == 512) alt = OV518_ALT_SIZE_512;
                else if (size == 640) alt = OV518_ALT_SIZE_640;
                else if (size == 768) alt = OV518_ALT_SIZE_768;
                else if (size == 896) alt = OV518_ALT_SIZE_896;
                else {
                        err("Set packet size: invalid size (%d)", size);
                        return -EINVAL;
                }
        } else {
                err("Set packet size: Invalid bridge type");
                return -EINVAL;
        }

        PDEBUG(3, "%d, alt=%d", size, alt);

        ov->packet_size = size;
        if (size > 0) {
                /* Program ISO FIFO size reg (packet number isn't included) */
                ov518_reg_w32(ov, 0x30, size, 2);

                if (ov->packet_numbering)
                        ++ov->packet_size;
        }

        if (usb_set_interface(ov->dev, ov->iface, alt) < 0) {
                err("Set packet size: set interface error");
                return -EBUSY;
        }

        /* Initialize the stream */
        if (reg_w(ov, 0x2f, 0x80) < 0)
                return -EIO;

        if (ov51x_restart(ov) < 0)
                return -EIO;

        if (ov51x_reset(ov, OV511_RESET_NOREGS) < 0)
                return -EIO;

        return 0;
}

/* Upload compression params and quantization tables. Returns 0 for success. */
static int
ov511_init_compression(struct usb_ov511 *ov)
{
        int rc = 0;

        if (!ov->compress_inited) {
                reg_w(ov, 0x70, phy);
                reg_w(ov, 0x71, phuv);
                reg_w(ov, 0x72, pvy);
                reg_w(ov, 0x73, pvuv);
                reg_w(ov, 0x74, qhy);
                reg_w(ov, 0x75, qhuv);
                reg_w(ov, 0x76, qvy);
                reg_w(ov, 0x77, qvuv);

                if (ov511_upload_quan_tables(ov) < 0) {
                        err("Error uploading quantization tables");
                        rc = -EIO;
                        goto out;
                }
        }

        ov->compress_inited = 1;
out:
        return rc;
}

/* Upload compression params and quantization tables. Returns 0 for success. */
static int
ov518_init_compression(struct usb_ov511 *ov)
{
        int rc = 0;

        if (!ov->compress_inited) {
                if (ov518_upload_quan_tables(ov) < 0) {
                        err("Error uploading quantization tables");
                        rc = -EIO;
                        goto out;
                }
        }

        ov->compress_inited = 1;
out:
        return rc;
}

/* -------------------------------------------------------------------------- */

/* Sets sensor's contrast setting to "val" */
static int
sensor_set_contrast(struct usb_ov511 *ov, unsigned short val)
{
        int rc;

        PDEBUG(3, "%d", val);

        if (ov->stop_during_set)
                if (ov51x_stop(ov) < 0)
                        return -EIO;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV6620:
        {
                rc = i2c_w(ov, OV7610_REG_CNT, val >> 8);
                if (rc < 0)
                        goto out;
                break;
        }
        case SEN_OV6630:
        {
                rc = i2c_w_mask(ov, OV7610_REG_CNT, val >> 12, 0x0f);
                if (rc < 0)
                        goto out;
                break;
        }
        case SEN_OV7620:
        {
                unsigned char ctab[] = {
                        0x01, 0x05, 0x09, 0x11, 0x15, 0x35, 0x37, 0x57,
                        0x5b, 0xa5, 0xa7, 0xc7, 0xc9, 0xcf, 0xef, 0xff
                };

                /* Use Y gamma control instead. Bit 0 enables it. */
                rc = i2c_w(ov, 0x64, ctab[val>>12]);
                if (rc < 0)
                        goto out;
                break;
        }
        case SEN_SAA7111A:
        {
                rc = i2c_w(ov, 0x0b, val >> 9);
                if (rc < 0)
                        goto out;
                break;
        }
        default:
        {
                PDEBUG(3, "Unsupported with this sensor");
                rc = -EPERM;
                goto out;
        }
        }

        rc = 0;         /* Success */
        ov->contrast = val;
out:
        if (ov51x_restart(ov) < 0)
                return -EIO;

        return rc;
}

/* Gets sensor's contrast setting */
static int
sensor_get_contrast(struct usb_ov511 *ov, unsigned short *val)
{
        int rc;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV6620:
                rc = i2c_r(ov, OV7610_REG_CNT);
                if (rc < 0)
                        return rc;
                else
                        *val = rc << 8;
                break;
        case SEN_OV6630:
                rc = i2c_r(ov, OV7610_REG_CNT);
                if (rc < 0)
                        return rc;
                else
                        *val = rc << 12;
                break;
        case SEN_OV7620:
                /* Use Y gamma reg instead. Bit 0 is the enable bit. */
                rc = i2c_r(ov, 0x64);
                if (rc < 0)
                        return rc;
                else
                        *val = (rc & 0xfe) << 8;
                break;
        case SEN_SAA7111A:
                *val = ov->contrast;
                break;
        default:
                PDEBUG(3, "Unsupported with this sensor");
                return -EPERM;
        }

        PDEBUG(3, "%d", *val);
        ov->contrast = *val;

        return 0;
}

/* -------------------------------------------------------------------------- */

/* Sets sensor's brightness setting to "val" */
static int
sensor_set_brightness(struct usb_ov511 *ov, unsigned short val)
{
        int rc;

        PDEBUG(4, "%d", val);

        if (ov->stop_during_set)
                if (ov51x_stop(ov) < 0)
                        return -EIO;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV76BE:
        case SEN_OV6620:
        case SEN_OV6630:
                rc = i2c_w(ov, OV7610_REG_BRT, val >> 8);
                if (rc < 0)
                        goto out;
                break;
        case SEN_OV7620:
                /* 7620 doesn't like manual changes when in auto mode */
                if (!ov->auto_brt) {
                        rc = i2c_w(ov, OV7610_REG_BRT, val >> 8);
                        if (rc < 0)
                                goto out;
                }
                break;
        case SEN_SAA7111A:
                rc = i2c_w(ov, 0x0a, val >> 8);
                if (rc < 0)
                        goto out;
                break;
        default:
                PDEBUG(3, "Unsupported with this sensor");
                rc = -EPERM;
                goto out;
        }

        rc = 0;         /* Success */
        ov->brightness = val;
out:
        if (ov51x_restart(ov) < 0)
                return -EIO;

        return rc;
}

/* Gets sensor's brightness setting */
static int
sensor_get_brightness(struct usb_ov511 *ov, unsigned short *val)
{
        int rc;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV76BE:
        case SEN_OV7620:
        case SEN_OV6620:
        case SEN_OV6630:
                rc = i2c_r(ov, OV7610_REG_BRT);
                if (rc < 0)
                        return rc;
                else
                        *val = rc << 8;
                break;
        case SEN_SAA7111A:
                *val = ov->brightness;
                break;
        default:
                PDEBUG(3, "Unsupported with this sensor");
                return -EPERM;
        }

        PDEBUG(3, "%d", *val);
        ov->brightness = *val;

        return 0;
}

/* -------------------------------------------------------------------------- */

/* Sets sensor's saturation (color intensity) setting to "val" */
static int
sensor_set_saturation(struct usb_ov511 *ov, unsigned short val)
{
        int rc;

        PDEBUG(3, "%d", val);

        if (ov->stop_during_set)
                if (ov51x_stop(ov) < 0)
                        return -EIO;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV76BE:
        case SEN_OV6620:
        case SEN_OV6630:
                rc = i2c_w(ov, OV7610_REG_SAT, val >> 8);
                if (rc < 0)
                        goto out;
                break;
        case SEN_OV7620:
//              /* Use UV gamma control instead. Bits 0 & 7 are reserved. */
//              rc = ov_i2c_write(ov->dev, 0x62, (val >> 9) & 0x7e);
//              if (rc < 0)
//                      goto out;
                rc = i2c_w(ov, OV7610_REG_SAT, val >> 8);
                if (rc < 0)
                        goto out;
                break;
        case SEN_SAA7111A:
                rc = i2c_w(ov, 0x0c, val >> 9);
                if (rc < 0)
                        goto out;
                break;
        default:
                PDEBUG(3, "Unsupported with this sensor");
                rc = -EPERM;
                goto out;
        }

        rc = 0;         /* Success */
        ov->colour = val;
out:
        if (ov51x_restart(ov) < 0)
                return -EIO;

        return rc;
}

/* Gets sensor's saturation (color intensity) setting */
static int
sensor_get_saturation(struct usb_ov511 *ov, unsigned short *val)
{
        int rc;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV76BE:
        case SEN_OV6620:
        case SEN_OV6630:
                rc = i2c_r(ov, OV7610_REG_SAT);
                if (rc < 0)
                        return rc;
                else
                        *val = rc << 8;
                break;
        case SEN_OV7620:
//              /* Use UV gamma reg instead. Bits 0 & 7 are reserved. */
//              rc = i2c_r(ov, 0x62);
//              if (rc < 0)
//                      return rc;
//              else
//                      *val = (rc & 0x7e) << 9;
                rc = i2c_r(ov, OV7610_REG_SAT);
                if (rc < 0)
                        return rc;
                else
                        *val = rc << 8;
                break;
        case SEN_SAA7111A:
                *val = ov->colour;
                break;
        default:
                PDEBUG(3, "Unsupported with this sensor");
                return -EPERM;
        }

        PDEBUG(3, "%d", *val);
        ov->colour = *val;

        return 0;
}

/* -------------------------------------------------------------------------- */

/* Sets sensor's hue (red/blue balance) setting to "val" */
static int
sensor_set_hue(struct usb_ov511 *ov, unsigned short val)
{
        int rc;

        PDEBUG(3, "%d", val);

        if (ov->stop_during_set)
                if (ov51x_stop(ov) < 0)
                        return -EIO;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV6620:
        case SEN_OV6630:
                rc = i2c_w(ov, OV7610_REG_RED, 0xFF - (val >> 8));
                if (rc < 0)
                        goto out;

                rc = i2c_w(ov, OV7610_REG_BLUE, val >> 8);
                if (rc < 0)
                        goto out;
                break;
        case SEN_OV7620:
// Hue control is causing problems. I will enable it once it's fixed.
#if 0
                rc = i2c_w(ov, 0x7a, (unsigned char)(val >> 8) + 0xb);
                if (rc < 0)
                        goto out;

                rc = i2c_w(ov, 0x79, (unsigned char)(val >> 8) + 0xb);
                if (rc < 0)
                        goto out;
#endif
                break;
        case SEN_SAA7111A:
                rc = i2c_w(ov, 0x0d, (val + 32768) >> 8);
                if (rc < 0)
                        goto out;
                break;
        default:
                PDEBUG(3, "Unsupported with this sensor");
                rc = -EPERM;
                goto out;
        }

        rc = 0;         /* Success */
        ov->hue = val;
out:
        if (ov51x_restart(ov) < 0)
                return -EIO;

        return rc;
}

/* Gets sensor's hue (red/blue balance) setting */
static int
sensor_get_hue(struct usb_ov511 *ov, unsigned short *val)
{
        int rc;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV6620:
        case SEN_OV6630:
                rc = i2c_r(ov, OV7610_REG_BLUE);
                if (rc < 0)
                        return rc;
                else
                        *val = rc << 8;
                break;
        case SEN_OV7620:
                rc = i2c_r(ov, 0x7a);
                if (rc < 0)
                        return rc;
                else
                        *val = rc << 8;
                break;
        case SEN_SAA7111A:
                *val = ov->hue;
                break;
        default:
                PDEBUG(3, "Unsupported with this sensor");
                return -EPERM;
        }

        PDEBUG(3, "%d", *val);
        ov->hue = *val;

        return 0;
}

/* -------------------------------------------------------------------------- */

static inline int
sensor_set_picture(struct usb_ov511 *ov, struct video_picture *p)
{
        int rc;

        PDEBUG(4, "sensor_set_picture");

        ov->whiteness = p->whiteness;

        /* Don't return error if a setting is unsupported, or rest of settings
         * will not be performed */

        rc = sensor_set_contrast(ov, p->contrast);
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_set_brightness(ov, p->brightness);
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_set_saturation(ov, p->colour);
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_set_hue(ov, p->hue);
        if (FATAL_ERROR(rc))
                return rc;

        return 0;
}

static int
sensor_get_picture(struct usb_ov511 *ov, struct video_picture *p)
{
        int rc;

        PDEBUG(4, "sensor_get_picture");

        /* Don't return error if a setting is unsupported, or rest of settings
         * will not be performed */

        rc = sensor_get_contrast(ov, &(p->contrast));
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_get_brightness(ov, &(p->brightness));
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_get_saturation(ov, &(p->colour));
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_get_hue(ov, &(p->hue));
        if (FATAL_ERROR(rc))
                return rc;

        p->whiteness = 105 << 8;

        return 0;
}

#if defined(CONFIG_PROC_FS) && defined(CONFIG_VIDEO_PROC_FS)
// FIXME: Exposure range is only 0x00-0x7f in interlace mode
/* Sets current exposure for sensor. This only has an effect if auto-exposure
 * is off */
static inline int
sensor_set_exposure(struct usb_ov511 *ov, unsigned char val)
{
        int rc;

        PDEBUG(3, "%d", val);

        if (ov->stop_during_set)
                if (ov51x_stop(ov) < 0)
                        return -EIO;

        switch (ov->sensor) {
        case SEN_OV6620:
        case SEN_OV6630:
        case SEN_OV7610:
        case SEN_OV7620:
        case SEN_OV76BE:
        case SEN_OV8600:
                rc = i2c_w(ov, 0x10, val);
                if (rc < 0)
                        goto out;

                break;
        case SEN_KS0127:
        case SEN_KS0127B:
        case SEN_SAA7111A:
                PDEBUG(3, "Unsupported with this sensor");
                return -EPERM;
        default:
                err("Sensor not supported for set_exposure");
                return -EINVAL;
        }

        rc = 0;         /* Success */
        ov->exposure = val;
out:
        if (ov51x_restart(ov) < 0)
                return -EIO;

        return rc;
}

/* Gets current exposure level from sensor, regardless of whether it is under
 * manual control. */
static int
sensor_get_exposure(struct usb_ov511 *ov, unsigned char *val)
{
        int rc;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV6620:
        case SEN_OV6630:
        case SEN_OV7620:
        case SEN_OV76BE:
        case SEN_OV8600:
                rc = i2c_r(ov, 0x10);
                if (rc < 0)
                        return rc;
                else
                        *val = rc;
                break;
        case SEN_KS0127:
        case SEN_KS0127B:
        case SEN_SAA7111A:
                val = 0;
                PDEBUG(3, "Unsupported with this sensor");
                return -EPERM;
        default:
                err("Sensor not supported for get_exposure");
                return -EINVAL;
        }

        PDEBUG(3, "%d", *val);
        ov->exposure = *val;

        return 0;
}
#endif /* CONFIG_PROC_FS && CONFIG_VIDEO_PROC_FS */

/* Turns on or off the LED. Only has an effect with OV511+/OV518(+) */
static inline void
ov51x_led_control(struct usb_ov511 *ov, int enable)
{
        PDEBUG(4, " (%s)", enable ? "turn on" : "turn off");

        if (ov->bridge == BRG_OV511PLUS)
                reg_w(ov, R511_SYS_LED_CTL, enable ? 1 : 0);
        else if (ov->bclass == BCL_OV518)
                reg_w_mask(ov, R518_GPIO_OUT, enable ? 0x02 : 0x00, 0x02);

        return;
}

/* Matches the sensor's internal frame rate to the lighting frequency.
 * Valid frequencies are:
 *      50 - 50Hz, for European and Asian lighting
 *      60 - 60Hz, for American lighting
 *
 * Tested with: OV7610, OV7620, OV76BE, OV6620
 * Unsupported: KS0127, KS0127B, SAA7111A
 * Returns: 0 for success
 */
static int
sensor_set_light_freq(struct usb_ov511 *ov, int freq)
{
        int sixty;

        PDEBUG(4, "%d Hz", freq);

        if (freq == 60)
                sixty = 1;
        else if (freq == 50)
                sixty = 0;
        else {
                err("Invalid light freq (%d Hz)", freq);
                return -EINVAL;
        }

        switch (ov->sensor) {
        case SEN_OV7610:
                i2c_w_mask(ov, 0x2a, sixty?0x00:0x80, 0x80);
                i2c_w(ov, 0x2b, sixty?0x00:0xac);
                i2c_w_mask(ov, 0x13, 0x10, 0x10);
                i2c_w_mask(ov, 0x13, 0x00, 0x10);
                break;
        case SEN_OV7620:
        case SEN_OV76BE:
        case SEN_OV8600:
                i2c_w_mask(ov, 0x2a, sixty?0x00:0x80, 0x80);
                i2c_w(ov, 0x2b, sixty?0x00:0xac);
                i2c_w_mask(ov, 0x76, 0x01, 0x01);
                break;
        case SEN_OV6620:
        case SEN_OV6630:
                i2c_w(ov, 0x2b, sixty?0xa8:0x28);
                i2c_w(ov, 0x2a, sixty?0x84:0xa4);
                break;
        case SEN_KS0127:
        case SEN_KS0127B:
        case SEN_SAA7111A:
                PDEBUG(5, "Unsupported with this sensor");
                return -EPERM;
        default:
                err("Sensor not supported for set_light_freq");
                return -EINVAL;
        }

        ov->lightfreq = freq;

        return 0;
}

/* If enable is true, turn on the sensor's banding filter, otherwise turn it
 * off. This filter tries to reduce the pattern of horizontal light/dark bands
 * caused by some (usually fluorescent) lighting. The light frequency must be
 * set either before or after enabling it with ov51x_set_light_freq().
 *
 * Tested with: OV7610, OV7620, OV76BE, OV6620.
 * Unsupported: KS0127, KS0127B, SAA7111A
 * Returns: 0 for success
 */
static inline int
sensor_set_banding_filter(struct usb_ov511 *ov, int enable)
{
        int rc;

        PDEBUG(4, " (%s)", enable ? "turn on" : "turn off");

        if (ov->sensor == SEN_KS0127 || ov->sensor == SEN_KS0127B
                || ov->sensor == SEN_SAA7111A) {
                PDEBUG(5, "Unsupported with this sensor");
                return -EPERM;
        }

        rc = i2c_w_mask(ov, 0x2d, enable?0x04:0x00, 0x04);
        if (rc < 0)
                return rc;

        ov->bandfilt = enable;

        return 0;
}

/* If enable is true, turn on the sensor's auto brightness control, otherwise
 * turn it off.
 *
 * Unsupported: KS0127, KS0127B, SAA7111A
 * Returns: 0 for success
 */
static inline int
sensor_set_auto_brightness(struct usb_ov511 *ov, int enable)
{
        int rc;

        PDEBUG(4, " (%s)", enable ? "turn on" : "turn off");

        if (ov->sensor == SEN_KS0127 || ov->sensor == SEN_KS0127B
                || ov->sensor == SEN_SAA7111A) {
                PDEBUG(5, "Unsupported with this sensor");
                return -EPERM;
        }

        rc = i2c_w_mask(ov, 0x2d, enable?0x10:0x00, 0x10);
        if (rc < 0)
                return rc;

        ov->auto_brt = enable;

        return 0;
}

/* If enable is true, turn on the sensor's auto exposure control, otherwise
 * turn it off.
 *
 * Unsupported: KS0127, KS0127B, SAA7111A
 * Returns: 0 for success
 */
static inline int
sensor_set_auto_exposure(struct usb_ov511 *ov, int enable)
{
        PDEBUG(4, " (%s)", enable ? "turn on" : "turn off");

        switch (ov->sensor) {
        case SEN_OV7610:
                i2c_w_mask(ov, 0x29, enable?0x00:0x80, 0x80);
                break;
        case SEN_OV6620:
        case SEN_OV7620:
        case SEN_OV76BE:
        case SEN_OV8600:
                i2c_w_mask(ov, 0x13, enable?0x01:0x00, 0x01);
                break;
        case SEN_OV6630:
                i2c_w_mask(ov, 0x28, enable?0x00:0x10, 0x10);
                break;
        case SEN_KS0127:
        case SEN_KS0127B:
        case SEN_SAA7111A:
                PDEBUG(5, "Unsupported with this sensor");
                return -EPERM;
        default:
                err("Sensor not supported for set_auto_exposure");
                return -EINVAL;
        }

        ov->auto_exp = enable;

        return 0;
}

/* Modifies the sensor's exposure algorithm to allow proper exposure of objects
 * that are illuminated from behind.
 *
 * Tested with: OV6620, OV7620
 * Unsupported: OV7610, OV76BE, KS0127, KS0127B, SAA7111A
 * Returns: 0 for success
 */
static int
sensor_set_backlight(struct usb_ov511 *ov, int enable)
{
        PDEBUG(4, " (%s)", enable ? "turn on" : "turn off");

        switch (ov->sensor) {
        case SEN_OV7620:
        case SEN_OV8600:
                i2c_w_mask(ov, 0x68, enable?0xe0:0xc0, 0xe0);
                i2c_w_mask(ov, 0x29, enable?0x08:0x00, 0x08);
                i2c_w_mask(ov, 0x28, enable?0x02:0x00, 0x02);
                break;
        case SEN_OV6620:
                i2c_w_mask(ov, 0x4e, enable?0xe0:0xc0, 0xe0);
                i2c_w_mask(ov, 0x29, enable?0x08:0x00, 0x08);
                i2c_w_mask(ov, 0x0e, enable?0x80:0x00, 0x80);
                break;
        case SEN_OV6630:
                i2c_w_mask(ov, 0x4e, enable?0x80:0x60, 0xe0);
                i2c_w_mask(ov, 0x29, enable?0x08:0x00, 0x08);
                i2c_w_mask(ov, 0x28, enable?0x02:0x00, 0x02);
                break;
        case SEN_OV7610:
        case SEN_OV76BE:
        case SEN_KS0127:
        case SEN_KS0127B:
        case SEN_SAA7111A:
                PDEBUG(5, "Unsupported with this sensor");
                return -EPERM;
        default:
                err("Sensor not supported for set_backlight");
                return -EINVAL;
        }

        ov->backlight = enable;

        return 0;
}

static inline int
sensor_set_mirror(struct usb_ov511 *ov, int enable)
{
        PDEBUG(4, " (%s)", enable ? "turn on" : "turn off");

        switch (ov->sensor) {
        case SEN_OV6620:
        case SEN_OV6630:
        case SEN_OV7610:
        case SEN_OV7620:
        case SEN_OV76BE:
        case SEN_OV8600:
                i2c_w_mask(ov, 0x12, enable?0x40:0x00, 0x40);
                break;
        case SEN_KS0127:
        case SEN_KS0127B:
        case SEN_SAA7111A:
                PDEBUG(5, "Unsupported with this sensor");
                return -EPERM;
        default:
                err("Sensor not supported for set_mirror");
                return -EINVAL;
        }

        ov->mirror = enable;

        return 0;
}

/* Returns number of bits per pixel (regardless of where they are located;
 * planar or not), or zero for unsupported format.
 */
static inline int
get_depth(int palette)
{
        switch (palette) {
        case VIDEO_PALETTE_GREY:    return 8;
        case VIDEO_PALETTE_YUV420:  return 12;
        case VIDEO_PALETTE_YUV420P: return 12; /* Planar */
        case VIDEO_PALETTE_RGB565:  return 16;
        case VIDEO_PALETTE_RGB24:   return 24;
        case VIDEO_PALETTE_YUV422:  return 16;
        case VIDEO_PALETTE_YUYV:    return 16;
        case VIDEO_PALETTE_YUV422P: return 16; /* Planar */
        default:                    return 0;  /* Invalid format */
        }
}

/* Bytes per frame. Used by read(). Return of 0 indicates error */
static inline long int
get_frame_length(struct ov511_frame *frame)
{
        if (!frame)
                return 0;
        else
                return ((frame->width * frame->height
                         * get_depth(frame->format)) >> 3);
}

static int
mode_init_ov_sensor_regs(struct usb_ov511 *ov, int width, int height,
                         int mode, int sub_flag, int qvga)
{
        int clock;

        /******** Mode (VGA/QVGA) and sensor specific regs ********/

        switch (ov->sensor) {
        case SEN_OV7610:
                i2c_w(ov, 0x14, qvga?0x24:0x04);
// FIXME: Does this improve the image quality or frame rate?
#if 0
                i2c_w_mask(ov, 0x28, qvga?0x00:0x20, 0x20);
                i2c_w(ov, 0x24, 0x10);
                i2c_w(ov, 0x25, qvga?0x40:0x8a);
                i2c_w(ov, 0x2f, qvga?0x30:0xb0);
                i2c_w(ov, 0x35, qvga?0x1c:0x9c);
#endif
                break;
        case SEN_OV7620:
//              i2c_w(ov, 0x2b, 0x00);
                i2c_w(ov, 0x14, qvga?0xa4:0x84);
                i2c_w_mask(ov, 0x28, qvga?0x00:0x20, 0x20);
                i2c_w(ov, 0x24, qvga?0x20:0x3a);
                i2c_w(ov, 0x25, qvga?0x30:0x60);
                i2c_w_mask(ov, 0x2d, qvga?0x40:0x00, 0x40);
                i2c_w_mask(ov, 0x67, qvga?0xf0:0x90, 0xf0);
                i2c_w_mask(ov, 0x74, qvga?0x20:0x00, 0x20);
                break;
        case SEN_OV76BE:
//              i2c_w(ov, 0x2b, 0x00);
                i2c_w(ov, 0x14, qvga?0xa4:0x84);
// FIXME: Enable this once 7620AE uses 7620 initial settings
#if 0
                i2c_w_mask(ov, 0x28, qvga?0x00:0x20, 0x20);
                i2c_w(ov, 0x24, qvga?0x20:0x3a);
                i2c_w(ov, 0x25, qvga?0x30:0x60);
                i2c_w_mask(ov, 0x2d, qvga?0x40:0x00, 0x40);
                i2c_w_mask(ov, 0x67, qvga?0xb0:0x90, 0xf0);
                i2c_w_mask(ov, 0x74, qvga?0x20:0x00, 0x20);
#endif
                break;
        case SEN_OV6620:
                i2c_w(ov, 0x14, qvga?0x24:0x04);
                break;
        case SEN_OV6630:
                i2c_w(ov, 0x14, qvga?0xa0:0x80);
                break;
        default:
                err("Invalid sensor");
                return -EINVAL;
        }

        /******** Palette-specific regs ********/

        if (mode == VIDEO_PALETTE_GREY) {
                if (ov->sensor == SEN_OV7610 || ov->sensor == SEN_OV76BE) {
                        /* these aren't valid on the OV6620/OV7620/6630? */
                        i2c_w_mask(ov, 0x0e, 0x40, 0x40);
                }

                if (ov->sensor == SEN_OV6630 && ov->bridge == BRG_OV518
                    && ov518_color) {
                        i2c_w_mask(ov, 0x12, 0x00, 0x10);
                        i2c_w_mask(ov, 0x13, 0x00, 0x20);
                } else {
                        i2c_w_mask(ov, 0x13, 0x20, 0x20);
                }
        } else {
                if (ov->sensor == SEN_OV7610 || ov->sensor == SEN_OV76BE) {
                        /* not valid on the OV6620/OV7620/6630? */
                        i2c_w_mask(ov, 0x0e, 0x00, 0x40);
                }

                /* The OV518 needs special treatment. Although both the OV518
                 * and the OV6630 support a 16-bit video bus, only the 8 bit Y
                 * bus is actually used. The UV bus is tied to ground.
                 * Therefore, the OV6630 needs to be in 8-bit multiplexed
                 * output mode */

                if (ov->sensor == SEN_OV6630 && ov->bridge == BRG_OV518
                    && ov518_color) {
                        i2c_w_mask(ov, 0x12, 0x10, 0x10);
                        i2c_w_mask(ov, 0x13, 0x20, 0x20);
                } else {
                        i2c_w_mask(ov, 0x13, 0x00, 0x20);
                }
        }

        /******** Clock programming ********/

        // FIXME: Test this with OV6630

        /* The OV6620 needs special handling. This prevents the 
         * severe banding that normally occurs */
        if (ov->sensor == SEN_OV6620 || ov->sensor == SEN_OV6630)
        {
                /* Clock down */

                i2c_w(ov, 0x2a, 0x04);

                if (ov->compress) {
//                      clock = 0;    /* This ensures the highest frame rate */
                        clock = 3;
                } else if (clockdiv == -1) {   /* If user didn't override it */
                        clock = 3;    /* Gives better exposure time */
                } else {
                        clock = clockdiv;
                }

                PDEBUG(4, "Setting clock divisor to %d", clock);

                i2c_w(ov, 0x11, clock);

                i2c_w(ov, 0x2a, 0x84);
                /* This next setting is critical. It seems to improve
                 * the gain or the contrast. The "reserved" bits seem
                 * to have some effect in this case. */
                i2c_w(ov, 0x2d, 0x85);
        }
        else
        {
                if (ov->compress) {
                        clock = 1;    /* This ensures the highest frame rate */
                } else if (clockdiv == -1) {   /* If user didn't override it */
                        /* Calculate and set the clock divisor */
                        clock = ((sub_flag ? ov->subw * ov->subh
                                  : width * height)
                                 * (mode == VIDEO_PALETTE_GREY ? 2 : 3) / 2)
                                 / 66000;
                } else {
                        clock = clockdiv;
                }

                PDEBUG(4, "Setting clock divisor to %d", clock);

                i2c_w(ov, 0x11, clock);
        }

        /******** Special Features ********/

        if (framedrop >= 0)
                i2c_w(ov, 0x16, framedrop);

        /* Test Pattern */
        i2c_w_mask(ov, 0x12, (testpat?0x02:0x00), 0x02);

        /* Enable auto white balance */
        i2c_w_mask(ov, 0x12, 0x04, 0x04);

        // This will go away as soon as ov51x_mode_init_sensor_regs()
        // is fully tested.
        /* 7620/6620/6630? don't have register 0x35, so play it safe */
        if (ov->sensor == SEN_OV7610 || ov->sensor == SEN_OV76BE) {
                if (width == 640 && height == 480)
                        i2c_w(ov, 0x35, 0x9e);
                else
                        i2c_w(ov, 0x35, 0x1e);
        }

        return 0;
}

static int
set_ov_sensor_window(struct usb_ov511 *ov, int width, int height, int mode,
                     int sub_flag)
{
        int ret;
        int hwsbase, hwebase, vwsbase, vwebase, hwsize, vwsize; 
        int hoffset, voffset, hwscale = 0, vwscale = 0;

        /* The different sensor ICs handle setting up of window differently.
         * IF YOU SET IT WRONG, YOU WILL GET ALL ZERO ISOC DATA FROM OV51x!!! */
        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV76BE:
                hwsbase = 0x38;
                hwebase = 0x3a;
                vwsbase = vwebase = 0x05;
                break;
        case SEN_OV6620:
        case SEN_OV6630:
                hwsbase = 0x38;
                hwebase = 0x3a;
                vwsbase = 0x05;
                vwebase = 0x06;
                break;
        case SEN_OV7620:
                hwsbase = 0x2f;         /* From 7620.SET (spec is wrong) */
                hwebase = 0x2f;
                vwsbase = vwebase = 0x05;
                break;
        default:
                err("Invalid sensor");
                return -EINVAL;
        }

        if (ov->sensor == SEN_OV6620 || ov->sensor == SEN_OV6630) {
                /* Note: OV518(+) does downsample on its own) */
                if ((width > 176 && height > 144)
                    || ov->bclass == BCL_OV518) {  /* CIF */
                        ret = mode_init_ov_sensor_regs(ov, width, height,
                                mode, sub_flag, 0);
                        if (ret < 0)
                                return ret;
                        hwscale = 1;
                        vwscale = 1;  /* The datasheet says 0; it's wrong */
                        hwsize = 352;
                        vwsize = 288;
                } else if (width > 176 || height > 144) {
                        err("Illegal dimensions");
                        return -EINVAL;
                } else {                            /* QCIF */
                        ret = mode_init_ov_sensor_regs(ov, width, height,
                                mode, sub_flag, 1);
                        if (ret < 0)
                                return ret;
                        hwsize = 176;
                        vwsize = 144;
                }
        } else {
                if (width > 320 && height > 240) {  /* VGA */
                        ret = mode_init_ov_sensor_regs(ov, width, height,
                                mode, sub_flag, 0);
                        if (ret < 0)
                                return ret;
                        hwscale = 2;
                        vwscale = 1;
                        hwsize = 640;
                        vwsize = 480;
                } else if (width > 320 || height > 240) {
                        err("Illegal dimensions");
                        return -EINVAL;
                } else {                            /* QVGA */
                        ret = mode_init_ov_sensor_regs(ov, width, height,
                                mode, sub_flag, 1);
                        if (ret < 0)
                                return ret;
                        hwscale = 1;
                        hwsize = 320;
                        vwsize = 240;
                }
        }

        /* Center the window */
        hoffset = ((hwsize - width) / 2) >> hwscale;
        voffset = ((vwsize - height) / 2) >> vwscale;

        /* FIXME! - This needs to be changed to support 160x120 and 6620!!! */
        if (sub_flag) {
                i2c_w(ov, 0x17, hwsbase+(ov->subx>>hwscale));
                i2c_w(ov, 0x18, hwebase+((ov->subx+ov->subw)>>hwscale));
                i2c_w(ov, 0x19, vwsbase+(ov->suby>>vwscale));
                i2c_w(ov, 0x1a, vwebase+((ov->suby+ov->subh)>>vwscale));
        } else {
                i2c_w(ov, 0x17, hwsbase + hoffset);
                i2c_w(ov, 0x18, hwebase + hoffset + (hwsize>>hwscale));
                i2c_w(ov, 0x19, vwsbase + voffset);
                i2c_w(ov, 0x1a, vwebase + voffset + (vwsize>>vwscale));
        }

#ifdef OV511_DEBUG
        if (dump_sensor)
                dump_i2c_regs(ov);
#endif

        return 0;
}

/* Set up the OV511/OV511+ with the given image parameters.
 *
 * Do not put any sensor-specific code in here (including I2C I/O functions)
 */
static int
ov511_mode_init_regs(struct usb_ov511 *ov,
                     int width, int height, int mode, int sub_flag)
{
        int hsegs, vsegs;

        if (sub_flag) {
                width = ov->subw;
                height = ov->subh;
        }

        PDEBUG(3, "width:%d, height:%d, mode:%d, sub:%d",
               width, height, mode, sub_flag);

        // FIXME: This should be moved to a 7111a-specific function once
        // subcapture is dealt with properly
        if (ov->sensor == SEN_SAA7111A) {
                if (width == 320 && height == 240) {
                        /* No need to do anything special */
                } else if (width == 640 && height == 480) {
                        /* Set the OV511 up as 320x480, but keep the
                         * V4L resolution as 640x480 */
                        width = 320;
                } else {
                        err("SAA7111A only allows 320x240 or 640x480");
                        return -EINVAL;
                }
        }

        /* Make sure width and height are a multiple of 8 */
        if (width % 8 || height % 8) {
                err("Invalid size (%d, %d) (mode = %d)", width, height, mode);
                return -EINVAL;
        }

        if (width < ov->minwidth || height < ov->minheight) {
                err("Requested dimensions are too small");
                return -EINVAL;
        }

        if (ov51x_stop(ov) < 0)
                return -EIO;

        if (mode == VIDEO_PALETTE_GREY) {
                reg_w(ov, R511_CAM_UV_EN, 0x00);
                reg_w(ov, R511_SNAP_UV_EN, 0x00);
                reg_w(ov, R511_SNAP_OPTS, 0x01);
        } else {
                reg_w(ov, R511_CAM_UV_EN, 0x01);
                reg_w(ov, R511_SNAP_UV_EN, 0x01);
                reg_w(ov, R511_SNAP_OPTS, 0x03);
        }

        /* Here I'm assuming that snapshot size == image size.
         * I hope that's always true. --claudio
         */
        hsegs = (width >> 3) - 1;
        vsegs = (height >> 3) - 1;

        reg_w(ov, R511_CAM_PXCNT, hsegs);
        reg_w(ov, R511_CAM_LNCNT, vsegs);
        reg_w(ov, R511_CAM_PXDIV, 0x00);
        reg_w(ov, R511_CAM_LNDIV, 0x00);

        /* YUV420, low pass filter on */
        reg_w(ov, R511_CAM_OPTS, 0x03);

        /* Snapshot additions */
        reg_w(ov, R511_SNAP_PXCNT, hsegs);
        reg_w(ov, R511_SNAP_LNCNT, vsegs);
        reg_w(ov, R511_SNAP_PXDIV, 0x00);
        reg_w(ov, R511_SNAP_LNDIV, 0x00);

        if (ov->compress) {
                /* Enable Y and UV quantization and compression */
                reg_w(ov, R511_COMP_EN, 0x07);
                reg_w(ov, R511_COMP_LUT_EN, 0x03);
                ov51x_reset(ov, OV511_RESET_OMNICE);
        }

        if (ov51x_restart(ov) < 0)
                return -EIO;

        return 0;
}

/* Sets up the OV518/OV518+ with the given image parameters
 *
 * OV518 needs a completely different approach, until we can figure out what
 * the individual registers do. Also, only 15 FPS is supported now.
 *
 * Do not put any sensor-specific code in here (including I2C I/O functions)
 */
static int
ov518_mode_init_regs(struct usb_ov511 *ov,
                     int width, int height, int mode, int sub_flag)
{
        int hsegs, vsegs, hi_res;

        if (sub_flag) {
                width = ov->subw;
                height = ov->subh;
        }

        PDEBUG(3, "width:%d, height:%d, mode:%d, sub:%d",
               width, height, mode, sub_flag);

        if (width % 16 || height % 8) {
                err("Invalid size (%d, %d)", width, height);
                return -EINVAL;
        }

        if (width < ov->minwidth || height < ov->minheight) {
                err("Requested dimensions are too small");
                return -EINVAL;
        }

        if (width >= 320 && height >= 240) {
                hi_res = 1;
        } else if (width >= 320 || height >= 240) {
                err("Invalid width/height combination (%d, %d)", width, height);
                return -EINVAL;
        } else {
                hi_res = 0;
        }

        if (ov51x_stop(ov) < 0)
                return -EIO;

        /******** Set the mode ********/

        reg_w(ov, 0x2b, 0);
        reg_w(ov, 0x2c, 0);
        reg_w(ov, 0x2d, 0);
        reg_w(ov, 0x2e, 0);
        reg_w(ov, 0x3b, 0);
        reg_w(ov, 0x3c, 0);
        reg_w(ov, 0x3d, 0);
        reg_w(ov, 0x3e, 0);

        if (ov->bridge == BRG_OV518 && ov518_color) {
                /* OV518 needs U and V swapped */
                i2c_w_mask(ov, 0x15, 0x00, 0x01);

                if (mode == VIDEO_PALETTE_GREY) {
                        /* Set 16-bit input format (UV data are ignored) */
                        reg_w_mask(ov, 0x20, 0x00, 0x08);

                        /* Set 8-bit (4:0:0) output format */
                        reg_w_mask(ov, 0x28, 0x00, 0xf0);
                        reg_w_mask(ov, 0x38, 0x00, 0xf0);
                } else {
                        /* Set 8-bit (YVYU) input format */
                        reg_w_mask(ov, 0x20, 0x08, 0x08);

                        /* Set 12-bit (4:2:0) output format */
                        reg_w_mask(ov, 0x28, 0x80, 0xf0);
                        reg_w_mask(ov, 0x38, 0x80, 0xf0);
                }
        } else {
                reg_w(ov, 0x28, (mode == VIDEO_PALETTE_GREY) ? 0x00:0x80);
                reg_w(ov, 0x38, (mode == VIDEO_PALETTE_GREY) ? 0x00:0x80);
        }

        hsegs = width / 16;
        vsegs = height / 4;

        reg_w(ov, 0x29, hsegs);
        reg_w(ov, 0x2a, vsegs);

        reg_w(ov, 0x39, hsegs);
        reg_w(ov, 0x3a, vsegs);

        /* Windows driver does this here; who knows why */
        reg_w(ov, 0x2f, 0x80);

        /******** Set the framerate (to 15 FPS) ********/

        /* Mode independent, but framerate dependent, regs */
        reg_w(ov, 0x51, 0x02);  /* Clock divider; lower==faster */
        reg_w(ov, 0x22, 0x18);
        reg_w(ov, 0x23, 0xff);

        if (ov->bridge == BRG_OV518PLUS)
                reg_w(ov, 0x21, 0x19);
        else
                reg_w(ov, 0x71, 0x19);  /* Compression-related? */

        // FIXME: Sensor-specific
        /* Bit 5 is what matters here. Of course, it is "reserved" */
        i2c_w(ov, 0x54, 0x23);

        reg_w(ov, 0x2f, 0x80);

        if (ov->bridge == BRG_OV518PLUS) {
                reg_w(ov, 0x24, 0x94);
                reg_w(ov, 0x25, 0x90);
                ov518_reg_w32(ov, 0xc4,    400, 2);     /* 190h   */
                ov518_reg_w32(ov, 0xc6,    540, 2);     /* 21ch   */
                ov518_reg_w32(ov, 0xc7,    540, 2);     /* 21ch   */
                ov518_reg_w32(ov, 0xc8,    108, 2);     /* 6ch    */
                ov518_reg_w32(ov, 0xca, 131098, 3);     /* 2001ah */
                ov518_reg_w32(ov, 0xcb,    532, 2);     /* 214h   */
                ov518_reg_w32(ov, 0xcc,   2400, 2);     /* 960h   */
                ov518_reg_w32(ov, 0xcd,     32, 2);     /* 20h    */
                ov518_reg_w32(ov, 0xce,    608, 2);     /* 260h   */
        } else {
                reg_w(ov, 0x24, 0x9f);
                reg_w(ov, 0x25, 0x90);
                ov518_reg_w32(ov, 0xc4,    400, 2);     /* 190h   */
                ov518_reg_w32(ov, 0xc6,    500, 2);     /* 1f4h   */
                ov518_reg_w32(ov, 0xc7,    500, 2);     /* 1f4h   */
                ov518_reg_w32(ov, 0xc8,    142, 2);     /* 8eh    */
                ov518_reg_w32(ov, 0xca, 131098, 3);     /* 2001ah */
                ov518_reg_w32(ov, 0xcb,    532, 2);     /* 214h   */
                ov518_reg_w32(ov, 0xcc,   2000, 2);     /* 7d0h   */
                ov518_reg_w32(ov, 0xcd,     32, 2);     /* 20h    */
                ov518_reg_w32(ov, 0xce,    608, 2);     /* 260h   */
        }

        reg_w(ov, 0x2f, 0x80);

        if (ov51x_restart(ov) < 0)
                return -EIO;

        /* Reset it just for good measure */
        if (ov51x_reset(ov, OV511_RESET_NOREGS) < 0)
                return -EIO;

        return 0;
}

/* This is a wrapper around the OV511, OV518, and sensor specific functions */
static int
mode_init_regs(struct usb_ov511 *ov,
               int width, int height, int mode, int sub_flag)
{
        int rc = 0;

        if (!ov || !ov->dev)
                return -EFAULT;

        if (ov->bclass == BCL_OV518) {
                rc = ov518_mode_init_regs(ov, width, height, mode, sub_flag);
        } else {
                rc = ov511_mode_init_regs(ov, width, height, mode, sub_flag);
        }

        if (FATAL_ERROR(rc))
                return rc;

        switch (ov->sensor) {
        case SEN_OV7610:
        case SEN_OV7620:
        case SEN_OV76BE:
        case SEN_OV8600:
        case SEN_OV6620:
        case SEN_OV6630:
                rc = set_ov_sensor_window(ov, width, height, mode, sub_flag);
                break;
        case SEN_KS0127:
        case SEN_KS0127B:
                err("KS0127-series decoders not supported yet");
                rc = -EINVAL;
                break;
        case SEN_SAA7111A:
//              rc = mode_init_saa_sensor_regs(ov, width, height, mode,
//                                             sub_flag);

                PDEBUG(1, "SAA status = 0x%02X", i2c_r(ov, 0x1f));
                break;
        default:
                err("Unknown sensor");
                rc = -EINVAL;
        }

        if (FATAL_ERROR(rc))
                return rc;

        /* Sensor-independent settings */
        rc = sensor_set_auto_brightness(ov, ov->auto_brt);
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_set_auto_exposure(ov, ov->auto_exp);
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_set_banding_filter(ov, bandingfilter);
        if (FATAL_ERROR(rc))
                return rc;

        if (ov->lightfreq) {
                rc = sensor_set_light_freq(ov, lightfreq);
                if (FATAL_ERROR(rc))
                        return rc;
        }

        rc = sensor_set_backlight(ov, ov->backlight);
        if (FATAL_ERROR(rc))
                return rc;

        rc = sensor_set_mirror(ov, ov->mirror);
        if (FATAL_ERROR(rc))
                return rc;

        return 0;
}

/* This sets the default image parameters. This is useful for apps that use
 * read() and do not set these.
 */
static int
ov51x_set_default_params(struct usb_ov511 *ov)
{
        int i;

        /* Set default sizes in case IOCTL (VIDIOCMCAPTURE) is not used
         * (using read() instead). */

        for (i = 0; i < OV511_NUMFRAMES; i++) {
                ov->frame[i].width = ov->maxwidth;
                ov->frame[i].height = ov->maxheight;
                ov->frame[i].bytes_read = 0;
                if (force_palette)
                        ov->frame[i].format = force_palette;
                else
                        ov->frame[i].format = VIDEO_PALETTE_RGB24;

                ov->frame[i].depth = get_depth(ov->frame[i].format);
        }

        PDEBUG(3, "%dx%d, %s", ov->maxwidth, ov->maxheight,
               symbolic(v4l1_plist, ov->frame[0].format));

        /* Initialize to max width/height, YUV420 or RGB24 (if supported) */
        if (mode_init_regs(ov, ov->maxwidth, ov->maxheight,
                           ov->frame[0].format, 0) < 0)
                return -EINVAL;

        return 0;
}

/**********************************************************************
 *
 * Video decoder stuff
 *
 **********************************************************************/

/* Set analog input port of decoder */
static int
decoder_set_input(struct usb_ov511 *ov, int input)
{
        PDEBUG(4, "port %d", input);

        switch (ov->sensor) {
        case SEN_SAA7111A:
        {
                /* Select mode */
                i2c_w_mask(ov, 0x02, input, 0x07);
                /* Bypass chrominance trap for modes 4..7 */
                i2c_w_mask(ov, 0x09, (input > 3) ? 0x80:0x00, 0x80);
                break;
        }
        default:
                return -EINVAL;
        }

        return 0;
}

/* Get ASCII name of video input */
static int
decoder_get_input_name(struct usb_ov511 *ov, int input, char *name)
{
        switch (ov->sensor) {
        case SEN_SAA7111A:
        {
                if (input < 0 || input > 7)
                        return -EINVAL;
                else if (input < 4)
                        sprintf(name, "CVBS-%d", input);
                else // if (input < 8)
                        sprintf(name, "S-Video-%d", input - 4);
                break;
        }
        default:
                sprintf(name, "%s", "Camera");
        }

        return 0;
}

/* Set norm (NTSC, PAL, SECAM, AUTO) */
static int
decoder_set_norm(struct usb_ov511 *ov, int norm)
{
        PDEBUG(4, "%d", norm);

        switch (ov->sensor) {
        case SEN_SAA7111A:
        {
                int reg_8, reg_e;

                if (norm == VIDEO_MODE_NTSC) {
                        reg_8 = 0x40;   /* 60 Hz */
                        reg_e = 0x00;   /* NTSC M / PAL BGHI */
                } else if (norm == VIDEO_MODE_PAL) {
                        reg_8 = 0x00;   /* 50 Hz */
                        reg_e = 0x00;   /* NTSC M / PAL BGHI */
                } else if (norm == VIDEO_MODE_AUTO) {
                        reg_8 = 0x80;   /* Auto field detect */
                        reg_e = 0x00;   /* NTSC M / PAL BGHI */
                } else if (norm == VIDEO_MODE_SECAM) {
                        reg_8 = 0x00;   /* 50 Hz */
                        reg_e = 0x50;   /* SECAM / PAL 4.43 */
                } else {
                        return -EINVAL;
                }

                i2c_w_mask(ov, 0x08, reg_8, 0xc0);
                i2c_w_mask(ov, 0x0e, reg_e, 0x70);
                break;
        }
        default:
                return -EINVAL;
        }

        return 0;
}

/**********************************************************************
 *
 * Color correction functions
 *
 **********************************************************************/

/*
 * Turn a YUV4:2:0 block into an RGB block
 *
 * Video4Linux seems to use the blue, green, red channel
 * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
 *
 * Color space conversion coefficients taken from the excellent
 * http://www.inforamp.net/~poynton/ColorFAQ.html
 * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
 * Y values are given for all 4 pixels, but the U (Pb)
 * and V (Pr) are assumed constant over the 2x2 block.
 *
 * To avoid floating point arithmetic, the color conversion
 * coefficients are scaled into 16.16 fixed-point integers.
 * They were determined as follows:
 *
 *      double brightness = 1.0;  (0->black; 1->full scale)
 *      double saturation = 1.0;  (0->greyscale; 1->full color)
 *      double fixScale = brightness * 256 * 256;
 *      int rvScale = (int)(1.402 * saturation * fixScale);
 *      int guScale = (int)(-0.344136 * saturation * fixScale);
 *      int gvScale = (int)(-0.714136 * saturation * fixScale);
 *      int buScale = (int)(1.772 * saturation * fixScale);
 *      int yScale = (int)(fixScale);
 */

/* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
#define LIMIT(x) ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)))

static inline void
move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v,
               int rowPixels, unsigned char * rgb, int bits)
{
        const int rvScale = 91881;
        const int guScale = -22553;
        const int gvScale = -46801;
        const int buScale = 116129;
        const int yScale  = 65536;
        int r, g, b;

        g = guScale * u + gvScale * v;
        if (force_rgb) {
                r = buScale * u;
                b = rvScale * v;
        } else {
                r = rvScale * v;
                b = buScale * u;
        }

        yTL *= yScale; yTR *= yScale;
        yBL *= yScale; yBR *= yScale;

        if (bits == 24) {
                /* Write out top two pixels */
                rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
                rgb[2] = LIMIT(r+yTL);

                rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
                rgb[5] = LIMIT(r+yTR);

                /* Skip down to next line to write out bottom two pixels */
                rgb += 3 * rowPixels;
                rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
                rgb[2] = LIMIT(r+yBL);

                rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
                rgb[5] = LIMIT(r+yBR);
        } else if (bits == 16) {
                /* Write out top two pixels */
                rgb[0] = ((LIMIT(b+yTL) >> 3) & 0x1F)
                        | ((LIMIT(g+yTL) << 3) & 0xE0);
                rgb[1] = ((LIMIT(g+yTL) >> 5) & 0x07)
                        | (LIMIT(r+yTL) & 0xF8);

                rgb[2] = ((LIMIT(b+yTR) >> 3) & 0x1F)
                        | ((LIMIT(g+yTR) << 3) & 0xE0);
                rgb[3] = ((LIMIT(g+yTR) >> 5) & 0x07)
                        | (LIMIT(r+yTR) & 0xF8);

                /* Skip down to next line to write out bottom two pixels */
                rgb += 2 * rowPixels;

                rgb[0] = ((LIMIT(b+yBL) >> 3) & 0x1F)
                        | ((LIMIT(g+yBL) << 3) & 0xE0);
                rgb[1] = ((LIMIT(g+yBL) >> 5) & 0x07)
                        | (LIMIT(r+yBL) & 0xF8);

                rgb[2] = ((LIMIT(b+yBR) >> 3) & 0x1F)
                        | ((LIMIT(g+yBR) << 3) & 0xE0);
                rgb[3] = ((LIMIT(g+yBR) >> 5) & 0x07)
                        | (LIMIT(r+yBR) & 0xF8);
        }
}

/**********************************************************************
 *
 * Raw data parsing
 *
 **********************************************************************/

/* Copies a 64-byte segment at pIn to an 8x8 block at pOut. The width of the
 * image at pOut is specified by w.
 */
static inline void
make_8x8(unsigned char *pIn, unsigned char *pOut, int w)
{
        unsigned char *pOut1 = pOut;
        int x, y;

        for (y = 0; y < 8; y++) {
                pOut1 = pOut;
                for (x = 0; x < 8; x++) {
                        *pOut1++ = *pIn++;
                }
                pOut += w;
        }
}

/*
 * For RAW BW (YUV 4:0:0) images, data show up in 256 byte segments.
 * The segments represent 4 squares of 8x8 pixels as follows:
 *
 *      0  1 ...  7    64  65 ...  71   ...  192 193 ... 199
 *      8  9 ... 15    72  73 ...  79        200 201 ... 207
 *           ...              ...                    ...
 *     56 57 ... 63   120 121 ... 127        248 249 ... 255
 *
 */ 
static void
yuv400raw_to_yuv400p(struct ov511_frame *frame,
                     unsigned char *pIn0, unsigned char *pOut0)
{
        int x, y;
        unsigned char *pIn, *pOut, *pOutLine;

        /* Copy Y */
        pIn = pIn0;
        pOutLine = pOut0;
        for (y = 0; y < frame->rawheight - 1; y += 8) {
                pOut = pOutLine;
                for (x = 0; x < frame->rawwidth - 1; x += 8) {
                        make_8x8(pIn, pOut, frame->rawwidth);
                        pIn += 64;
                        pOut += 8;
                }
                pOutLine += 8 * frame->rawwidth;
        }
}

/*
 * For YUV 4:2:0 images, the data show up in 384 byte segments.
 * The first 64 bytes of each segment are U, the next 64 are V.  The U and
 * V are arranged as follows:
 *
 *      0  1 ...  7
 *      8  9 ... 15
 *           ...   
 *     56 57 ... 63
 *
 * U and V are shipped at half resolution (1 U,V sample -> one 2x2 block).
 *
 * The next 256 bytes are full resolution Y data and represent 4 squares
 * of 8x8 pixels as follows:
 *
 *      0  1 ...  7    64  65 ...  71   ...  192 193 ... 199
 *      8  9 ... 15    72  73 ...  79        200 201 ... 207
 *           ...              ...                    ...
 *     56 57 ... 63   120 121 ... 127   ...  248 249 ... 255
 *
 * Note that the U and V data in one segment represent a 16 x 16 pixel
 * area, but the Y data represent a 32 x 8 pixel area. If the width is not an
 * even multiple of 32, the extra 8x8 blocks within a 32x8 block belong to the
 * next horizontal stripe.
 *
 * If dumppix module param is set, _parse_data just dumps the incoming segments,
 * verbatim, in order, into the frame. When used with vidcat -f ppm -s 640x480
 * this puts the data on the standard output and can be analyzed with the
 * parseppm.c utility I wrote.  That's a much faster way for figuring out how
 * these data are scrambled.
 */

/* Converts from raw, uncompressed segments at pIn0 to a YUV420P frame at pOut0.
 *
 * FIXME: Currently only handles width and height that are multiples of 16
 */
static void
yuv420raw_to_yuv420p(struct ov511_frame *frame,
                     unsigned char *pIn0, unsigned char *pOut0)
{
        int k, x, y;
        unsigned char *pIn, *pOut, *pOutLine;
        const unsigned int a = frame->rawwidth * frame->rawheight;
        const unsigned int w = frame->rawwidth / 2;

        /* Copy U and V */
        pIn = pIn0;
        pOutLine = pOut0 + a;
        for (y = 0; y < frame->rawheight - 1; y += 16) {
                pOut = pOutLine;
                for (x = 0; x < frame->rawwidth - 1; x += 16) {
                        make_8x8(pIn, pOut, w);
                        make_8x8(pIn + 64, pOut + a/4, w);
                        pIn += 384;
                        pOut += 8;
                }
                pOutLine += 8 * w;
        }

        /* Copy Y */
        pIn = pIn0 + 128;
        pOutLine = pOut0;
        k = 0;
        for (y = 0; y < frame->rawheight - 1; y += 8) {
                pOut = pOutLine;
                for (x = 0; x < frame->rawwidth - 1; x += 8) {
                        make_8x8(pIn, pOut, frame->rawwidth);
                        pIn += 64;
                        pOut += 8;
                        if ((++k) > 3) {
                                k = 0;
                                pIn += 128;
                        }
                }
                pOutLine += 8 * frame->rawwidth;
        }
}

/*
 * fixFrameRGBoffset--
 * My camera seems to return the red channel about 1 pixel
 * low, and the blue channel about 1 pixel high. After YUV->RGB
 * conversion, we can correct this easily. OSL 2/24/2000.
 */
static void
fixFrameRGBoffset(struct ov511_frame *frame)
{
        int x, y;
        int rowBytes = frame->width*3, w = frame->width;
        unsigned char *rgb = frame->data;
        const int shift = 1;  /* Distance to shift pixels by, vertically */

        /* Don't bother with little images */
        if (frame->width < 400)
                return;

        /* This only works with RGB24 */
        if (frame->format != VIDEO_PALETTE_RGB24)
                return;

        /* Shift red channel up */
        for (y = shift; y < frame->height; y++) {
                int lp = (y-shift)*rowBytes;     /* Previous line offset */
                int lc = y*rowBytes;             /* Current line offset */
                for (x = 0; x < w; x++)
                        rgb[lp+x*3+2] = rgb[lc+x*3+2]; /* Shift red up */
        }

        /* Shift blue channel down */
        for (y = frame->height-shift-1; y >= 0; y--) {
                int ln = (y + shift) * rowBytes;  /* Next line offset */
                int lc = y * rowBytes;            /* Current line offset */
                for (x = 0; x < w; x++)
                        rgb[ln+x*3+0] = rgb[lc+x*3+0]; /* Shift blue down */
        }
}

/**********************************************************************
 *
 * Decompression
 *
 **********************************************************************/

/* Chooses a decompression module, locks it, and sets ov->decomp_ops
 * accordingly. Returns -ENXIO if decompressor is not available, otherwise
 * returns 0 if no other error.
 */
static int
request_decompressor(struct usb_ov511 *ov)
{
        if (!ov)
                return -ENODEV;

        if (ov->decomp_ops) {
                err("ERROR: Decompressor already requested!");
                return -EINVAL;
        }

        lock_kernel();

        /* Try to get MMX, and fall back on no-MMX if necessary */
        if (ov->bclass == BCL_OV511) {
                if (ov511_mmx_decomp_ops) {
                        PDEBUG(3, "Using OV511 MMX decompressor");
                        ov->decomp_ops = ov511_mmx_decomp_ops;
                } else if (ov511_decomp_ops) {
                        PDEBUG(3, "Using OV511 decompressor");
                        ov->decomp_ops = ov511_decomp_ops;
                } else {
                        err("No decompressor available");
                }
        } else if (ov->bclass == BCL_OV518) {
                if (ov518_mmx_decomp_ops) {
                        PDEBUG(3, "Using OV518 MMX decompressor");
                        ov->decomp_ops = ov518_mmx_decomp_ops;
                } else if (ov518_decomp_ops) {
                        PDEBUG(3, "Using OV518 decompressor");
                        ov->decomp_ops = ov518_decomp_ops;
                } else {
                        err("No decompressor available");
                }
        } else {
                err("Unknown bridge");
        }

        if (ov->decomp_ops) {
                if (!ov->decomp_ops->owner) {
                        ov->decomp_ops = NULL;
                        unlock_kernel();
                        return -ENOSYS;
                }
                __MOD_INC_USE_COUNT(ov->decomp_ops->owner);
                unlock_kernel();
                return 0;
        } else {
                unlock_kernel();
                return -ENOSYS;
        }
}

/* Unlocks decompression module and nulls ov->decomp_ops. Safe to call even
 * if ov->decomp_ops is NULL.
 */
static void
release_decompressor(struct usb_ov511 *ov)
{
        int released = 0;       /* Did we actually do anything? */

        if (!ov)
                return;

        lock_kernel();

        if (ov->decomp_ops && ov->decomp_ops->owner) {
                __MOD_DEC_USE_COUNT(ov->decomp_ops->owner);
                released = 1;
        }

        ov->decomp_ops = NULL;

        unlock_kernel();

        if (released)
                PDEBUG(3, "Decompressor released");
}

static void
decompress(struct usb_ov511 *ov, struct ov511_frame *frame,
           unsigned char *pIn0, unsigned char *pOut0)
{
        if (!ov->decomp_ops)
                if (request_decompressor(ov))
                        return;

        PDEBUG(4, "Decompressing %d bytes", frame->bytes_recvd);

        if (frame->format == VIDEO_PALETTE_GREY
            && ov->decomp_ops->decomp_400) {
                int ret = ov->decomp_ops->decomp_400(
                        pIn0,
                        pOut0,
                        frame->compbuf,
                        frame->rawwidth,
                        frame->rawheight,
                        frame->bytes_recvd);
                PDEBUG(4, "DEBUG: decomp_400 returned %d", ret);
        } else if (frame->format != VIDEO_PALETTE_GREY
                   && ov->decomp_ops->decomp_420) {
                int ret = ov->decomp_ops->decomp_420(
                        pIn0,
                        pOut0,
                        frame->compbuf,
                        frame->rawwidth,
                        frame->rawheight,
                        frame->bytes_recvd);
                PDEBUG(4, "DEBUG: decomp_420 returned %d", ret);
        } else {
                err("Decompressor does not support this format");
        }
}

/**********************************************************************
 *
 * Format conversion
 *
 **********************************************************************/

/* Converts from planar YUV420 to RGB24. */
static void
yuv420p_to_rgb(struct ov511_frame *frame,
               unsigned char *pIn0, unsigned char *pOut0, int bits)
{
        const int numpix = frame->width * frame->height;
        const int bytes = bits >> 3;
        int i, j, y00, y01, y10, y11, u, v;
        unsigned char *pY = pIn0;
        unsigned char *pU = pY + numpix;
        unsigned char *pV = pU + numpix / 4;
        unsigned char *pOut = pOut0;

        for (j = 0; j <= frame->height - 2; j += 2) {
                for (i = 0; i <= frame->width - 2; i += 2) {
                        y00 = *pY;
                        y01 = *(pY + 1);
                        y10 = *(pY + frame->width);
                        y11 = *(pY + frame->width + 1);
                        u = (*pU++) - 128;
                        v = (*pV++) - 128;

                        move_420_block(y00, y01, y10, y11, u, v,
                                       frame->width, pOut, bits);

                        pY += 2;
                        pOut += 2 * bytes;
                }
                pY += frame->width;
                pOut += frame->width * bytes;
        }
}

/* Converts from planar YUV420 to YUV422 (YUYV). */
static void
yuv420p_to_yuv422(struct ov511_frame *frame,
                  unsigned char *pIn0, unsigned char *pOut0)
{
        const int numpix = frame->width * frame->height;
        int i, j;
        unsigned char *pY = pIn0;
        unsigned char *pU = pY + numpix;
        unsigned char *pV = pU + numpix / 4;
        unsigned char *pOut = pOut0;

        for (i = 0; i < numpix; i++) {
                *pOut = *(pY + i);
                pOut += 2;
        }

        pOut = pOut0 + 1;
        for (j = 0; j <= frame->height - 2 ; j += 2) {
                for (i = 0; i <= frame->width - 2; i += 2) {
                        int u = *pU++;
                        int v = *pV++;

                        *pOut = u;
                        *(pOut+2) = v;
                        *(pOut+frame->width*2) = u;
                        *(pOut+frame->width*2+2) = v;
                        pOut += 4;
                }
                pOut += (frame->width * 2);
        }
}

/* Converts pData from planar YUV420 to planar YUV422 **in place**. */
static void
yuv420p_to_yuv422p(struct ov511_frame *frame, unsigned char *pData)
{
        const int numpix = frame->width * frame->height;
        const int w = frame->width;
        int j;
        unsigned char *pIn, *pOut;

        /* Clear U and V */
        memset(pData + numpix + numpix / 2, 127, numpix / 2);

        /* Convert V starting from beginning and working forward */
        pIn = pData + numpix + numpix / 4;
        pOut = pData + numpix +numpix / 2;
        for (j = 0; j <= frame->height - 2; j += 2) {
                memmove(pOut, pIn, w/2);
                memmove(pOut + w/2, pIn, w/2);
                pIn += w/2;
                pOut += w;
        }

        /* Convert U, starting from end and working backward */
        pIn = pData + numpix + numpix / 4;
        pOut = pData + numpix + numpix / 2;
        for (j = 0; j <= frame->height - 2; j += 2) {
                pIn -= w/2;
                pOut -= w;
                memmove(pOut, pIn, w/2);
                memmove(pOut + w/2, pIn, w/2);
        }
}

/* Fuses even and odd fields together, and doubles width.
 * INPUT: an odd field followed by an even field at pIn0, in YUV planar format
 * OUTPUT: a normal YUV planar image, with correct aspect ratio
 */
static void
deinterlace(struct ov511_frame *frame, int rawformat,
            unsigned char *pIn0, unsigned char *pOut0)
{
        const int fieldheight = frame->rawheight / 2;
        const int fieldpix = fieldheight * frame->rawwidth;
        const int w = frame->width;
        int x, y;
        unsigned char *pInEven, *pInOdd, *pOut;

        PDEBUG(5, "fieldheight=%d", fieldheight);

        if (frame->rawheight != frame->height) {
                err("invalid height");
                return;
        }

        if ((frame->rawwidth * 2) != frame->width) {
                err("invalid width");
                return;
        }

        /* Y */
        pInOdd = pIn0;
        pInEven = pInOdd + fieldpix;
        pOut = pOut0;
        for (y = 0; y < fieldheight; y++) {
                for (x = 0; x < frame->rawwidth; x++) {
                        *pOut = *pInEven;
                        *(pOut+1) = *pInEven++;
                        *(pOut+w) = *pInOdd;
                        *(pOut+w+1) = *pInOdd++;
                        pOut += 2;
                }
                pOut += w;
        }

        if (rawformat == RAWFMT_YUV420) {
        /* U */
                pInOdd = pIn0 + fieldpix * 2;
                pInEven = pInOdd + fieldpix / 4;
                for (y = 0; y < fieldheight / 2; y++) {
                        for (x = 0; x < frame->rawwidth / 2; x++) {
                                *pOut = *pInEven;
                                *(pOut+1) = *pInEven++;
                                *(pOut+w/2) = *pInOdd;
                                *(pOut+w/2+1) = *pInOdd++;
                                pOut += 2;
                        }
                        pOut += w/2;
                }
        /* V */
                pInOdd = pIn0 + fieldpix * 2 + fieldpix / 2;
                pInEven = pInOdd + fieldpix / 4;
                for (y = 0; y < fieldheight / 2; y++) {
                        for (x = 0; x < frame->rawwidth / 2; x++) {
                                *pOut = *pInEven;
                                *(pOut+1) = *pInEven++;
                                *(pOut+w/2) = *pInOdd;
                                *(pOut+w/2+1) = *pInOdd++;
                                pOut += 2;
                        }
                        pOut += w/2;
                }
        }
}

static void
ov51x_postprocess_grey(struct usb_ov511 *ov, struct ov511_frame *frame)
{
                /* Deinterlace frame, if necessary */
                if (ov->sensor == SEN_SAA7111A && frame->rawheight >= 480) {
                        if (frame->compressed)
                                decompress(ov, frame, frame->rawdata,
                                                 frame->tempdata);
                        else
                                yuv400raw_to_yuv400p(frame, frame->rawdata,
                                                     frame->tempdata);

                        deinterlace(frame, RAWFMT_YUV400, frame->tempdata,
                                    frame->data);
                } else {
                        if (frame->compressed)
                                decompress(ov, frame, frame->rawdata,
                                                 frame->data);
                        else
                                yuv400raw_to_yuv400p(frame, frame->rawdata,
                                                     frame->data);
                }
}

/* Process raw YUV420 data into the format requested by the app. Conversion
 * between V4L formats is allowed.
 */
static void
ov51x_postprocess_yuv420(struct usb_ov511 *ov, struct ov511_frame *frame)
{
        /* Process frame->rawdata to frame->tempdata */
        if (frame->compressed)
                decompress(ov, frame, frame->rawdata, frame->tempdata);
        else
                yuv420raw_to_yuv420p(frame, frame->rawdata, frame->tempdata);

        /* Deinterlace frame, if necessary */
        if (ov->sensor == SEN_SAA7111A && frame->rawheight >= 480) {
                memcpy(frame->rawdata, frame->tempdata,
                        MAX_RAW_DATA_SIZE(frame->width, frame->height));
                deinterlace(frame, RAWFMT_YUV420, frame->rawdata,
                            frame->tempdata);
        }

        /* Frame should be (width x height) and not (rawwidth x rawheight) at
         * this point. */

        /* Process frame->tempdata to frame->data */
        switch (frame->format) {
        case VIDEO_PALETTE_RGB565:
                yuv420p_to_rgb(frame, frame->tempdata, frame->data, 16);
                break;
        case VIDEO_PALETTE_RGB24:
                yuv420p_to_rgb(frame, frame->tempdata, frame->data, 24);
                break;
        case VIDEO_PALETTE_YUV422:
        case VIDEO_PALETTE_YUYV:
                yuv420p_to_yuv422(frame, frame->tempdata, frame->data);
                break;
        case VIDEO_PALETTE_YUV420:
        case VIDEO_PALETTE_YUV420P:
                memcpy(frame->data, frame->tempdata,
                        MAX_RAW_DATA_SIZE(frame->width, frame->height));
                break;
        case VIDEO_PALETTE_YUV422P:
                /* Data is converted in place, so copy it in advance */
                memcpy(frame->data, frame->tempdata,
                        MAX_RAW_DATA_SIZE(frame->width, frame->height));

                yuv420p_to_yuv422p(frame, frame->data);
                break;
        default:
                err("Cannot convert YUV420 to %s",
                    symbolic(v4l1_plist, frame->format));
        }

        if (fix_rgb_offset)
                fixFrameRGBoffset(frame);
}

/* Post-processes the specified frame. This consists of:
 *      1. Decompress frame, if necessary
 *      2. Deinterlace frame and scale to proper size, if necessary
 *      3. Convert from YUV planar to destination format, if necessary
 *      4. Fix the RGB offset, if necessary
 */
static void
ov51x_postprocess(struct usb_ov511 *ov, struct ov511_frame *frame)
{
        if (dumppix) {
                memset(frame->data, 0,
                        MAX_DATA_SIZE(ov->maxwidth, ov->maxheight));
                PDEBUG(4, "Dumping %d bytes", frame->bytes_recvd);
                memcpy(frame->data, frame->rawdata, frame->bytes_recvd);
        } else {
                switch (frame->format) {
                case VIDEO_PALETTE_GREY:
                        ov51x_postprocess_grey(ov, frame);
                        break;
                case VIDEO_PALETTE_YUV420:
                case VIDEO_PALETTE_YUV420P:
                case VIDEO_PALETTE_RGB565:
                case VIDEO_PALETTE_RGB24:
                case VIDEO_PALETTE_YUV422:
                case VIDEO_PALETTE_YUYV:
                case VIDEO_PALETTE_YUV422P:
                        ov51x_postprocess_yuv420(ov, frame);
                        break;
                default:
                        err("Cannot convert data to %s",
                            symbolic(v4l1_plist, frame->format));
                }
        }
}

/**********************************************************************
 *
 * OV51x data transfer, IRQ handler
 *
 **********************************************************************/

static inline void
ov511_move_data(struct usb_ov511 *ov, unsigned char *in, int n)
{
        int num, offset;
        int pnum = in[ov->packet_size - 1];             /* Get packet number */
        int max_raw = MAX_RAW_DATA_SIZE(ov->maxwidth, ov->maxheight);
        struct ov511_frame *frame = &ov->frame[ov->curframe];
        struct timeval *ts;

        /* SOF/EOF packets have 1st to 8th bytes zeroed and the 9th
         * byte non-zero. The EOF packet has image width/height in the
         * 10th and 11th bytes. The 9th byte is given as follows:
         *
         * bit 7: EOF
         *     6: compression enabled
         *     5: 422/420/400 modes
         *     4: 422/420/400 modes
         *     3: 1
         *     2: snapshot button on
         *     1: snapshot frame
         *     0: even/odd field
         */

        if (printph) {
                info("ph(%3d): %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x",
                     pnum, in[0], in[1], in[2], in[3], in[4], in[5], in[6],
                     in[7], in[8], in[9], in[10], in[11]);
        }

        /* Check for SOF/EOF packet */
        if ((in[0] | in[1] | in[2] | in[3] | in[4] | in[5] | in[6] | in[7]) ||
            (~in[8] & 0x08))
                goto check_middle;

        /* Frame end */
        if (in[8] & 0x80) {
                ts = (struct timeval *)(frame->data
                      + MAX_FRAME_SIZE(ov->maxwidth, ov->maxheight));
                do_gettimeofday(ts);

                /* Get the actual frame size from the EOF header */
                frame->rawwidth = ((int)(in[9]) + 1) * 8;
                frame->rawheight = ((int)(in[10]) + 1) * 8;

                PDEBUG(4, "Frame end, frame=%d, pnum=%d, w=%d, h=%d, recvd=%d",
                        ov->curframe, pnum, frame->rawwidth, frame->rawheight,
                        frame->bytes_recvd);

                /* Validate the header data */
                RESTRICT_TO_RANGE(frame->rawwidth, ov->minwidth, ov->maxwidth);
                RESTRICT_TO_RANGE(frame->rawheight, ov->minheight,
                                  ov->maxheight);

                /* Don't allow byte count to exceed buffer size */
                RESTRICT_TO_RANGE(frame->bytes_recvd, 8, max_raw);

                if (frame->scanstate == STATE_LINES) {
                        int nextf;

                        frame->grabstate = FRAME_DONE;  // FIXME: Is this right?

                        if (waitqueue_active(&frame->wq)) {
                                frame->grabstate = FRAME_DONE;
                                wake_up_interruptible(&frame->wq);
                        }

                        /* If next frame is ready or grabbing,
                         * point to it */
                        nextf = (ov->curframe + 1) % OV511_NUMFRAMES;
                        if (ov->frame[nextf].grabstate == FRAME_READY
                            || ov->frame[nextf].grabstate == FRAME_GRABBING) {
                                ov->curframe = nextf;
                                ov->frame[nextf].scanstate = STATE_SCANNING;
                        } else {
                                if (frame->grabstate == FRAME_DONE) {
                                        PDEBUG(4, "** Frame done **");
                                } else {
                                        PDEBUG(4, "Frame not ready? state = %d",
                                                ov->frame[nextf].grabstate);
                                }

                                ov->curframe = -1;
                        }
                } else {
                        PDEBUG(5, "Frame done, but not scanning");
                }
                /* Image corruption caused by misplaced frame->segment = 0
                 * fixed by carlosf@conectiva.com.br
                 */
        } else {
                /* Frame start */
                PDEBUG(4, "Frame start, framenum = %d", ov->curframe);

                /* Check to see if it's a snapshot frame */
                /* FIXME?? Should the snapshot reset go here? Performance? */
                if (in[8] & 0x02) {
                        frame->snapshot = 1;
                        PDEBUG(3, "snapshot detected");
                }

                frame->scanstate = STATE_LINES;
                frame->bytes_recvd = 0;
                frame->compressed = in[8] & 0x40;
        }

check_middle:
        /* Are we in a frame? */
        if (frame->scanstate != STATE_LINES) {
                PDEBUG(5, "Not in a frame; packet skipped");
                return;
        }

        /* If frame start, skip header */
        if (frame->bytes_recvd == 0)
                offset = 9;
        else
                offset = 0;

        num = n - offset - 1;

        /* Dump all data exactly as received */
        if (dumppix == 2) {
                frame->bytes_recvd += n - 1;
                if (frame->bytes_recvd <= max_raw)
                        memcpy(frame->rawdata + frame->bytes_recvd - (n - 1),
                                in, n - 1);
                else
                        PDEBUG(3, "Raw data buffer overrun!! (%d)",
                                frame->bytes_recvd - max_raw);
        } else if (!frame->compressed && !remove_zeros) {
                frame->bytes_recvd += num;
                if (frame->bytes_recvd <= max_raw)
                        memcpy(frame->rawdata + frame->bytes_recvd - num,
                                in + offset, num);
                else
                        PDEBUG(3, "Raw data buffer overrun!! (%d)",
                                frame->bytes_recvd - max_raw);
        } else { /* Remove all-zero FIFO lines (aligned 32-byte blocks) */
                int b, read = 0, allzero, copied = 0;
                if (offset) {
                        frame->bytes_recvd += 32 - offset;      // Bytes out
                        memcpy(frame->rawdata,  in + offset, 32 - offset);
                        read += 32;
                }

                while (read < n - 1) {
                        allzero = 1;
                        for (b = 0; b < 32; b++) {
                                if (in[read + b]) {
                                        allzero = 0;
                                        break;
                                }
                        }

                        if (allzero) {
                                /* Don't copy it */
                        } else {
                                if (frame->bytes_recvd + copied + 32 <= max_raw)
                                {
                                        memcpy(frame->rawdata
                                                + frame->bytes_recvd + copied,
                                                in + read, 32);
                                        copied += 32;
                                } else {
                                        PDEBUG(3, "Raw data buffer overrun!!");
                                }
                        }
                        read += 32;
                }

                frame->bytes_recvd += copied;
        }
}

static inline void
ov518_move_data(struct usb_ov511 *ov, unsigned char *in, int n)
{
        int max_raw = MAX_RAW_DATA_SIZE(ov->maxwidth, ov->maxheight);
        struct ov511_frame *frame = &ov->frame[ov->curframe];
        struct timeval *ts;

        /* Don't copy the packet number byte */
        if (ov->packet_numbering)
                --n;

        /* A false positive here is likely, until OVT gives me
         * the definitive SOF/EOF format */
        if ((!(in[0] | in[1] | in[2] | in[3] | in[5])) && in[6]) {
                if (printph) {
                        info("ph: %2x %2x %2x %2x %2x %2x %2x %2x", in[0],
                             in[1], in[2], in[3], in[4], in[5], in[6], in[7]);
                }

                if (frame->scanstate == STATE_LINES) {
                        PDEBUG(4, "Detected frame end/start");
                        goto eof;