user/w3cam/v4l.c

   1 /*
   2  * v4l.c
   3  *
   4  * Copyright (C) 2001 Rasca, Berlin
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software
  18  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  19  */
  20
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <sys/types.h>
  24 #include <sys/ioctl.h>
  25 #include <sys/mman.h>
  26 #include <fcntl.h>
  27 #include <unistd.h>
  28 #include <linux/types.h>
  29 #include <linux/videodev.h>
  30 #include "v4l.h"
  31
  32 #define min(a,b) ((a) < (b) ? (a) : (b))
  33 #define max(a,b) ((a) > (b) ? (a) : (b))
  34
  35 /*
  36  * set the input and norm for the video4linux device
  37  */
  38 int
  39 v4l_set_input (int fd, int input, int norm)
  40 {
  41         struct video_channel vid_chnl;
  42
  43         if (input != INPUT_DEFAULT || norm != NORM_DEFAULT) {
  44                 if (vid_chnl.channel != INPUT_DEFAULT)
  45                         vid_chnl.channel = input;
  46                 else
  47                         vid_chnl.channel = 0;
  48                 vid_chnl.norm = -1;
  49                 if (ioctl (fd, VIDIOCGCHAN, &vid_chnl) == -1) {
  50                         perror ("ioctl (VIDIOCGCHAN)");
  51                         return -1;
  52                 } else {
  53                         if (input != 0)
  54                                 vid_chnl.channel = input;
  55                         if (norm != NORM_DEFAULT)
  56                                 vid_chnl.norm    = norm;
  57                         if (ioctl (fd, VIDIOCSCHAN, &vid_chnl) == -1) {
  58                                 perror ("ioctl (VIDIOCSCHAN)");
  59                                 return -1;
  60                         }
  61                 }
  62         }
  63         return 0;
  64 }
  65
  66 /*
  67  * check the size and readjust if necessary
  68  */
  69 int
  70 v4l_check_size (int fd, int *width, int *height)
  71 {
  72         struct video_capability vid_caps;
  73
  74         if (ioctl (fd, VIDIOCGCAP, &vid_caps) == -1) {
  75                 perror ("ioctl (VIDIOCGCAP)");
  76                 return -1;
  77         }
  78         /* readjust if necessary */
  79         if (*width > vid_caps.maxwidth || *width < vid_caps.minwidth) {
  80                 *width = min (*width, vid_caps.maxwidth);
  81                 *width = max (*width, vid_caps.minwidth);
  82                 fprintf (stderr, "readjusting width to %d\n", *width);
  83         }
  84         if (*height > vid_caps.maxheight || *height < vid_caps.minheight) {
  85                 *height = min (*height, vid_caps.maxheight);
  86                 *height = max (*height, vid_caps.minheight);
  87                 fprintf (stderr, "readjusting height to %d\n", *height);
  88         }
  89         return 0;
  90 }
  91
  92 /*
  93  * check the requested palette and adjust if possible
  94  * seems not to work :-(
  95  */
  96 int
  97 v4l_check_palette (int fd, int *palette)
  98 {
  99         struct video_picture vid_pic;
 100
 101         if (!palette)
 102                 return -1;
 103
 104         if (ioctl (fd, VIDIOCGPICT, &vid_pic) == -1) {
 105                 perror ("ioctl (VIDIOCGPICT)");
 106                 return -1;
 107         }
 108         vid_pic.palette = *palette;
 109         if (ioctl (fd, VIDIOCSPICT, &vid_pic) == -1) {
 110                 /* try YUV420P
 111                  */
 112                 fprintf (stderr, "failed\n");
 113                 vid_pic.palette = *palette = VIDEO_PALETTE_YUV420P;
 114                 if (ioctl (fd, VIDIOCSPICT, &vid_pic) == -1) {
 115                         perror ("ioctl (VIDIOCSPICT) to YUV");
 116                         /* ok, try grayscale..
 117                          */
 118                         vid_pic.palette = *palette = VIDEO_PALETTE_GREY;
 119                         if (ioctl (fd, VIDIOCSPICT, &vid_pic) == -1) {
 120                                 perror ("ioctl (VIDIOCSPICT) to GREY");
 121                                 return -1;
 122                         }
 123                 }
 124         }
 125         return 0;
 126 }
 127
 128 /*
 129  * check if driver supports mmap'ed buffer
 130  */
 131 int
 132 v4l_check_mmap (int fd, int *size)
 133 {
 134         struct video_mbuf vid_buf;
 135
 136         if (ioctl (fd, VIDIOCGMBUF, &vid_buf) == -1) {
 137                 return -1;
 138         }
 139         if (size)
 140                 *size = vid_buf.size;
 141         return 0;
 142 }
 143
 144 /*
 145  * mute sound if available
 146  */
 147 int
 148 v4l_mute_sound (int fd)
 149 {
 150         struct video_capability vid_caps;
 151         struct video_audio vid_aud;
 152
 153         if (ioctl (fd, VIDIOCGCAP, &vid_caps) == -1) {
 154                 perror ("ioctl (VIDIOCGCAP)");
 155                 return -1;
 156         }
 157         if (vid_caps.audios > 0) {
 158                 /* mute the sound */
 159                 if (ioctl (fd, VIDIOCGAUDIO, &vid_aud) == -1) {
 160                         return -1;
 161         } else {
 162             vid_aud.flags = VIDEO_AUDIO_MUTE;
 163             if (ioctl (fd, VIDIOCSAUDIO, &vid_aud) == -1)
 164                                 return -1;
 165         }
 166     }
 167         return 0;
 168 }
 169
 170 /*
 171  * Turn a YUV4:2:0 block into an RGB block
 172  *
 173  * Video4Linux seems to use the blue, green, red channel
 174  * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
 175  *
 176  * Color space conversion coefficients taken from the excellent
 177  * http://www.inforamp.net/~poynton/ColorFAQ.html
 178  * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
 179  * Y values are given for all 4 pixels, but the U (Pb)
 180  * and V (Pr) are assumed constant over the 2x2 block.
 181  *
 182  * To avoid floating point arithmetic, the color conversion
 183  * coefficients are scaled into 16.16 fixed-point integers.
 184  * They were determined as follows:
 185  *
 186  *      double brightness = 1.0;  (0->black; 1->full scale)
 187  *      double saturation = 1.0;  (0->greyscale; 1->full color)
 188  *      double fixScale = brightness * 256 * 256;
 189  *      int rvScale = (int)(1.402 * saturation * fixScale);
 190  *      int guScale = (int)(-0.344136 * saturation * fixScale);
 191  *      int gvScale = (int)(-0.714136 * saturation * fixScale);
 192  *      int buScale = (int)(1.772 * saturation * fixScale);
 193  *      int yScale = (int)(fixScale);
 194  */
 195
 196 /* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
 197 #define LIMIT(x) ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)))
 198
 199 /*
 200  */
 201 static inline void
 202 v4l_copy_420_block (int yTL, int yTR, int yBL, int yBR, int u, int v,
 203         int rowPixels, unsigned char * rgb, int bits)
 204 {
 205         const int rvScale = 91881;
 206         const int guScale = -22553;
 207         const int gvScale = -46801;
 208         const int buScale = 116129;
 209         const int yScale  = 65536;
 210         int r, g, b;
 211
 212         g = guScale * u + gvScale * v;
 213         r = rvScale * v;
 214         b = buScale * u;
 215
 216         yTL *= yScale; yTR *= yScale;
 217         yBL *= yScale; yBR *= yScale;
 218
 219         if (bits == 24) {
 220                 /* Write out top two pixels */
 221                 rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL); rgb[2] = LIMIT(r+yTL);
 222                 rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR); rgb[5] = LIMIT(r+yTR);
 223
 224                 /* Skip down to next line to write out bottom two pixels */
 225                 rgb += 3 * rowPixels;
 226                 rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL); rgb[2] = LIMIT(r+yBL);
 227                 rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR); rgb[5] = LIMIT(r+yBR);
 228         } else if (bits == 16) {
 229                 /* Write out top two pixels */
 230                 rgb[0] = ((LIMIT(b+yTL) >> 3) & 0x1F) | ((LIMIT(g+yTL) << 3) & 0xE0);
 231                 rgb[1] = ((LIMIT(g+yTL) >> 5) & 0x07) | (LIMIT(r+yTL) & 0xF8);
 232
 233                 rgb[2] = ((LIMIT(b+yTR) >> 3) & 0x1F) | ((LIMIT(g+yTR) << 3) & 0xE0);
 234                 rgb[3] = ((LIMIT(g+yTR) >> 5) & 0x07) | (LIMIT(r+yTR) & 0xF8);
 235
 236                 /* Skip down to next line to write out bottom two pixels */
 237                 rgb += 2 * rowPixels;
 238
 239                 rgb[0] = ((LIMIT(b+yBL) >> 3) & 0x1F) | ((LIMIT(g+yBL) << 3) & 0xE0);
 240                 rgb[1] = ((LIMIT(g+yBL) >> 5) & 0x07) | (LIMIT(r+yBL) & 0xF8);
 241
 242                 rgb[2] = ((LIMIT(b+yBR) >> 3) & 0x1F) | ((LIMIT(g+yBR) << 3) & 0xE0);
 243                 rgb[3] = ((LIMIT(g+yBR) >> 5) & 0x07) | (LIMIT(r+yBR) & 0xF8);
 244         }
 245 }
 246
 247 /*
 248  */
 249 static inline void
 250 v4l_copy_422_block (int yTL, int yTR, int u, int v,
 251         int rowPixels, unsigned char * rgb, int bits)
 252 {
 253         const int rvScale = 91881;
 254         const int guScale = -22553;
 255         const int gvScale = -46801;
 256         const int buScale = 116129;
 257         const int yScale  = 65536;
 258         int r, g, b;
 259
 260         g = guScale * u + gvScale * v;
 261         r = rvScale * v;
 262         b = buScale * u;
 263
 264         yTL *= yScale; yTR *= yScale;
 265
 266         if (bits == 24) {
 267                 /* Write out top two pixels */
 268                 rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL); rgb[2] = LIMIT(r+yTL);
 269                 rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR); rgb[5] = LIMIT(r+yTR);
 270
 271         } else if (bits == 16) {
 272                 /* Write out top two pixels */
 273                 rgb[0] = ((LIMIT(b+yTL) >> 3) & 0x1F) | ((LIMIT(g+yTL) << 3) & 0xE0);
 274                 rgb[1] = ((LIMIT(g+yTL) >> 5) & 0x07) | (LIMIT(r+yTL) & 0xF8);
 275
 276                 rgb[2] = ((LIMIT(b+yTR) >> 3) & 0x1F) | ((LIMIT(g+yTR) << 3) & 0xE0);
 277                 rgb[3] = ((LIMIT(g+yTR) >> 5) & 0x07) | (LIMIT(r+yTR) & 0xF8);
 278         }
 279 }
 280
 281 /*
 282  * convert a YUV420P to a rgb image
 283  */
 284 int
 285 v4l_yuv420p2rgb (unsigned char *rgb_out, unsigned char *yuv_in,
 286                 int width, int height, int bits)
 287 {
 288         const int numpix = width * height;
 289         const unsigned int bytes = bits >> 3;
 290         int h, w, y00, y01, y10, y11, u, v;
 291         unsigned char *pY = yuv_in;
 292         unsigned char *pU = pY + numpix;
 293         unsigned char *pV = pU + numpix / 4;
 294         unsigned char *pOut = rgb_out;
 295
 296         if (!rgb_out || !yuv_in)
 297                 return -1;
 298
 299         for (h = 0; h <= height - 2; h += 2) {
 300                 for (w = 0; w <= width - 2; w += 2) {
 301                         y00 = *(pY);
 302                         y01 = *(pY + 1);
 303                         y10 = *(pY + width);
 304                         y11 = *(pY + width + 1);
 305                         u = (*pU++) - 128;
 306                         v = (*pV++) - 128;
 307
 308                         v4l_copy_420_block (y00, y01, y10, y11, u, v, width, pOut, bits);
 309
 310                         pY += 2;
 311                         pOut += bytes << 1;
 312
 313                 }
 314                 pY += width;
 315                 pOut += width * bytes;
 316         }
 317         return 0;
 318 }
 319
 320 /*
 321  * convert a YUV422P to a rgb image
 322  */
 323 int
 324 v4l_yuv422p2rgb (unsigned char *rgb_out, unsigned char *yuv_in,
 325                 int width, int height, int bits)
 326 {
 327         const int numpix = width * height;
 328         const unsigned int bytes = bits >> 3;
 329         int h, w, y00, y01, u, v;
 330         unsigned char *pY = yuv_in;
 331         unsigned char *pU = pY + numpix;
 332         unsigned char *pV = pU + numpix / 2;
 333         unsigned char *pOut = rgb_out;
 334
 335         if (!rgb_out || !yuv_in)
 336                 return -1;
 337
 338         for (h = 0; h < height; h += 1) {
 339                 for (w = 0; w <= width - 2; w += 2) {
 340                         y00 = *(pY);
 341                         y01 = *(pY + 1);
 342                         u = (*pU++) - 128;
 343                         v = (*pV++) - 128;
 344
 345                         v4l_copy_422_block (y00, y01, u, v, width, pOut, bits);
 346
 347                         pY += 2;
 348                         pOut += bytes << 1;
 349
 350                 }
 351                 //pY += width;
 352                 //pOut += width * bytes;
 353         }
 354         return 0;
 355 }