r5u870v2/webcam.c

#include "shared.h"
#include "webcam.h"
#include "wdm.h"
#include "urb.h"




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





/****************************
*			UTILS			*
****************************/

int r5u870_control_msg( struct driverInstance* this, int write, int class, u8 request, u16 value, u16 index, void* data, u16 size ){
	
	char* dbuf = NULL;
	int res;
	
	if( size ){
		dbuf = kmalloc( size, GFP_KERNEL );
		if ( !dbuf )return -ENOMEM;
		if ( write ) memcpy( dbuf, data, size );
	}
	
	//int usb_control_msg(struct usb_device * dev, unsigned int pipe, __u8 request,__u8 requesttype, __u16 value, __u16 index, void * data, __u16 size, int timeout );
	res = usb_control_msg( 
		this->usbDevice,
		write  ? usb_sndctrlpipe( this->usbDevice, 0 ) : usb_rcvctrlpipe( this->usbDevice, 0 ),
		request,
		( write ? USB_DIR_OUT : USB_DIR_IN ) | ( class ? (USB_TYPE_CLASS | USB_RECIP_INTERFACE) : (USB_TYPE_VENDOR | USB_RECIP_DEVICE) ),
		value, 
		index, 
		dbuf, 
		size, 
		1000 
	);
	
	if( dbuf ) {
		if( !write ) memcpy( data, dbuf, size );
		kfree( dbuf );
	}
	return res;
}





static int r5u870_set_gen_reg( struct driverInstance* this,  int cmd, int reg, int val ){
	
	int res = r5u870_control_msg( this, 1, 0, cmd, val, reg, NULL, 0 );
	if( res < 0 ){
		printk( KERN_ERR "[%s] %s %04x/%04x/%04x failed: %d\n", KBUILD_MODNAME, __FUNCTION__, cmd, reg, val, res);
		return res;
	}
	return 0;
}

static int r5u870_set_reg_wdm( struct driverInstance* this, int reg, int val ){
	return r5u870_set_gen_reg( this, 0xc2, reg, val);
}



/****************************
*		MICROCODE			*
****************************/


static int r5u870_microcode_get_state( struct driverInstance* this ){
	
	char buf[1];
	int res;
	
	res = r5u870_control_msg( this, 0, 0, 0xa4, 0, 0, buf, 1 );
	if ((res != 1) || ((buf[0] != 0) && (buf[0] != 1))) {
		printk( KERN_ERR "[%s] command 0xa4 failed: %d", KBUILD_MODNAME, res);
		return res < 0 ? res : -EIO;
	}
	
	printk( KERN_NOTICE "[%s] camera reports %s microcode state\n", KBUILD_MODNAME, buf[0] ? "positive" : "negative" );
	
	return (buf[0] == 0) ? -ENOENT : 0;
}

static int r5u870_microcode_get_ver( struct driverInstance* this, int *verp ){
	
	char buf[2];
	int res;
	
	res = r5u870_control_msg( this, 0, 0, 0xc3, 0, 0x0e, buf, 2 );
	if (res != 2) {
		printk( KERN_ERR "[%s] command 0xa3 failed: %d\n", KBUILD_MODNAME, res );
		return res < 0 ? res : -EIO;
	}
	
	res = le16_to_cpup((__le16 *) buf);
	printk( KERN_NOTICE "[%s] camera reports version %04x\n", KBUILD_MODNAME, res );
	*verp = res;
	return 0;
}

static int r5u870_microcode_reset( struct driverInstance* this ){
	
	int res;
	printk( KERN_NOTICE "[%s] sending microcode reset command\n", KBUILD_MODNAME );
	msleep( 100 );// The Windows driver waits 1sec
	res = r5u870_set_gen_reg( this, 0xa6, 0, 0 );
	if ( !res ) msleep(200);
	return res;
}


static int r5u870_microcode_upload( struct driverInstance* this ){
	
	const struct firmware *fws;
	char *pgbuf;
	const u8 *dptr;
	int tolerance = 3;
	int i, rem, addr, len, res = 0;
	
	pgbuf = (char *) kmalloc(64, GFP_KERNEL);
	if( !pgbuf ) return -ENOMEM;
	
	printk( KERN_NOTICE "[%s] loading microcode file \"%s\"\n", KBUILD_MODNAME, this->webcamModel->ucodeFile );
	
	res = request_firmware( &fws, this->webcamModel->ucodeFile, &this->usbDevice->dev );
	
	if( res ) {
		printk( KERN_ERR "[%s] Microcode file \"%s\" is missing\n", KBUILD_MODNAME, this->webcamModel->ucodeFile );
		kfree( pgbuf );
		return res;
	}
	
	i = 0;
	dptr = fws->data;
	rem = fws->size;
	while( rem ){
		if( rem < 3 ){
			printk( KERN_ERR "[%s] Microcode file msg %d is incomplete", KBUILD_MODNAME, i );
			res = -EINVAL;
			break;
		}
		
		len = dptr[0];
		addr = dptr[1] | (dptr[2] << 8);
		dptr += 3;
		rem -= 3;
		
		if ((rem < len) || (len > 64)) {
			printk( KERN_ERR "[%s] Microcode file msg %d has bad length %d\n", KBUILD_MODNAME, i, len );
			res = -EINVAL;
			break;
		}
		
		/*
		 * The USB stack has issues with the initseq data if
		 * initseq points into the vmalloc arena.  This is
		 * the case for microcode embedded in a module, or
		 * data loaded by request_firmware().
		 *
		 * As a workaround, we memcpy() into a kmalloc page.
		 */
		memcpy(pgbuf, dptr, len);
		dptr += len;
		rem -= len;
		
	retry:
		// TODO: Maybe make this use r5u870_control_msg or similar? 
		res = usb_control_msg(
			this->usbDevice,
			usb_sndctrlpipe( this->usbDevice, 0 ),
			0xa0,
			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
			addr,
			0,
			pgbuf,
			len,
			1000
		);
		
		if (res < 0) {
			if (tolerance--) goto retry;
			printk( KERN_ERR "[%s] command a0[%d] failed: %d\n", KBUILD_MODNAME, i, res);
			break;
		}
		if (res != len) {
			printk( KERN_ERR "[%s] command a0[%d] failed: %d (exp %d)\n", KBUILD_MODNAME, i, res, len);
			res = -EIO;
			break;
		}
		
		i++;
	}
	
	release_firmware( fws );
	kfree( pgbuf );
	return res;
}


static int r5u870_microcode_enable( struct driverInstance* this ){
	
	char buf[1];
	int res;
	
	buf[0] = 1;
	res = r5u870_control_msg( this, 1, 0, 0xa1, 0, 0, buf, 1 );
	if( res != 1 ) {
		printk( KERN_ERR "[%s] command 0xa1 failed: %d\n", KBUILD_MODNAME, res );
		return res < 0 ? res : -EIO;
	}
	
	return 0;
}

static int webcamMicrocode( struct driverInstance* this ){//r5u870_dev_init
	
	int mcver;
	int res;
	
	if ( !this->webcamModel->ucodeFile ) return 0;
	
	res = r5u870_microcode_get_state( this );
	if ( res && (res != -ENOENT) ) return res;
	
	if( !res ){
		res = r5u870_microcode_get_ver( this, &mcver );
		if ( res ) return res;
	
		if ( mcver != this->webcamModel->ucodeVersion ){
			res = r5u870_microcode_reset( this );
			if( res ) return res;
			res = -ENOENT;
		}
	}
	
	if( res == -ENOENT ){
		res = r5u870_microcode_upload( this );
		if( res < 0 ) return res;
		
		res = r5u870_microcode_enable( this );
		if( res ) return res;
		
		res = r5u870_microcode_get_ver( this, &mcver );
		if( res ) return res;
	}
	
	if( mcver != this->webcamModel->ucodeVersion ){
		printk( KERN_ERR "[%s] Unexpected microcode version (exp:%04x got:%04x)\n", KBUILD_MODNAME, this->webcamModel->ucodeVersion, mcver);
	}
	
	//Halt capture in case it's running (broken driver?)
	res = this->vh_cap_stop( this );
	if (res < 0) return res;
	
	return 0;
}

/****************************
*			FORMAT			*
****************************/

int webcamTryFormat( struct driverInstance* this, struct v4l2_pix_format* requestedFormat, char setFlag ){
	
	const struct r5u870_pixelformat* format;
	const struct r5u870_resolution* resolutionTable;
	const struct r5u870_resolution* resolution;
	int i;
	
	format = NULL;
	for ( i = 0; i < this->webcamFormatsCount; i++ ) {
		if ( this->webcamFormats[i].pixelformat == requestedFormat->pixelformat ) {
			format = &this->webcamFormats[i];
			break;
		}
	}
	if ( format == NULL ) return -EINVAL;
	
	resolutionTable = format->resolutionTable;
	if ( !resolutionTable || !resolutionTable[0].width ){
		printk( KERN_ERR "[%s] invalid resolution table\n", KBUILD_MODNAME );
		return -EINVAL;
	}
	
	//Find the most acceptable resolution
	resolution = NULL;
	for( i = 0; resolutionTable[i].width > 0; i++ ){
		if ( !resolution ){
			resolution = &resolutionTable[i];
		}else if( resolution->width > requestedFormat->width ){
			if( resolutionTable[i].width < resolution->width ) resolution = &resolutionTable[i];
		}else if( resolution->height > requestedFormat->height ){
			if( resolutionTable[i].width <= requestedFormat->width && resolutionTable[i].height < resolution->height ) resolution = &resolutionTable[i];
		}else if( resolutionTable[i].width <= requestedFormat->width && resolutionTable[i].height <= requestedFormat->height &&
			( resolutionTable[i].width > resolution->width || ( resolutionTable[i].width == resolution->width && resolutionTable[i].height > resolution->height ) ) ){
			resolution = &resolutionTable[i];
		}
	}
	
	if( (requestedFormat->width > 1) && (requestedFormat->height > 1) ){
		if ( (resolution->width > requestedFormat->width) || (resolution->height > requestedFormat->height) ) {
			printk( KERN_WARNING "[%s] Bad size request. Returning -EINVAL.\n", KBUILD_MODNAME );
			return -EINVAL;
		}
	}
	
	printk( KERN_NOTICE "[%s] Requested %s %dx%d. Using %dx%d.\n", 
		KBUILD_MODNAME, 
		format->description, 
		requestedFormat->width,
		requestedFormat->height, 
		resolution->width, 
		resolution->height
	);
	
	memset( requestedFormat, 0, sizeof(*requestedFormat) );
	requestedFormat->width = resolution->width;
	requestedFormat->height = resolution->height;
	requestedFormat->pixelformat = format->pixelformat;
	requestedFormat->bytesperline = requestedFormat->width * 2;
	requestedFormat->sizeimage = requestedFormat->width * requestedFormat->height * 2;
	requestedFormat->field = V4L2_FIELD_INTERLACED;
	requestedFormat->colorspace = V4L2_COLORSPACE_SMPTE170M;
	
	if( !setFlag ) return 0;
	
	if( (this->webcamCurrentPixelformat.width != requestedFormat->width) ||
		(this->webcamCurrentPixelformat.height != requestedFormat->height) ||
		(this->webcamCurrentPixelformat.pixelformat != requestedFormat->pixelformat) ||
		(this->webcamCurrentPixelformat.sizeimage != requestedFormat->sizeimage) ){
		this->vh_configured = 0;
	}
	
	this->webcamCurrentPixelformat = *requestedFormat;
	this->webcamCurrentFormat = format;
	this->webcamCurrentResolution = resolution;
	
	return 0;
}

/****************************
*			WDM				*
****************************/

//Set the frame size and data format.
//Do not call this function with the isochronous stream active.
//r5u870_set_fmt_wdm
static int webcamApplyFormat_wdm( struct driverInstance* this ){
	
	int res;
	
	msleep(1);
	res = r5u870_set_gen_reg( this, 0xc5, 2, this->webcamCurrentFormat->formatId );
	if( res ) return res;
	msleep(1);
	
	res = r5u870_set_gen_reg( this, 0xc5, 0, this->webcamCurrentResolution->width );
	if( res ) return res;
	msleep(1);
	
	res = r5u870_set_gen_reg( this, 0xc5, 1, this->webcamCurrentResolution->height );
	if( res ) return res;
	msleep(5);
	
	return 0;
}

//Turn frame grabbing on or off (WDM).
//This will also turn on or off the LED.
static inline int webcamSetCaptureState_wdm( struct driverInstance* this, int val ){
	return r5u870_set_gen_reg( this, 0xc4, val, 0 );
}

static int r5u870_cap_stop_wdm( struct driverInstance* this ){
	return webcamSetCaptureState_wdm( this, 0 );
}

static int webcamSetControlCallback_wdm( struct v4l2_ctrl* control ){
	
	struct driverInstance* this = container_of( control->handler, struct driverInstance, webcamControlHandler );
	struct webcam_control* webcamControl;
	int err = 0;
	
	if( atomic_read( &this->webcamControlRegisterIsEnabled ) ){
		if( !control->priv ){
			printk( KERN_ERR "[%s] %s err: control->priv == NULL\n", KBUILD_MODNAME, __FUNCTION__ );
			return -EINVAL;
		}
		webcamControl = control->priv;
		err = r5u870_set_reg_wdm( this, webcamControl->reg, control->val );
		
		if( err )printk( KERN_ERR "[%s](%s) id: %u, val: %i, err: %i\n", KBUILD_MODNAME, __FUNCTION__, control->id, control->val, err );
	}else err = -EBUSY;
	
	return err;
}

static int webcamAddControls_wdm( struct driverInstance* this ){
	
	int err = 0;
	int i;
	
	//TODO: Find a beautiful way to calculate the number of controls
	v4l2_ctrl_handler_init( &this->webcamControlHandler, 19 );
	
	for( i = 0; this->webcamModel->controls_wdm[i] != R5U870_WDM_CTRL_LAST; i++ ){
		v4l2_ctrl_new_custom(
			&this->webcamControlHandler,
			&r5u870_wdm_ctrls[ this->webcamModel->controls_wdm[i] ].control, 
			&r5u870_wdm_ctrls[ this->webcamModel->controls_wdm[i] ]
		);
		if( this->webcamControlHandler.error )break;
	}
	
	if( this->webcamControlHandler.error ){
		err = this->webcamControlHandler.error;
		v4l2_ctrl_handler_free( &this->webcamControlHandler );
	}else this->v4l2Device.ctrl_handler = &this->webcamControlHandler;
	
	return err;
}




/*
 * r5u870_decide_pkt_wdm
 *
 * Based on the size of an isochronous data packet, this function
 * decides whether to copy the packet into the frame buffer and possibly
 * complete the frame, or to discard both the packet and the frame.
 *
 * Returns:
 * 	 0			Frame is done
 *	-EAGAIN		Append packet to frame, frame is not done
 *	-EPIPE		Discard frame and packet
 *	-EIO		The device is nonresponsive, abort
 */
static int r5u870_decide_pkt_wdm( struct driverInstance* this, int pktstatus, int pktlen, const u8* pktdata, int* start ){
	
	int ret = -EAGAIN;
	*start = 0;
	
	if( pktstatus ){//Abort current frame
		if( printk_ratelimit() )printk( KERN_NOTICE "[%s] frame abort: packet status %d\n", KBUILD_MODNAME, pktstatus );
		this->vh_frame_accum = -1;
		this->vh_emptypkts = 0;
		ret = -EPIPE;
		
	}else if( !pktlen ){
		if( ++this->vh_emptypkts == R5U870_EMPTYPKT_FRAME_DELIM ){
			if(this->vh_frame_accum == -1) ret = -EPIPE;//Frame was previously aborted
			else if( this->vh_frame_accum == this->webcamCurrentPixelformat.sizeimage ) ret = 0;//Complete frame
			else{//Not enough data in frame sequence
				if( printk_ratelimit() ) printk( KERN_NOTICE "[%s] frame abort: Frame seq too short (exp:%d got:%d)\n", KBUILD_MODNAME, this->webcamCurrentPixelformat.sizeimage, this->vh_frame_accum );
				ret = -EPIPE;
			}
			
			if( !this->vh_firstframe ){//Always reject the first frame
				this->vh_firstframe = 1;
				this->vh_frame_accum = -1;
				ret = -EPIPE;
			}else this->vh_frame_accum = 0;
			
		}else if( this->vh_emptypkts >= R5U870_EMPTYPKT_GIVE_UP ){
			if( printk_ratelimit() ) printk( KERN_NOTICE "[%s] %d empty packets, giving up\n", KBUILD_MODNAME, this->vh_emptypkts );
			ret = -EIO;
		}
		
	}else{
		this->vh_emptypkts = 0;
		if( this->vh_frame_accum == -1 ) ret = -EPIPE;//Frame was previously aborted
		else if( (this->vh_frame_accum + pktlen) <= this->webcamCurrentPixelformat.sizeimage ) this->vh_frame_accum += pktlen;//Append this data
		else{//Oversized frame, abort
			if( printk_ratelimit() ) printk( KERN_NOTICE "[%s] frame abort: Frame seq too long\n", KBUILD_MODNAME );
			this->vh_frame_accum = -1;
			ret = -EPIPE;
		}
	}
	
	return ret;
}


/****************************
*			CAPTURE			*
****************************/

//r5u870_config_iso_ep
static int webcamConfigInterface( struct driverInstance* this ){
	
	int res;
	
	if( !this->vh_configured ){
		this->vh_ctrl_sync = 0;
		
		res = usbcam_choose_altsetting( this );
		if( res ){
			printk( KERN_ERR "[%s] need %d B/s, no altsetting provides\n", KBUILD_MODNAME, this->webcamCurrentResolution->requiredBandwidth );
			return res;
		}
		
		printk( KERN_NOTICE "[%s] using altsetting %d\n", KBUILD_MODNAME, this->usbCurrentAltsetting );
	}
	
	res = usb_set_interface( this->usbDevice, this->urbstreamInterfaceNumber, this->usbCurrentAltsetting );
	if( res ){
		printk( KERN_ERR "[%s] could not set altsetting: %d\n", KBUILD_MODNAME, res );
		return res;
	}
	
	return 0;
}

int webcamCaptureStart( struct driverInstance* this ){
	
	int res;
	
	if( atomic_read( &this->webcamIsCaptuing ) )return 0;
	
	if( !this->webcamModel->isUvc ){
		res = webcamConfigInterface( this );
		if( res )return res;
	}
	
	atomic_set( &this->webcamControlRegisterIsEnabled, 1 );
	
	if( !this->vh_configured ){
		v4l2_ctrl_handler_setup( &this->webcamControlHandler );
		res = this->webcamApplyFormat( this );
		if( res ){
			printk( KERN_ERR "[%s] could not configure capture: %d\n", KBUILD_MODNAME, res );
			goto out_set_idle;
		}
		
		if( this->webcamModel->rm_no_ctrl_reload ) this->vh_ctrl_sync = 1;
	}
	
	if( this->webcamModel->isUvc ){
		res = webcamConfigInterface( this );
		if( res ) goto out_set_idle;
	}
	
	this->vh_configured = 1;
	
	res = usbcam_urbstream_config_iso( this );
	if( res < 0 ) {
		printk( KERN_ERR "[%s] urbstream init failed: %d\n", KBUILD_MODNAME, res );
		goto out_set_idle;
	}
	
	atomic_set( &this->webcamIsCaptuing, 1 );
	
	if( !this->webcamModel->isUvc ){
		res = webcamSetCaptureState_wdm( this, 1 );
		if( res )goto out_cleanup_urbstream;
	}
	
	if( !this->vh_ctrl_sync ){
		//Reload the control values after changing res/format
		v4l2_ctrl_handler_setup( &this->webcamControlHandler );
		this->vh_ctrl_sync = 1;
	}
	
	printk( KERN_NOTICE "[%s] starting capture\n", KBUILD_MODNAME );
	
	this->vh_firstframe = 0;
	this->vh_frame_accum = -1;
	this->vh_framebuf_offset = 0;
	this->vh_emptypkts = R5U870_EMPTYPKT_FRAME_DELIM - 1;
	
	res = usbcam_urbstream_start( this );
	if(res) goto out_stop_capture;
	
	return 0;
	
out_stop_capture:
	(void) this->vh_cap_stop( this );
out_cleanup_urbstream:
	usbcam_urbstream_cleanup( this );
out_set_idle:
	usb_set_interface( this->usbDevice, this->urbstreamInterfaceNumber, 0 );
	atomic_set( &this->webcamControlRegisterIsEnabled, 0 );
	atomic_set( &this->webcamIsCaptuing, 0 );
	return res;
}

void webcamCaptureStop( struct driverInstance* this ){//r5u870_do_stop
	
	struct videobuffer_capsule* cursor;
	struct videobuffer_capsule* tmp;
	unsigned long flags = 0;
	
	if( atomic_read( &this->webcamIsCaptuing ) ){
		atomic_set( &this->webcamIsCaptuing, 0 );
		atomic_set( &this->webcamControlRegisterIsEnabled, 0 );
		
		usbcam_urbstream_stop( this );
		
		spin_lock_irqsave( &this->buflistSpinlock, flags );
		atomic_set( &this->buflistIsDisabled, 1 );
		list_for_each_entry_safe( cursor, tmp, &this->buflist, listItem ){
			list_del( &cursor->listItem );
			vb2_buffer_done( &cursor->videobuffer, VB2_BUF_STATE_ERROR );
		}
		spin_unlock_irqrestore( &this->buflistSpinlock, flags );
		
		if( !atomic_read( &this->usbDeviceIsDisconnected ) ){
			usb_set_interface( this->usbDevice, this->urbstreamInterfaceNumber, 0);
			this->vh_cap_stop( this );
		}
		
		usbcam_urbstream_cleanup( this );
		
		printk( KERN_NOTICE "[%s] Webcam capture stopped\n", KBUILD_MODNAME );
	}
}

/****************************
*	PACKET PROCESSING		*
****************************/

static inline struct videobuffer_capsule* usbcam_capture_curframe( struct driverInstance* this ){
	return list_empty( &this->buflist ) ? NULL : list_entry( this->buflist.next, struct videobuffer_capsule, listItem );
}

static void usbcam_curframe_complete( struct driverInstance* this, enum vb2_buffer_state bufstate ){
	
	struct videobuffer_capsule* curframe;
	unsigned long flags = 0;
	
	spin_lock_irqsave( &this->buflistSpinlock, flags );
	curframe = usbcam_capture_curframe( this );
	if( !curframe ){
		spin_unlock_irqrestore( &this->buflistSpinlock, flags );
		if( printk_ratelimit() ) printk( KERN_WARNING "[%s] No videobuffers in queue.\n", KBUILD_MODNAME );
		return;
	}
	list_del( &curframe->listItem );
	spin_unlock_irqrestore( &this->buflistSpinlock, flags );
	
	curframe->videobuffer.timestamp = ktime_get_ns();
	vb2_buffer_done( &curframe->videobuffer, bufstate );
}

//#define EAGAIN          11
//#define EPIPE           32 
//#define EPROTO          71
//#define EOVERFLOW       75

//static void r5u870_iso_packet_done(struct usbcam_dev *udp, struct usbcam_urbstream *usp,  const uint8_t *pktdata, int pktlen, int pktstatus) 
void r5u870_iso_packet_done( struct driverInstance* this, const uint8_t *pktdata, int pktlen, int pktstatus ) {
	
	struct videobuffer_capsule* currentVideobuffer;
	int res, start = 0;
	unsigned long flags = 0;
	
	res = this->vh_decide_pkt( this, pktstatus, pktlen, (u8 *) pktdata, &start );
	
	switch( res ) {
		case -EPIPE:
			if( this->vh_framebuf_offset ){
				this->vh_framebuf_offset = 0;
				usbcam_curframe_complete( this, VB2_BUF_STATE_ERROR );
			}
			break;
			
		case 0:
		case -EAGAIN:
			if( pktlen ) {
				spin_lock_irqsave( &this->buflistSpinlock, flags );
				currentVideobuffer = usbcam_capture_curframe( this );
				if( !currentVideobuffer ){
					spin_unlock_irqrestore( &this->buflistSpinlock, flags );
					
					//Some programs get stupid when starting the capture and do not have time 
					//to queue videobuffers, so we just display a warning and continue working.
					if( printk_ratelimit() ) printk( KERN_WARNING "[%s] Running out of videobuffers.\n", KBUILD_MODNAME );
					//webcamCaptureStop( this );
					break;
				}
				
				BUG_ON( pktlen - start + this->vh_framebuf_offset > vb2_plane_size( &currentVideobuffer->videobuffer, 0 ) );
				
				//This is our one and only memcpy. It's kind of hard to get around doing this, as we
				//cannot predict into which isochronous packets the camera will choose to return data.
				memcpy( vb2_plane_vaddr( &currentVideobuffer->videobuffer, 0 ) + this->vh_framebuf_offset, pktdata + start, pktlen - start );
				this->vh_framebuf_offset += (pktlen - start);
				//printk( KERN_NOTICE "(%s) offset now: %d\n", __FUNCTION__, this->vh_framebuf_offset );
				spin_unlock_irqrestore( &this->buflistSpinlock, flags );
			}
			
			if( !res ) {
				this->vh_framebuf_offset = 0;
				usbcam_curframe_complete( this, VB2_BUF_STATE_DONE );
			}
			break;
			
		case -EIO:
			webcamCaptureStop( this );
			break;
			
		default:
			BUG();
	}
}


/****************************
*			INIT			*
****************************/


void webcamRelease( struct driverInstance* this ){//r5u870_usbcam_release
	
	struct claimed_interface* interfaceTemp;
	struct claimed_interface* interfaceCursor;
	
	webcamCaptureStop( this );
	v4l2_ctrl_handler_free( &this->webcamControlHandler );
	
	list_for_each_entry_safe( interfaceCursor, interfaceTemp, &this->claimedInterfacesList, listItem ){
		usb_set_intfdata( interfaceCursor->interface, NULL );
		usb_put_intf( interfaceCursor->interface );
		usb_driver_release_interface( this->usbDeviceDriver, interfaceCursor->interface );
		list_del( &interfaceCursor->listItem );
	}
}


/*
 * Someone clever at HP decided to use 05ca:1870 for two distinct devices.
 * The Pavilion dv1xxx machines all seem to have the less common of the
 * two.  There is no known, working method to distinguish the devices
 * using USB commands only.  We resort to reading the model number out
 * of DMI.
 */

static int dv1000 = 2;
module_param( dv1000, int, S_IRUGO | S_IWUSR );
MODULE_PARM_DESC( dv1000, "HP dv1000 detect mode (0=no,1=yes,2=DMI)" );

static int r5u870_check_hp_dv1000( void ){
	const char *prod_name;
	if ( !dv1000 ) return 0;
	if ( dv1000 == 1 ) return 1;
	prod_name = dmi_get_system_info( DMI_PRODUCT_NAME );
	if ( !prod_name ) printk( KERN_INFO "r5u870: No DMI model found\n" );
	else {
		printk( KERN_INFO "r5u870: Found DMI model: \"%s\"\n",  prod_name );
		if ( !strncmp( prod_name, "HP Pavilion dv1000", 18 ) && !isdigit( prod_name[18] ) ) return 1;
	}
	return 0;
}


static const struct r5u870_model* r5u870_find_model( int driver_info ){
	if ( driver_info == R5U870_DI_HP_PAVWC_WDM ) {
		if ( r5u870_check_hp_dv1000() ) driver_info = R5U870_DI_HP_WEBCAM1K;
	}
	if ( ( driver_info <= R5U870_DI_INVALID ) || ( driver_info >= ARRAY_SIZE(r5u870_models) ) ) return NULL;
	if ( !r5u870_models[driver_info].modelName ) return NULL;
	return &r5u870_models[driver_info];
}


int webcamInit( struct driverInstance* this, const struct usb_device_id* devid ){
	
	int res = 0;
	
	this->vh_ctrl_ifnum = -1;
	this->urbstreamInterfaceNumber = -1;
	this->urbstreamMinPacketSize = -1;
	
	this->webcamModel = r5u870_find_model( devid->driver_info );
	
	if( !this->webcamModel ){
		printk( KERN_ERR "[%s] no suitable model descriptor for %04x:%04x\n", 
		KBUILD_MODNAME, le16_to_cpu( this->usbDevice->descriptor.idVendor ), le16_to_cpu( this->usbDevice->descriptor.idProduct ) );
		return -ENODEV;
	}
	
	this->webcamFormats = this->webcamModel->pixelformats;
	this->webcamFormatsCount = this->webcamModel->pixelformatsCount;
	
	if( this->webcamModel->isUvc ) {
		this->webcamApplyFormat = webcamApplyFormat_uvc;
		this->vh_cap_stop = r5u870_cap_stop_uvc;
		this->vh_decide_pkt = r5u870_decide_pkt_uvc;
	}else{
		this->webcamApplyFormat = webcamApplyFormat_wdm;
		this->vh_cap_stop = r5u870_cap_stop_wdm;
		this->vh_decide_pkt = r5u870_decide_pkt_wdm;
	}
	
	printk( KERN_INFO "[%s] Detected %s\n", KBUILD_MODNAME, this->webcamModel->modelName );
	snprintf( this->videoDevice.name, sizeof( this->videoDevice.name ), "%s", this->webcamModel->modelName );
	
	res = webcamMicrocode( this );
	if(res < 0){
		printk( KERN_ERR "[%s] webcamMicrocode failed: %d\n", KBUILD_MODNAME, res );
		goto out_failed;
	}
	
	if ( this->webcamModel->isUvc ) {//This appears to be a UVC VideoControl interface.
		res = r5u870_uvc_parse_vc( this );
		if (res < 0) {
			printk( KERN_ERR "[%s] UVC setup failed: %d \n", KBUILD_MODNAME, res );
			goto out_failed;
		}
	} else {//We are looking at a proprietary Ricoh interface 
		this->urbstreamInterfaceNumber = this->usbInterface->altsetting->desc.bInterfaceNumber;
		this->urbstreamEndpoint = 6;
	}
	
	if( this->webcamModel->controls_wdm ){
		res = webcamAddControls_wdm( this );
		if ( res ) {
			printk( KERN_ERR "[%s] Vendor control setup failed: %d\n", KBUILD_MODNAME, res );
			goto out_failed;
		}
	}
	
	//Configure the usbcam pixel format and control arrays
	if( !this->webcamFormatsCount ){
		printk( KERN_ERR "[%s] No pixel formats detected\n", KBUILD_MODNAME );
		res = -ENODEV;
		goto out_failed;
	}
	
	/* Set the default format */
	this->webcamCurrentFormat = &this->webcamFormats[0];
	this->webcamCurrentResolution = &this->webcamCurrentFormat->resolutionTable[0];
	this->webcamCurrentPixelformat.width = this->webcamCurrentResolution->width;
	this->webcamCurrentPixelformat.height = this->webcamCurrentResolution->height;
	this->webcamCurrentPixelformat.pixelformat = this->webcamCurrentFormat->pixelformat;
	this->webcamCurrentPixelformat.bytesperline = this->webcamCurrentPixelformat.width * 2;
	this->webcamCurrentPixelformat.sizeimage = this->webcamCurrentPixelformat.width * this->webcamCurrentPixelformat.height * 2;
	this->webcamCurrentPixelformat.colorspace = V4L2_COLORSPACE_SMPTE170M;
	this->webcamCurrentPixelformat.field = V4L2_FIELD_INTERLACED;
	
	return 0;
	
out_failed:
	webcamRelease( this );
	return res;
}