camv4l.c

   1 #include "camv4l.h"
   2 /**
   3  *  \file  camv4l.c
   4  *
   5  *  \brief This file implement function.
   6  *
   7  */
   8
   9 ///Definitions
  10 int PAL[] ={VIDEO_PALETTE_YUV420P,VIDEO_PALETTE_RGB565,VIDEO_PALETTE_RGB24,VIDEO_PALETTE_RGB32};
  11
  12 #define LEN_PALLETE     4
  13
  14 int SIZE[] = { 640,480,384,288,352,288,320,240,192,144,176,144,160,120 };
  15
  16
  17 static int GetVideoPict (struct vdIn *vd);
  18
  19 static int SetVideoPict (struct vdIn *vd);
  20
  21 static int GetDepth ( int format);
  22
  23 static int probePalette ( struct vdIn *vd );
  24
  25 static int probeSize ( struct vdIn *vd );
  26
  27
  28 int init_v4l (struct vdIn *vd)
  29 {
  30         int f;
  31         int erreur = 0;
  32
  33         if ((vd->fd = open (vd->videodevice, O_RDWR)) == -1){
  34       perror ("ERROR opening V4L interface \n");
  35       exit (1);
  36         }
  37         if (ioctl (vd->fd, VIDIOCGCAP, &(vd->videocap)) == -1){
  38       printf ("wrong device\n");
  39       exit (1);
  40         }
  41         printf ("Camera found: %s \n", vd->videocap.name);
  42         snprintf (vd->cameraname, 32, "%s", vd->videocap.name);
  43
  44         erreur = GetVideoPict (vd);
  45         vd->formatIn = vd->videopict.palette;
  46         vd->bppIn = vd->videopict.depth;
  47         vd->flipUV = 0;
  48
  49   /* To Compute the estimate frame size perhaps not need !!!
  50   if ((vd->bppIn = GetDepth (vd->formatIn)) < 0)
  51     {
  52       perror ("getdepth  failed \n");
  53       exit (1);
  54     }
  55    */
  56         if (ioctl (vd->fd, VIDIOCGCHAN, &vd->videochan) == -1){
  57       printf ("did not support Video_channel\n");
  58       vd->cameratype = UNKNOW;
  59    } else {
  60       printf ("Bridge found: %s \n", vd->videochan.name);
  61       snprintf (vd->bridge, 9, "%s", vd->videochan.name);
  62       vd->cameratype = UNKNOW;// GetStreamId (vd->videochan.name);
  63         }
  64
  65         if (vd->cameratype == UNKNOW){
  66         /* process others cams default parameters should be set */
  67                 printf ("StreamId: %d Unknow Camera\n", vd->cameratype);
  68                 printf("Probe Pallete\n");
  69                 if (probePalette (vd ) < 0) {
  70                         printf ("Sorry cannot probe Palette for that Unknow Camera\n");
  71                         exit (0);
  72                 }
  73                 printf("Probe Size\n");
  74                 if (probeSize (vd ) < 0) {
  75                         printf ("Sorry cannot probe Size for that Unknow Camera\n");
  76                         exit (0);
  77                 }
  78         } else {
  79                 printf("Find camera with supported bridge\n");
  80         }
  81
  82         /**********************************************************/
  83         /*          alloc the frame buffer to read                */
  84         vd->framesizeIn = (vd->hdrwidth * vd->hdrheight * (vd->bppIn >> 3));
  85         /***************************            *******************/
  86         if (vd->grabMethod){
  87       printf (" grabbing method default MMAP asked \n");
  88       // MMAP VIDEO acquisition
  89       memset (&(vd->videombuf), 0, sizeof (vd->videombuf));
  90       if (ioctl (vd->fd, VIDIOCGMBUF, &(vd->videombuf)) < 0){
  91                         perror (" init VIDIOCGMBUF FAILED\n");
  92                 }
  93
  94                 printf ("VIDIOCGMBUF size %d  frames %d  offets[0]=%d offsets[1]=%d\n",
  95                 vd->videombuf.size, vd->videombuf.frames,
  96                 vd->videombuf.offsets[0], vd->videombuf.offsets[1]);
  97
  98       vd->pFramebuffer = (unsigned char *) mmap (0, vd->videombuf.size, PROT_READ | PROT_WRITE,MAP_SHARED, vd->fd, 0);
  99       vd->mmapsize = vd->videombuf.size;
 100       vd->vmmap.height = vd->hdrheight;
 101       vd->vmmap.width = vd->hdrwidth;
 102       vd->vmmap.format = vd->formatIn;
 103
 104                 for (f = 0; f < vd->videombuf.frames; f++){
 105                         vd->vmmap.frame = f;
 106                         if (ioctl (vd->fd, VIDIOCMCAPTURE, &(vd->vmmap))){
 107                       perror ("cmcapture");
 108                         }
 109                 }
 110                 vd->vmmap.frame = 0;
 111
 112         } else {
 113       /* read method */
 114                 printf (" grabbing method READ asked \n");
 115       if (ioctl (vd->fd, VIDIOCGWIN, &(vd->videowin)) < 0){
 116                         perror ("VIDIOCGWIN failed \n");
 117                 }
 118
 119                 vd->videowin.height = vd->hdrheight;
 120       vd->videowin.width = vd->hdrwidth;
 121
 122                 if (ioctl (vd->fd, VIDIOCSWIN, &(vd->videowin)) < 0){
 123                         perror ("VIDIOCSWIN failed \n");
 124                 }
 125       printf ("VIDIOCSWIN height %d  width %d \n", vd->videowin.height, vd->videowin.width);
 126         }
 127
 128         vd->pixTmp = (unsigned char *) malloc (vd->framesizeIn);
 129
 130         return erreur;
 131 }
 132
 133
 134 int close_v4l (struct vdIn *vd){
 135         if (vd->grabMethod){
 136                 printf ("unmapping frame buffer\n");
 137       munmap (vd->pFramebuffer, vd->mmapsize);
 138         }
 139         printf ("freeing frame buffer\n");
 140         free (vd->pixTmp);
 141
 142         printf ("close v4l \n");
 143         close (vd->fd);
 144         return 0;
 145 }
 146
 147
 148 static void flipUV (unsigned char *src, int format, int w, int h){
 149         __u32 *lpix;
 150         __u16 *pix;
 151         __u8 *V;
 152         __u8 *U;
 153         __u8 *savC = NULL;
 154         unsigned char tmp;
 155         int sizetransfert = 0;
 156         int i;
 157
 158         if (format == VIDEO_PALETTE_RAW_JPEG){
 159       /*nothing todo */
 160       return;
 161         }
 162
 163         switch (format){
 164     case VIDEO_PALETTE_YUV420P:{
 165                 /* allocate a helper buffer */
 166                 sizetransfert = (w * h >> 2);
 167                 savC = (__u8 *) realloc (savC, (size_t) sizetransfert);
 168                 U = src + (w * h);
 169                 V = src + (w * h) + sizetransfert;
 170                 /* save U */
 171                 memcpy (savC, U, sizetransfert);
 172                 /* flip UV */
 173                 memcpy (U, V, sizetransfert);
 174                 /* restore U */
 175                 memcpy (V, savC, sizetransfert);
 176                 free (savC);
 177                 savC = NULL;
 178                 }
 179       break;
 180         case VIDEO_PALETTE_RGB565: {
 181                 pix = (__u16 *) src;
 182                 for (i = 0; i < (w * h); i++){
 183                         pix[i] = (((pix[i] & 0xF800) >> 11) | ((pix[i] & 0x001F) << 11) | (pix[i] & 0x07E0));
 184                 }
 185         }
 186       break;
 187    case VIDEO_PALETTE_RGB24:{
 188                 for (i = 0; i < (w * h * 3); i += 3){
 189                         tmp = src[i];
 190                         src[i] = src[i + 2];
 191                         src[i + 2] = tmp;
 192                 }
 193    }
 194       break;
 195    case VIDEO_PALETTE_RGB32:{
 196                 lpix = (__u32 *) src;
 197                 for (i = 0; i < (w * h); i++){
 198                     lpix[i] = (((lpix[i] & 0x00FF0000) >> 16) | ((lpix[i] & 0x000000FF) << 16)
 199                | (lpix[i] & 0x0000FF00));
 200                 }
 201    }
 202       break;
 203    default:
 204          break;
 205   }
 206 }
 207
 208
 209 int grab (struct vdIn *vd){
 210         int ff;
 211         int len;
 212         int status=0;
 213         //int count = 0;
 214         int size;
 215         int erreur = 0;
 216
 217         if (vd->grabMethod){
 218       ff = vd->vmmap.frame;
 219       vd->vmmap.height = vd->hdrheight;
 220       vd->vmmap.width = vd->hdrwidth;
 221       vd->vmmap.format = vd->formatIn;
 222       if (ioctl (vd->fd, VIDIOCSYNC, &ff) < 0){
 223                         perror ("cvsync err\n");
 224                         erreur = -1;
 225                 }
 226       vd->vmmap.frame = ff;
 227       memcpy (vd->pixTmp,vd->pFramebuffer + vd->videombuf.offsets[vd->vmmap.frame], vd->framesizeIn);
 228
 229       if (vd->flipUV){
 230                         printf("Flip UV\n");
 231                         flipUV (vd->pixTmp, vd->formatIn, vd->hdrwidth, vd->hdrheight);
 232                 }
 233
 234       if ((ioctl (vd->fd, VIDIOCMCAPTURE, &(vd->vmmap))) < 0){
 235                         perror ("cmcapture");
 236                         printf (">>cmcapture err %d\n", status);
 237                         erreur = -1;
 238                 }
 239       vd->vmmap.frame = (vd->vmmap.frame + 1) % vd->videombuf.frames;
 240       //printf("frame nb %d\n",vd->vmmap.frame);
 241         } else {
 242       /* read method */
 243       size = vd->framesizeIn;
 244       len = read (vd->fd, vd->pixTmp, size);
 245       if (len != size){
 246                         printf ("v4l read error\n");
 247                         printf ("len %d asked %d \n", len, size);
 248                         erreur = -1;
 249                 }
 250       if (vd->flipUV){
 251                         flipUV (vd->pixTmp, vd->formatIn, vd->hdrwidth, vd->hdrheight);
 252                 }
 253         }
 254         return erreur;
 255 }
 256
 257 int setPalette (struct vdIn *vd)
 258 {
 259         vd->bppIn = GetDepth (vd->formatIn);
 260         vd->videopict.palette = vd->formatIn;
 261         vd->videopict.depth = vd->bppIn;
 262         SetVideoPict (vd);
 263         vd->framesizeIn = ((vd->hdrwidth * vd->hdrheight * vd->bppIn) >> 3);
 264         vd->pixTmp = (unsigned char *) realloc (vd->pixTmp, (size_t) vd->framesizeIn);
 265
 266         return 1;
 267 }
 268
 269 /* probe palette and set a default one for unknow cams*/
 270 static int probePalette ( struct vdIn *vd )
 271 {
 272         struct video_picture pict;
 273         int masq = 0x2;
 274         int i;
 275         int availpal = 0;
 276         int defaut = 1;
 277         /* initialize the internal struct */
 278         if (ioctl (vd->fd, VIDIOCGPICT, &pict) < 0)
 279                 {
 280                 perror ("Couldnt get videopict params with VIDIOCGPICT\n");
 281                 return -1;
 282                  }
 283         /* try each palette we have we skip raw_jpeg */
 284         for(i = 0; i < LEN_PALLETE ; i++){
 285                 pict.palette = PAL[i];
 286                 /* maybe correct the bug on qca driver depth always 24 ? */
 287                 pict.depth = GetDepth (PAL[i]);
 288                 printf("try palette %d depth %d\n",pict.palette,pict.depth);
 289                 if (ioctl (vd->fd, VIDIOCSPICT, &pict) < 0){
 290                         printf("Couldn't set palette first try %d \n", PAL[i]);
 291                 }
 292                 if (ioctl (vd->fd, VIDIOCGPICT, &pict) < 0){
 293                 printf("Couldnt get palette %d \n", PAL[i]);
 294         }
 295                 if (pict.palette != PAL[i]){
 296                         printf("Damned second try fail \n");
 297                 }
 298                 else {
 299                         availpal = availpal | masq ;
 300                         printf("Available  palette %d \n", PAL[i]);
 301                         if (defaut){
 302                                 defaut = 0;
 303                                 vd->formatIn = PAL[i];
 304                                 vd->bppIn = GetDepth (PAL[i]);
 305                         }
 306                 }
 307                 masq = masq << 1;
 308         }
 309         vd->palette = availpal;
 310         printf("Vybrana paleta je %d\n",availpal);
 311         //should set default palette here ?
 312         return 1;
 313 }
 314
 315
 316 /* probe size and set a default one for unknow cams */
 317 static int probeSize ( struct vdIn *vd )
 318 {
 319         struct video_window win;
 320         int maxw,minw,maxh,minh;
 321         int masq = 0x1;
 322         int i = 0;
 323         int defaut = 1 ;
 324         /* initialize de parameters */
 325         maxw = vd->videocap.maxwidth;
 326         minw = vd->videocap.minwidth;
 327         maxh = vd->videocap.maxheight;
 328         minh = vd->videocap.minheight;
 329         printf("probe size in \n");
 330
 331         while (SIZE[i] > maxw){
 332                 printf("skip size %d x %d\n",SIZE[i],SIZE[i+1]);
 333                 i += 2;
 334                 masq = masq << 1;
 335                 if (i > 13) break;
 336         }
 337         /* initialize the internal struct */
 338         if (ioctl (vd->fd, VIDIOCGWIN, &win) < 0) {
 339                 perror ("VIDIOCGWIN failed \n");
 340                 return -1;
 341         }
 342         /* now i is on the first possible width */
 343         while ((SIZE[i] >= minw) && i < 13) {
 344                 win.width = SIZE[i];
 345                 win.height = SIZE[i+1];
 346                 if (ioctl (vd->fd, VIDIOCSWIN, &win) < 0) {
 347                 printf ("VIDIOCSWIN reject width %d  height %d \n",
 348                         win.width, win.height);
 349         } else {
 350                         vd->sizeothers = vd->sizeothers | masq;
 351                         printf ("Available Resolutions width %d  heigth %d \n",
 352                         win.width, win.height);
 353                         if (defaut){
 354                                 vd->hdrwidth = win.width;
 355                                 vd->hdrheight = win.height;
 356                                 defaut = 0;
 357                         }
 358                 }
 359                 masq = masq << 1 ;
 360                 i += 2;
 361         }
 362         return 1;
 363 }
 364
 365 int changeSize (struct vdIn *vd)
 366 {
 367         int erreur;
 368         erreur = GetVideoPict (vd);
 369         vd->formatIn = vd->videopict.palette;
 370         vd->bppIn = vd->videopict.depth;
 371         /* To Compute the estimate frame size perhaps not need !!! */
 372         if ((vd->bppIn = GetDepth (vd->formatIn)) < 0){
 373                 perror ("getdepth  failed \n");
 374       exit (1);
 375         }
 376         if (vd->grabMethod){
 377       vd->vmmap.height = vd->hdrheight;
 378       vd->vmmap.width = vd->hdrwidth;
 379       vd->vmmap.format = vd->formatIn;
 380    } else {
 381                 if (ioctl (vd->fd, VIDIOCGWIN, &vd->videowin) < 0)
 382                         perror ("VIDIOCGWIN failed \n");
 383       vd->videowin.height = vd->hdrheight;
 384       vd->videowin.width = vd->hdrwidth;
 385       if (ioctl (vd->fd, VIDIOCSWIN, &vd->videowin) < 0)
 386                         perror ("VIDIOCSWIN failed \n");
 387
 388       printf ("VIDIOCGWIN height %d  width %d \n",vd->videowin.height, vd->videowin.width);
 389    }
 390         vd->framesizeIn = ((vd->hdrwidth * vd->hdrheight * vd->bppIn) >> 3);
 391         vd->pixTmp =
 392    (unsigned char *) realloc (vd->pixTmp, (size_t) vd->framesizeIn);
 393         return 1;
 394 }
 395
 396
 397 static int GetVideoPict (struct vdIn *vd)
 398 {
 399   if (ioctl (vd->fd, VIDIOCGPICT, &vd->videopict) < 0){
 400       perror ("Couldnt get videopict params with VIDIOCGPICT\n");
 401       return -1;
 402   }
 403
 404   printf ("VIDIOCGPICT brightnes=%d hue=%d color=%d contrast=%d whiteness=%d"
 405           "depth=%d palette=%d\n", vd->videopict.brightness,
 406           vd->videopict.hue, vd->videopict.colour, vd->videopict.contrast,
 407           vd->videopict.whiteness, vd->videopict.depth,
 408           vd->videopict.palette);
 409
 410   return 0;
 411 }
 412
 413 static int SetVideoPict (struct vdIn *vd)
 414 {
 415         if (ioctl (vd->fd, VIDIOCSPICT, &vd->videopict) < 0){
 416       perror ("Couldnt set videopict params with VIDIOCSPICT\n");
 417       return -1;
 418         }
 419
 420         printf ("VIDIOCSPICT brightnes=%d hue=%d color=%d contrast=%d whiteness=%d"
 421           "depth=%d palette=%d\n", vd->videopict.brightness,
 422           vd->videopict.hue, vd->videopict.colour, vd->videopict.contrast,
 423           vd->videopict.whiteness, vd->videopict.depth,
 424           vd->videopict.palette);
 425
 426   return 0;
 427 }
 428
 429 static int GetDepth(int format)
 430 {
 431   int depth;
 432   switch (format)
 433     {
 434     case VIDEO_PALETTE_RAW_JPEG:
 435       {
 436         depth = 8;              // be sure spca50x ask raw data
 437       }
 438       break;
 439     case VIDEO_PALETTE_YUV420P:
 440       {
 441         depth = (8 * 3) >> 1;
 442       }
 443       break;
 444     case VIDEO_PALETTE_RGB565:
 445       depth = 16;
 446       break;
 447     case VIDEO_PALETTE_RGB24:
 448       depth = 24;
 449       break;
 450     case VIDEO_PALETTE_RGB32:
 451       {
 452         depth = 32;
 453       }
 454       break;
 455     default:
 456       depth = -1;
 457       break;
 458     }
 459   return depth;
 460 }
 461
 462 __u8 getBrightness (struct vdIn * vdin)
 463 {
 464   if (GetVideoPict (vdin) < 0)
 465     {
 466       printf (" Error getBrightness \n");
 467       return 0;
 468     }
 469   return ((vdin->videopict.brightness) >> 8);
 470 }
 471
 472 void setBrightness (struct vdIn *vdin, __u8 bright)
 473 {
 474   vdin->videopict.brightness = bright << 8;
 475   if (SetVideoPict (vdin) < 0)
 476     {
 477       printf (" Error setBrightness \n");
 478     }
 479
 480 }
 481
 482 __u8 getContrast (struct vdIn *vdin)
 483 {
 484   if (GetVideoPict (vdin) < 0)
 485     {
 486       printf (" Error getContrast \n");
 487       return 0;
 488     }
 489   return ((vdin->videopict.contrast) >> 8);
 490 }
 491
 492 void setContrast (struct vdIn *vdin, __u8 contrast)
 493 {
 494   vdin->videopict.contrast = contrast << 8;
 495   if (SetVideoPict (vdin) < 0)
 496     {
 497       printf (" Error setContrast \n");
 498     }
 499 }
 500 __u8 getColors (struct vdIn *vdin)
 501 {
 502   if (GetVideoPict (vdin) < 0)
 503     {
 504       printf (" Error getColors \n");
 505       return 0;
 506     }
 507   return ((vdin->videopict.colour) >> 8);
 508 }
 509
 510 void setColors (struct vdIn *vdin, __u8 colors)
 511 {
 512   vdin->videopict.colour = colors << 8;
 513   if (SetVideoPict (vdin) < 0)
 514     {
 515       printf (" Error setColors \n");
 516     }
 517 }