v4l2.c

2440mmc-and-camera-linux-driver 2440mmc-and-camera-linux-driver

/*
 * TEST V4L2
 *
 *   [e-mail]
 *
 *   preview : 240x160 overlay on 240x320 16bpp LCD
 *
 *   capture : 640x480
 *
 *
 */
#include <sys/time.h>
#include <sys/types.h>
#include <asm/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/mman.h>
#include <errno.h>

#include <jpeglib.h>

#include <linux/fs.h>
#include <linux/kernel.h>
#include "videodev.h"
#include <linux/fb.h>

#define PIXFMT_NUM 5
#define INPUT_NUM 5
#define CTRL_NUM 100

#define V4L2_DEV_NODE "/dev/v4l2/capture0"
#define FB_DEV_NODE "/dev/fb/0"

#define RGB_FILE "rgb.jpg"
#define YUV420_FILE "yuv420.jpg"

typedef struct v4l2_input    V_INPUT;
typedef struct v4l2_format   V_FORMAT;
typedef struct v4l2_fmtdesc  V_FMTDESC;
typedef struct v4l2_queryctrl V_QRYCTRL;

typedef struct fb_var_screeninfo F_VINFO;

static struct jpeg_compress_struct cinfo;
static int quality = 100;
static struct jpeg_error_mgr jerr;

static char *name_of_ctrl_type[] = {
	[V4L2_CTRL_TYPE_INTEGER] = "INTEGER",
	[V4L2_CTRL_TYPE_BOOLEAN] = "BOOLEAN",
	[V4L2_CTRL_TYPE_MENU] = "MENU",
	[V4L2_CTRL_TYPE_BUTTON] = "BUTTON",
};

static void v4l2_print_settings(int fd, 
		V_INPUT *inp, V_FORMAT *fmt, V_FMTDESC *pix, V_QRYCTRL *qc)
{
	int i;
	int pixnum = 0;
	int inputnum = 0;
	int ctrlnum = 0;
	V_INPUT *inp_0 = inp;
	V_FMTDESC *pix_0 = pix;
	V_QRYCTRL *qc_0 = qc;

	for (i = 0; i < INPUT_NUM; i++) {
		inp->index = i;
		if (ioctl(fd, VIDIOC_ENUMINPUT, inp) < 0) {
			break;
		}
		inp++;
		inputnum++;
	}

	for (i = 0; i < PIXFMT_NUM; i++) {
		pix->index = i;
		if (ioctl(fd, VIDIOC_ENUM_PIXFMT, pix) < 0)
			break;
		pix++;
		pixnum++;
	}

	for (i = 0; i < CTRL_NUM; i++) {
		qc->id = V4L2_CID_BASE + i;	
		if (ioctl(fd, VIDIOC_QUERYCTRL, qc) < 0)
			break;
		if (!(qc->flags & (V4L2_CTRL_FLAG_DISABLED | V4L2_CTRL_FLAG_GRABBED))) {
			ctrlnum++;
			qc++;
		}
	}
	for (i = 0; i < CTRL_NUM; i++) {
		qc->id = V4L2_CID_PRIVATE_BASE + i;
		if (ioctl(fd, VIDIOC_QUERYCTRL, qc) < 0)
			break;
		if (!(qc->flags & (V4L2_CTRL_FLAG_DISABLED | V4L2_CTRL_FLAG_GRABBED))) {
			ctrlnum++;
			qc++;
		}
	}
	qc->id = 0;

	fmt->type = V4L2_BUF_TYPE_CAPTURE;
	if (ioctl(fd, VIDIOC_G_FMT, fmt) < 0)
		perror("getting video format");


	printf("   available inputs are:\n");
	inp = inp_0;
	for (i = 0; i < inputnum; i++) {
		printf("     [%02d] - %s\n", inp->index, inp->name);
		inp++;
	}
	printf("\n");

	printf("   available fomats are:\n");
	pix = pix_0;
	for (i = 0; i < pixnum; i++) {
		printf("     [%02d] - %-15s (%d bpp), 0x%x\n",
		       pix->index, pix->description, pix->depth, pix->pixelformat);
		pix++;
	}
	printf("\n");

	printf("   available controls are:\n");
	qc = qc_0;
	for (i = 0; i < ctrlnum; i++) {
		printf("     [%2d] - \"%s\" [%7s] %d~%d (step:%d default:%d)\n", i
				, qc->name
				, name_of_ctrl_type[qc->type]
				, qc->minimum, qc->maximum, qc->step, qc->default_value);
		qc++;
	}
	printf("\n");

	printf("   driver's current selection is:\n");
	inp = inp_0;
	printf("     %d x %d @ %d bpp from input [%d] (%s)\n",
	       fmt->fmt.pix.width, fmt->fmt.pix.height, fmt->fmt.pix.depth, 
		   inp->index, inp->name);
	printf("\n");
}

static void v4l2_config_for_preview(int fd, V_FORMAT *fmt, V_FMTDESC *pix)
{
	int i;
	V_FORMAT fmt_req;
	V_FORMAT fmt_ret;

	for (i=0; i < PIXFMT_NUM; i++) {
		if (pix->pixelformat == V4L2_PIX_FMT_RGB565)
			break;
		pix++;
	}
	if (i == PIXFMT_NUM) {
		printf("there is not pixel format for preview\n");	
		exit (EXIT_FAILURE);
	}

	memcpy(&fmt_req, fmt, sizeof(V_FORMAT));
	fmt_req.type = V4L2_BUF_TYPE_CAPTURE;
	fmt_req.fmt.pix.width = 240;
	fmt_req.fmt.pix.height = 160;
	fmt_req.fmt.pix.depth = 16;
	fmt_req.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565;

	if (ioctl(fd, VIDIOC_S_FMT, &fmt_req) < 0) {
		perror("VIDIOC_S_FMT");
		exit (EXIT_FAILURE);
	}

	fmt_ret.type = V4L2_BUF_TYPE_CAPTURE;
	if (ioctl(fd, VIDIOC_G_FMT, &fmt_ret) < 0)
		perror("getting video format");

	printf("   driver's modified selection is:\n");
	printf("     %d x %d @ %d bpp, RGB565\n",
	       fmt_ret.fmt.pix.width, fmt_ret.fmt.pix.height, 
		   fmt_ret.fmt.pix.depth);
	printf("\n");
}

static void v4l2_config_for_rgb_capture(int fd, V_FORMAT *fmt, V_FMTDESC *pix)
{
	int i;
	V_FORMAT fmt_req;
	V_FORMAT fmt_ret;

	for (i=0; i < PIXFMT_NUM; i++) {
		if (pix->pixelformat == V4L2_PIX_FMT_RGB32)
			break;
		pix++;
	}
	if (i == PIXFMT_NUM) {
		printf("there is not pixel format for capture\n");	
		exit (EXIT_FAILURE);
	}

	memcpy(&fmt_req, fmt, sizeof(V_FORMAT));
	fmt_req.type = V4L2_BUF_TYPE_CAPTURE;
	fmt_req.fmt.pix.width = 640;
	fmt_req.fmt.pix.height = 480;
	fmt_req.fmt.pix.depth = 32;
	fmt_req.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB32;

	if (ioctl(fd, VIDIOC_S_FMT, &fmt_req) < 0) {
		perror("VIDIOC_S_FMT");
		exit (EXIT_FAILURE);
	}

	fmt_ret.type = V4L2_BUF_TYPE_CAPTURE;
	if (ioctl(fd, VIDIOC_G_FMT, &fmt_ret) < 0)
		perror("getting video format");

	printf("   driver's modified selection is:\n");
	printf("     %d x %d @ %d bpp, RGB32\n",
	       fmt_ret.fmt.pix.width, fmt_ret.fmt.pix.height, 
		   fmt_ret.fmt.pix.depth);
	printf("\n");
}

static void v4l2_config_for_yuv420_capture(int fd, V_FORMAT *fmt, V_FMTDESC *pix)
{
	int i;
	V_FORMAT fmt_req;
	V_FORMAT fmt_ret;

	for (i=0; i < PIXFMT_NUM; i++) {
		if (pix->pixelformat == V4L2_PIX_FMT_YUV420)
			break;
		pix++;
	}
	if (i == PIXFMT_NUM) {
		printf("there is not pixel format for capture\n");	
		exit (EXIT_FAILURE);
	}

	memcpy(&fmt_req, fmt, sizeof(V_FORMAT));
	fmt_req.type = V4L2_BUF_TYPE_CAPTURE;
	fmt_req.fmt.pix.width = 640;
	fmt_req.fmt.pix.height = 480;
	fmt_req.fmt.pix.depth = 12;
	fmt_req.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;

	if (ioctl(fd, VIDIOC_S_FMT, &fmt_req) < 0) {
		perror("VIDIOC_S_FMT");
		exit (EXIT_FAILURE);
	}

	fmt_ret.type = V4L2_BUF_TYPE_CAPTURE;
	if (ioctl(fd, VIDIOC_G_FMT, &fmt_ret) < 0)
		perror("getting video format");

	printf("   driver's modified selection is:\n");
	printf("     %d x %d @ %d bpp, YUV420\n",
	       fmt_ret.fmt.pix.width, fmt_ret.fmt.pix.height, 
		   fmt_ret.fmt.pix.depth);
	printf("\n");
}

static void v4l2_show_on_fb(int fd, char *fbmem, int frames)
{
	int i;
	int ret;
	char preview_buf[240*160*2];

	for (i = 0; i < frames; i++) {
		if ((ret = read (fd, &preview_buf, 240*160*2)) < 0) {
			perror("read");
			return;
		}
#if 0
		{
		int y;
		for (y = 0; y < 160; y++) 
			memcpy(fbmem + 240*2*y, preview_buf + 240*2*y, 240*2);
		}
#else
		memcpy(fbmem, &preview_buf, 240*160*2);
#endif
		printf("\r%3d", i);
		fflush(stdout);
	}

	printf("\n");
}

#if 0
static void v4l2_save_rgb32_to_jpg(int fd)
{
	char rgb32buf[640*480*4];
	unsigned int y,x = 0;
	FILE *jpg_fp = NULL;
	unsigned long *rgb32 = (unsigned long*)&rgb32buf[0];
	unsigned long *rgb32_p = rgb32;
	unsigned char *rgb24 = &rgb32buf[0];
	unsigned long tmp;
	char *base;
	JSAMPROW row_pointer[1] = {(JSAMPROW)rgb24};

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

	jpg_fp = fopen(RGB_FILE, "wb");
	if (!jpg_fp) {
		perror(RGB_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);

	/* xRGB -> RGB */
	for (y = 0; y < 480; y++) {
		rgb32_p = rgb32 + y*640;
		base = rgb24;
		for (x = 0; x < 640; x++) {
			tmp = *(rgb32_p + x);
			*base++ = (tmp >> 16) & 0xff;
			*base++ = (tmp >>  8) & 0xff;
			*base++ = (tmp >>  0) & 0xff;
		}
		jpeg_write_scanlines(&cinfo, row_pointer, 1);
	}

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

	if (jpg_fp)
		fclose(jpg_fp);

	printf("   save to \"%s\"\n", RGB_FILE);
}
#endif


static inline void v4l2_save_rgb16_to_jpg(int cam_fp, int width, int height)
{
	int x, y;
	FILE *jpg_fp = NULL;
	char file_name[100];
	unsigned char *rgb24 = NULL;
	unsigned short *rgb16 = NULL;
	JSAMPROW row_pointer[1];

	rgb24 = (unsigned char*)malloc(width*3);
	rgb16 = (unsigned short*)malloc(width*height*2);
	row_pointer[0] = (JSAMPROW)rgb24;

	 /* read from device */
	if (read(cam_fp, (char*)rgb16, width*height*2) < 0) {
		perror("read()");
		goto err;
	}

	sprintf(&file_name[0], "rgb.jpg");
	/* file create/open, note to "wb" */
	jpg_fp = fopen(&file_name[0], "wb");
	if (!jpg_fp) {
		perror(&file_name[0]);
		goto err;
	}

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

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