marvell/services/camera/camera.c

/* 
*	V4L2 video capture example
* 
*	This program is provided with the V4L2 API
* 
*	Copyright (C) 2023 ASR Microelectronics Co., Ltd.
*/

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <assert.h>
#include <getopt.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <errno.h>
#include <linux/videodev2.h>
#include <sys/mman.h>
#include <sys/time.h>

#define TRUE            (1)
#define FALSE           (0)

#define IMAGE           "/tmp/camera/image"
#define DIR           "/tmp/camera"

#define IMAGEWIDTH      640
#define IMAGEHEIGHT     480
#define FRAME_NUM       4

int fd;
struct v4l2_buffer buf;
static int frame_count = 4;
static char *dev_name = "/dev/video0";

struct buffer
{
	void * start;
	unsigned int length;
	long long int timestamp;
} *buffers;

static int v4l2_init(void)
{
	struct v4l2_capability cap;
	struct v4l2_fmtdesc fmtdesc;
	struct v4l2_format fmt;

	if ((fd = open(dev_name, O_RDWR)) == -1) 
	{
		printf("Error opening V4L interface\n");
		exit(EXIT_FAILURE);
	}

	if (ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) 
	{
		printf("Error opening device %s: unable to query device.\n",dev_name);
		exit(EXIT_FAILURE);
	}
	else
	{
		printf("driver:\t\t%s\n",cap.driver);
		printf("card:\t\t%s\n",cap.card);
		printf("bus_info:\t%s\n",cap.bus_info);
		printf("capabilities:\t%x\n",cap.capabilities);
		
		if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == V4L2_CAP_VIDEO_CAPTURE) 
		{
			printf("Device %s: supports capture.\n",dev_name);
		}

		if ((cap.capabilities & V4L2_CAP_STREAMING) == V4L2_CAP_STREAMING) 
		{
			printf("Device %s: supports streaming.\n",dev_name);
		}
	}

	fmtdesc.index=0;
	fmtdesc.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
	printf("Support format:\r\n");
	while(ioctl(fd,VIDIOC_ENUM_FMT,&fmtdesc)!=-1)
	{
		printf("\t%d.%s, 0x%x\r\n",fmtdesc.index+1,fmtdesc.description, fmtdesc.pixelformat);
		fmtdesc.index++;
	}

	struct v4l2_format fmt_test;
	fmt_test.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt_test.fmt.pix.pixelformat=V4L2_PIX_FMT_YVYU;
	if(ioctl(fd,VIDIOC_TRY_FMT,&fmt_test)==-1)
	{
		printf("not support format YVYU!\n");
	}
	else
	{
		printf("support format YVYU, 0x%x\r\n", V4L2_PIX_FMT_YVYU);
	}

	printf("set fmt...\r\n");
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YVYU;

	fmt.fmt.pix.height = IMAGEHEIGHT;
	fmt.fmt.pix.width = IMAGEWIDTH;
	fmt.fmt.pix.field = V4L2_FIELD_NONE;
	printf("fmt.type:\t\t%d\n", fmt.type);
	printf("pix.pixelformat:\t%c%c%c%c\n", fmt.fmt.pix.pixelformat & 0xFF,
			 (fmt.fmt.pix.pixelformat >> 8) & 0xFF, (fmt.fmt.pix.pixelformat >> 16) & 0xFF,
			 (fmt.fmt.pix.pixelformat >> 24) & 0xFF);
	printf("pix.height:\t\t%d\n", fmt.fmt.pix.height);
	printf("pix.width:\t\t%d\n", fmt.fmt.pix.width);
	printf("pix.field:\t\t%d\n", fmt.fmt.pix.field);
	if(ioctl(fd, VIDIOC_S_FMT, &fmt) == -1)
	{
		printf("Unable to set format\n");
		exit(EXIT_FAILURE);
	}

	printf("get fmt...\r\n");
	if(ioctl(fd, VIDIOC_G_FMT, &fmt) == -1)
	{
		printf("Unable to get format\n");
		exit(EXIT_FAILURE);
	}

	printf("fmt.type:\t\t%d\n",fmt.type);
	printf("pix.pixelformat:\t%c%c%c%c\n", fmt.fmt.pix.pixelformat & 0xFF,
		 (fmt.fmt.pix.pixelformat >> 8) & 0xFF, (fmt.fmt.pix.pixelformat >> 16) & 0xFF,
		 (fmt.fmt.pix.pixelformat >> 24) & 0xFF);
	printf("pix.height:\t\t%d\n", fmt.fmt.pix.height);
	printf("pix.width:\t\t%d\n", fmt.fmt.pix.width);
	printf("pix.field:\t\t%d\n", fmt.fmt.pix.field);

	return TRUE;
}

static int v4l2_mem_ops(void)
{
	unsigned int n_buffers;
	struct v4l2_requestbuffers req;
	
	req.count = FRAME_NUM;
	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory = V4L2_MEMORY_MMAP;
	if(ioctl(fd, VIDIOC_REQBUFS, &req)==-1)
	{
		printf("request for buffers error\n");
		return FALSE;
	}

	buffers = malloc(req.count * sizeof(*buffers));
	if (!buffers)
	{
		printf ("out of memory!\n");
		exit(EXIT_FAILURE);
	}
	
	for (n_buffers = 0; n_buffers < FRAME_NUM; n_buffers++)
	{
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index = n_buffers;

		if (ioctl (fd, VIDIOC_QUERYBUF, &buf) == -1)
		{
			printf("query buffer error\n");
			exit(EXIT_FAILURE);
		}

		buffers[n_buffers].length = buf.length;
		buffers[n_buffers].start = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
		if (buffers[n_buffers].start == MAP_FAILED)
		{
			printf("buffer map error\n");
			exit(EXIT_FAILURE);
		}
	}

	return TRUE;	
}

static int v4l2_frame_process(void)
{
	unsigned int n_buffers;
	enum v4l2_buf_type type;
	char file_name[100];
	char index_str[10];
	long long int extra_time = 0;
	long long int cur_time = 0;
	long long int last_time = 0;

	if(access(DIR, 0) !=0 )
	{
		if(mkdir(DIR, 0755)==-1)
		{
			printf("mkdir %s error\n", DIR);
			return FALSE;
		}
	}

	for (n_buffers = 0; n_buffers < FRAME_NUM; n_buffers++)
	{
		buf.index = n_buffers;
		ioctl(fd, VIDIOC_QBUF, &buf);
	}
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ioctl(fd, VIDIOC_STREAMON, &type);

	int loop = 0;
	while(loop < frame_count)
	{
		n_buffers = loop % FRAME_NUM;
		fd_set fds;
		struct timeval tv;
		int r;

		FD_ZERO(&fds);
		FD_SET(fd, &fds);

		tv.tv_sec = 2;
		tv.tv_usec = 0;

		r = select(fd + 1, &fds, NULL, NULL, &tv);
		if (-1 == r) {
			if (EINTR == errno)
				continue;
			printf("select failed, err: %d\n", errno);
			return FALSE;
		}

		if (0 == r) {
			printf("select failed, err: %d\n", errno);
			return FALSE;
		}

		buf.index = n_buffers;
		ioctl(fd, VIDIOC_DQBUF, &buf);

		buffers[n_buffers].timestamp = buf.timestamp.tv_sec*1000000 + buf.timestamp.tv_usec;
		cur_time = buffers[n_buffers].timestamp;
		extra_time = cur_time - last_time;
		last_time = cur_time;
		printf("time_deta:%lld\n\n", extra_time);

		memset(file_name, 0, sizeof(file_name));
		memset(index_str, 0, sizeof(index_str));
		sprintf(index_str, "%d", loop%2);
		strcpy(file_name, IMAGE);
		strcat(file_name, index_str);
		strcat(file_name, ".jpg");

		FILE *fp2 = fopen(file_name, "wb");
		if(!fp2)
		{
			printf("open %s error\r\n",file_name);
			return(FALSE);
		}
		fwrite(buffers[n_buffers].start, buffers[n_buffers].length, 1, fp2);
		fclose(fp2);
		printf("save %s OK\r\n", file_name);

		ioctl(fd, VIDIOC_QBUF, &buf);

		loop++;
	}
	return TRUE;	
}

static int v4l2_release(void)
{
	unsigned int n_buffers;
	enum v4l2_buf_type type;

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ioctl(fd, VIDIOC_STREAMOFF, &type);
	
	for(n_buffers=0;n_buffers<FRAME_NUM;n_buffers++)
	{
		munmap(buffers[n_buffers].start,buffers[n_buffers].length);
	}

	free(buffers);
	
	close(fd);
	return TRUE;
}

static void usage(FILE *fp, char **argv)
{
		fprintf(fp,
				 "Usage: %s [options]\n\n"
				 "Options:\n"
				 "-d | --device name Video device name [%s]\n"
				 "-h | --help		 Print this message\n"
				 "-c | --count		 Number of frames to grab [%i]\n"
				 "",
				 argv[0], dev_name, frame_count);
}

static const char short_options[] = "d:hc:";

static const struct option
long_options[] = {
		{ "device", required_argument, NULL, 'd' },
		{ "help", no_argument, NULL, 'h' },
		{ "count", required_argument, NULL, 'c' },
		{ 0, 0, 0, 0 }
};


int main(int argc, char **argv)
{
	dev_name = "/dev/video0";
	
	for (;;) {
		int idx;
		int c;

		c = getopt_long(argc, argv,
				short_options, long_options, &idx);

		if (-1 == c)
			break;

		switch (c) {
		case 0:
			break;

		case 'd':
			dev_name = optarg;
			break;
		case 'h':
			usage(stdout, argv);
			exit(EXIT_SUCCESS);
		case 'c':
			frame_count = strtol(optarg, NULL, 0);
			if (errno)
				exit(EXIT_FAILURE);
			break;

		default:
			usage(stderr, argv);
			exit(EXIT_FAILURE);
		}
	}

	v4l2_init();

	v4l2_mem_ops();

	v4l2_frame_process();

	v4l2_release();
	
	return TRUE;
}