src/mmap_v4l.c

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