udevとusbカメラのソースコード


//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; }