//usb
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>
#include <getopt.h>
#include <fcntl.h>
#include <errno.h>
#include <signal.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <sys/select.h>
#include <linux/types.h>
#include <linux/netlink.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sched.h>
#include <pthread.h>
#include <asm/types.h>
#include <linux/fb.h>
#include <linux/kd.h>
#include <linux/vt.h>
#include <linux/videodev.h>
#include <linux/videodev2.h>
#undef asmlinkage
#ifdef __i386__
#define asmlinkage __attribute__((regparm(0)))
#else
#define asmlinkage
#endif
#define UDEV_MAX(a,b) ((a) > (b) ? (a) : (b))
#define udev_list_entry_foreach(entry, first) for (entry = first;entry != NULL; entry = udev_list_entry_get_next(entry))
static int debug;
static int udev_exit;
static void asmlinkage sig_handler(int signum)
{
if (signum == SIGINT || signum == SIGTERM)
udev_exit = 1;
}
#define CAM_ACTION_ADD "add"
#define CAM_ACTION_REMOVE "remove"
#define CAM_DEVNAME "video0"
#define MAX_BUFFER 3
#define MAXLOOPCOUNT 300
#define WIDTH 640
#define HEIGHT 480
#define DEGREE 0
#define DEGREE1 180
#define DEV_NAME_TVOUT "/dev/video1"
#define FB_FILE "/dev/fb0"
#define VIDEO_INPUT_DEV "/dev/video0"
#define SUCCESS 0
#define FAILURE -1
/***********************************************************
netlink
*************************************************************/
#define UEVENT_BUFFER_SIZE 2048
static int display_fd = 0;
static char display_dev_name[20] = {"/dev/video1"};
struct buf_info {
int index;
unsigned int length;
char *start;
};
typedef struct _fb_v4l
{
int fbfd ;
char *fbp;
struct fb_var_screeninfo vinfo;
struct fb_fix_screeninfo finfo;
}fb_v41;
fb_v41 vd;
static struct buf_info display_buff_info[MAX_BUFFER];
static int my_numbuffers = MAX_BUFFER;
static int dispheight, dispwidth, sizeimage;
struct v4l2_buffer mybuf;
void *displaybuffer;
unsigned int nbufsx = 8;
unsigned int nbufs = 1;
unsigned int input = 0;
unsigned int skip = 0;
void *mem[1];///////before is 32 V4L_BUFFERS_MAX
unsigned int pixelformat = V4L2_PIX_FMT_YUYV;////////
unsigned int ratenum = 1;
unsigned int ratefs = 30;
unsigned int i;
int ret,cam_dev;
char *tmpBuffer=NULL, *dispBuffer=NULL,*tmpBuffer_tst,*pYbuf,*pYtemp,*svideotemp;
int actADDBool =0;
int actREMOVEBool =0;
int devnameBool = 0;
int width = 640 ;
int height = 480;
int no_cam =0;
int had_cam =0;
int add_cam =0;
int del_cam =0;
/******************************************************************************
Function Definitions of TV_OUT
******************************************************************************/
static int releaseDisplay();
static void startDisplay();
static void stopDisplay();
/***********************************my funcktion ********************************************/
/********************************************************************************************
yuv processing
***********************************************************************************************/
void yuv_to_framebuffer(fb_v41 *vd, int width, int height, int xoffset, int yoffset, unsigned short *buffer)
{
int x, y, location;
unsigned short *loca_ptr;
for(y = 0; y < height; y++){
location = xoffset * 2 + (y + yoffset) * vd->finfo.line_length;
loca_ptr = (unsigned short *) (vd->fbp + location);
for(x = 0; x < width; x++){
*(loca_ptr + x) = *buffer++;//rgb565
}
}
}
int open_framebuffer(char *ptr,fb_v41 *vd)//ret = open_framebuffer(FB_FILE, &vd);
{
int fbfd,screensize;
fbfd = open( ptr, O_RDWR);
if (fbfd < 0) {
printf("Error: cannot open framebuffer device.%x
",fbfd);
return 0;
}
printf("The framebuffer device was opened successfully.
");
vd->fbfd = fbfd;
if (ioctl(fbfd, FBIOGET_FSCREENINFO, &vd->finfo)) {
printf("Error reading fixed information.
");
return 0;
}
if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vd->vinfo)) {
printf("Error reading variable information.
");
return 0;
}
printf("%dx%d, %dbpp, xoffset=%d ,yoffset=%d
", vd->vinfo.xres, vd->vinfo.yres, vd->vinfo.bits_per_pixel,vd->vinfo.xoffset,vd->vinfo.yoffset );
screensize = vd->vinfo.xres * vd->vinfo.yres * vd->vinfo.bits_per_pixel / 8;
vd->fbp = (char *)mmap(0,screensize,PROT_READ|PROT_WRITE,MAP_SHARED,fbfd,0);
if ((int)vd->fbp == -1) {
printf("Error: failed to map framebuffer device to memory.
");
return 0;
}
printf("The framebuffer device was mapped to memory successfully.
");
return 1;
}
void color_bar(char *addr, int w, int h, int order);
/*
This routine unmaps all the buffers
This is the final step.
*/
static int releaseDisplay()
{
int i;
for (i = 0; i < my_numbuffers; i++) {
munmap(display_buff_info[i].start,
display_buff_info[i].length);
display_buff_info[i].start = NULL;
}
close(display_fd);
display_fd = 0;
return 0;
}
/*
Starts Streaming
*/
static void startDisplay()
{
int a = V4L2_BUF_TYPE_VIDEO_OUTPUT, ret;
ret = ioctl(display_fd, VIDIOC_STREAMON, &a);
if (ret < 0) {
perror("VIDIOC_STREAMON
");
close(display_fd);
exit(1);
}
}
/*
Stops Streaming
*/
static void stopDisplay()
{
int ret, a = V4L2_BUF_TYPE_VIDEO_OUTPUT;
ret = ioctl(display_fd, VIDIOC_STREAMOFF, &a);
if(ret < 0) {
perror("Error in stopping display
");
close(display_fd);
exit(1);
}
}
static short ycbcr[8] = {
(0x1F << 11) | (0x3F << 5) | (0x1F),
(0x00 << 11) | (0x00 << 5) | (0x00),
(0x1F << 11) | (0x00 << 5) | (0x00),
(0x00 << 11) | (0x3F << 5) | (0x00),
(0x00 << 11) | (0x00 << 5) | (0x1F),
(0x1F << 11) | (0x3F << 5) | (0x00),
(0x1F << 11) | (0x00 << 5) | (0x1F),
(0x00 << 11) | (0x3F << 5) | (0x1F),
};
void color_bar(char *addr, int width, int height, int order)
{
unsigned short *ptr = (unsigned short *)addr + order*width;
int i, j, k;
for(i = 0 ; i < 8 ; i ++) {
for(j = 0 ; j < height / 8 ; j ++) {
for(k = 0 ; k < width / 2 ; k ++, ptr++)
*ptr = ycbcr[i];
if((unsigned int)ptr > (unsigned int)addr +
width*height)
ptr = (unsigned short *)addr;
}
}
}
void set_rotation(int degree)
{
struct v4l2_control control;
int ret = 0;
//control.id = V4L2_CID_ROTATION;
control.id = V4L2_CID_BASE+31;
control.value = degree;
ret = ioctl(display_fd, VIDIOC_S_CTRL, &control);
if (ret < 0) {
perror("VIDIOC_S_CTRL
");
close(display_fd);
exit(0);
}
}
void set_format(int width, int height, int degree)
{
struct v4l2_format fmt;
int ret = 0;
fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
ret = ioctl(display_fd, VIDIOC_G_FMT, &fmt);
if(ret<0) {
perror("Get Format failed
");
exit(1);
}
/* Set the image size to VGA and pixel format to RGB565 */
if(degree == 0 || degree == 180) {
fmt.fmt.pix.width = width;
fmt.fmt.pix.height = height;
} else {
fmt.fmt.pix.width = height;
fmt.fmt.pix.height = width;
}
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;//V4L2_PIX_FMT_YUYV V4L2_PIX_FMT_RGB565
ret = ioctl(display_fd, VIDIOC_S_FMT, &fmt);
if(ret<0) {
perror("Set Format failed
");
exit(1);
}
}
void get_format(int *dispheight, int *dispwidth, int *sizeimage)
{
struct v4l2_format fmt;
int ret = 0;
fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
ret = ioctl(display_fd, VIDIOC_G_FMT, &fmt);
if(ret<0){
perror("Get Format failed
");
close(display_fd);
exit(1);
}
*dispheight = fmt.fmt.pix.height;
*dispwidth = fmt.fmt.pix.bytesperline;
*sizeimage = fmt.fmt.pix.sizeimage;
}
void setup_buffers()
{
struct v4l2_requestbuffers reqbuf;
int ret = 0, i;
reqbuf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
reqbuf.count = my_numbuffers;
reqbuf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(display_fd, VIDIOC_REQBUFS, &reqbuf);
if (ret < 0) {
perror("Could not allocate the buffers
");
close(display_fd);
exit(1);
}
memset(&mybuf,0,sizeof mybuf);
my_numbuffers = reqbuf.count;
for(i = 0 ; i < reqbuf.count ; i ++) {
/* query */
mybuf.index = i;
mybuf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
mybuf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(display_fd, VIDIOC_QUERYBUF, &mybuf);
if (ret < 0) {
perror("quering for buffer info failed
");
close(display_fd);
exit(1);
}
/* mmap */
display_buff_info[i].length = mybuf.length;
display_buff_info[i].index = i;
display_buff_info[i].start =
mmap(NULL, mybuf.length, PROT_READ | PROT_WRITE,
MAP_SHARED, display_fd, mybuf.m.offset);
if ((unsigned int) display_buff_info[i].
start == MAP_SHARED) {
printf("Cannot mmap = %d buffer
", i);
close(display_fd);
exit(1);
}
memset(display_buff_info[i].start, 0x80, mybuf.length);
/* Fill up the buffers with the values.*/
color_bar(display_buff_info[i].start, dispwidth, dispheight,0);
}
for(i = 0 ; i < reqbuf.count ; i ++) {
mybuf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
mybuf.memory = V4L2_MEMORY_MMAP;
mybuf.index = i;
ret = ioctl(display_fd, VIDIOC_QBUF, &mybuf);
if (ret < 0) {
perror("VIDIOC_QBUF
");
close(display_fd);
exit(1);
}
}
}
void open_display()
{
struct v4l2_capability capability;
int mode = O_RDWR;
display_fd = open((const char *)display_dev_name, mode);
if(display_fd == -1) {
perror("failed to open display device
");
exit(1);
}
if (ioctl(display_fd, VIDIOC_QUERYCAP, &capability) < 0) {
perror("VIDIOC_QUERYCAP");
exit(1);
}
}
int My_svideo_start(int width, int height)
{
void *displaybuffer;
int ret = 0;
/* Setting parameters for 90 degree rotation */
/* open display channel */
open_display();
/* Set the rotation angle */
set_rotation(DEGREE);
/* set the format according to the rotation value selected */
set_format(width,height,DEGREE);
/* Get the negotiatied format */
get_format(&dispheight, &dispwidth, &sizeimage);
setup_buffers();
printf("Image rotated by 90 degree
");
/* Start Displaying */
startDisplay();
printf("open tv display dev success
");
return 0;
}
int My_svideo_stop()
{
stopDisplay();
/* close display channel */
releaseDisplay();
close(display_fd);
return 0;
}
static int video_open(const char *devname)
{
struct v4l2_capability cap;
int dev, ret;
dev = open(devname, O_RDWR);
if (dev < 0) {
printf("Error opening device %s: %d.
", devname, errno);
return dev;
}
memset(&cap, 0, sizeof cap);
ret = ioctl(dev, VIDIOC_QUERYCAP, &cap);
if (ret < 0) {
printf("Error opening device %s: unable to query device.
",
devname);
close(dev);
return ret;
}
if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
printf("Error opening device %s: video capture not supported.
",
devname);
close(dev);
return -EINVAL;
}
printf("Device %s opened: %s.
", devname, cap.card);
return dev;
}
int enum_frame_intervals(int dev, __u32 pixfmt, __u32 width, __u32 height)
{
int ret;
struct v4l2_frmivalenum fival;
memset(&fival, 0, sizeof(fival));
fival.index = 0;
fival.pixel_format = pixfmt;
fival.width = width;
fival.height = height;
printf("\tTime interval between frame: ");
while ((ret = ioctl(dev, VIDIOC_ENUM_FRAMEINTERVALS, &fival)) == 0) {
if (fival.type == V4L2_FRMIVAL_TYPE_DISCRETE) {
printf("%u/%u, ",
fival.discrete.numerator, fival.discrete.denominator);
} else if (fival.type == V4L2_FRMIVAL_TYPE_CONTINUOUS) {
printf("{min { %u/%u } .. max { %u/%u } }, ",
fival.stepwise.min.numerator, fival.stepwise.min.numerator,
fival.stepwise.max.denominator, fival.stepwise.max.denominator);
break;
} else if (fival.type == V4L2_FRMIVAL_TYPE_STEPWISE) {
printf("{min { %u/%u } .. max { %u/%u } / "
"stepsize { %u/%u } }, ",
fival.stepwise.min.numerator, fival.stepwise.min.denominator,
fival.stepwise.max.numerator, fival.stepwise.max.denominator,
fival.stepwise.step.numerator, fival.stepwise.step.denominator);
break;
}
fival.index++;
}
printf("
");
if (ret != 0 && errno != EINVAL) {
perror("ERROR enumerating frame intervals");
return errno;
}
return 0;
}
static int enum_frame_sizes(int dev, __u32 pixfmt)
{
int ret;
struct v4l2_frmsizeenum fsize;
memset(&fsize, 0, sizeof(fsize));
fsize.index = 0;
fsize.pixel_format = pixfmt;
while ((ret = ioctl(dev, VIDIOC_ENUM_FRAMESIZES, &fsize)) == 0) {
if (fsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
printf("{ discrete: width = %u, height = %u }
",
fsize.discrete.width, fsize.discrete.height);
ret = enum_frame_intervals(dev, pixfmt,
fsize.discrete.width, fsize.discrete.height);
if (ret != 0)
printf(" Unable to enumerate frame sizes.
");
} else if (fsize.type == V4L2_FRMSIZE_TYPE_CONTINUOUS) {
printf("{ continuous: min { width = %u, height = %u } .. "
"max { width = %u, height = %u } }
",
fsize.stepwise.min_width, fsize.stepwise.min_height,
fsize.stepwise.max_width, fsize.stepwise.max_height);
printf(" Refusing to enumerate frame intervals.
");
break;
} else if (fsize.type == V4L2_FRMSIZE_TYPE_STEPWISE) {
printf("{ stepwise: min { width = %u, height = %u } .. "
"max { width = %u, height = %u } / "
"stepsize { width = %u, height = %u } }
",
fsize.stepwise.min_width, fsize.stepwise.min_height,
fsize.stepwise.max_width, fsize.stepwise.max_height,
fsize.stepwise.step_width, fsize.stepwise.step_height);
printf(" Refusing to enumerate frame intervals.
");
break;
}
fsize.index++;
}
if (ret != 0 && errno != EINVAL) {
perror("ERROR enumerating frame sizes");
return errno;
}
return 0;
}
static void video_list_formats(int dev)
{
struct v4l2_fmtdesc fmt;
int ret;
memset(&fmt, 0, sizeof(fmt));
fmt.index = 0;
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
while ((ret = ioctl(dev, VIDIOC_ENUM_FMT, &fmt)) == 0) {
printf("{ pixelformat = '%c%c%c%c', description = '%s' }
",
fmt.pixelformat & 0xFF, (fmt.pixelformat >> 8) & 0xFF,
(fmt.pixelformat >> 16) & 0xFF, (fmt.pixelformat >> 24) & 0xFF,
fmt.description);
ret = enum_frame_sizes(dev, fmt.pixelformat);
if(ret != 0)
printf(" Unable to enumerate frame sizes.
");
fmt.index++;
}
if (errno != EINVAL) {
perror("ERROR enumerating frame formats");
}
}
static int video_get_input(int dev)
{
__u32 input;
int ret;
ret = ioctl(dev, VIDIOC_G_INPUT, &input);
if (ret < 0) {
printf("Unable to get current input: %s.
", strerror(errno));
return ret;
}
return input;
}
static int video_set_format(int dev, unsigned int w, unsigned int h, unsigned int format)
{
struct v4l2_format fmt;
int ret;
printf("video_set_format: width: %u height: %u
");
memset(&fmt, 0, sizeof fmt);
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = w;
fmt.fmt.pix.height = h;
fmt.fmt.pix.pixelformat = format;
fmt.fmt.pix.field = V4L2_FIELD_ANY;
ret = ioctl(dev, VIDIOC_S_FMT, &fmt);
if (ret < 0) {
printf("Unable to set format: %d.
", errno);
return ret;
}
printf("Video format set: width: %u height: %u buffer size: %u
",
fmt.fmt.pix.width, fmt.fmt.pix.height, fmt.fmt.pix.sizeimage);
return 0;
}
static int video_set_framerate(int dev,unsigned int numerator,unsigned int denominator)
{
struct v4l2_streamparm parm;
int ret;
memset(&parm, 0, sizeof parm);
parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ret = ioctl(dev, VIDIOC_G_PARM, &parm);
if (ret < 0) {
printf("Unable to get frame rate: %d.
", errno);
return ret;
}
printf("Current frame rate: %u/%u
",
parm.parm.capture.timeperframe.numerator,
parm.parm.capture.timeperframe.denominator);
parm.parm.capture.timeperframe.numerator = numerator;
parm.parm.capture.timeperframe.denominator = denominator;
ret = ioctl(dev, VIDIOC_S_PARM, &parm);
if (ret < 0) {
printf("Unable to set frame rate: %d.
", errno);
return ret;
}
ret = ioctl(dev, VIDIOC_G_PARM, &parm);
if (ret < 0) {
printf("Unable to get frame rate: %d.
", errno);
return ret;
}
printf("Frame rate set: %u/%u
",
parm.parm.capture.timeperframe.numerator,
parm.parm.capture.timeperframe.denominator);
return 0;
}
static int video_reqbufs(int dev, int nbufs)
{
struct v4l2_requestbuffers rb;
int ret;
memset(&rb, 0, sizeof rb);
rb.count = nbufs;
rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
rb.memory = V4L2_MEMORY_MMAP;
ret = ioctl(dev, VIDIOC_REQBUFS, &rb);
if (ret < 0) {
printf("Unable to allocate buffers: %d.
", errno);
return ret;
}
printf("%u buffers allocated.
", rb.count);
return rb.count;
}
static void video_map(int dev, int nbufs, void **mem)
{
struct v4l2_buffer buf;
int i,ret;
for (i = 0; i < nbufs; ++i) {
memset(&buf, 0, sizeof buf);
buf.index = i;
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(dev, VIDIOC_QUERYBUF, &buf);
if (ret < 0) {
printf("Unable to query buffer %u (%d).
", i, errno);
close(dev);
return 1;
}
printf("length: %u offset: %u
", buf.length, buf.m.offset);
mem[i] = mmap(0, buf.length, PROT_READ, MAP_SHARED, dev, buf.m.offset);
if (mem[i] == MAP_FAILED) {
printf("Unable to map buffer %u (%d)
", i, errno);
close(dev);
return 1;
}
printf("Buffer %u mapped at address %p.
", i, mem[i]);
}
/* Queue the buffers. */
for (i = 0; i < nbufs; ++i) {
memset(&buf, 0, sizeof buf);
buf.index = i;
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(dev, VIDIOC_QBUF, &buf);
if (ret < 0) {
printf("Unable to queue buffer (%d).
", errno);
close(dev);
return 1;
}
}
return;
}
static int video_enable(int dev, int enable)
{
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
int ret;
ret = ioctl(dev, enable ? VIDIOC_STREAMON : VIDIOC_STREAMOFF, &type);
if (ret < 0) {
printf("Unable to %s capture: %d.
",
enable ? "start" : "stop", errno);
return ret;
}
return 0;
}
/******************************************************************************
* waitForFrame
******************************************************************************/
static inline int waitForFrame(int fd)
{
struct timeval tv;
fd_set fds;
int ret;
FD_ZERO(&fds);
FD_SET(fd, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
ret = select(fd + 1, &fds, NULL, NULL, &tv);
if (ret == -1) {
// ERR("Select failed capture device (%s)
", strerror(errno));
return FAILURE;
}
if (ret == 0) {
//ERR("Select timed out
");
return FAILURE;
}
return SUCCESS;
}
void my_memcpy(void *dest, const void * src,int size)
{
#define L1_BUFF 4096
long temp_array[L1_BUFF/sizeof(long)];
while(size/L1_BUFF){
memcpy(temp_array,src,L1_BUFF);
memcpy(dest, temp_array,L1_BUFF);
src =(void*)((char *)src+L1_BUFF);
dest =(void*)((char *)dest+L1_BUFF);
size -=L1_BUFF;
}
memcpy(temp_array,src,size);
memcpy(dest,temp_array,size);
}
static inline void yuv_to_rgb16(unsigned char y,
unsigned char u,
unsigned char v,
unsigned char *rgb)
{
register int r,g,b;
int rgb16;
r = (1192 * (y - 16) + 1634 * (v - 128) ) >> 10;
g = (1192 * (y - 16) - 833 * (v - 128) - 400 * (u -128) ) >> 10;
b = (1192 * (y - 16) + 2066 * (u - 128) ) >> 10;
r = r > 255 ? 255 : r < 0 ? 0 : r;
g = g > 255 ? 255 : g < 0 ? 0 : g;
b = b > 255 ? 255 : b < 0 ? 0 : b;
rgb16 = (int)(((r >> 3)<<11) | ((g >> 2) << 5)| ((b >> 3) << 0));
*rgb = (unsigned char)(rgb16 & 0xFF);
rgb++;
*rgb = (unsigned char)((rgb16 & 0xFF00) >> 8);
}
void convert(unsigned char *buf, unsigned char *rgb, int width, int height)
{
int x,y,z=0;
int blocks;
blocks = (width * height) * 2;
for (y = 0; y < blocks; y+=4) {
unsigned char Y1, Y2, U, V;
///////////////////////////////////////
Y1 = buf[y + 0];
U = buf[y + 1];
Y2 = buf[y + 2];
V = buf[y + 3];
//////////////////////////////////////
yuv_to_rgb16(Y1, U, V, &rgb[y]);
yuv_to_rgb16(Y2, U, V, &rgb[y + 2]);
//yuv_to_rgb16(Y1, 0x80, 0x80, &rgb[y]);
//yuv_to_rgb16(Y2, 0x80, 0x80, &rgb[y + 2]);
}
}
int open_usb_cam_dev(int sizeImg)
{
/* Open the video device. */
cam_dev = video_open(VIDEO_INPUT_DEV);
if (cam_dev < 0)
return 1;
video_list_formats(cam_dev);
ret = video_get_input(cam_dev);
printf("Input %d selected
", ret);
/* Set the video format. */
if (video_set_format(cam_dev, width, height, pixelformat) < 0) {
close(cam_dev);
return 1;
}
/* Set the frame rate. */
if (video_set_framerate(cam_dev,ratenum,ratefs) < 0) {
close(cam_dev);
return 1;
}
/* Allocate buffers. */
if ((int)(nbufs = video_reqbufs(cam_dev, nbufs)) < 0)
{
close(cam_dev);
return 1;
}
/* Map the buffers. */
video_map(cam_dev,nbufs, mem);
/* Start streaming. */
video_enable(cam_dev, 1);
printf("open the camera success
");
return cam_dev;
}
int usb_cam_video_display()
{
struct v4l2_buffer buf;
if (waitForFrame(cam_dev) == FAILURE) { return 1;}
if(!tmpBuffer)
tmpBuffer = malloc(width * height * 2);
if(!dispBuffer)
dispBuffer = malloc(width * height * 2);
memset(&buf, 0, sizeof buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(cam_dev, VIDIOC_DQBUF, &buf);
if (ret < 0) {
//printf("Video Input Unable to dequeue buffer (%d).
", errno);
//ERR(" Video Input VIDIOC_DQBUF failed (%s)
", strerror(errno));
// BREAK_LOOP(THREAD_FAILURE);
close(cam_dev);
return 1;
}
printf("ok 1
");
my_memcpy(tmpBuffer, mem[buf.index],width * height * sizeof(char)*2);
printf("ok 1-1
");
convert(tmpBuffer, dispBuffer, width, height);
printf("ok 1-2
");
yuv_to_framebuffer(&vd, width, height, 0, 0, (unsigned short *)dispBuffer);
printf("ok 1-3
");
if (waitForFrame(display_fd) == FAILURE) { return 0;}
memset(&mybuf, 0, sizeof mybuf);
mybuf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
mybuf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(display_fd, VIDIOC_DQBUF, &mybuf);
if (ret < 0) {
perror("SVIDEO VIDIOC_DQBUF
");
return 1;
}
displaybuffer = display_buff_info[mybuf.index].start;
//color_bar(displaybuffer, dispwidth, dispheight,200%(dispheight/2));
memcpy(displaybuffer, mem[buf.index], width * height * 2);
printf("ok 2
");
/* Now queue it back to display it */
mybuf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
mybuf.memory = V4L2_MEMORY_MMAP;
mybuf.index = mybuf.index;
ret = ioctl(display_fd, VIDIOC_QBUF, &mybuf);
if (ret < 0) {
perror("SVIDEO VIDIOC_QBUF
");
return 1;
}
/* Requeue the buffer. */
ret = ioctl(cam_dev, VIDIOC_QBUF, &buf);
if (ret < 0) {
printf("Video Input requeue buffer (%d).
", errno);
close(cam_dev);
return 1;
}
printf("ok 3
");
return 0;
}
int usb_cam_video_display_close()
{
My_svideo_stop();
/* Stop streaming. */
video_enable(cam_dev, 0);
close(cam_dev);
//free(dispBuffer);
//free(tmpBuffer);
return 0;
}
static void print_device(struct udev_device *device, const char *source, int env)
{
struct timeval tv;
struct timezone tz;
gettimeofday(&tv, &tz);
printf("%-6s[%llu.%06u] %-8s %s (%s)
",
source,
(unsigned long long) tv.tv_sec, (unsigned int) tv.tv_usec,
udev_device_get_action(device),
udev_device_get_devpath(device),
udev_device_get_subsystem(device));
if (env) {
struct udev_list_entry *list_entry;
udev_list_entry_foreach(list_entry, udev_device_get_properties_list_entry(device))
printf("%s=%s
", udev_list_entry_get_name(list_entry),udev_list_entry_get_value(list_entry));
printf("
");
}
}
int killcam=0;
int udevadm_monitor(struct udev *udev)
{
struct sigaction act;
int env = 0;
int print_kernel = 0;
int print_udev = 0;
struct udev_monitor *udev_monitor = NULL;
struct udev_monitor *kernel_monitor = NULL;
fd_set readfds;
int rc = 0;
if (!print_kernel && !print_udev) {
print_kernel = 1;
print_udev =1;
}
if (getuid() != 0 && print_kernel) {
fprintf(stderr, "root privileges needed to subscribe to kernel events/n");
goto out;
}
/* set signal handlers */
memset(&act, 0x00, sizeof(struct sigaction));
act.sa_handler = (void (*)(int)) sig_handler;
sigemptyset(&act.sa_mask);
act.sa_flags = SA_RESTART;
sigaction(SIGINT, &act, NULL);
sigaction(SIGTERM, &act, NULL);
printf("monitor will print the received events for:/n");
if (print_udev) {
udev_monitor = udev_monitor_new_from_socket(udev, "@/org/kernel/udev/monitor");
if (udev_monitor == NULL) {
rc = 1;
goto out;
}
if (udev_monitor_enable_receiving(udev_monitor) < 0) {
rc = 2;
goto out;
}
printf("UDEV the event which udev sends out after rule processing/n");
}
if (print_kernel) {
kernel_monitor = udev_monitor_new_from_netlink(udev, "udev"); //\u8fd9\u91cc\u7684udev\u6e90\u7801\u4e2d\u6ca1\u6709"udev"\u8fd9\u4e2a\u53c2\u6570\uff0c\u4e0d\u52a0\u8fdb\u53bb\u8fd4\u56de\u503c\u5c31\u4e3aNULL\uff0c\u6240\u4ee5\u8981\u52a0\u8fd9\u4e2a
if (kernel_monitor == NULL) {
rc = 3;
printf("udev_monitor_new_from_netlink() error/n");
goto out;
}
if (udev_monitor_enable_receiving(kernel_monitor) < 0) {
rc = 4;
goto out;
}
printf("UEVENT the kernel uevent/n");
}
printf("/n");
while (!udev_exit) {
#if 1
int fdcount;
FD_ZERO(&readfds);
if (kernel_monitor != NULL)
FD_SET(udev_monitor_get_fd(kernel_monitor), &readfds);
if (udev_monitor != NULL)
FD_SET(udev_monitor_get_fd(udev_monitor), &readfds);
fdcount = select(UDEV_MAX(udev_monitor_get_fd(kernel_monitor), udev_monitor_get_fd(udev_monitor))+1,
&readfds, NULL, NULL, NULL);
if (fdcount < 0) {
if (errno != EINTR)
fprintf(stderr, "error receiving uevent message: %m/n");
continue;
}
#endif
#if 1
if ((kernel_monitor != NULL) && FD_ISSET(udev_monitor_get_fd(kernel_monitor), &readfds)) {
struct udev_device *device;
device = udev_monitor_receive_device(kernel_monitor);
if (device == NULL)
continue;
if(killcam == 0 && !strcmp(udev_device_get_action(device),"remove")){
no_cam =1;
del_cam =1;
usb_cam_video_display_close();
killcam = 1;
}
else
{
if(killcam ==1 && !strcmp(udev_device_get_action(device),"add"))
{
had_cam =1;
add_cam =1;
}
}
print_device(device, "UEVENT", env);
udev_device_unref(device);
}
#endif
#if 1
if ((udev_monitor != NULL) && FD_ISSET(udev_monitor_get_fd(udev_monitor), &readfds)) {
struct udev_device *device;
device = udev_monitor_receive_device(udev_monitor);
if (device == NULL)
continue;
print_device(device, "UDEV", env);
//usb_cam_video_display();//????????
udev_device_unref(device);
//usleep(30000);
}
#endif
//usb_cam_video_display();//????????
}
out:
udev_monitor_unref(udev_monitor);
udev_monitor_unref(kernel_monitor);
return rc;
}
static void sig(int sig)
{
usb_cam_video_display_close();
exit(1);
}
int sizeimg;
void all_in_one()
{
sizeimg= width * height * sizeof(char) *2;
open_usb_cam_dev(sizeimg);
My_svideo_start( width, height);
ret = open_framebuffer(FB_FILE, &vd);//#define FB_FILE "/dev/fb0" open the screen buffer
return;
}
void thread(void )
{
sizeimg = width * height * sizeof(char) *2;
open_usb_cam_dev(sizeimg);
My_svideo_start( width, height);
ret = open_framebuffer(FB_FILE, &vd);//#define FB_FILE "/dev/fb0" open the screen buffer
signal(SIGSEGV, sig);
signal(SIGINT, sig);
signal(SIGTERM, sig);
if (ret == 0)
{
printf("open framebuffer error!
");
return 0;
}
else
{
printf("open framebuffer success!
");
}
while (1) {
if(no_cam==1 && del_cam == 1)
{
//usb_cam_video_display_close();
continue;
}
if(had_cam == 1 && add_cam ==1)
{
all_in_one();
had_cam = 0;
no_cam =0;
}
ret=usb_cam_video_display();//????????
if(ret ==1)
{
//exit(1);
break;
}
usleep(30000);
}
usb_cam_video_display_close();
}
int main(int argc, char *argv[])
{
struct udev *udev;
int rc = 1;
udev = udev_new();
pthread_t id;
int i,ret1;
ret1=pthread_create(&id,NULL,(void *) thread,NULL);
if(ret1!=0){
printf ("Create pthread error!
");
exit (1);
}
if (udev == NULL)
goto out;
udevadm_monitor(udev);
goto out;
rc = 2;
out:
pthread_join(id,NULL);
udev_unref(udev);
return rc;
}