cpia.c

是关于linux2.5.1的完全源码

static int skipcount(int count, int fmt)
{
	switch(fmt) {
	case VIDEO_PALETTE_GREY:
	case VIDEO_PALETTE_RGB555:
	case VIDEO_PALETTE_RGB565:
	case VIDEO_PALETTE_YUV422:
	case VIDEO_PALETTE_YUYV:
	case VIDEO_PALETTE_UYVY:
		return 2*count;
	case VIDEO_PALETTE_RGB24:
		return 3*count;
	case VIDEO_PALETTE_RGB32:
		return 4*count;
	default:
		return 0;
	}
}

static int parse_picture(struct cam_data *cam, int size)
{
	u8 *obuf, *ibuf, *end_obuf;
	int ll, in_uyvy, compressed, origsize, out_fmt;

	/* make sure params don't change while we are decoding */
	down(&cam->param_lock);

	obuf = cam->decompressed_frame.data;
	end_obuf = obuf+CPIA_MAX_FRAME_SIZE;
	ibuf = cam->raw_image;
	origsize = size;
	out_fmt = cam->vp.palette;

	if ((ibuf[0] != MAGIC_0) || (ibuf[1] != MAGIC_1)) {
		LOG("header not found\n");
		up(&cam->param_lock);
		return -1;
	}

	if ((ibuf[16] != VIDEOSIZE_QCIF) && (ibuf[16] != VIDEOSIZE_CIF)) {
		LOG("wrong video size\n");
		up(&cam->param_lock);
		return -1;
	}
	
	if (ibuf[17] != SUBSAMPLE_422) {
		LOG("illegal subtype %d\n",ibuf[17]);
		up(&cam->param_lock);
		return -1;
	}
	
	if (ibuf[18] != YUVORDER_YUYV && ibuf[18] != YUVORDER_UYVY) {
		LOG("illegal yuvorder %d\n",ibuf[18]);
		up(&cam->param_lock);
		return -1;
	}
	in_uyvy = ibuf[18] == YUVORDER_UYVY;
	
#if 0
	/* FIXME: ROI mismatch occurs when switching capture sizes */
	if ((ibuf[24] != cam->params.roi.colStart) ||
	    (ibuf[25] != cam->params.roi.colEnd) ||
	    (ibuf[26] != cam->params.roi.rowStart) ||
	    (ibuf[27] != cam->params.roi.rowEnd)) {
		LOG("ROI mismatch\n");
		up(&cam->param_lock);
		return -1;
	}
#endif
	
	if ((ibuf[28] != NOT_COMPRESSED) && (ibuf[28] != COMPRESSED)) {
		LOG("illegal compression %d\n",ibuf[28]);
		up(&cam->param_lock);
		return -1;
	}
	compressed = (ibuf[28] == COMPRESSED);
	
	if (ibuf[29] != NO_DECIMATION) {
		LOG("decimation not supported\n");
		up(&cam->param_lock);
		return -1;
	}
	
	cam->params.yuvThreshold.yThreshold = ibuf[30];
	cam->params.yuvThreshold.uvThreshold = ibuf[31];
	cam->params.status.systemState = ibuf[32];
	cam->params.status.grabState = ibuf[33];
	cam->params.status.streamState = ibuf[34];
	cam->params.status.fatalError = ibuf[35];
	cam->params.status.cmdError = ibuf[36];
	cam->params.status.debugFlags = ibuf[37];
	cam->params.status.vpStatus = ibuf[38];
	cam->params.status.errorCode = ibuf[39];
	cam->fps = ibuf[41];
	up(&cam->param_lock);
	
	ibuf += FRAME_HEADER_SIZE;
	size -= FRAME_HEADER_SIZE;
	ll = ibuf[0] | (ibuf[1] << 8);
	ibuf += 2;

	while (size > 0) {
		size -= (ll+2);
		if (size < 0) {
			LOG("Insufficient data in buffer\n");
			return -1;
		}

		while (ll > 1) {
			if (!compressed || (compressed && !(*ibuf & 1))) {
				obuf += yuvconvert(ibuf, obuf, out_fmt,
				                   in_uyvy, cam->mmap_kludge);
				ibuf += 4;
				ll -= 4;
			} else {
				/*skip compressed interval from previous frame*/
				int skipsize = skipcount(*ibuf >> 1, out_fmt);
				obuf += skipsize;
				if (obuf > end_obuf) {
					LOG("Insufficient data in buffer\n");
					return -1;
				}
				++ibuf;
				ll--;
			}
		}
		if (ll == 1) {
			if (*ibuf != EOL) {
				LOG("EOL not found giving up after %d/%d"
				    " bytes\n", origsize-size, origsize);
				return -1;
			}

			ibuf++; /* skip over EOL */

			if ((size > 3) && (ibuf[0] == EOI) && (ibuf[1] == EOI) &&
			   (ibuf[2] == EOI) && (ibuf[3] == EOI)) {
			 	size -= 4;
				break;
			}

			if (size > 1) {
				ll = ibuf[0] | (ibuf[1] << 8);
				ibuf += 2; /* skip over line length */
			}
		} else {
			LOG("line length was not 1 but %d after %d/%d bytes\n",
			    ll, origsize-size, origsize);
			return -1;
		}
	}
	
	cam->decompressed_frame.count = obuf-cam->decompressed_frame.data;

	return cam->decompressed_frame.count;
}

/* InitStreamCap wrapper to select correct start line */
static inline int init_stream_cap(struct cam_data *cam)
{
	return do_command(cam, CPIA_COMMAND_InitStreamCap,
	                  0, cam->params.streamStartLine, 0, 0);
}

/* update various camera modes and settings */
static void dispatch_commands(struct cam_data *cam)
{
	down(&cam->param_lock);
	if (cam->cmd_queue==COMMAND_NONE) {
		up(&cam->param_lock);
		return;
	}
	DEB_BYTE(cam->cmd_queue);
	DEB_BYTE(cam->cmd_queue>>8);
	if (cam->cmd_queue & COMMAND_SETCOLOURPARAMS)
		do_command(cam, CPIA_COMMAND_SetColourParams,
		           cam->params.colourParams.brightness,
		           cam->params.colourParams.contrast,
		           cam->params.colourParams.saturation, 0);

	if (cam->cmd_queue & COMMAND_SETCOMPRESSION)
		do_command(cam, CPIA_COMMAND_SetCompression,
		           cam->params.compression.mode,
			   cam->params.compression.decimation, 0, 0);

	if (cam->cmd_queue & COMMAND_SETFORMAT) {
		do_command(cam, CPIA_COMMAND_SetFormat,
	        	   cam->params.format.videoSize,
	        	   cam->params.format.subSample,
	        	   cam->params.format.yuvOrder, 0);
		do_command(cam, CPIA_COMMAND_SetROI,
	        	   cam->params.roi.colStart, cam->params.roi.colEnd,
	        	   cam->params.roi.rowStart, cam->params.roi.rowEnd);
		cam->first_frame = 1;
	}

	if (cam->cmd_queue & COMMAND_SETCOMPRESSIONTARGET)
		do_command(cam, CPIA_COMMAND_SetCompressionTarget,
		           cam->params.compressionTarget.frTargeting,
		           cam->params.compressionTarget.targetFR,
		           cam->params.compressionTarget.targetQ, 0);

	if (cam->cmd_queue & COMMAND_SETYUVTHRESH)
		do_command(cam, CPIA_COMMAND_SetYUVThresh,
		           cam->params.yuvThreshold.yThreshold,
		           cam->params.yuvThreshold.uvThreshold, 0, 0);

	if (cam->cmd_queue & COMMAND_SETECPTIMING)
		do_command(cam, CPIA_COMMAND_SetECPTiming,
		           cam->params.ecpTiming, 0, 0, 0);

	if (cam->cmd_queue & COMMAND_SETCOMPRESSIONPARAMS)
		do_command_extended(cam, CPIA_COMMAND_SetCompressionParams,
		            0, 0, 0, 0,
		            cam->params.compressionParams.hysteresis,
		            cam->params.compressionParams.threshMax,
		            cam->params.compressionParams.smallStep,
		            cam->params.compressionParams.largeStep,
		            cam->params.compressionParams.decimationHysteresis,
		            cam->params.compressionParams.frDiffStepThresh,
		            cam->params.compressionParams.qDiffStepThresh,
		            cam->params.compressionParams.decimationThreshMod);

	if (cam->cmd_queue & COMMAND_SETEXPOSURE)
		do_command_extended(cam, CPIA_COMMAND_SetExposure,
		                    cam->params.exposure.gainMode,
		                    cam->params.exposure.expMode,
		                    cam->params.exposure.compMode,
		                    cam->params.exposure.centreWeight,
		                    cam->params.exposure.gain,
		                    cam->params.exposure.fineExp,
		                    cam->params.exposure.coarseExpLo,
		                    cam->params.exposure.coarseExpHi,
		                    cam->params.exposure.redComp,
		                    cam->params.exposure.green1Comp,
		                    cam->params.exposure.green2Comp,
		                    cam->params.exposure.blueComp);

	if (cam->cmd_queue & COMMAND_SETCOLOURBALANCE) {
		if (cam->params.colourBalance.balanceModeIsAuto) {
			do_command(cam, CPIA_COMMAND_SetColourBalance,
				   2, 0, 0, 0);
		} else {
			do_command(cam, CPIA_COMMAND_SetColourBalance,
				   1,
				   cam->params.colourBalance.redGain,
				   cam->params.colourBalance.greenGain,
				   cam->params.colourBalance.blueGain);
			do_command(cam, CPIA_COMMAND_SetColourBalance,
				   3, 0, 0, 0);
		}
	}

	if (cam->cmd_queue & COMMAND_SETSENSORFPS)
		do_command(cam, CPIA_COMMAND_SetSensorFPS,
		           cam->params.sensorFps.divisor,
		           cam->params.sensorFps.baserate, 0, 0);

	if (cam->cmd_queue & COMMAND_SETAPCOR)
		do_command(cam, CPIA_COMMAND_SetApcor,
		           cam->params.apcor.gain1,
		           cam->params.apcor.gain2,
		           cam->params.apcor.gain4,
		           cam->params.apcor.gain8);

	if (cam->cmd_queue & COMMAND_SETFLICKERCTRL)
		do_command(cam, CPIA_COMMAND_SetFlickerCtrl,
		           cam->params.flickerControl.flickerMode,
		           cam->params.flickerControl.coarseJump,
		           cam->params.flickerControl.allowableOverExposure, 0);

	if (cam->cmd_queue & COMMAND_SETVLOFFSET)
		do_command(cam, CPIA_COMMAND_SetVLOffset,
		           cam->params.vlOffset.gain1,
		           cam->params.vlOffset.gain2,
		           cam->params.vlOffset.gain4,
		           cam->params.vlOffset.gain8);

	if (cam->cmd_queue & COMMAND_PAUSE)
		do_command(cam, CPIA_COMMAND_EndStreamCap, 0, 0, 0, 0);

	if (cam->cmd_queue & COMMAND_RESUME)
		init_stream_cap(cam);

	up(&cam->param_lock);
	cam->cmd_queue = COMMAND_NONE;
	return;
}

/* kernel thread function to read image from camera */
static void fetch_frame(void *data)
{
	int image_size, retry;
	struct cam_data *cam = (struct cam_data *)data;
	unsigned long oldjif, rate, diff;

	/* Allow up to two bad images in a row to be read and
	 * ignored before an error is reported */
	for (retry = 0; retry < 3; ++retry) {
		if (retry)
			DBG("retry=%d\n", retry);

		if (!cam->ops)
			continue;

		/* load first frame always uncompressed */
		if (cam->first_frame &&
		    cam->params.compression.mode != CPIA_COMPRESSION_NONE)
			do_command(cam, CPIA_COMMAND_SetCompression,
				   CPIA_COMPRESSION_NONE,
				   NO_DECIMATION, 0, 0);

		/* init camera upload */
		if (do_command(cam, CPIA_COMMAND_SetGrabMode,
			       CPIA_GRAB_CONTINUOUS, 0, 0, 0))
			continue;

		if (do_command(cam, CPIA_COMMAND_GrabFrame, 0,
			       cam->params.streamStartLine, 0, 0))
			continue;

		if (cam->ops->wait_for_stream_ready) {
			/* loop until image ready */
			do_command(cam, CPIA_COMMAND_GetCameraStatus,0,0,0,0);
			while (cam->params.status.streamState != STREAM_READY) {
				cond_resched();

				current->state = TASK_INTERRUPTIBLE;

				/* sleep for 10 ms, hopefully ;) */
				schedule_timeout(10*HZ/1000);
				if (signal_pending(current))
					return;

				do_command(cam, CPIA_COMMAND_GetCameraStatus,
				           0, 0, 0, 0);
			}
		}

		/* grab image from camera */
		cond_resched();

		oldjif = jiffies;
		image_size = cam->ops->streamRead(cam->lowlevel_data,
						  cam->raw_image, 0);
		if (image_size <= 0) {
			DBG("streamRead failed: %d\n", image_size);
			continue;
		}

		rate = image_size * HZ / 1024;
		diff = jiffies-oldjif;
		cam->transfer_rate = diff==0 ? rate : rate/diff;
			/* diff==0 ? unlikely but possible */

		/* camera idle now so dispatch queued commands */
		dispatch_commands(cam);

		/* Update our knowledge of the camera state - FIXME: necessary? */
		do_command(cam, CPIA_COMMAND_GetColourBalance, 0, 0, 0, 0);
		do_command(cam, CPIA_COMMAND_GetExposure, 0, 0, 0, 0);

		/* decompress and convert image to by copying it from
		 * raw_image to decompressed_frame
		 */
		cond_resched();

		cam->image_size = parse_picture(cam, image_size);
		if (cam->image_size <= 0)
			DBG("parse_picture failed %d\n", cam->image_size);
		else
			break;
	}

	if (retry < 3) {
		/* FIXME: this only works for double buffering */
		if (cam->frame[cam->curframe].state == FRAME_READY) {
			memcpy(cam->frame[cam->curframe].data,
			       cam->decompressed_frame.data,
			       cam->decompressed_frame.count);
			cam->frame[cam->curframe].state = FRAME_DONE;
		} else
			cam->decompressed_frame.state = FRAME_DONE;

#if 0
		if (cam->first_frame &&
		    cam->params.compression.mode != CPIA_COMPRESSION_NONE) {
			cam->first_frame = 0;
			cam->cmd_queue |= COMMAND_SETCOMPRESSION;
		}
#else
		if (cam->first_frame) {
			cam->first_frame = 0;
			cam->cmd_queue |= COMMAND_SETCOMPRESSION;
			cam->cmd_queue |= COMMAND_SETEXPOSURE;
		}
#endif
	}
}

static int capture_frame(struct cam_data *cam, struct video_mmap *vm)
{
	int retval = 0;

	if (!cam->frame_buf) {
		/* we do lazy allocation */
		if ((retval = allocate_frame_buf(cam)))
			return retval;
	}
	
	/* FIXME: the first frame seems to be captured by the camera
	   without regards to any initial settings, so we throw away
	   that one, the next one is generated with our settings
	   (exposure, color balance, ...)
	*/
	if (cam->first_frame) {
		cam->curframe = vm->frame;
		cam->frame[cam->curframe].state = FRAME_READY;
		fetch_frame(cam);
		if (cam->frame[cam->curframe].state != FRAME_DONE)
			retval = -EIO;
	}
	cam->curframe = vm->frame;
	cam->frame[cam->curframe].state = FRAME_READY;
	fetch_frame(cam);
	if (cam->frame[cam->curframe].state != FRAME_DONE)
		retval=-EIO;

	return retval;
}
  
static int goto_high_power(struct cam_data *cam)
{
	if (do_command(cam, CPIA_COMMAND_GotoHiPower, 0, 0, 0, 0))
		return -1;
	mdelay(100);		/* windows driver does it too */
	if (do_command(cam, CPIA_COMMAND_GetCameraStatus, 0, 0, 0, 0))
		return -1;
	if (cam->params.status.systemState == HI_POWER_STATE) {
		DBG("camera now in HIGH power state\n");
		return