cpia.c

自己根据lkd和情境分析

			*rgb++ = LIMIT(b+y);
			*rgb++ = LIMIT(r+y1);
			*rgb++ = LIMIT(g+y1);
			*rgb = LIMIT(b+y1);
		}
		return 6;
	case VIDEO_PALETTE_RGB32:
		if (mmap_kludge) {
			*rgb++ = LIMIT(b+y);
			*rgb++ = LIMIT(g+y);
			*rgb++ = LIMIT(r+y);
			rgb++;
			*rgb++ = LIMIT(b+y1);
			*rgb++ = LIMIT(g+y1);
			*rgb = LIMIT(r+y1);
		} else {
			*rgb++ = LIMIT(r+y);
			*rgb++ = LIMIT(g+y);
			*rgb++ = LIMIT(b+y);
			rgb++;
			*rgb++ = LIMIT(r+y1);
			*rgb++ = LIMIT(g+y1);
			*rgb = LIMIT(b+y1);
		}
		return 8;
	case VIDEO_PALETTE_GREY:
		*rgb++ = y;
		*rgb = y1;
		return 2;
	case VIDEO_PALETTE_YUV422:
	case VIDEO_PALETTE_YUYV:
		*rgb++ = y;
		*rgb++ = u;
		*rgb++ = y1;
		*rgb = v;
		return 4;
	case VIDEO_PALETTE_UYVY:
		*rgb++ = u;
		*rgb++ = y;
		*rgb++ = v;
		*rgb = y1;
		return 4;
	default:
		DBG("Empty: %d\n", out_fmt);
		return 0;
	}
}

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) {
				if (current->need_resched)
					schedule();

				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 */
		if (current->need_resched)
			schedule();

		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
		 */
		if (current->need_resched)
			schedule();

		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_SETCOMPRESSIO