src/v4l-player.c

   1 /*
   2  * transsip - the telephony network
   3  * By Daniel Borkmann <daniel@transsip.org>
   4  * Copyright 2011 Daniel Borkmann <dborkma@tik.ee.ethz.ch>,
   5  * Swiss federal institute of technology (ETH Zurich)
   6  * Subject to the GPL, version 2.
   7  */
   8
   9 /*  libgtk2.0-dev */
  10 /* Compile with: gcc v4ltest.c `pkg-config --libs --cflags gtk+-2.0` */
  11
  12 /* TODO: clean code, use gtk_image_set_from_pixbuf() */
  13
  14 #include <stdio.h>
  15 #include <stdlib.h>
  16 #include <stdint.h>
  17 #include <unistd.h>
  18 #include <assert.h>
  19 #include <fcntl.h>
  20 #include <getopt.h>
  21 #include <errno.h>
  22 #include <string.h>
  23 #include <gtk/gtk.h>
  24 #include <sys/stat.h>
  25 #include <sys/types.h>
  26 #include <sys/time.h>
  27 #include <sys/mman.h>
  28 #include <sys/ioctl.h>
  29 #include <asm/types.h>
  30 #include <linux/videodev2.h>
  31
  32 static GtkWidget *window;
  33 static GtkWidget *image;
  34
  35 struct buffer {
  36         void *start;
  37         size_t length;
  38 };
  39
  40 static void errno_exit(const char *s)
  41 {
  42         fprintf(stderr, "%s error %d, %s\n", s, errno, strerror(errno));
  43         exit (EXIT_FAILURE);
  44 }
  45
  46 static int xioctl(int fd, int request, void *arg)
  47 {
  48         int r;
  49
  50         assert(arg);
  51
  52         do r = ioctl(fd, request, arg);
  53         while(r < 0 && EINTR == errno);
  54
  55         return r;
  56 }
  57
  58 #define CLIP(color) (unsigned char) (((color) > 0xFF) ?                   \
  59                                     0xff : (((color) < 0) ? 0 : (color)))
  60
  61 void v4lconvert_yuyv_to_rgb24(const unsigned char *src, unsigned char *dest,
  62                               int width, int height)
  63 {
  64         /* From: Hans de Goede <j.w.r.degoede@hhs.nl> */
  65         int j;
  66
  67         while(--height >= 0) {
  68                 for(j = 0; j < width; j += 2) {
  69                         int u = src[1];
  70                         int v = src[3];
  71                         int u1 = (((u - 128) << 7) +  (u - 128)) >> 6;
  72                         int rg = (((u - 128) << 1) +  (u - 128) +
  73                                  ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
  74                         int v1 = (((v - 128) << 1) +  (v - 128)) >> 1;
  75
  76                         *dest++ = CLIP(src[0] + v1);
  77                         *dest++ = CLIP(src[0] - rg);
  78                         *dest++ = CLIP(src[0] + u1);
  79
  80                         *dest++ = CLIP(src[2] + v1);
  81                         *dest++ = CLIP(src[2] - rg);
  82                         *dest++ = CLIP(src[2] + u1);
  83
  84                         src += 4;
  85                 }
  86         }
  87 }
  88
  89 static void convert_v4l_image_and_display(unsigned char *img, size_t len)
  90 {
  91         unsigned char img2[640*480*3] = {0};
  92         v4lconvert_yuyv_to_rgb24(img, img2, 640, 480);
  93
  94         GdkPixbuf *pb = gdk_pixbuf_new_from_data(img2, GDK_COLORSPACE_RGB,
  95                                                  FALSE, 24/3, 640, 480, 640*3,
  96                                                  NULL, NULL);
  97         if(image != NULL)
  98                 gtk_container_remove(GTK_CONTAINER(window), image);
  99         image = gtk_image_new_from_pixbuf(pb);
 100         gtk_container_add(GTK_CONTAINER(window), image);
 101         gtk_widget_show_all(window);
 102 }
 103
 104 static void process_v4l_image(unsigned char *img, size_t len)
 105 {
 106         assert(img);
 107         convert_v4l_image_and_display(img, len);
 108 }
 109
 110 static int read_v4l_frame(int fd, unsigned int n_buffers,
 111                           struct buffer *buffers)
 112 {
 113         struct v4l2_buffer buf;
 114
 115         assert(buffers);
 116
 117         memset(&buf, 0, sizeof(buf));
 118
 119         buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 120         buf.memory = V4L2_MEMORY_MMAP;
 121
 122         if(xioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
 123                 switch(errno) {
 124                 case EAGAIN:
 125                         return 0;
 126                 case EIO:
 127                 default:
 128                         errno_exit("VIDIOC_DQBUF");
 129                 }
 130         }
 131
 132         assert(buf.index < n_buffers);
 133
 134         process_v4l_image(buffers[buf.index].start, buffers[buf.index].length);
 135         if(xioctl(fd, VIDIOC_QBUF, &buf) < 0)
 136                 errno_exit("VIDIOC_QBUF");
 137
 138         return 1;
 139 }
 140
 141 static int open_v4l_device(const char *dev_name)
 142 {
 143         int fd;
 144         struct stat st;
 145
 146         assert(dev_name);
 147
 148         if(stat(dev_name, &st) < 0) {
 149                 fprintf(stderr, "Cannot identify %s: %d, %s\n",
 150                         dev_name, errno, strerror(errno));
 151                 exit(EXIT_FAILURE);
 152         }
 153
 154         if(!S_ISCHR(st.st_mode)) {
 155                 fprintf(stderr, "%s is no device\n", dev_name);
 156                 exit(EXIT_FAILURE);
 157         }
 158
 159         fd = open (dev_name, O_RDWR | O_NONBLOCK, 0);
 160         if(fd < 0) {
 161                 fprintf(stderr, "Cannot open %s: %d, %s\n",
 162                         dev_name, errno, strerror(errno));
 163                 exit(EXIT_FAILURE);
 164         }
 165
 166         return fd;
 167 }
 168
 169 static void close_vl4_device(int fd)
 170 {
 171         if(fd < 0)
 172                 return;
 173         close(fd);
 174 }
 175
 176 static int start_v4l_capturing(int fd, unsigned int n_buffers)
 177 {
 178         unsigned int i;
 179         enum v4l2_buf_type type;
 180
 181         for(i = 0; i < n_buffers; ++i) {
 182                 struct v4l2_buffer buf;
 183
 184                 memset(&buf, 0, sizeof(buf));
 185
 186                 buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 187                 buf.memory = V4L2_MEMORY_MMAP;
 188                 buf.index  = i;
 189
 190                 if(xioctl(fd, VIDIOC_QBUF, &buf) < 0)
 191                         errno_exit("VIDIOC_QBUF");
 192         }
 193
 194         type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 195
 196         if(xioctl(fd, VIDIOC_STREAMON, &type) < 0)
 197                 errno_exit("VIDIOC_STREAMON");
 198
 199         return 0;
 200 }
 201
 202 static int stop_v4l_capturing(int fd)
 203 {
 204         enum v4l2_buf_type type;
 205
 206         type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 207
 208         if(xioctl(fd, VIDIOC_STREAMOFF, &type) < 0)
 209                 errno_exit("VIDIOC_STREAMOFF");
 210
 211         return 0;
 212 }
 213
 214 static int init_mmap(int fd, const char *dev_name, unsigned int *n_buffers,
 215                      struct buffer **buffers)
 216 {
 217         struct v4l2_requestbuffers req;
 218
 219         assert(buffers);
 220         assert(n_buffers);
 221         assert(dev_name);
 222
 223         memset(&req, 0, sizeof(req));
 224
 225         req.count  = 4;
 226         req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 227         req.memory = V4L2_MEMORY_MMAP;
 228
 229         if(xioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
 230                 if(EINVAL == errno) {
 231                         fprintf(stderr, "%s does not support memory mapping\n",
 232                                 dev_name);
 233                         exit(EXIT_FAILURE);
 234                 } else {
 235                         errno_exit("VIDIOC_REQBUFS");
 236                 }
 237         }
 238
 239         if(req.count < 2) {
 240                 fprintf(stderr, "Insufficient buffer memory on %s\n",
 241                         dev_name);
 242                 exit(EXIT_FAILURE);
 243         }
 244
 245         (*buffers) = calloc(req.count, sizeof (**buffers));
 246         if(!(*buffers)) {
 247                 fprintf (stderr, "Out of memory\n");
 248                 exit(EXIT_FAILURE);
 249         }
 250
 251         for((*n_buffers) = 0; (*n_buffers) < req.count; ++(*n_buffers)) {
 252                 struct v4l2_buffer buf;
 253
 254                 memset(&buf, 0, sizeof(buf));
 255
 256                 buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 257                 buf.memory = V4L2_MEMORY_MMAP;
 258                 buf.index  = (*n_buffers);
 259
 260                 if(xioctl(fd, VIDIOC_QUERYBUF, &buf) < 0)
 261                         errno_exit("VIDIOC_QUERYBUF");
 262
 263                 (*buffers)[(*n_buffers)].length = buf.length;
 264                 (*buffers)[(*n_buffers)].start = mmap(NULL, buf.length,
 265                                                    PROT_READ | PROT_WRITE,
 266                                                    MAP_SHARED, fd, buf.m.offset);
 267                 if(MAP_FAILED == (*buffers)[(*n_buffers)].start)
 268                         errno_exit("mmap");
 269         }
 270
 271         return 0;
 272 }
 273
 274 static int init_v4l_device(int fd, const char *dev_name, unsigned int *n_buffers,
 275                            struct buffer **buffers)
 276 {
 277         unsigned int min;
 278
 279         struct v4l2_capability cap;
 280         struct v4l2_cropcap cropcap;
 281         struct v4l2_crop crop;
 282         struct v4l2_format fmt;
 283
 284         if(xioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
 285                 if(EINVAL == errno) {
 286                         fprintf(stderr, "%s is no V4L2 device\n", dev_name);
 287                         exit(EXIT_FAILURE);
 288                 } else {
 289                         errno_exit("VIDIOC_QUERYCAP");
 290                 }
 291         }
 292
 293         if(!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
 294                 fprintf(stderr, "%s is no video capture device\n", dev_name);
 295                 exit(EXIT_FAILURE);
 296         }
 297
 298         if(!(cap.capabilities & V4L2_CAP_STREAMING)) {
 299                 fprintf(stderr, "%s does not support streaming i/o\n", dev_name);
 300                 exit(EXIT_FAILURE);
 301         }
 302
 303         memset(&cropcap, 0, sizeof(cropcap));
 304
 305         cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 306
 307         if(xioctl(fd, VIDIOC_CROPCAP, &cropcap) == 0) {
 308                 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 309                 crop.c    = cropcap.defrect;
 310
 311                 if(xioctl(fd, VIDIOC_S_CROP, &crop) < 0) {
 312                         switch(errno) {
 313                         case EINVAL:
 314                                 /* Cropping not supported. */
 315                                 break;
 316                         default:
 317                                 /* Errors ignored. */
 318                                 break;
 319                         }
 320                 }
 321         } else {
 322                 /* Errors ignored. */
 323         }
 324
 325         memset(&fmt, 0, sizeof(fmt));
 326
 327         fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 328         fmt.fmt.pix.width       = 640;//640;
 329         fmt.fmt.pix.height      = 480;//480;
 330         fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
 331         fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;
 332
 333         if(xioctl(fd, VIDIOC_S_FMT, &fmt) < 0)
 334                 errno_exit("VIDIOC_S_FMT");
 335
 336         /* XXX: VIDIOC_S_FMT may change width and height. */
 337
 338         /* Buggy driver paranoia. */
 339         min = fmt.fmt.pix.width * 2;
 340         if(fmt.fmt.pix.bytesperline < min)
 341                 fmt.fmt.pix.bytesperline = min;
 342         min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
 343         if(fmt.fmt.pix.sizeimage < min)
 344                 fmt.fmt.pix.sizeimage = min;
 345
 346         return init_mmap(fd, dev_name, n_buffers, buffers);
 347 }
 348
 349 static int cleanup_v4l_device(int fd, unsigned int n_buffers,
 350                               struct buffer *buffers)
 351 {
 352         unsigned int i;
 353
 354         for(i = 0; i < n_buffers; ++i)
 355                 if(munmap(buffers[i].start, buffers[i].length) < 0)
 356                         errno_exit("munmap");
 357         free(buffers);
 358         return 0;
 359 }
 360
 361 static void destroy(void) {
 362         gtk_main_quit();
 363 }
 364
 365 /* XXX: Remove here */
 366 int fd;
 367 unsigned int v4l_n_buffers;
 368 struct buffer *v4l_buffers;
 369
 370 gboolean update_v4l_image(gpointer null)
 371 {
 372         read_v4l_frame(fd, v4l_n_buffers, v4l_buffers);
 373         return TRUE;
 374 }
 375
 376 int main(int argc, char **argv)
 377 {
 378         const char *dev_name = "/dev/video0";
 379
 380         gtk_init(&argc, &argv);
 381
 382         window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
 383         gtk_window_set_title(GTK_WINDOW(window), "V4L Player");
 384         gtk_window_set_default_size(GTK_WINDOW(window), 640, 480);
 385         gtk_window_set_position(GTK_WINDOW(window), GTK_WIN_POS_NONE);
 386         gtk_window_set_resizable(GTK_WINDOW(window), FALSE);
 387
 388         gtk_signal_connect(GTK_OBJECT(window), "destroy",
 389                            GTK_SIGNAL_FUNC(destroy), NULL);
 390
 391         fd = open_v4l_device(dev_name);
 392         init_v4l_device(fd, dev_name, &v4l_n_buffers, &v4l_buffers);
 393         start_v4l_capturing(fd, v4l_n_buffers);
 394
 395         gint func_ref = g_timeout_add(10, update_v4l_image, NULL);
 396         gtk_widget_show_all(window);
 397         gtk_main();
 398         g_source_remove(func_ref);
 399
 400         stop_v4l_capturing(fd);
 401         cleanup_v4l_device(fd, v4l_n_buffers, v4l_buffers);
 402         close_vl4_device(fd);
 403
 404         return 0;
 405 }
 406