src/v4l-player.c

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