qc-driver.c

Webcam Linux driver for Quickcam

		if (qcdebug&QC_DEBUGADAPTATION) PDEBUG("Decreasing exposure");
		ctrl->controlalg = EXPCONTROL_SATURATED;
		newexposure = ctrl->exposure / 2;
	} else {
		deviation = target - midvalue;
		if (ABS(deviation) < adaptation_min) {
			/* For small variations, adapt linearly */
			if (qcdebug&QC_DEBUGADAPTATION) PDEBUG("Small deviation %i",deviation);
			ctrl->controlalg = EXPCONTROL_FLOAT;
			newexposure = small_adapt * SGN(deviation) + ctrl->exposure;
		} else {
			/* Try using Newton method for estimating correct exposure value */
			ctrl->controlalg = EXPCONTROL_NEWTON;
			dmidvalue = midvalue       - ctrl->oldmidvalue;
			dexposure = ctrl->exposure - ctrl->oldexposure;
			if (ABS(dmidvalue) <  dmidvalue_min || 
			    ABS(dexposure) <  dexposure_min ||
			    SGN(dmidvalue) != SGN(dexposure))
			{
				/* Can not estimate delta with Newton method, just guess */
				if (ctrl->olddelta < 2) {
					if (qcdebug&QC_DEBUGADAPTATION) PDEBUG("Best guessing");
					smoothdelta = bestguess;
				} else {
					Bool cross = SGN(midvalue-target) != SGN(ctrl->oldmidvalue-target);
					smoothdelta = cross ? (ctrl->olddelta / 2) : (ctrl->olddelta * 3 / 2);
					if (qcdebug&QC_DEBUGADAPTATION) PDEBUG("Change more exposure, smoothdelta=%i",smoothdelta);
				}
			} else {
				/* Everything is well, use here actual Newton method */
				delta       = (256 - underestimate) * dexposure / dmidvalue;
				smoothdelta = (delta_speed*delta + (256-delta_speed)*ctrl->olddelta) / 256;
				if (qcdebug&QC_DEBUGADAPTATION) PDEBUG("Using Newton, delta=%i",delta);
			}
		}
		/* Compute new exposure based on guessed/computed delta */
		smoothdelta = CLIP(smoothdelta, delta_min,delta_max);
		dexposure = deviation * smoothdelta / 256;
		/* Newton works linearly, but exposure/brightness are not linearly related */
		/* The following test fixes the worst deficiencies due to that (I hope) */
		if (-dexposure > ctrl->exposure/2)
			dexposure = -ctrl->exposure/2;
		newexposure = dexposure + ctrl->exposure;
		ctrl->olddelta = smoothdelta;
	}

	newexposure       = CLIP(newexposure, 2,65535);

	if (qcdebug&QC_DEBUGADAPTATION) 
		PDEBUG("midval=%i dev=%i dmidv=%i dexp=%i smdelta=%i olddelta=%i newexp=%i gain=%i",
		midvalue,deviation,dmidvalue,dexposure,smoothdelta,ctrl->olddelta,newexposure,ctrl->gain);

	ctrl->oldexposure = ctrl->exposure;
	ctrl->exposure    = newexposure;
	ctrl->oldmidvalue = midvalue;
	*ret_exposure     = newexposure;
	*ret_gain         = ctrl->gain;
#else
	/* This code is for measuring the delay between an exposure settings and until
	 * it becomes in effect. Only useful for developing the adaptation algorithm. */
	/* Some delays: when a setting is changed at frame number #0,
	 * it becomes in effect in frame xx for	exposure	gain
	 * QuickCam Web/0850/normal mode	4		4
	 * QuickCam Web/0850/compressed mode	5		5
	 * QuickCam Express/840			2		1-5
	 *
	 */
	static int exp = 0;
	static int gain = 0;
	static const int changedel = 20;
	static int state = 0;
	static int framenum = 0;
	PRINTK(KERN_CRIT,"Measuring: framenum=%i, midvalue=%i",framenum,midvalue);
	if ((framenum%changedel)==0) {
		switch (state) {
		default:
		case 0:
			PRINTK(KERN_CRIT,"Measuring: set to black");
			exp = 0;
			gain = 0;
			break;
		case 1:
			PRINTK(KERN_CRIT,"Measuring: changing exposure");
			exp = 65535;
			break;
		case 2:
			PRINTK(KERN_CRIT,"Measuring: changing gain");
			gain = 32535;
			break;
		}
		state = ((state+1) % 3);
	}
	*ret_exposure = exp;
	*ret_gain = gain;
	framenum++;
#endif
}

/* }}} */

/* }}} */
/* {{{ [fold] **** qc_frame:  Frame capturing functions ************************* */

/* From /usr/src/linux/Documentation/smp.tex:
 * + Kernel mode process (e.g. system calls):
 *   - No other kernel mode processes may run simultaneously/pre-empt
 *     (kernel mode processes are atomic with respect to each other)
 *     (Does not hold for 2.6.x)
 *   - Exception is voluntary sleeping, in which case re-entry is allowed
 *     (Does not hold for 2.6.x)
 *   - Interrupts may pre-empt (but return to same process)
 *     (interrupts can be disabled if necessary)
 * + Interrupt mode execution
 *   - Kernel mode process may not pre-empt/execute simultaneously
 *   - Other interrupts may pre-empt, however same interrupt is not nested
 */

/* We have here a quite typical producer-consumer scheme:
 * Interrupt routine produces more frame data, while
 * kernel mode processes consume it
 * Read: Linux Device Drivers, Alessandro Rubini et al, 2nd edition, pg. 279
 * "Using Circular Buffers"
 */

/* Initialization and cleanup routines, called from kernel mode processes */
/* {{{ [fold] qc_frame_init(struct quickcam *qc) */
static int qc_frame_init(struct quickcam *qc)
{
	struct qc_frame_data *fd = &qc->frame_data;
	int n;

	if (qcdebug&QC_DEBUGFRAME || qcdebug&QC_DEBUGINIT) PDEBUG("qc_frame_init(qc=%p)",qc);
	TEST_BUGR(qc==NULL || fd==NULL);
	TEST_BUGR(in_interrupt());
	fd->rawdatabuf = vmalloc(FRAME_DATASIZE * FRAME_BUFFERS);
	if (!fd->rawdatabuf) return -ENOMEM;
	memset(fd->rawdatabuf, 0, FRAME_DATASIZE * FRAME_BUFFERS);	/* Never let user access random kernel data */
	fd->head       = 0;		/* First buffer to fill */
	fd->tail       = 0;		/* First buffer to get */
	spin_lock_init(&fd->tail_lock);
	fd->tail_in_use= FALSE;
	init_waitqueue_head(&fd->wq);
	fd->waiting    = 0;
	fd->exiting    = FALSE;
	for (n=0; n<FRAME_BUFFERS; n++) fd->buffers[n].rawdatalen = 0;
	fd->lost_frames = 0;
	IDEBUG_INIT(*fd);
	return 0;
}
/* }}} */
/* {{{ [fold] qc_frame_exit(struct quickcam *qc) */
/* This function must be called with qc->lock acquired 
 * (it may release it temporarily and sleep) */
static void qc_frame_exit(struct quickcam *qc)
{
	struct qc_frame_data *fd = &qc->frame_data;
#if PARANOID
	unsigned long startjiffy = jiffies;
#endif
	if (qcdebug&QC_DEBUGFRAME || qcdebug&QC_DEBUGINIT) PDEBUG("qc_frame_exit(qc=%p,tail=%i,head=%i)",qc,fd->tail,fd->head);
	TEST_BUG(in_interrupt());
	TEST_BUG(qc==NULL || fd==NULL);
	fd->exiting = TRUE;
	fd->maxrawdatalen = 0;		/* Hopefully stops all ongoing captures, might need locking though */
	wake_up(&fd->wq);
	if (qcdebug&QC_DEBUGFRAME) PDEBUG("waiting=%i",fd->waiting);
	if (qcdebug&QC_DEBUGMUTEX) PDEBUG("up(%p) in qc_frame_exit() : %i", qc, sem_getcount(&qc->lock));
	up(&qc->lock);			/* The lock was down when entering this function */
	while (fd->waiting > 0) {
		schedule();
#if PARANOID
		if (jiffies-startjiffy > 60*HZ) {
			PRINTK(KERN_CRIT,"Wait queue never completing!! (waiting=%i)",fd->waiting);
			break;
		}
#endif
	}
	if (qcdebug&QC_DEBUGMUTEX) PDEBUG("down(%p) in qc_frame_exit() : %i", qc, sem_getcount(&qc->lock));
	down(&qc->lock);
	vfree(fd->rawdatabuf);
	POISON(fd->rawdatabuf);
	IDEBUG_EXIT(*fd);
}
/* }}} */

/* Consumer routines, called from kernel mode processes */
/* {{{ [fold] qc_frame_get(struct quickcam *qc, unsigned char **buf) */
/* Wait until next frame is ready and return the frame length
 * and set buf to point to the frame. If error happens,
 * return standard Linux negative error number.
 * qc_frame_free() must be called after the frame is not needed anymore.
 * qc->lock must be acquired when entering this routine
 * (it may release it temporarily and sleep).
 */
static int qc_frame_get(struct quickcam *qc, unsigned char **buf)
{
	struct qc_frame_data *fd = &qc->frame_data;
	int ret;

	TEST_BUGR(qc==NULL || fd==NULL || fd->tail_in_use);
	TEST_BUGR(in_interrupt());
	IDEBUG_TEST(*fd);

	/* Wait until the next frame is available */
	if (qcdebug&QC_DEBUGFRAME) PDEBUG("qc_frame_get/consume(qc=%p,tail=%i,head=%i)",qc,fd->tail,fd->head);
	fd->waiting++;
	if (qcdebug&QC_DEBUGMUTEX) PDEBUG("up(%p) in qc_frame_get() : %i", qc, sem_getcount(&qc->lock));
	up(&qc->lock);					/* Release lock while waiting */

	ret = wait_event_interruptible(fd->wq, fd->head!=fd->tail || fd->exiting);	//FIXME:What if we get -ERESTARTSYS?
	if(ret == -ERESTARTSYS) {
	  PDEBUG("wait_event_interruptible() returned ERESTARTSYS");
	}
	if (qcdebug&QC_DEBUGMUTEX) PDEBUG("down(%p) in qc_frame_get() : %i", qc, sem_getcount(&qc->lock));
	down(&qc->lock);
	if (!ret) {
		if (!fd->exiting) {
			unsigned int t;
			unsigned long flags;
			spin_lock_irqsave(&fd->tail_lock, flags);
			fd->tail_in_use = TRUE;
			t = fd->tail;
			spin_unlock_irqrestore(&fd->tail_lock, flags);
			if (qcdebug&QC_DEBUGFRAME) PDEBUG("qc_frame_get/consume(qc=%p,tail=%i,head=%i,tail->rawdatalen=%i), got frame",qc,t,fd->head,fd->buffers[t].rawdatalen);
			*buf = fd->rawdatabuf + t*FRAME_DATASIZE;
			ret  = fd->buffers[t].rawdatalen;
		} else {
			ret = -ENODATA;
		}
	}
	fd->waiting--;
	fd->lost_frames = 0;
	if (ret<0 && (qcdebug&(QC_DEBUGERRORS|QC_DEBUGFRAME))) PDEBUG("failed qc_frame_get()=%i",ret);
	return ret;
}
/* }}} */
/* {{{ [fold] qc_frame_free(struct quickcam *qc) */
/* Free up the last frame returned from qc_frame_get() (it must be called first) */
static inline void qc_frame_free(struct quickcam *qc)
{
	struct qc_frame_data *fd = &qc->frame_data;
	unsigned long flags;
	TEST_BUG(qc==NULL || fd==NULL);
	TEST_BUG(in_interrupt());
	TEST_BUG(fd->head==fd->tail);			/* The current fd->tail is not available to be freed! */
	IDEBUG_TEST(*fd);
	if (qcdebug&QC_DEBUGFRAME) PDEBUG("qc_frame_free/consume(qc=%p,tail=%i,head=%i)",qc,fd->tail,fd->head);
	/* Free up previous frame and advance to next */
	spin_lock_irqsave(&fd->tail_lock, flags);
	fd->tail_in_use = FALSE;
	fd->tail = (fd->tail + 1) % FRAME_BUFFERS;	
	spin_unlock_irqrestore(&fd->tail_lock, flags);
}
/* }}} */
/* {{{ [fold] qc_frame_test(struct quickcam *qc) */
/* Return TRUE if next frame is immediately available, FALSE otherwise. */
static inline Bool qc_frame_test(struct quickcam *qc)
{
	struct qc_frame_data *fd = &qc->frame_data;
	IDEBUG_TEST(*fd);
	if (qcdebug&QC_DEBUGFRAME) PDEBUG("qc_frame_test/consume(qc=%p,tail=%i,head=%i)",qc,fd->tail,fd->head);
	return fd->head != fd->tail;
}
/* }}} */

/* Producer routines, called from interrupt context */
/* {{{ [fold] qc_frame_begin(struct quickcam *qc) */
/* Begin capturing next frame from camera. If buffer is full, the frame will be lost */
static void qc_frame_begin(struct quickcam *qc)
{
	struct qc_frame_data *fd = &qc->frame_data;
	int framesize, h;
	TEST_BUG(qc==NULL || fd==NULL);
	IDEBUG_TEST(*fd);
	if (qcdebug&QC_DEBUGFRAME) PDEBUG("qc_frame_begin/produce(qc=%p,tail=%i,head=%i)",qc,fd->tail,fd->head);
	if (fd->exiting) return;
	TEST_BUG(fd->rawdatabuf==NULL);
	h = fd->head;
	fd->buffers[h].rawdatalen = 0;

	/* Use sensor information to get the framesize (i.e. how much we expect to receive bytes per image) */
	/* FIXME: should compute data size differently in compressed mode */
	framesize = qc->sensor_data.width * qc->sensor_data.height;
	fd->maxrawdatalen = MIN(framesize, FRAME_DATASIZE);
}
/* }}} */
/* {{{ [fold] qc_frame_add(struct quickcam *qc, unsigned char *data, int datalen) */
/* Store more data for a frame, return nonzero if too much data or other error */
static int qc_frame_add(struct quickcam *qc, unsigned char *data, int datalen)
{
	struct qc_frame_data *fd = &qc->frame_data;
	int h = fd->head;
	int bytes;

	TEST_BUGR(qc==NULL || fd==NULL);
	IDEBUG_TEST(*fd);
	TEST_BUGR(fd->rawdatabuf==NULL);
	if (fd->maxrawdatalen <= fd->buffers[h].rawdatalen) {
	  //if (qcdebug&QC_DEBUGERRORS) PDEBUG("buffer disabled, maxrawdatalen=%i rawdatalen=%i datalen=%i",fd->maxrawdatalen,fd->buffers[h].rawdatalen, datalen);
		return -EBUSY;
	}
	bytes = MIN(datalen, fd->maxrawdatalen - fd->buffers[h].rawdatalen);
	memcpy(fd->rawdatabuf + h*FRAME_DATASIZE + fd->buffers[h].rawdatalen, data, bytes);

	fd->buffers[h].rawdatalen += bytes;
	if (bytes < datalen) {
		if (qcdebug&QC_DEBUGERRORS) PRINTK(KERN_ERR,"out of buffer space by %i, maxrawdatalen=%i rawdatalen=%i datalen=%i", datalen-bytes,fd->maxrawdatalen,fd->buffers[h].rawdatalen, datalen);
		return -ENOSPC;
	}
	return 0;
}
/* }}} */
/* {{{ [fold] qc_frame_end(struct quickcam *qc) */
/* Finished capturing most recent frame from camera */
/* (may be premature end, in which case some data is missing) */
static void qc_frame_end(struct quickcam *qc)
{
	static const int minrawdatalen = 32*32;	/* If frame length is less than this many bytes, discard it */
	struct qc_frame_data *fd = &qc->frame_data;
	unsigned int t, h;
	unsigned long flags;
	Bool lost_frame;
	TEST_BUG(qc==NULL || fd==NULL);
	h = fd->head;
	if (qcdebug&QC_DEBUGFRAME) PDEBUG("qc_frame_end/produce(qc=%p,tail=%i,head=%i), got %i bytes",qc,fd->tail,h,fd->buffers[h].rawdatalen);
	IDEBUG_TEST(*fd);
	fd->maxrawdatalen = 0;			/* Stop frame data capturing */
#if DUMPDATA
	PDEBUG("frame_end: got %i bytes", fd->buffers[h].rawdatalen);
#endif
	if (fd->buffers[h].rawdatalen < minrawdatalen) {
		/* No enough data in buffer, don't advance index */
		if (qcdebug&QC_DEBUGERRORS) PDEBUG("discarding frame with only %u bytes", fd->buffers[h].rawdatalen);
		return;
	}
	h = (h + 1) % FRAME_BUFFERS;		/* Select next frame buffer to fill */

	lost_frame = FALSE;
	spin_lock_irqsave(&fd->tail_lock, flags);
	t = fd->tail;
	if (t == h) {
		lost_frame = TRUE;
		/* FIXME: the below should work fine for two buffers, but not so well for more. It should be possible
		 * to drop oldest frame even when the current tail is in use. */
		if (fd->tail_in_use) {
			/* Can not drop the oldest frame, it is in use. Drop the newest frame */
			h = (h + FRAME_BUFFERS - 1) % FRAME_BUFFERS;		/* Decrease head by one back to the original */
			if (qcdebug&QC_DEBUGFRAME) PDEBUG("dropping newest frame");
		} else {
			/* Drop the oldest frame */
			fd->tail = (t + 1) % FRAME_BUFFERS;			/* Drop the oldest frame away */
			if (qcdebug&QC_DEBUGFRAME) PDEBUG("dropping oldest frame");
		}
	}
	spin_unlock_irqrestore(&fd->tail_lock, flags);
	if (lost_frame) {
		if (qcdebug&QC_DEBUGCOMMON || qcdebug&QC_DEBUGFRAME) PRINTK(KERN_NOTICE,"frame lost");
		fd->lost_frames++;
		if (fd->lost_frames > 10) {
			/* Here we should call qc_isoc_stop() to stop isochronous
			 * streaming since the application is clearly not reading frames at al