jpeg_stdio_dest(&cinfo, jpg_fp);

	cinfo.image_width = width;
	cinfo.image_height = height;
	cinfo.input_components = 3;
	cinfo.in_color_space = JCS_RGB;

	jpeg_set_defaults(&cinfo);

	jpeg_set_quality(&cinfo, quality, TRUE);

	jpeg_start_compress(&cinfo, TRUE);

	/* RGB565 -> RGB888, compress */
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			*(rgb24 + x*3+0) = ((*(rgb16 + y*width + x) & 0xf800) >> 8) & 0xff;
			*(rgb24 + x*3+1) = ((*(rgb16 + y*width + x) & 0x07e0) >> 3) & 0xff;
			*(rgb24 + x*3+2) = ((*(rgb16 + y*width + x) & 0x001f) << 3) & 0xff;
		}
		jpeg_write_scanlines(&cinfo, row_pointer, 1);
	}

	jpeg_finish_compress(&cinfo);
	jpeg_destroy_compress(&cinfo);

	fclose(jpg_fp);

err:
	if (rgb24)
		free(rgb24);
	if (rgb16)
		free(rgb16);
}
#define XLATTABSIZE      256
#define MulDiv(x, y, z)	((long)((int) x * (int) y) / (int) z)

//#define CLIP(x)	min_t(int, 255, max_t(int, 0, (x)))
#define CLIP(x) {if(x<0) x=0;if(x>255) x=255;}

int XlatY[XLATTABSIZE] = { 0 };
int XlatV_B[XLATTABSIZE] = { 0 };
int XlatV_G[XLATTABSIZE] = { 0 };
int XlatU_G[XLATTABSIZE] = { 0 };
int XlatU_R[XLATTABSIZE] = { 0 };

#define MIN(a,b) ((a)>(b) ? (b):(a))
#define MAX(a,b) ((a)>(b) ? (a):(b))

static void inline init_yuvtable (void)
{
	int i, j;

	for (i = 0; i < XLATTABSIZE; i++) {
#if ORIG_XLAT
		j = MIN(253, MAX(16, i));
#else
		j = (255 * i + 110) / 220;	// scale up
		j = MIN(255, MAX(j, 16));
#endif
		// orig: XlatY[i] = (int ) j;
		XlatY[i] = j-16;
	}

	for (i = 0; i < XLATTABSIZE; i++) {
#if ORIG_XLAT
		j = MIN(240, MAX(16, i));
		j -= 128;
#else
		j = i - 128;		// make signed
		if (j < 0)
			j++;			// noise reduction
		j = (127 * j + 56) / 112;	// scale up
		j = MIN(127, MAX(-128, j));
#endif

		XlatV_B[i] = MulDiv (j, 1000, 564);	/* j*219/126 */
		XlatV_G[i] = MulDiv (j, 1100, 3328);
		XlatU_G[i] = MulDiv (j, 3100, 4207);
		XlatU_R[i] = MulDiv (j, 1000, 713);
	}
}

void inline yuv_convert_rgb24(unsigned char *rawY, unsigned char *rawU, 
		unsigned char *rawV, unsigned char *rgb, int size)
{
	unsigned short  buf1, buf3;
	int   red;
	int   blue;
	int   green;
	unsigned long   cnt;
	int    Y, U, V;

	for ( cnt = 0 ; cnt < size; cnt +=2){
		buf1 = *(rawY+cnt) & 0xff;  // Y data
		buf3 = *(rawY+cnt+1) & 0xff;  // Y data

		U = *(rawV+cnt/2) & 0xff;
		V = *(rawU+cnt/2) & 0xff;

#if MORE_QUALITY
		Y = buf1;
#else
		Y = ((buf1+buf3)/2);
#endif

		red = XlatY[Y] + XlatU_R[U];
		CLIP(red);
		green = XlatY[Y] - XlatV_G[V] - XlatU_G[U];
		CLIP(green);
		blue = XlatY[Y] + XlatV_B[V];
		CLIP(blue);

		*rgb ++ = red & 0xff;
		*rgb ++ = green & 0xff;
		*rgb ++ = blue & 0xff;

#if MORE_QUALITY
		Y = buf3;
		red = XlatY[Y] + XlatU_R[U];
		CLIP(red);
		green = XlatY[Y] - XlatV_G[V] - XlatU_G[U];
		CLIP(green);
		blue = XlatY[Y] + XlatV_B[V];
		CLIP(blue);
#endif
		*rgb ++ = red & 0xff;
		*rgb ++ = green & 0xff;
		*rgb ++ = blue & 0xff;
	}
}
static void v4l2_save_yuv420_to_jpg(int fd)
{
	char yuv420buf[640*480*2];
	char rgb24buf[640*480*3];
	char *yuv420 = &yuv420buf[0];
	char *rgb24 = &rgb24buf[0];
	FILE *jpg_fp = NULL;
	int i;

	JSAMPROW row_pointer[480];

	for (i=0; i<480; i++)
		row_pointer[i] = (JSAMPROW)&rgb24buf[640*i*3];

	if ((read (fd, &yuv420buf, 640*480*2)) < 0) {
		perror("read");
		return;
	}

	/*
	 * work-arround for CPU bug
	 *
	 * retry
	 */
	if ((read (fd, &yuv420buf, 640*480*2)) < 0) {
		perror("read");
		return;
	}

	jpg_fp = fopen(YUV420_FILE, "wb");
	if (!jpg_fp) {
		perror(YUV420_FILE);
		return;
	}

	cinfo.err = jpeg_std_error(&jerr);
	jpeg_create_compress(&cinfo);

	jpeg_stdio_dest(&cinfo, jpg_fp);

	cinfo.image_width = 640;
	cinfo.image_height = 480;
	cinfo.input_components = 3;
	cinfo.in_color_space = JCS_RGB;

	jpeg_set_defaults(&cinfo);

	jpeg_set_quality(&cinfo, quality, TRUE);
	
	jpeg_start_compress(&cinfo, TRUE);

	/* YUV420 -> RGB24 */
	yuv_convert_rgb24( 
			yuv420, yuv420 + 640*480, yuv420 + 640*480/2*3,
			rgb24, 640*480);

	jpeg_write_scanlines(&cinfo, row_pointer, 480);

	jpeg_finish_compress(&cinfo);
	jpeg_destroy_compress(&cinfo);

	if (jpg_fp)
		fclose(jpg_fp);

	printf("   save to \"%s\"\n", YUV420_FILE);
}

void v4l2_adjust_hw_to_default(int fd, V_QRYCTRL *qc)
{
	int i;
	V_QRYCTRL *qc_0 = qc;
	struct v4l2_control vc;

	/* integer */
	for (i=0; i<CTRL_NUM; i++) {
		if (!(qc->id))
			break;
		if (qc->type == V4L2_CTRL_TYPE_INTEGER) {

			vc.id = qc->id;

#if 0
			vc.value = qc->minimum;
			if (ioctl(fd, VIDIOC_S_CTRL, &vc) < 0) {
				perror("VIDIOC_S_CTRL");
				continue;
			}
			vc.value = qc->maximum;
			if (ioctl(fd, VIDIOC_S_CTRL, &vc) < 0) {
				perror("VIDIOC_S_CTRL");
				continue;
			}
#endif
			vc.value = qc->default_value;
			if (ioctl(fd, VIDIOC_S_CTRL, &vc) < 0) {
				perror("VIDIOC_S_CTRL");
				continue;
			}
			printf("   set [%s] to %d\n", qc->name, vc.value);
		}
		qc++;
	}

	/* boolean */
	qc = qc_0;
	for (i=0; i<CTRL_NUM; i++) {
		if (!(qc->id))
			break;
		vc.id = qc->id;
		if (qc->type == V4L2_CTRL_TYPE_BOOLEAN) {
			vc.value = qc->default_value;
			if (ioctl(fd, VIDIOC_S_CTRL, &vc) < 0) {
				perror("VIDIOC_S_CTRL");
				continue;
			}
			printf("   set [%s] to %s\n", qc->name, vc.value ? "TRUE":"FALSE");
		}
		qc++;
	}
}

static unsigned int fb_grab(int fd, char **fbmem)
{
	F_VINFO modeinfo;
	unsigned int length;

	if (ioctl(fd, FBIOGET_VSCREENINFO, &modeinfo) < 0) {
		perror("FBIOGET_VSCREENINFO");
		exit (EXIT_FAILURE);
	}
	length = modeinfo.xres * modeinfo.yres * (modeinfo.bits_per_pixel >> 3);

	printf("  %d x %d, %d bpp\n",
			modeinfo.xres, modeinfo.yres, modeinfo.bits_per_pixel);

	*fbmem = mmap(0, length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
	if (*fbmem < 0) {
		perror("mmap()");
		length = 0;
	}

	return length;
}

static void fb_ungrab(char **fbmem, unsigned int length)
{
	if (*fbmem)
		munmap(*fbmem, length);
}


int main(int argc, char *argv[])
{
	int v4l2_fd = -1;
	int fb_fd = -1;
	char *fbmem = NULL;
	unsigned int fb_length = 0;
	V_FORMAT fmt;
	V_INPUT inp[INPUT_NUM];
	V_QRYCTRL qc[CTRL_NUM];
	V_FMTDESC pix[PIXFMT_NUM];
	int preview_frames = 180;
	int tmp;

	if (argc > 1) {
		if (sscanf(argv[1], "%d", &tmp) == 1)
			preview_frames = tmp;
	}

	printf("Start Main \n");
	v4l2_fd = open(V4L2_DEV_NODE, O_RDWR);
	if (v4l2_fd < 0) {
		perror(V4L2_DEV_NODE);
		goto out;
	}

	printf("Before openning FB \n");
	fb_fd = open(FB_DEV_NODE, O_RDWR);
	if (fb_fd < 0) {
		perror(FB_DEV_NODE);
		goto out;
	}

	printf("\n");
	printf("======================\n");
	printf("FB : Driver Capacities\n");
	printf("======================\n");
	fflush(stdout);
	if ((fb_length = fb_grab(fb_fd, &fbmem)) == 0)
		goto out;
	memset(fbmem, 0, fb_length);

	printf("\n");
	printf("========================\n");
	printf("V4L2 : Driver Capacities\n");
	printf("========================\n");
	fflush(stdout);
	v4l2_print_settings(v4l2_fd, &inp[0], &fmt, &pix[0], &qc[0]);

	printf("\n");

	printf("===================================\n");
	printf("V4L2 : Adjust Camera H/W to default\n");
	printf("===================================\n");
	fflush(stdout);
	v4l2_adjust_hw_to_default(v4l2_fd, &qc[0]);

	printf("\n");

	printf("============================\n");
	printf("V4L2 : Configure for Preview\n");
	printf("============================\n");
	fflush(stdout);
	v4l2_config_for_preview(v4l2_fd, &fmt, &pix[0]);

	printf("\n");

	printf("===============================\n");
	printf("V4L2 : Show %d frames on LCD\n", preview_frames);
	printf("===============================\n");
	fflush(stdout);
	v4l2_show_on_fb(v4l2_fd, fbmem, preview_frames);
	
	printf("\n");

	printf("===================================\n");
	printf("V4L2 : Configure for YUV420 Capture\n");
	printf("===================================\n");
	fflush(stdout);
	v4l2_config_for_yuv420_capture(v4l2_fd, &fmt, &pix[0]);

	printf("\n");

	printf("=====================================\n");
	printf("V4L2 : Capture 1 YUV420 frame to JPEG\n");
	printf("=====================================\n");
	fflush(stdout);
	init_yuvtable();
	v4l2_save_yuv420_to_jpg(v4l2_fd);
	
	printf("\n");

out:

	if (v4l2_fd > 0)
		close(v4l2_fd);

	fb_ungrab(&fbmem, fb_length);

	if (fb_fd > 0)
		close(fb_fd);

	return 0;
}