src/v4l_player.c

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