newtx/hardware/cis/gvideo.cpp

#include "gvideo.h"
#include <iostream>
#include <fcntl.h> /* low-level i/o */
#include <unistd.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <dlfcn.h>
#include <linux/videodev2.h>
#include "linux/v4l2-subdev.h"

#include <cstring>
#include <poll.h>
#include <malloc.h>

#include <base/utils.h>

static const std::string loggername = "gVideo";

using namespace std;
#define CLEAR(x) memset(&(x), 0, sizeof(x))
#define LOG printf

int file_size(const char* filename)
{
	struct stat statbuf;
	int ret = stat(filename, &statbuf);
	int size = -1;
	if (ret == 0)
		size = statbuf.st_size;
	return size;
}

gVideo::gVideo()
		: fd(-1),
		  buffers(0),
		  bStart(false) ,
          dev_name("/dev/video3")
{
}

gVideo::~gVideo()
{
	stop();
	close();
}

bool gVideo::is_open()
{
	return fd >= 0;
}

void gVideo::close()
 {
	if (is_open()) {
		stop();
		uninit_device();
		close_device();
	}
}

void gVideo::open(int width,int height) {
	if (is_open())
		return;

	open_device();
	if (is_open()) {
		//init_device();
		set_size(width, height);
		start();
	}
}

void gVideo::start() 
{
	std::lock_guard<std::mutex> lck(m_lock);
	if (bStart)
		return;

	start_capturing();
	bStart = true;
}

void gVideo::stop() {
	std::lock_guard<std::mutex> lck(m_lock);
	if (bStart) {
		stop_capturing();
		bStart = false;
	}
}

void gVideo::grab(int color, bool bcrrect, int timeout, int flatpara)
{
}

bool gVideo::hasframe() {
	return wait(0);
}

bool gVideo::wait(int msTimeout) {
	pollfd pfd;
	pfd.fd = this->fd;
	pfd.events = POLLIN | POLLRDNORM;

	if (poll(&pfd, 1, msTimeout) > 0) {
		return pfd.revents;
	}
	return false;
}

void* gVideo::read_frame(int timeout, size_t& size, int& ind) {
	size = 0;
	ind = -1;
	if (!wait(timeout)){
		utils::to_log(LOG_LEVEL_FATAL, "read frame time out\n");
		return 0;
	}
	v4l2_buffer buf;
	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_USERPTR;
	if (-1 == ioctl(fd, VIDIOC_DQBUF, &buf)) {
		LOG_ERROR(utils::format_string("VIDIOC_DQBUF  error!!! index :%d\n", buf.index));
		return 0;
	}
	// move to release_v4l2_memory method ...
	// if (-1 == ioctl(fd, VIDIOC_QBUF, &buf)) {
	// 	LOG_ERROR(utils::format_string("VIDIOC_QBUF error!!! index :%d\n", buf.index));
	// 	return 0;
	// }
	ind = buf.index;
	size = buffers[ind].length;
	
	return buffers[ind].start;
}
int gVideo::add_v4l2_memory(int ind)
{
	v4l2_buffer buf;
	int			ret = 0;
	struct 		v4l2_plane planes[VIDEO_MAX_PLANES];

	CLEAR(buf);
	CLEAR(planes);
	for(int j = 0; j <  VIDEO_MAX_PLANES; j++)
	{
		planes[j].length = buf_size_;
		planes[j].m.userptr =(unsigned long)buffers[ind].start;
	}
	buf.index = ind;
	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
	buf.m.planes = planes;
	buf.length = 1;
	if (-1 == ioctl(fd, VIDIOC_QBUF, &buf))
	{
		ret = errno;
		utils::to_log(LOG_LEVEL_FATAL, "VIDIOC_QBUF(%d) failed: %s\n", ind, strerror(ret));
	}

	return ret;
}
int gVideo::get_mem_count(void)
{
	return n_buffers;
}

void gVideo::set_size(int width,int height){
	if(!buffers.empty())
		uninit_device();
		
	v4l_width=width;
	v4l_height=height;
	init_device();
}

void gVideo::stop_capturing(void) {
	enum v4l2_buf_type type;

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (-1 == ioctl(fd, VIDIOC_STREAMOFF, &type))
		utils::to_log(LOG_LEVEL_FATAL, "streamo off\n");
}


void gVideo::start_capturing(void)
{
	enum v4l2_buf_type type;
	printf("start_capturing::buf_size_  = %d.\n",buf_size_);
	for (int i = 0; i < n_buffers; ++i)
	{
		struct v4l2_buffer buf;
		struct v4l2_plane planes[VIDEO_MAX_PLANES];
		CLEAR(buf);
		CLEAR(planes);
		for(int j = 0; j <  VIDEO_MAX_PLANES; j++)
		{
			//planes[j].length = buffers[i].length;
			planes[j].length = buf_size_;
			planes[j].m.userptr =(unsigned long)buffers[i].start;
		}
		buf.index = i;
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
		buf.m.planes = planes;
		buf.length = 1;

		if (-1 == ioctl(fd, VIDIOC_QBUF, &buf))
			LOG_ERROR(utils::format_string("VIDIOC_QBUF Error %d\n", i));
		else
		    LOG_ERROR(utils::format_string("VIDIOC_QBUF %d\n", i));
	}

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (-1 == ioctl(fd, VIDIOC_STREAMON, &type))
		utils::to_log(LOG_LEVEL_FATAL, "VIDIOC_STREAMON\n");
}

void gVideo::uninit_device(void)
{
	if (!buffers.empty())
	{
		struct v4l2_requestbuffers req;
		CLEAR(req);
		req.count = 0;
		req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		req.memory = V4L2_MEMORY_USERPTR;

		if (-1 == ioctl(fd, VIDIOC_REQBUFS, &req))
		{
			if (EINVAL == errno)
				utils::to_log(LOG_LEVEL_FATAL, "does not support user pointer i/o\n");
			else
				utils::to_log(LOG_LEVEL_FATAL, "VIDIOC_REQBUFS\n");
		}
		else
		{
			LOG_TRACE(utils::format_string("rest frame count: %d\n", req.count));
		}

		for (int i = 0; i < n_buffers; ++i)
			free(buffers[i].start);

		buffers.clear();
	}
}

void gVideo::init_mmap(void)
{

	struct v4l2_requestbuffers req;
	int ret;
	unsigned int length;
	unsigned int offset;
	unsigned int page_size;
	int buffer_size = v4l_width * v4l_height * 3;
	printf("init_mmap::buduiqi:changdu: buffer_size= %d\n", buffer_size);
	page_size = getpagesize();
	buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
	printf("init_mmap::shijichangdu:v4l_width = %d,v4l_height = %d\n", v4l_width,v4l_height);
	printf("init_mmap::duiqi:page_size = %d,buffer_size = %d\n", page_size,buffer_size);

	CLEAR(req);

	//6.VIDIOC_REQBUFS
	req.count = v4l_buffer_count;
	//req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	//req.memory = V4L2_MEMORY_MMAP;
	req.memory = V4L2_MEMORY_USERPTR;
	printf("set v4l_buffer_count = %d\n", req.count);
	printf("set v4l_buffer_type = %d\n", req.type);
	printf("set v4l_buffer_memory = %d\n", req.memory);

	ret = ioctl(fd, VIDIOC_REQBUFS, &req);
	if (ret < 0) {
		printf("Unable to request buffers: %s (%d).\n", strerror(errno),errno);
	}
	else {
		printf("%s VIDIOC_REQBUFS sucess\n",dev_name.c_str());
	}
	printf("%u buffers requested.\n", req.count);

	buffers.resize(req.count);
	if (buffers.empty()) {
		printf("Out of memory\n");
	}
	
	//7.VIDIOC_QUERYBUF
	for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
		const char *ts_type, *ts_source;
		struct v4l2_buffer buf;
		struct v4l2_plane planes[VIDEO_MAX_PLANES];
		CLEAR(buf);
		CLEAR(planes);
		buf.index = n_buffers;
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		//buf.memory = V4L2_MEMORY_MMAP;
		buf.memory = V4L2_MEMORY_USERPTR;
		buf.length = VIDEO_MAX_PLANES;
		buf.m.planes = planes;

		ret = ioctl(fd, VIDIOC_QUERYBUF, &buf);
		if (ret < 0) {
			printf("Unable to query buffer %u: %s (%d).\n", n_buffers,strerror(errno), errno);

		}
		else {
			printf("index %d VIDIOC_QUERYBUF sucess.\n",n_buffers);

		}
		//get_ts_flags(buf.flags, &ts_type, &ts_source);
		//printf("length: %u offset: %u timestamp type/source: %s/%s\n",
		//      buf.length, buf.m.offset, ts_type, ts_source);

		//mmap
		unsigned int length;
		unsigned int offset;
		length = buf.m.planes[0].length;
		offset = buf.m.planes[0].m.mem_offset;
		buf_size_ = buf.m.planes[0].length;
		printf("VIDIOC_QUERYBUF：Buffer %u length  = %d.\n",n_buffers,length);
		printf("VIDIOC_QUERYBUF：Buffer %u offset  = %d.\n",n_buffers,offset);
		printf("VIDIOC_QUERYBUF：Buffer %u buf_size_  = %d.\n",n_buffers,buf_size_);
	}
	
	for (n_buffers = 0; n_buffers < buffers.size(); ++n_buffers)
	{
		buffers[n_buffers].length = buf_size_;
		buffers[n_buffers].start = memalign(/* boundary */ 1024, buf_size_);
		printf("memalign:buffers[%d].length = %d\n", n_buffers,buffers[n_buffers].length);
		printf("memalign:buffers[%d].start = %p\n", n_buffers,buffers[n_buffers].start);

		if (!buffers[n_buffers].start)
		{
			fprintf(stderr, "Out of memory\n");
			exit(EXIT_FAILURE);
		}
	}
	// unsigned int page_size;
	// int buffer_size = v4l_width * v4l_height * 3;

	// page_size = getpagesize();
	// buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);

	// struct v4l2_requestbuffers req;
	// CLEAR(req);
	// req.count = v4l_buffer_count;
	// LOG_TRACE(utils::format_string("frame count: %d", req.count));
	// req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	// req.memory = V4L2_MEMORY_USERPTR;

	// if (-1 == ioctl(fd, VIDIOC_REQBUFS, &req))
	// {
	// 	if (EINVAL == errno)
	// 		LOG_ERROR("%s does not support user pointer i/o");
	// 	else
	// 		LOG_ERROR("VIDIOC_REQBUFS");
	// }

	// buffers.resize(req.count);

	// for (n_buffers = 0; n_buffers < buffers.size(); ++n_buffers)
	// {
	// 	buffers[n_buffers].length = buffer_size;
	// 	buffers[n_buffers].start = memalign(/* boundary */ page_size, buffer_size);

	// 	if (!buffers[n_buffers].start)
	// 		LOG_ERROR("Out of memory");
	// }

	// struct v4l2_requestbuffers req;
	// CLEAR(req);

	// req.count = v4l_buffer_count;

	// LOG("frame count: %d \n",req.count);
	// req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	// req.memory = V4L2_MEMORY_MMAP;

	// if (-1 == ioctl(fd, VIDIOC_REQBUFS, &req)) {
	// 	if (EINVAL == errno) {
	// 		LOG("%s does not support " "memory mapping\n", dev_name.c_str());
	// 	}
	// 	else {
	// 		LOG("VIDIOC_REQBUFS \n");
	// 	}
	// }

	// LOG("frame count:%d \n" ,req.count);

	// if (req.count < 2) {
	// 	LOG("Insufficient buffer memory on %s\n", dev_name.c_str());
	// }

	// buffers.resize(req.count);
	// LOG("frame count: %d \n", req.count);

	// if (buffers.empty()) {
	// 	LOG("Out of memory\n");
	// }

	// for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
	// 	struct v4l2_buffer buf;
	// 	CLEAR(buf);
	// 	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	// 	buf.memory = V4L2_MEMORY_MMAP;
	// 	buf.index = n_buffers;

	// 	if (-1 == ioctl(fd, VIDIOC_QUERYBUF, &buf))
	// 		LOG("VIDIOC_QUERYBUF \n");
	// 	LOG("mmap buffer len:%d \n",buf.length);
	// 	buffers[n_buffers].length = buf.length;
	// 	buffers[n_buffers].start =
	// 			mmap(NULL /* start anywhere */,
	// 				 buf.length,
	// 				 PROT_READ | PROT_WRITE /* required */,
	// 				 MAP_SHARED /* recommended */,
	// 				 fd, buf.m.offset);

	// 	if (MAP_FAILED == buffers[n_buffers].start){
	// 		LOG("MAP_FAILED!!! %d \n",buf.length);
	// 	}
	// }
}

void printCamInfo(int fd)
{ //获取摄像头所支持的分辨率
	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	struct v4l2_fmtdesc fmt_1;
	struct v4l2_frmsizeenum frmsize;
	struct v4l2_frmivalenum frmival;
	fmt_1.index = 0;
	fmt_1.type = type;
	while (ioctl(fd, VIDIOC_ENUM_FMT, &fmt_1) >= 0)
	{
		frmsize.pixel_format = fmt_1.pixelformat;
		frmsize.index = 0;
		while (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) >= 0)
		{
		 	if(frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE)
				LOG_TRACE(utils::format_string("line:%d %dx%d\n", __LINE__, frmsize.discrete.width, frmsize.discrete.height));
			else if (frmsize.type == V4L2_FRMSIZE_TYPE_STEPWISE)
				LOG_TRACE(utils::format_string("line:%d %dx%d\n", __LINE__, frmsize.discrete.width, frmsize.discrete.height));
			frmsize.index++;
		}
		fmt_1.index++;
	}
}

void gVideo::init_device(void) {
	struct v4l2_capability cap;
	struct v4l2_format fmt;
	printCamInfo(fd);
	if (-1 == ioctl(fd, VIDIOC_QUERYCAP, &cap)) {
		if (EINVAL == errno) 
			LOG_ERROR(utils::format_string("%s is no V4L2 device\n", dev_name.c_str()));
		else 
			utils::to_log(LOG_LEVEL_DEBUG, "VIDIOC_QUERYCAP\n");
	}

	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) 
		LOG_ERROR(utils::format_string("%s is no video capture device\n", dev_name.c_str()));

	if (!(cap.capabilities & V4L2_CAP_STREAMING)) 
		LOG_ERROR(utils::format_string("%s does not support streaming i/o\n", dev_name.c_str()));

	CLEAR(fmt);
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.width = v4l_width;
	fmt.fmt.pix.height = v4l_height;
	LOG_TRACE(utils::format_string("v4l2 init_device width:%d, height:%d\n", fmt.fmt.pix.width, fmt.fmt.pix.height));
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
	fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;

	if (-1 == ioctl(fd, VIDIOC_S_FMT, &fmt))
		utils::to_log(LOG_LEVEL_DEBUG, "VIDIOC_S_FMT error\n");
	init_mmap();
}

void gVideo::close_device(void) {
	if (-1 == ::close(fd))
		utils::to_log(LOG_LEVEL_DEBUG, "close\n");

	fd = -1;
}

void gVideo::open_device(void)
{
	if((fd = ::open(dev_name.c_str(), O_RDWR /* required */ /*| O_NONBLOCK*/, 0)) == -1)
		LOG_ERROR(utils::format_string("Cannot open %s\n", dev_name.c_str()));
}