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\n",fbfd);
        return 0;
    }
    printf("The framebuffer device was opened successfully.\n");
    
    vd->fbfd = fbfd;
    
    if (ioctl(fbfd, FBIOGET_FSCREENINFO, &vd->finfo)) {
        printf("Error reading fixed information.\n");
        return 0;
    }
    
    if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vd->vinfo)) {
        printf("Error reading variable information.\n");
        return 0;
    }
    
    printf("%dx%d, %dbpp, xoffset=%d ,yoffset=%d \n", 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.\n");
        return 0;
    }
    
    printf("The framebuffer device was mapped to memory successfully.\n");
    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\n");
        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\n");
        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\n");
        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\n");
        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\n");
        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\n");
        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\n");
        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\n");
            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\n", 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\n");
            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\n");
        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\n");
    /* Start Displaying */
    startDisplay();
    
    printf("open tv display dev success \n");
    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.\n", 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.\n",
               devname);
        close(dev);
        return ret;
    }
    
    if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
        printf("Error opening device %s: video capture not supported.\n",
               devname);
        close(dev);
        return -EINVAL;
    }
    
    printf("Device %s opened: %s.\n", 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("\n");
    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 }\n",
                   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.\n");
        } else if (fsize.type == V4L2_FRMSIZE_TYPE_CONTINUOUS) {
            printf("{ continuous: min { width = %u, height = %u } .. "
                   "max { width = %u, height = %u } }\n",
                   fsize.stepwise.min_width, fsize.stepwise.min_height,
                   fsize.stepwise.max_width, fsize.stepwise.max_height);
            printf("  Refusing to enumerate frame intervals.\n");
            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 } }\n",
                   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.\n");
            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' }\n",
               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.\n");
        
        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.\n", 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\n");
    
    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.\n", errno);
        return ret;
    }
    
    printf("Video format set: width: %u height: %u buffer size: %u\n",
           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.\n", errno);
        return ret;
    }
    
    printf("Current frame rate: %u/%u\n",
           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.\n", errno);
        return ret;
    }
    
    ret = ioctl(dev, VIDIOC_G_PARM, &parm);
    if (ret < 0) {
        printf("Unable to get frame rate: %d.\n", errno);
        return ret;
    }
    
    printf("Frame rate set: %u/%u\n",
           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.\n", errno);
        return ret;
    }
    
    printf("%u buffers allocated.\n", 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).\n", i, errno);
            close(dev);
            return 1;
        }
        printf("length: %u offset: %u\n", 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)\n", i, errno);
            close(dev);
            return 1;
        }
        printf("Buffer %u mapped at address %p.\n", 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).\n", 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.\n",
               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)\n", strerror(errno));
        return FAILURE;
    }
    
    if (ret == 0) {
        //ERR("Select timed out\n");
        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\n", 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 \n");
    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).\n", errno);
        //ERR(" Video Input VIDIOC_DQBUF failed (%s)\n", strerror(errno));
        // BREAK_LOOP(THREAD_FAILURE);
        close(cam_dev);
        return 1;
    }
    printf("ok 1\n");
    my_memcpy(tmpBuffer, mem[buf.index],width * height * sizeof(char)*2);
    printf("ok 1-1\n");
    convert(tmpBuffer, dispBuffer, width, height);
    printf("ok 1-2\n");
    yuv_to_framebuffer(&vd, width, height, 0, 0, (unsigned short *)dispBuffer);
    printf("ok 1-3\n");
    
    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\n");
        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\n");
    
    /* 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\n");
        return 1;
    }
   
    /* Requeue the buffer. */
    ret = ioctl(cam_dev, VIDIOC_QBUF, &buf);
    if (ret < 0) {
        printf("Video Input requeue buffer (%d).\n", errno);
        close(cam_dev);
        return 1;
    }
    printf("ok 3\n");
    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)\n",
           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\n", udev_list_entry_get_name(list_entry),udev_list_entry_get_value(list_entry));
        
        printf("\n");
    }
}

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!\n");
        return 0;    
    }
    else
    {
        printf("open framebuffer success!\n");
    }

    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!\n");
     exit (1);
    }

    
    if (udev == NULL)
        goto out;
    udevadm_monitor(udev);
    goto out;
    rc = 2;
out:
    pthread_join(id,NULL);
    udev_unref(udev);
    return rc;
}