cpia2_usb.c

V4l driver for DVB HD

/****************************************************************************
 *
 *  Filename: cpia2_usb.c
 *
 *  Copyright 2001, STMicrolectronics, Inc.
 *      Contact:  [e-mail]
 *
 *  Description:
 *     This is a USB driver for CPia2 based video cameras.
 *     The infrastructure of this driver is based on the cpia usb driver by
 *     Jochen Scharrlach and Johannes Erdfeldt.
 *
 *  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.
 *
 *  Stripped of 2.4 stuff ready for main kernel submit by
 *		Alan Cox <[e-mail]>
 ****************************************************************************/

#include <linux/kernel.h>
#include <linux/slab.h>
#include <linux/usb.h>

#include "cpia2.h"

static int frame_sizes[] = {
	0,	// USBIF_CMDONLY
	0, 	// USBIF_BULK
	128, 	// USBIF_ISO_1
	384, 	// USBIF_ISO_2
	640, 	// USBIF_ISO_3
	768, 	// USBIF_ISO_4
	896, 	// USBIF_ISO_5
	1023, 	// USBIF_ISO_6
};

#define FRAMES_PER_DESC    10
#define FRAME_SIZE_PER_DESC   frame_sizes[cam->cur_alt]

static void process_frame(struct camera_data *cam);
static void cpia2_usb_complete(struct urb *urb, struct pt_regs *);
static int cpia2_usb_probe(struct usb_interface *intf,
			   const struct usb_device_id *id);
static void cpia2_usb_disconnect(struct usb_interface *intf);

static void free_sbufs(struct camera_data *cam);
static void add_APPn(struct camera_data *cam);
static void add_COM(struct camera_data *cam);
static int submit_urbs(struct camera_data *cam);
static int set_alternate(struct camera_data *cam, unsigned int alt);
static int configure_transfer_mode(struct camera_data *cam, unsigned int alt);

static struct usb_device_id cpia2_id_table[] = {
	{USB_DEVICE(0x0553, 0x0100)},
	{USB_DEVICE(0x0553, 0x0140)},
	{USB_DEVICE(0x0553, 0x0151)},  /* STV0676 */
	{}			/* Terminating entry */
};
MODULE_DEVICE_TABLE(usb, cpia2_id_table);

static struct usb_driver cpia2_driver = {
	.name		= "cpia2",
	.probe		= cpia2_usb_probe,
	.disconnect	= cpia2_usb_disconnect,
	.id_table	= cpia2_id_table
};


/******************************************************************************
 *
 *  process_frame
 *
 *****************************************************************************/
static void process_frame(struct camera_data *cam)
{
	static int frame_count = 0;

	unsigned char *inbuff = cam->workbuff->data;

	DBG("Processing frame #%d, current:%d\n",
	    cam->workbuff->num, cam->curbuff->num);

	if(cam->workbuff->length > cam->workbuff->max_length)
		cam->workbuff->max_length = cam->workbuff->length;

	if ((inbuff[0] == 0xFF) && (inbuff[1] == 0xD8)) {
		frame_count++;
	} else {
		cam->workbuff->status = FRAME_ERROR;
		DBG("Start of frame not found\n");
		return;
	}

	/***
	 * Now the output buffer should have a JPEG image in it.
	 ***/
	if(!cam->first_image_seen) {
		/* Always skip the first image after streaming
		 * starts. It is almost certainly corrupt. */
		cam->first_image_seen = 1;
		cam->workbuff->status = FRAME_EMPTY;
		return;
	}
	if (cam->workbuff->length > 3) {
		if(cam->mmapped &&
		   cam->workbuff->length < cam->workbuff->max_length) {
			/* No junk in the buffers */
			memset(cam->workbuff->data+cam->workbuff->length,
			       0, cam->workbuff->max_length-
				  cam->workbuff->length);
		}
		cam->workbuff->max_length = cam->workbuff->length;
		cam->workbuff->status = FRAME_READY;

		if(!cam->mmapped && cam->num_frames > 2) {
			/* During normal reading, the most recent
			 * frame will be read.  If the current frame
			 * hasn't started reading yet, it will never
			 * be read, so mark it empty.  If the buffer is
			 * mmapped, or we have few buffers, we need to
			 * wait for the user to free the buffer.
			 *
			 * NOTE: This is not entirely foolproof with 3
			 * buffers, but it would take an EXTREMELY
			 * overloaded system to cause problems (possible
			 * image data corruption).  Basically, it would
			 * need to take more time to execute cpia2_read
			 * than it would for the camera to send
			 * cam->num_frames-2 frames before problems
			 * could occur.
			 */
			cam->curbuff->status = FRAME_EMPTY;
		}
		cam->curbuff = cam->workbuff;
		cam->workbuff = cam->workbuff->next;
		DBG("Changed buffers, work:%d, current:%d\n",
		    cam->workbuff->num, cam->curbuff->num);
		return;
	} else {
		DBG("Not enough data for an image.\n");
	}

	cam->workbuff->status = FRAME_ERROR;
	return;
}

/******************************************************************************
 *
 *  add_APPn
 *
 *  Adds a user specified APPn record
 *****************************************************************************/
static void add_APPn(struct camera_data *cam)
{
	if(cam->APP_len > 0) {
		cam->workbuff->data[cam->workbuff->length++] = 0xFF;
		cam->workbuff->data[cam->workbuff->length++] = 0xE0+cam->APPn;
		cam->workbuff->data[cam->workbuff->length++] = 0;
		cam->workbuff->data[cam->workbuff->length++] = cam->APP_len+2;
		memcpy(cam->workbuff->data+cam->workbuff->length,
		       cam->APP_data, cam->APP_len);
		cam->workbuff->length += cam->APP_len;
	}
}

/******************************************************************************
 *
 *  add_COM
 *
 *  Adds a user specified COM record
 *****************************************************************************/
static void add_COM(struct camera_data *cam)
{
	if(cam->COM_len > 0) {
		cam->workbuff->data[cam->workbuff->length++] = 0xFF;
		cam->workbuff->data[cam->workbuff->length++] = 0xFE;
		cam->workbuff->data[cam->workbuff->length++] = 0;
		cam->workbuff->data[cam->workbuff->length++] = cam->COM_len+2;
		memcpy(cam->workbuff->data+cam->workbuff->length,
		       cam->COM_data, cam->COM_len);
		cam->workbuff->length += cam->COM_len;
	}
}

/******************************************************************************
 *
 *  cpia2_usb_complete
 *
 *  callback when incoming packet is received
 *****************************************************************************/
static void cpia2_usb_complete(struct urb *urb, struct pt_regs *regs)
{
	int i;
	unsigned char *cdata;
	static int frame_ready = false;
	struct camera_data *cam = (struct camera_data *) urb->context;

	if (urb->status!=0) {
		if (!(urb->status == -ENOENT ||
		      urb->status == -ECONNRESET ||
		      urb->status == -ESHUTDOWN))
		{
			DBG("urb->status = %d!\n", urb->status);
		}
		DBG("Stopping streaming\n");
		return;
	}

	if (!cam->streaming || !cam->present || cam->open_count == 0) {
		LOG("Will now stop the streaming: streaming = %d, "
		    "present=%d, open_count=%d\n",
		    cam->streaming, cam->present, cam->open_count);
		return;
	}

	/***
	 * Packet collater
	 ***/
	//DBG("Collating %d packets\n", urb->number_of_packets);
	for (i = 0; i < urb->number_of_packets; i++) {
		u16 checksum, iso_checksum;
		int j;
		int n = urb->iso_frame_desc[i].actual_length;
		int st = urb->iso_frame_desc[i].status;

		if(cam->workbuff->status == FRAME_READY) {
			struct framebuf *ptr;
			/* Try to find an available buffer */
			DBG("workbuff full, searching\n");
			for (ptr = cam->workbuff->next;
			     ptr != cam->workbuff;
			     ptr = ptr->next)
			{
				if (ptr->status == FRAME_EMPTY) {
					ptr->status = FRAME_READING;
					ptr->length = 0;
					break;
				}
			}
			if (ptr == cam->workbuff)
				break; /* No READING or EMPTY buffers left */

			cam->workbuff = ptr;
		}

		if (cam->workbuff->status == FRAME_EMPTY ||
		    cam->workbuff->status == FRAME_ERROR) {
			cam->workbuff->status = FRAME_READING;
			cam->workbuff->length = 0;
		}

		//DBG("   Packet %d length = %d, status = %d\n", i, n, st);
		cdata = urb->transfer_buffer + urb->iso_frame_desc[i].offset;

		if (st) {
			LOG("cpia2 data error: [%d] len=%d, status = %d\n",
			    i, n, st);
			if(!ALLOW_CORRUPT)
				cam->workbuff->status = FRAME_ERROR;
			continue;
		}

		if(n<=2)
			continue;

		checksum = 0;
		for(j=0; j<n-2; ++j)
			checksum += cdata[j];
		iso_checksum = cdata[j] + cdata[j+1]*256;
		if(checksum != iso_checksum) {
			LOG("checksum mismatch: [%d] len=%d, calculated = %x, checksum = %x\n",
			    i, n, (int)checksum, (int)iso_checksum);
			if(!ALLOW_CORRUPT) {
				cam->workbuff->status = FRAME_ERROR;
				continue;
			}
		}
		n -= 2;

		if(cam->workbuff->status != FRAME_READING) {
			if((0xFF == cdata[0] && 0xD8 == cdata[1]) ||
			   (0xD8 == cdata[0] && 0xFF == cdata[1] &&
			    0 != cdata[2])) {
				/* frame is skipped, but increment total
				 * frame count anyway */
				cam->frame_count++;
			}
			DBG("workbuff not reading, status=%d\n",
			    cam->workbuff->status);
			continue;
		}

		if (cam->frame_size < cam->workbuff->length + n) {
			ERR("buffer overflow! length: %d, n: %d\n",
			    cam->workbuff->length, n);
			cam->workbuff->status = FRAME_ERROR;
			if(cam->workbuff->length > cam->workbuff->max_length)
				cam->workbuff->max_length =
					cam->workbuff->length;
			continue;
		}

		if (cam->workbuff->length == 0) {
			int data_offset;
			if ((0xD8 == cdata[0]) && (0xFF == cdata[1])) {
				data_offset = 1;
			} else if((0xFF == cdata[0]) && (0xD8 == cdata[1])
				  && (0xFF == cdata[2])) {
				data_offset = 2;
			} else {
				DBG("Ignoring packet, not beginning!\n");
				continue;
			}
			DBG("Start of frame pattern found\n");
			do_gettimeofday(&cam->workbuff->timestamp);
			cam->workbuff->seq = cam->frame_count++;
			cam->workbuff->data[0] = 0xFF;
			cam->workbuff->data[1] = 0xD8;
			cam->workbuff->length = 2;
			add_APPn(cam);
			add_COM(cam);
			memcpy(cam->workbuff->data+cam->workbuff->length,
			       cdata+data_offset, n-data_offset);
			cam->workbuff->length += n-data_offset;
		} else if (cam->workbuff->length > 0) {
			memcpy(cam->workbuff->data + cam->workbuff->length,
			       cdata, n);
			cam->workbuff->length += n;
		}

		if ((cam->workbuff->length >= 3) &&
		    (cam->workbuff->data[cam->workbuff->length - 3] == 0xFF) &&
		    (cam->workbuff->data[cam->workbuff->length - 2] == 0xD9) &&
		    (cam->workbuff->data[cam->workbuff->length - 1] == 0xFF)) {
			frame_ready = true;
			cam->workbuff->data[cam->workbuff->length - 1] = 0;
			cam->workbuff->length -= 1;
		} else if ((cam->workbuff->length >= 2) &&
		   (cam->workbuff->data[cam->workbuff->length - 2] == 0xFF) &&
		   (cam->workbuff->data[cam->workbuff->length - 1] == 0xD9)) {
			frame_ready = true;
		}

		if (frame_ready) {
			DBG("Workbuff image size = %d\n",cam->workbuff->length);
			process_frame(cam);

			frame_ready = false;

			if (waitqueue_active(&cam->wq_stream))
				wake_up_interruptible(&cam->wq_stream);
		}
	}

	if(cam->streaming) {
		/* resubmit */
		urb->dev = cam->dev;
		if ((i = usb_submit_urb(urb, GFP_ATOMIC)) != 0)
			ERR("%s: usb_submit_urb ret %d!\n", __func__, i);
	}
}

/******************************************************************************
 *
 * configure_transfer_mode
 *
 *****************************************************************************/
static int configure_transfer_mode(struct camera_data *cam, unsigned int alt)
{
	static unsigned char iso_regs[8][4] = {
		{0x00, 0x00, 0x00, 0x00},
		{0x00, 0x00, 0x00, 0x00},
		{0xB9, 0x00, 0x00, 0x7E},
		{0xB9, 0x00, 0x01, 0x7E},
		{0xB9, 0x00, 0x02, 0x7E},
		{0xB9, 0x00, 0x02, 0xFE},
		{0xB9, 0x00, 0x03, 0x7E},
		{0xB9, 0x00, 0x03, 0xFD}
	};
	struct cpia2_command cmd;
	unsigned char reg;

	if(!cam->present)
		return -ENODEV;

	/***
	 * Write the isoc registers according to the alternate selected
	 ***/
	cmd.direction = TRANSFER_WRITE;
	cmd.buffer.block_data[0] = iso_regs[alt][0];
	cmd.buffer.block_data[1] = iso_regs[alt][1];
	cmd.buffer.block_data[2] = iso_regs[alt][2];
	cmd.buffer.block_data[3] = iso_regs[alt][3];
	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
	cmd.start = CPIA2_VC_USB_ISOLIM;
	cmd.reg_count = 4;
	cpia2_send_command(cam, &cmd);

	/***
	 * Enable relevant streams before starting polling.
	 * First read USB Stream Config Register.
	 ***/
	cmd.direction = TRANSFER_READ;
	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
	cmd.start = CPIA2_VC_USB_STRM;
	cmd.reg_count = 1;
	cpia2_send_command(cam, &cmd);
	reg = cmd.buffer.block_data[0];

	/* Clear iso, bulk, and int */
	reg &= ~(CPIA2_VC_USB_STRM_BLK_ENABLE |
		 CPIA2_VC_USB_STRM_ISO_ENABLE |
		 CPIA2_VC_USB_STRM_INT_ENABLE);

	if (alt == USBIF_BULK) {
		DBG("Enabling bulk xfer\n");
		reg |= CPIA2_VC_USB_STRM_BLK_ENABLE;	/* Enable Bulk */
		cam->xfer_mode = XFER_BULK;
	} else if (alt >= USBIF_ISO_1) {
		DBG("Enabling ISOC xfer\n");
		reg |= CPIA2_VC_USB_STRM_ISO_ENABLE;
		cam->xfer_mode = XFER_ISOC;
	}

	cmd.buffer.block_data[0] = reg;
	cmd.direction = TRANSFER_WRITE;
	cmd.start = CPIA2_VC_USB_STRM;
	cmd.reg_count = 1;
	cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
	cpia2_send_command(cam, &cmd);

	return 0;
}

/******************************************************************************
 *
 * cpia2_usb_change_streaming_alternate
 *
 *****************************************************************************/
int cpia2_usb_change_streaming_alternate(struct camera_data *cam,
					 unsigned int alt)
{
	int ret = 0;