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 #include <stdio.h>
  11 #include <stdlib.h>
  12 #include <stdint.h>
  13 #include <unistd.h>
  14 #include <assert.h>
  15 #include <fcntl.h>
  16 #include <getopt.h>
  17 #include <errno.h>
  18 #include <string.h>
  19 #include <sys/stat.h>
  20 #include <sys/types.h>
  21 #include <sys/time.h>
  22 #include <sys/mman.h>
  23 #include <asm/types.h>
  24 #include <linux/videodev2.h>
  25
  26 #include "mmap_v4l.h"
  27 #include "comp_x86.h"
  28 #include "xmalloc.h"
  29 #include "die.h"
  30
  31 #define CLIP(color) (unsigned char) (((color) > 0xFF) ?  \
  32                     0xff : (((color) < 0) ? 0 : (color)))
  33
  34 void convert_v4l_yuyv_to_rgb24(const uint8_t *src, uint8_t *dest,
  35                                int width, int height)
  36 {
  37         /* From: Hans de Goede <j.w.r.degoede@hhs.nl> */
  38         int j;
  39
  40         while(--height >= 0) {
  41                 for(j = 0; j < width; j += 2) {
  42                         int u = src[1];
  43                         int v = src[3];
  44                         int u1 = (((u - 128) << 7) +  (u - 128)) >> 6;
  45                         int rg = (((u - 128) << 1) +  (u - 128) +
  46                                   ((v - 128) << 2) + ((v - 128)  << 1)) >> 3;
  47                         int v1 = (((v - 128) << 1) +  (v - 128)) >> 1;
  48
  49                         *dest++ = CLIP(src[0] + v1);
  50                         *dest++ = CLIP(src[0] - rg);
  51                         *dest++ = CLIP(src[0] + u1);
  52                         *dest++ = CLIP(src[2] + v1);
  53                         *dest++ = CLIP(src[2] - rg);
  54                         *dest++ = CLIP(src[2] + u1);
  55                         src += 4;
  56                 }
  57         }
  58 }
  59
  60 int read_v4l_frame(int fd, unsigned int nbuffs,
  61                    struct v4l_buff *buffs,
  62                    int (*rx_v4l_cb)(uint8_t *frame, size_t len))
  63 {
  64         int ret, rc;
  65         struct v4l2_buffer buf;
  66
  67         if (unlikely(!buffs || !nbuffs))
  68                 return -EINVAL;
  69
  70         memset(&buf, 0, sizeof(buf));
  71         buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  72         buf.memory = V4L2_MEMORY_MMAP;
  73
  74         ret = xioctl(fd, VIDIOC_DQBUF, &buf);
  75         if (ret < 0) {
  76                 switch(errno) {
  77                 case EAGAIN:
  78                         return 0;
  79                 case EIO:
  80                 default:
  81                         panic("I/O dequeue error!\n");
  82                 }
  83         }
  84
  85         if (unlikely(buf.index >= nbuffs))
  86                 panic("I/O overflow error!\n");
  87
  88         rc = rx_v4l_cb(buffs[buf.index].start, buffs[buf.index].length);
  89
  90         ret = xioctl(fd, VIDIOC_QBUF, &buf);
  91         if (ret < 0)
  92                 panic("I/O enqueue error!\n");
  93
  94         return rc;
  95 }
  96
  97 int open_v4l_device(char *dev)
  98 {
  99         int fd, ret;
 100         struct stat st;
 101
 102         if (!dev)
 103                 dev = "/dev/video0";
 104
 105         memset(&st, 0, sizeof(st));
 106         ret = stat(dev, &st);
 107         if (ret < 0)
 108                 panic("Cannot identify %s!\n", dev);
 109
 110         if (!S_ISCHR(st.st_mode))
 111                 panic("%s is no device!\n", dev);
 112
 113         fd = open(dev, O_RDWR | O_NONBLOCK, 0);
 114         if (fd < 0)
 115                 panic("Cannot open %s!\n", dev);
 116
 117         return fd;
 118 }
 119
 120 void close_vl4_device(int fd)
 121 {
 122         if (fd < 0)
 123                 return;
 124         close(fd);
 125 }
 126
 127 int start_v4l_capturing(int fd, unsigned int nbuffs)
 128 {
 129         int ret;
 130         unsigned int i;
 131         enum v4l2_buf_type type;
 132
 133         for (i = 0; i < nbuffs; ++i) {
 134                 struct v4l2_buffer buf;
 135
 136                 memset(&buf, 0, sizeof(buf));
 137                 buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 138                 buf.memory = V4L2_MEMORY_MMAP;
 139                 buf.index  = i;
 140
 141                 ret = xioctl(fd, VIDIOC_QBUF, &buf);
 142                 if (ret < 0)
 143                         panic("I/O enqueue error!\n");
 144         }
 145
 146         type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 147         ret = xioctl(fd, VIDIOC_STREAMON, &type);
 148         if (ret < 0)
 149                 panic("Cannot start capture!\n");
 150
 151         return 0;
 152 }
 153
 154 int stop_v4l_capturing(int fd)
 155 {
 156         int ret;
 157         enum v4l2_buf_type type;
 158
 159         type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 160         ret = xioctl(fd, VIDIOC_STREAMOFF, &type);
 161         if (ret < 0)
 162                 panic("Cannot stop capture!\n");
 163
 164         return 0;
 165 }
 166
 167 int init_mmap(int fd, const char *dev, unsigned int *nbuffs,
 168               struct v4l_buff **buffs, int frames)
 169 {
 170         int ret;
 171         struct v4l2_requestbuffers setup;
 172
 173         if (unlikely(!dev || !nbuffs || !buffs))
 174                 return -EINVAL;
 175         if (frames == 0)
 176                 frames = 3; /* default */
 177
 178         memset(&setup, 0, sizeof(setup));
 179         setup.count  = frames; /* queuelen */
 180         setup.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 181         setup.memory = V4L2_MEMORY_MMAP;
 182
 183         ret = xioctl(fd, VIDIOC_REQBUFS, &setup);
 184         if (ret < 0) {
 185                 if (EINVAL == errno)
 186                         panic("No mmap support!\n");
 187                 else
 188                         panic("I/O error!\n");
 189         }
 190
 191         if (setup.count < 2)
 192                 panic("Insufficient buffer memory!\n");
 193
 194         (*buffs) = xzmalloc(setup.count * sizeof(**buffs));
 195
 196         for ((*nbuffs) = 0; (*nbuffs) < setup.count; ++(*nbuffs)) {
 197                 struct v4l2_buffer buf;
 198
 199                 memset(&buf, 0, sizeof(buf));
 200                 buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 201                 buf.memory = V4L2_MEMORY_MMAP;
 202                 buf.index  = (*nbuffs);
 203
 204                 ret = xioctl(fd, VIDIOC_QUERYBUF, &buf);
 205                 if (ret < 0)
 206                         panic("Cannot query buf!\n");
 207
 208                 (*buffs)[(*nbuffs)].length = buf.length;
 209                 (*buffs)[(*nbuffs)].start = mmap(NULL, buf.length,
 210                                                  PROT_READ | PROT_WRITE,
 211                                                  MAP_SHARED, fd, buf.m.offset);
 212                 if (MAP_FAILED == (*buffs)[(*nbuffs)].start)
 213                         panic("Error on mmap!\n");
 214         }
 215
 216         return 0;
 217 }
 218
 219 void set_v4l_ctl_or_die(int fd, unsigned int id, int value)
 220 {
 221         int ret;
 222         struct v4l2_control control_s;
 223         struct v4l2_queryctrl queryctrl;
 224
 225         memset(&queryctrl, 0, sizeof(queryctrl));
 226         queryctrl.id = id;
 227
 228         ret = xioctl(fd, VIDIOC_QUERYCTRL, &queryctrl);
 229         if (ret < 0)
 230                 panic("Cannot query cmd id %x!\n", id);
 231         if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
 232                 panic("Cmd id %x disabled!\n", id);
 233         if (queryctrl.type != V4L2_CTRL_TYPE_BOOLEAN &&
 234             queryctrl.type != V4L2_CTRL_TYPE_INTEGER &&
 235             queryctrl.type != V4L2_CTRL_TYPE_MENU)
 236                 panic("Cmd id %x has unsupported query type!\n", id);
 237         if (value < queryctrl.minimum)
 238                 value = queryctrl.minimum;
 239         if (value > queryctrl.maximum)
 240                 value = queryctrl.maximum;
 241         if (value < 0)
 242                 value = queryctrl.default_value;
 243
 244         memset(&control_s, 0, sizeof(control_s));
 245         control_s.id = id;
 246         control_s.value = value;
 247
 248         ret = xioctl(fd, VIDIOC_S_CTRL, &control_s);
 249         if (ret < 0)
 250                 panic("Cannot set cmd id %x!\n", id);
 251 }
 252
 253 int init_v4l_device(int fd, int width, int height)
 254 {
 255         int ret;
 256         v4l2_std_id esid0;
 257         struct v4l2_capability cap;
 258         struct v4l2_control control;
 259         struct v4l2_cropcap cropcap;
 260         struct v4l2_crop crop;
 261         struct v4l2_format fmt;
 262         struct v4l2_streamparm fps;
 263
 264         memset(&cap, 0, sizeof(cap));
 265         ret = xioctl(fd, VIDIOC_QUERYCAP, &cap);
 266         if (ret < 0) {
 267                 if(EINVAL == errno)
 268                         panic("This is no V4L2 device!\n");
 269                 else
 270                         panic("V4L2 error on device!\n");
 271         }
 272
 273         if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0)
 274                 panic("This is no video capture device!\n");
 275
 276         if ((cap.capabilities & V4L2_CAP_STREAMING) == 0)
 277                 panic("No streaming support for this device!\n");
 278
 279         memset(&cropcap, 0, sizeof(cropcap));
 280         cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 281
 282         ret = xioctl(fd, VIDIOC_CROPCAP, &cropcap);
 283         if (ret == 0) {
 284                 memset(&crop, 0, sizeof(crop));
 285                 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 286                 crop.c    = cropcap.defrect;
 287
 288                 ret = xioctl(fd, VIDIOC_S_CROP, &crop);
 289                 if (ret < 0) {
 290                         switch(errno) {
 291                         case EINVAL:
 292                                 /* Cropping not supported. */
 293                         default:
 294                                 break;
 295                         }
 296                 }
 297         }
 298
 299         /* This is for the buggy AMD driver, we need to do this first! */
 300         memset(&control, 0, sizeof(control));
 301         control.id    = V4L2_CID_CAM_INIT;
 302         control.value = 0;
 303
 304         ret = xioctl(fd, VIDIOC_S_CTRL, &control);
 305         if (ret < 0)
 306                 panic("%s cannot set cam init!\n");
 307
 308         /* Aldebaran setting */
 309         if (width == 640 && height == 480)
 310                 esid0 = 0x08000000UL; /*VGA*/
 311         else if (width == 320 && height == 240)
 312                 esid0 = 0x04000000UL; /*QVGA*/
 313         else
 314                 panic("Panic over invalid width/height settings!\n");
 315
 316         ret = xioctl(fd, VIDIOC_S_STD, &esid0);
 317         if (ret < 0)
 318                 panic("Cannot set Aldebaran video i/o std!\n");
 319
 320         memset(&fmt, 0, sizeof(fmt));
 321         fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 322         fmt.fmt.pix.width       = width;
 323         fmt.fmt.pix.height      = height;
 324         fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
 325         fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;
 326
 327         ret = xioctl(fd, VIDIOC_S_FMT, &fmt);
 328         if (ret < 0)
 329                 panic("Cannot set v4l format!\n");
 330
 331         set_v4l_ctl_or_die(fd, V4L2_CID_AUTO_WHITE_BALANCE, 0);
 332         set_v4l_ctl_or_die(fd, V4L2_CID_AUTOGAIN, 0);
 333         set_v4l_ctl_or_die(fd, V4L2_CID_HUE_AUTO, 0);
 334         set_v4l_ctl_or_die(fd, V4L2_CID_EXPOSURE_AUTO, 0);
 335         set_v4l_ctl_or_die(fd, V4L2_CID_HFLIP, 0);
 336         set_v4l_ctl_or_die(fd, V4L2_CID_VFLIP, 0);
 337
 338         memset(&fps, 0, sizeof(fps));
 339         fps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 340
 341         ret = xioctl(fd, VIDIOC_G_PARM, &fps);
 342         if (ret < 0)
 343                 panic("Cannot query fps param!\n");
 344
 345         fps.parm.capture.timeperframe.numerator   = 1;
 346         fps.parm.capture.timeperframe.denominator = 30;
 347
 348         ret = xioctl(fd, VIDIOC_S_PARM, &fps);
 349         if (ret < 0)
 350                 panic("Cannot set fps param!\n");
 351
 352         return 0;
 353 }
 354
 355 int cleanup_v4l_device(int fd, unsigned int nbuffs,
 356                        struct v4l_buff *buffs)
 357 {
 358         int ret;
 359         unsigned int i;
 360
 361         for (i = 0; i < nbuffs; ++i) {
 362                 ret = munmap(buffs[i].start, buffs[i].length);
 363                 if (ret < 0)
 364                         panic("Cannot munmap!\n");
 365         }
 366
 367         xfree(buffs);
 368         return 0;
 369 }
 370
 371 void debug_v4l_struct_buffer(struct v4l2_buffer *img)
 372 {
 373         printf("index: %u\n", img->index);
 374         printf("type:  %u\n", img->type);
 375         printf("bytes used: %u\n", img->bytesused);
 376         printf("flags: %u (0x%x), mapped:%d, queued:%d, done:%d\n",
 377                 img->flags, img->flags,
 378                (img->flags & V4L2_BUF_FLAG_MAPPED),
 379                (img->flags & V4L2_BUF_FLAG_QUEUED),
 380                (img->flags & V4L2_BUF_FLAG_DONE));
 381         printf("field: %u\n", img->field);
 382         printf("sequence: %u\n", img->sequence);
 383         printf("length: %u\n", img->length);
 384         printf("input: %u\n", img->input);
 385         printf("reserved: %u\n\n", img->reserved);
 386 }
 387
 388