sn9c102_core.c

Linux Kernel 2.6.9 for OMAP1710

/***************************************************************************
 * V4L2 driver for SN9C10[12] PC Camera Controllers                        *
 *                                                                         *
 * Copyright (C) 2004 by Luca Risolia <[e-mail]>       *
 *                                                                         *
 * 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/module.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/param.h>
#include <linux/moduleparam.h>
#include <linux/errno.h>
#include <linux/slab.h>
#include <linux/string.h>
#include <linux/device.h>
#include <linux/fs.h>
#include <linux/time.h>
#include <linux/delay.h>
#include <linux/stddef.h>
#include <linux/compiler.h>
#include <linux/ioctl.h>
#include <linux/poll.h>
#include <linux/stat.h>
#include <linux/mm.h>
#include <linux/vmalloc.h>
#include <linux/page-flags.h>
#include <asm/page.h>
#include <asm/uaccess.h>

#include "sn9c102.h"

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

MODULE_DEVICE_TABLE(usb, sn9c102_id_table);

MODULE_AUTHOR(SN9C102_MODULE_AUTHOR " " SN9C102_AUTHOR_EMAIL);
MODULE_DESCRIPTION(SN9C102_MODULE_NAME);
MODULE_VERSION(SN9C102_MODULE_VERSION);
MODULE_LICENSE(SN9C102_MODULE_LICENSE);

static short video_nr[] = {[0 ... SN9C102_MAX_DEVICES-1] = -1};
static unsigned int nv;
module_param_array(video_nr, short, nv, 0444);
MODULE_PARM_DESC(video_nr,
                 "\n<-1|n[,...]> Specify V4L2 minor mode number."
                 "\n -1 = use next available (default)"
                 "\n  n = use minor number n (integer >= 0)"
                 "\nYou can specify up to "__MODULE_STRING(SN9C102_MAX_DEVICES)
                 " cameras this way."
                 "\nFor example:"
                 "\nvideo_nr=-1,2,-1 would assign minor number 2 to"
                 "\nthe second camera and use auto for the first"
                 "\none and for every other camera."
                 "\n");

#ifdef SN9C102_DEBUG
static unsigned short debug = SN9C102_DEBUG_LEVEL;
module_param(debug, ushort, 0644);
MODULE_PARM_DESC(debug,
                 "\n<n> Debugging information level, from 0 to 3:"
                 "\n0 = none (use carefully)"
                 "\n1 = critical errors"
                 "\n2 = significant informations"
                 "\n3 = more verbose messages"
                 "\nLevel 3 is useful for testing only, when only "
                 "one device is used."
                 "\nDefault value is "__MODULE_STRING(SN9C102_DEBUG_LEVEL)"."
                 "\n");
#endif

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

typedef char sn9c102_sof_header_t[12];
typedef char sn9c102_eof_header_t[4];

static sn9c102_sof_header_t sn9c102_sof_header[] = {
	{0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96, 0x00},
	{0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96, 0x01},
};


static sn9c102_eof_header_t sn9c102_eof_header[] = {
	{0x00, 0x00, 0x00, 0x00},
	{0x40, 0x00, 0x00, 0x00},
	{0x80, 0x00, 0x00, 0x00},
	{0xc0, 0x00, 0x00, 0x00},
};

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

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);
	ret = __pa(kva);
	return ret;
}


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

	size = PAGE_ALIGN(size);

	mem = vmalloc_32((unsigned long)size);
	if (!mem)
		return NULL;

	memset(mem, 0, size);

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

	return mem;
}


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

	if (!mem)
		return;

	size = PAGE_ALIGN(size);

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

	vfree(mem);
}


static u32 sn9c102_request_buffers(struct sn9c102_device* cam, u32 count)
{
	struct v4l2_pix_format* p = &(cam->sensor->pix_format);
	const size_t imagesize = (p->width * p->height * p->priv)/8;
	void* buff = NULL;
	u32 i;

	if (count > SN9C102_MAX_FRAMES)
		count = SN9C102_MAX_FRAMES;

	cam->nbuffers = count;
	while (cam->nbuffers > 0) {
		if ((buff = rvmalloc(cam->nbuffers * imagesize)))
			break;
		cam->nbuffers--;
	}

	for (i = 0; i < cam->nbuffers; i++) {
		cam->frame[i].bufmem = buff + i*imagesize;
		cam->frame[i].buf.index = i;
		cam->frame[i].buf.m.offset = i*imagesize;
		cam->frame[i].buf.length = imagesize;
		cam->frame[i].buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		cam->frame[i].buf.sequence = 0;
		cam->frame[i].buf.field = V4L2_FIELD_NONE;
		cam->frame[i].buf.memory = V4L2_MEMORY_MMAP;
		cam->frame[i].buf.flags = 0;
	}

	return cam->nbuffers;
}


static void sn9c102_release_buffers(struct sn9c102_device* cam)
{
	if (cam->nbuffers) {
		rvfree(cam->frame[0].bufmem,
		       cam->nbuffers * cam->frame[0].buf.length);
		cam->nbuffers = 0;
	}
}


static void sn9c102_empty_framequeues(struct sn9c102_device* cam)
{
	u32 i;

	INIT_LIST_HEAD(&cam->inqueue);
	INIT_LIST_HEAD(&cam->outqueue);

	for (i = 0; i < SN9C102_MAX_FRAMES; i++) {
		cam->frame[i].state = F_UNUSED;
		cam->frame[i].buf.bytesused = 0;
	}
}


static void sn9c102_queue_unusedframes(struct sn9c102_device* cam)
{
	unsigned long lock_flags;
	u32 i;

	for (i = 0; i < cam->nbuffers; i++)
		if (cam->frame[i].state == F_UNUSED) {
			cam->frame[i].state = F_QUEUED;
			spin_lock_irqsave(&cam->queue_lock, lock_flags);
			list_add_tail(&cam->frame[i].frame, &cam->inqueue);
			spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
		}
}

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

int sn9c102_write_reg(struct sn9c102_device* cam, u8 value, u16 index)
{
	struct usb_device* udev = cam->usbdev;
	u8* buff = cam->control_buffer;
	int res;

	*buff = value;

	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x08, 0x41,
	                      index, 0, buff, 1, SN9C102_CTRL_TIMEOUT);
	if (res < 0) {
		DBG(3, "Failed to write a register (value 0x%02X, index "
		       "0x%02X, error %d)", value, index, res)
		return -1;
	}

	cam->reg[index] = value;

	return 0;
}


/* NOTE: reading some registers always returns 0 */
static int sn9c102_read_reg(struct sn9c102_device* cam, u16 index)
{
	struct usb_device* udev = cam->usbdev;
	u8* buff = cam->control_buffer;
	int res;

	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
	                      index, 0, buff, 1, SN9C102_CTRL_TIMEOUT);
	if (res < 0)
		DBG(3, "Failed to read a register (index 0x%02X, error %d)",
		    index, res)

	return (res >= 0) ? (int)(*buff) : -1;
}


int sn9c102_pread_reg(struct sn9c102_device* cam, u16 index)
{
	if (index > 0x1f)
		return -EINVAL;

	return cam->reg[index];
}


static int
sn9c102_i2c_wait(struct sn9c102_device* cam, struct sn9c102_sensor* sensor)
{
	int i, r;

	for (i = 1; i <= 5; i++) {
		r = sn9c102_read_reg(cam, 0x08);
		if (r < 0)
			return -EIO;
		if (r & 0x04)
			return 0;
		if (sensor->frequency & SN9C102_I2C_400KHZ)
			udelay(5*8);
		else
			udelay(16*8);
	}
	return -EBUSY;
}


static int
sn9c102_i2c_detect_read_error(struct sn9c102_device* cam, 
                              struct sn9c102_sensor* sensor)
{
	int r;
	r = sn9c102_read_reg(cam, 0x08);
	return (r < 0 || (r >= 0 && !(r & 0x08))) ? -EIO : 0;
}


static int
sn9c102_i2c_detect_write_error(struct sn9c102_device* cam, 
                               struct sn9c102_sensor* sensor)
{
	int r;
	r = sn9c102_read_reg(cam, 0x08);
	return (r < 0 || (r >= 0 && (r & 0x08))) ? -EIO : 0;
}


int 
sn9c102_i2c_try_read(struct sn9c102_device* cam,
                     struct sn9c102_sensor* sensor, u8 address)
{
	struct usb_device* udev = cam->usbdev;
	u8* data = cam->control_buffer;
	int err = 0, res;

	/* Write cycle - address */
	data[0] = ((sensor->interface == SN9C102_I2C_2WIRES) ? 0x80 : 0) |
	          ((sensor->frequency & SN9C102_I2C_400KHZ) ? 0x01 : 0) | 0x10;
	data[1] = sensor->slave_write_id;
	data[2] = address;
	data[7] = 0x10;
	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x08, 0x41,
	                      0x08, 0, data, 8, SN9C102_CTRL_TIMEOUT);
	if (res < 0)
		err += res;

	err += sn9c102_i2c_wait(cam, sensor);

	/* Read cycle - 1 byte */
	data[0] = ((sensor->interface == SN9C102_I2C_2WIRES) ? 0x80 : 0) |
	          ((sensor->frequency & SN9C102_I2C_400KHZ) ? 0x01 : 0) |
	          0x10 | 0x02;
	data[1] = sensor->slave_read_id;
	data[7] = 0x10;
	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x08, 0x41,
	                      0x08, 0, data, 8, SN9C102_CTRL_TIMEOUT);
	if (res < 0)
		err += res;

	err += sn9c102_i2c_wait(cam, sensor);

	/* The read byte will be placed in data[4] */
	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
	                      0x0a, 0, data, 5, SN9C102_CTRL_TIMEOUT);
	if (res < 0)
		err += res;

	err += sn9c102_i2c_detect_read_error(cam, sensor);

	if (err)
		DBG(3, "I2C read failed for %s image sensor", sensor->name)

	PDBGG("I2C read: address 0x%02X, value: 0x%02X", address, data[4])

	return err ? -1 : (int)data[4];
}


int 
sn9c102_i2c_try_raw_write(struct sn9c102_device* cam,
                          struct sn9c102_sensor* sensor, u8 n, u8 data0,
                          u8 data1, u8 data2, u8 data3, u8 data4, u8 data5)
{
	struct usb_device* udev = cam->usbdev;
	u8* data = cam->control_buffer;
	int err = 0, res;

	/* Write cycle. It usually is address + value */
	data[0] = ((sensor->interface == SN9C102_I2C_2WIRES) ? 0x80 : 0) |
	          ((sensor->frequency & SN9C102_I2C_400KHZ) ? 0x01 : 0)
	          | ((n - 1) << 4);
	data[1] = data0;
	data[2] = data1;
	data[3] = data2;
	data[4] = data3;
	data[5] = data4;
	data[6] = data5;
	data[7] = 0x10;
	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x08, 0x41,
	                      0x08, 0, data, 8, SN9C102_CTRL_TIMEOUT);
	if (res < 0)
		err += res;

	err += sn9c102_i2c_wait(cam, sensor);
	err += sn9c102_i2c_detect_write_error(cam, sensor);

	if (err)
		DBG(3, "I2C write failed for %s image sensor", sensor->name)

	PDBGG("I2C write: %u bytes, data0 = 0x%02X, data1 = 0x%02X, "
	      "data2 = 0x%02X, data3 = 0x%02X, data4 = 0x%02X, data5 = 0x%02X",
	      n, data0, data1, data2, data3, data4, data5)

	return err ? -1 : 0;
}


int 
sn9c102_i2c_try_write(struct sn9c102_device* cam,
                      struct sn9c102_sensor* sensor, u8 address, u8 value)
{
	return sn9c102_i2c_try_raw_write(cam, sensor, 3, 
	                                 sensor->slave_write_id, address,
	                                 value, 0, 0, 0);
}


int sn9c102_i2c_read(struct sn9c102_device* cam, u8 address)
{
	if (!cam->sensor)
		return -1;

	return sn9c102_i2c_try_read(cam, cam->sensor, address);
}


int sn9c102_i2c_write(struct sn9c102_device* cam, u8 address, u8 value)
{
	if (!cam->sensor)
		return -1;

	return sn9c102_i2c_try_write(cam, cam->sensor, address, value);
}

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

static void* sn9c102_find_sof_header(void* mem, size_t len)
{
	size_t soflen = sizeof(sn9c102_sof_header_t), i;
	u8 j, n = sizeof(sn9c102_sof_header) / soflen;

	for (i = 0; (len >= soflen) && (i <= len - soflen); i++)
		for (j = 0; j < n; j++)
			/* It's enough to compare 7 bytes */
			if (!memcmp(mem + i, sn9c102_sof_header[j], 7))
				/* Skips the header */
				return mem + i + soflen;

	return NULL;
}


static void* sn9c102_find_eof_header(void* mem, size_t len)
{
	size_t eoflen = sizeof(sn9c102_eof_header_t), i;
	unsigned j, n = sizeof(sn9c102_eof_header) / eoflen;

	for (i = 0; (len >= eoflen) && (i <= len - eoflen); i++)
		for (j = 0; j < n; j++)
			if (!memcmp(mem + i, sn9c102_eof_header[j], eoflen))
				return mem + i;

	return NULL;
}


static void sn9c102_urb_complete(struct urb *urb, struct pt_regs* regs)
{
	struct sn9c102_device* cam = urb->context;
	struct sn9c102_frame_t** f;
	unsigned long lock_flags;
	u8 i;
	int err = 0;

	if (urb->status == -ENOENT)
		return;

	f = &cam->frame_current;

	if (cam->stream == STREAM_INTERRUPT) {
		cam->stream = STREAM_OFF;
		if ((*f))
			(*f)->state = F_QUEUED;
		DBG(3, "Stream interrupted")
		wake_up_interruptible(&cam->wait_stream);
	}

	if ((cam->state & DEV_DISCONNECTED)||(cam->state & DEV_MISCONFIGURED))
		return;

	if (cam->stream == STREAM_OFF || list_empty(&cam->inqueue))
		goto resubmit_urb;

	if (!(*f))
		(*f) = list_entry(cam->inqueue.next, struct sn9c102_frame_t,
		                  frame);

	for (i = 0; i < urb->number_of_packets; i++) {
		unsigned int img, len, status;
		void *pos, *sof, *eof;

		len = urb->iso_frame_desc[i].actual_length;
		status = urb->iso_frame_desc[i].status;
		pos = urb->iso_frame_desc[i].offset + urb->transfer_buffer;

		if (status) {
			DBG(3, "Error in isochronous frame")
			(*f)->state = F_ERROR;
			continue;
		}

		PDBGG("Isochrnous frame: length %u, #%u i", len, i)

		/*
		   NOTE: It is probably correct to assume that SOF and EOF
		         headers do not occur between two consecutive packets,
		         but who knows..Whatever is the truth, this assumption
		         doesn't introduce bugs.
		*/

redo:
		sof = sn9c102_find_sof_header(pos, len);
		if (!sof) {
			eof = sn9c102_find_eof_header(pos, len);
			if ((*f)->state == F_GRABBING) {
end_of_frame:
				img = len;

				if (eof)
					img = (eof > pos) ? eof - pos - 1 : 0;

				if ((*f)->buf.bytesused+img>(*f)->buf.length) {
					u32 b = (*f)->buf.bytesused + img -
					        (*f)->buf.length;
					img = (*f)->buf.length - 
					      (*f)->buf.bytesused;
					DBG(3, "Expected EOF not found: "
					       "video frame cut")
					if (eof)
						DBG(3, "Exceeded limit: +%u "
						       "bytes", (unsigned)(b))
				}

				memcpy((*f)->bufmem + (*f)->buf.bytesused, pos,
				       img);

				if ((*f)->buf.bytesused == 0)
					do_gettimeofday(&(*f)->buf.timestamp);

				(*f)->buf.bytesused += img;

				if ((*f)->buf.bytesused == (*f)->buf.length) {
					u32 b = (*f)->buf.bytesused;
					(*f)->state = F_DONE;
					(*f)->buf.sequence= ++cam->frame_count;
					spin_lock_irqsave(&cam->queue_lock,
					                  lock_flags);
					list_move_tail(&(*f)->frame,
					               &cam->outqueue);
					if (!list_empty(&cam->inqueue))
						(*f) = list_entry(
						        cam->inqueue.next,
						        struct sn9c102_frame_t,
						        frame );
					else
						(*f) = NULL;
					spin_unlock_irqrestore(&cam->queue_lock
					                       , lock_flags);
					DBG(3, "Video frame captured: "
					       "%lu bytes", (unsigned long)(b))

					if (!(*f))
						goto resubmit_urb;

				} else if (eof) {
					(*f)->state = F_ERROR;
					DBG(3, "Not expected EOF after %lu "
					       "bytes of image data", 
					  (unsigned long)((*f)->buf.bytesused))
				}

				if (sof) /* (1) */
					goto start_of_frame;

			} else if (eof) {
				DBG(3, "EOF without SOF")
				continue;

			} else {
				PDBGG("Ignoring pointless isochronous frame")
				continue;
			}

		} else if ((*f)->state == F_QUEUED || (*f)->state == F_ERROR) {
start_of_frame:
			(*f)->state = F_GRABBING;
			(*f)->buf.bytesused = 0;
			len -= (sof - pos);
			pos = sof;
			DBG(3, "SOF detected: new video frame")
			if (len)
				goto redo;

		} else if ((*f)->state == F_GRABBING) {
			eof = sn9c102_find_eof_header(pos, len);
			if (eof && eof < sof)