cpia.c

优龙2410linux2.6.8内核源代码

	data[7] = l;

	retval = cam->ops->transferCmd(cam->lowlevel_data, cmd, data);
	if (retval)
		DBG("%x - failed\n", command);

	return retval;
}

/**********************************************************************
 *
 * Colorspace conversion
 *
 **********************************************************************/
#define LIMIT(x) ((((x)>0xffffff)?0xff0000:(((x)<=0xffff)?0:(x)&0xff0000))>>16)

static int convert420(unsigned char *yuv, unsigned char *rgb, int out_fmt,
                      int linesize, int mmap_kludge)
{
	int y, u, v, r, g, b, y1;
	
	/* Odd lines use the same u and v as the previous line.
	 * Because of compression, it is necessary to get this
	 * information from the decoded image. */
	switch(out_fmt) {
	case VIDEO_PALETTE_RGB555:
		y = (*yuv++ - 16) * 76310;
		y1 = (*yuv - 16) * 76310;
		r = ((*(rgb+1-linesize)) & 0x7c) << 1;
		g = ((*(rgb-linesize)) & 0xe0) >> 4 |
		    ((*(rgb+1-linesize)) & 0x03) << 6;
		b = ((*(rgb-linesize)) & 0x1f) << 3;
		u = (-53294 * r - 104635 * g + 157929 * b) / 5756495;
		v = (157968 * r - 132278 * g - 25690 * b) / 5366159;
		r = 104635 * v;
		g = -25690 * u - 53294 * v;
		b = 132278 * u;
		*rgb++ = ((LIMIT(g+y) & 0xf8) << 2) | (LIMIT(b+y) >> 3);
		*rgb++ = ((LIMIT(r+y) & 0xf8) >> 1) | (LIMIT(g+y) >> 6);
		*rgb++ = ((LIMIT(g+y1) & 0xf8) << 2) | (LIMIT(b+y1) >> 3);
		*rgb = ((LIMIT(r+y1) & 0xf8) >> 1) | (LIMIT(g+y1) >> 6);
		return 4;
	case VIDEO_PALETTE_RGB565:
		y = (*yuv++ - 16) * 76310;
		y1 = (*yuv - 16) * 76310;
		r = (*(rgb+1-linesize)) & 0xf8;
		g = ((*(rgb-linesize)) & 0xe0) >> 3 |
		    ((*(rgb+1-linesize)) & 0x07) << 5;
		b = ((*(rgb-linesize)) & 0x1f) << 3;
		u = (-53294 * r - 104635 * g + 157929 * b) / 5756495;
		v = (157968 * r - 132278 * g - 25690 * b) / 5366159;
		r = 104635 * v;
		g = -25690 * u - 53294 * v;
		b = 132278 * u;
		*rgb++ = ((LIMIT(g+y) & 0xfc) << 3) | (LIMIT(b+y) >> 3);
		*rgb++ = (LIMIT(r+y) & 0xf8) | (LIMIT(g+y) >> 5);
		*rgb++ = ((LIMIT(g+y1) & 0xfc) << 3) | (LIMIT(b+y1) >> 3);
		*rgb = (LIMIT(r+y1) & 0xf8) | (LIMIT(g+y1) >> 5);
		return 4;
		break;
	case VIDEO_PALETTE_RGB24:
	case VIDEO_PALETTE_RGB32:
		y = (*yuv++ - 16) * 76310;
		y1 = (*yuv - 16) * 76310;
		if (mmap_kludge) {
			r = *(rgb+2-linesize);
			g = *(rgb+1-linesize);
			b = *(rgb-linesize);
		} else {
			r = *(rgb-linesize);
			g = *(rgb+1-linesize);
			b = *(rgb+2-linesize);
		}
		u = (-53294 * r - 104635 * g + 157929 * b) / 5756495;
		v = (157968 * r - 132278 * g - 25690 * b) / 5366159;
		r = 104635 * v;
		g = -25690 * u + -53294 * v;
		b = 132278 * u;
		if (mmap_kludge) {
			*rgb++ = LIMIT(b+y);
			*rgb++ = LIMIT(g+y);
			*rgb++ = LIMIT(r+y);
			if(out_fmt == VIDEO_PALETTE_RGB32)
				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);
			if(out_fmt == VIDEO_PALETTE_RGB32)
				rgb++;
			*rgb++ = LIMIT(r+y1);
			*rgb++ = LIMIT(g+y1);
			*rgb = LIMIT(b+y1);
		}
		if(out_fmt == VIDEO_PALETTE_RGB32)
			return 8;
		return 6;
	case VIDEO_PALETTE_YUV422:
	case VIDEO_PALETTE_YUYV:
		y = *yuv++;
		u = *(rgb+1-linesize);
		y1 = *yuv;
		v = *(rgb+3-linesize);
		*rgb++ = y;
		*rgb++ = u;
		*rgb++ = y1;
		*rgb = v;
		return 4;
	case VIDEO_PALETTE_UYVY:
		u = *(rgb-linesize);
		y = *yuv++;
		v = *(rgb+2-linesize);
		y1 = *yuv;
		*rgb++ = u;
		*rgb++ = y;
		*rgb++ = v;
		*rgb = y1;
		return 4;
	case VIDEO_PALETTE_GREY:
		*rgb++ = *yuv++;
		*rgb = *yuv;
		return 2;
	default:
		DBG("Empty: %d\n", out_fmt);
		return 0;
	}
}


static int yuvconvert(unsigned char *yuv, unsigned char *rgb, int out_fmt,
                      int in_uyvy, int mmap_kludge)
{
	int y, u, v, r, g, b, y1;

	switch(out_fmt) {
	case VIDEO_PALETTE_RGB555:
	case VIDEO_PALETTE_RGB565:
	case VIDEO_PALETTE_RGB24:
	case VIDEO_PALETTE_RGB32:
		if (in_uyvy) {
			u = *yuv++ - 128;
			y = (*yuv++ - 16) * 76310;
			v = *yuv++ - 128;
			y1 = (*yuv - 16) * 76310;
		} else {
			y = (*yuv++ - 16) * 76310;
			u = *yuv++ - 128;
			y1 = (*yuv++ - 16) * 76310;
			v = *yuv - 128;
		}
		r = 104635 * v;
		g = -25690 * u + -53294 * v;
		b = 132278 * u;
		break;
	default:
		y = *yuv++;
		u = *yuv++;
		y1 = *yuv++;
		v = *yuv;
		/* Just to avoid compiler warnings */
		r = 0;
		g = 0;
		b = 0;
		break;
	}
	switch(out_fmt) {
	case VIDEO_PALETTE_RGB555:
		*rgb++ = ((LIMIT(g+y) & 0xf8) << 2) | (LIMIT(b+y) >> 3);
		*rgb++ = ((LIMIT(r+y) & 0xf8) >> 1) | (LIMIT(g+y) >> 6);
		*rgb++ = ((LIMIT(g+y1) & 0xf8) << 2) | (LIMIT(b+y1) >> 3);
		*rgb = ((LIMIT(r+y1) & 0xf8) >> 1) | (LIMIT(g+y1) >> 6);
		return 4;
	case VIDEO_PALETTE_RGB565:
		*rgb++ = ((LIMIT(g+y) & 0xfc) << 3) | (LIMIT(b+y) >> 3);
		*rgb++ = (LIMIT(r+y) & 0xf8) | (LIMIT(g+y) >> 5);
		*rgb++ = ((LIMIT(g+y1) & 0xfc) << 3) | (LIMIT(b+y1) >> 3);
		*rgb = (LIMIT(r+y1) & 0xf8) | (LIMIT(g+y1) >> 5);
		return 4;
	case VIDEO_PALETTE_RGB24:
		if (mmap_kludge) {
			*rgb++ = LIMIT(b+y);
			*rgb++ = LIMIT(g+y);
			*rgb++ = LIMIT(r+y);
			*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++ = 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:
		return count;
	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, decimation, even_line, origsize, out_fmt;
	int rows, cols, linesize, subsample_422;

	/* 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_420 && ibuf[17] != SUBSAMPLE_422) {
		LOG("illegal subtype %d\n",ibuf[17]);
		up(&cam->param_lock);
		return -1;
	}
	subsample_422 = ibuf[17] == SUBSAMPLE_422;
	
	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 ((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;
	}
	cols = 8*(ibuf[25] - ibuf[24]);
	rows = 4*(ibuf[27] - ibuf[26]);

	
	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 && ibuf[29] != DECIMATION_ENAB) {
		LOG("illegal decimation %d\n",ibuf[29]);
		up(&cam->param_lock);
		return -1;
	}
	decimation = (ibuf[29] == DECIMATION_ENAB);	

	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);
	
	linesize = skipcount(cols, out_fmt);
	ibuf += FRAME_HEADER_SIZE;
	size -= FRAME_HEADER_SIZE;
	ll = ibuf[0] | (ibuf[1] << 8);
	ibuf += 2;
	even_line = 1;

	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))) {
				if(subsample_422 || even_line) {
				obuf += yuvconvert(ibuf, obuf, out_fmt,
				                   in_uyvy, cam->mmap_kludge);
				ibuf += 4;
				ll -= 4;
			} else {
					/* SUBSAMPLE_420 on an odd line */
					obuf += convert420(ibuf, obuf,
					                   out_fmt, linesize,
			                		   cam->mmap_kludge);
					ibuf += 2;
					ll -= 2;
				}
			} else {
				/*skip compressed interval from previous frame*/
				obuf += skipcount(*ibuf >> 1, out_fmt);
				if (obuf > end_obuf) {
					LOG("Insufficient buffer size\n");
					return -1;
				}
				++ibuf;
				ll--;
			}
		}
		if (ll == 1) {
			if (*ibuf != EOL) {
				DBG("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(decimation) {
				/* skip the odd lines for now */
				obuf += linesize;
			}

			if (size > 1) {
				ll = ibuf[0] | (ibuf[1] << 8);
				ibuf += 2; /* skip over line length */
			}
			if(!decimation)
				even_line = !even_line;
		} else {
			LOG("line length was not 1 but %d after %d/%d bytes\n",
			    ll, origsize-size, origsize);
			return -1;
		}
	}
	
	if(decimation) {
		/* interpolate odd rows */
		int i, j;
		u8 *prev, *next;
		prev = cam->decompressed_frame.data;
		obuf = prev+linesize;
		next = obuf+linesize;
		for(i=1; i<rows-1; i+=2) {
			for(j=0; j<linesize; ++j) {
				*obuf++ = ((int)*prev++ + *next++) / 2;
			}
			prev += linesize;
			obuf += linesize;
			next += linesize;
		}
		/* last row is odd, just copy previous row */
		memcpy(obuf, prev, linesize);
	}

	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);
}


/*  find_over_exposure
 *    Finds a suitable value of OverExposure for use with SetFlickerCtrl
 *    Some calculat