utils.c

机器人仿真软件

/*******************************************************************************
#               uvcview: Sdl video Usb Video Class grabber           .         #
#This package work with the Logitech UVC based webcams with the mjpeg feature. #
#All the decoding is in user space with the embedded jpeg decoder              #
#.                                                                             #
#               Copyright (C) 2005 2006 Laurent Pinchart &&  Michel Xhaard     #
#                                                                              #
# This program is free software; you can redistribute it and/or modify         #
# it under the terms of the GNU General Public License as published by         #
# the Free Software Foundation; either version 2 of the License, or            #
# (at your option) any later version.                                          #
#                                                                              #
# This program is distributed in the hope that it will be useful,              #
# but WITHOUT ANY WARRANTY; without even the implied warranty of               #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                #
# GNU General Public License for more details.                                 #
#                                                                              #
# You should have received a copy of the GNU General Public License            #
# along with this program; if not, write to the Free Software                  #
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA    #
#                                                                              #
*******************************************************************************/

#include "utils.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <linux/types.h>
#include <string.h>
#include <fcntl.h>
#include <wait.h>
#include <time.h>
#include <limits.h>

#define ISHIFT 11

#define IFIX(a) ((int)((a) * (1 << ISHIFT) + .5))
#define IMULT(a, b) (((a) * (b)) >> ISHIFT)
#define ITOINT(a) ((a) >> ISHIFT)

#ifndef __P
# define __P(x) x
#endif

/* special markers */
#define M_BADHUFF       -1
#define M_EOF              0x80

struct jpeg_decdata {
    int dcts[6 * 64 + 16];
    int out[64 * 6];
    int dquant[3][64];
};

struct in {
    unsigned char *p;
    unsigned int bits;
    int left;
    int marker;

    int (*func) __P((void *));
    void *data;
};

/*********************************/
struct dec_hufftbl;
struct enc_hufftbl;

union hufftblp {
    struct dec_hufftbl *dhuff;
    struct enc_hufftbl *ehuff;
};

struct scan {
    int dc;                     /* old dc value */

    union hufftblp hudc;
    union hufftblp huac;
    int next;                     /* when to switch to next scan */

    int cid;                     /* component id */
    int hv;                     /* horiz/vert, copied from comp */
    int tq;                     /* quant tbl, copied from comp */
};

/*********************************/

#define DECBITS 10              /* seems to be the optimum */

struct dec_hufftbl {
    int maxcode[17];
    int valptr[16];
    unsigned char vals[256];
    unsigned int llvals[1 << DECBITS];
};

static void decode_mcus
__P((struct in *, int *, int, struct scan *, int *));
static int dec_readmarker __P((struct in *));
static void dec_makehuff
__P((struct dec_hufftbl *, int *, unsigned char *));

static void setinput __P((struct in *, unsigned char *));
/*********************************/

#undef PREC
#define PREC int

static void idctqtab __P((unsigned char *, PREC *));
static void idct __P((int *, int *, PREC *, PREC, int));
static void scaleidctqtab __P((PREC *, PREC));

/*********************************/

static void initcol __P((PREC[][64]));

static void col221111 __P((int *, unsigned char *, int));
static void yuyvtopict __P((int *, unsigned char *, int));

/*********************************/

#define M_SOI       0xd8
#define M_APP0       0xe0
#define M_DQT       0xdb
#define M_SOF0       0xc0
#define M_DHT   0xc4
#define M_DRI       0xdd
#define M_SOS       0xda
#define M_RST0       0xd0
#define M_EOI       0xd9
#define M_COM       0xfe

static unsigned char *datap;

static int getbyte(void)
{
    return *datap++;
}

static int getword(void)
{
    int c1, c2;
    c1 = *datap++;
    c2 = *datap++;
    return c1 << 8 | c2;
}

struct comp {
    int cid;
    int hv;
    int tq;
};

#define MAXCOMP 4
struct jpginfo {
    int nc;                     /* number of components */
    int ns;                     /* number of scans */
    int dri;                     /* restart interval */
    int nm;                     /* mcus til next marker */
    int rm;                     /* next restart marker */
};

static struct jpginfo info;
static struct comp comps[MAXCOMP];

static struct scan dscans[MAXCOMP];

static unsigned char quant[4][64];

static struct dec_hufftbl dhuff[4];

#define dec_huffdc (dhuff + 0)
#define dec_huffac (dhuff + 2)

static struct in in;

static int readtables(int till)
{
    int m, l, i, j, lq, pq, tq;
    int tc, th, tt;

    for (;;) {
       if (getbyte() != 0xff)
           return -1;
       if ((m = getbyte()) == till)
           break;

       switch (m) {
       case 0xc2:
           return 0;

       case M_DQT:
           lq = getword();
           while (lq > 2) {
              pq = getbyte();
              tq = pq & 15;
              if (tq > 3)
                  return -1;
              pq >>= 4;
              if (pq != 0)
                  return -1;
              for (i = 0; i < 64; i++)
                  quant[tq][i] = getbyte();
              lq -= 64 + 1;
           }
           break;

       case M_DHT:
           l = getword();
           while (l > 2) {
              int hufflen[16], k;
              unsigned char huffvals[256];

              tc = getbyte();
              th = tc & 15;
              tc >>= 4;
              tt = tc * 2 + th;
              if (tc > 1 || th > 1)
                  return -1;
              for (i = 0; i < 16; i++)
                  hufflen[i] = getbyte();
              l -= 1 + 16;
              k = 0;
              for (i = 0; i < 16; i++) {
                  for (j = 0; j < hufflen[i]; j++)
                     huffvals[k++] = getbyte();
                  l -= hufflen[i];
              }
              dec_makehuff(dhuff + tt, hufflen, huffvals);
           }
           break;

       case M_DRI:
           l = getword();
           info.dri = getword();
           break;

       default:
           l = getword();
           while (l-- > 2)
              getbyte();
           break;
       }
    }
    return 0;
}

static void dec_initscans(void)
{
    int i;

    info.nm = info.dri + 1;
    info.rm = M_RST0;
    for (i = 0; i < info.ns; i++)
       dscans[i].dc = 0;
}

static int dec_checkmarker(void)
{
    int i;

    if (dec_readmarker(&in) != info.rm)
       return -1;
    info.nm = info.dri;
    info.rm = (info.rm + 1) & ~0x08;
    for (i = 0; i < info.ns; i++)
       dscans[i].dc = 0;
    return 0;
}

int jpeg_decode(unsigned char **pic, unsigned char *buf, int *width,
              int *height)
{
    struct jpeg_decdata *decdata;
    int i, j, m, tac, tdc;
    int intwidth, intheight;
    int mcusx, mcusy, mx, my;
    int max[6];
    int err = 0;
    int enc411 = 1;
    decdata = (struct jpeg_decdata *) malloc(sizeof(struct jpeg_decdata));
    if (!decdata) {
       err = -1;
       goto error;
    }
    if (buf == NULL) {
       err = -1;
       goto error;
    }
    datap = buf;
    if (getbyte() != 0xff) {
       err = ERR_NO_SOI;
       goto error;
    }
    if (getbyte() != M_SOI) {
       err = ERR_NO_SOI;
       goto error;
    }
    if (readtables(M_SOF0)) {
       err = ERR_BAD_TABLES;
       goto error;
    }
    getword();
    i = getbyte();
    if (i != 8) {
       err = ERR_NOT_8BIT;
       goto error;
    }
    intheight = getword();
    intwidth = getword();

    //if ((intheight & 15) || (intwidth & 15)){
    if ((intheight & 7) || (intwidth & 15)) {
       err = ERR_BAD_WIDTH_OR_HEIGHT;
       goto error;
    }
    info.nc = getbyte();
    if (info.nc > MAXCOMP) {
       err = ERR_TOO_MANY_COMPPS;
       goto error;
    }
    for (i = 0; i < info.nc; i++) {
       int h, v;
       comps[i].cid = getbyte();
       comps[i].hv = getbyte();
       v = comps[i].hv & 15;
       h = comps[i].hv >> 4;
       comps[i].tq = getbyte();
       if (h > 3 || v > 3) {
           err = ERR_ILLEGAL_HV;
           goto error;
       }
       if (comps[i].tq > 3) {
           err = ERR_QUANT_TABLE_SELECTOR;
           goto error;
       }
    }
    if (readtables(M_SOS)) {
       err = ERR_BAD_TABLES;
       goto error;
    }
    getword();
    info.ns = getbyte();
    if (info.ns != 3) {
       err = ERR_NOT_YCBCR_221111;
       goto error;
    }
    for (i = 0; i < 3; i++) {
       dscans[i].cid = getbyte();
       tdc = getbyte();
       tac = tdc & 15;
       tdc >>= 4;
       if (tdc > 1 || tac > 1) {
           err = ERR_QUANT_TABLE_SELECTOR;
           goto error;
       }
       for (j = 0; j < info.nc; j++)
           if (comps[j].cid == dscans[i].cid)
              break;
       if (j == info.nc) {
           err = ERR_UNKNOWN_CID_IN_SCAN;
           goto error;
       }
       dscans[i].hv = comps[j].hv;
       dscans[i].tq = comps[j].tq;
       dscans[i].hudc.dhuff = dec_huffdc + tdc;
       dscans[i].huac.dhuff = dec_huffac + tac;
    }

    i = getbyte();
    j = getbyte();
    m = getbyte();

    if (i != 0 || j != 63 || m != 0) {
       err = ERR_NOT_SEQUENTIAL_DCT;
       goto error;
    }

    if (dscans[0].cid != 1 || dscans[1].cid != 2 || dscans[2].cid != 3) {
       err = ERR_NOT_YCBCR_221111;
       goto error;
    }

    if (dscans[1].hv != 0x11 || dscans[2].hv != 0x11) {
       err = ERR_NOT_YCBCR_221111;
       goto error;
    }
    /* if internal width and external are not the same or heigth too 
       and pic not allocated realloc the good size and mark the change 
       need 1 macroblock line more ?? */
    if (intwidth != *width || intheight != *height || *pic == NULL) {
       *width = intwidth;
       *height = intheight;
       // BytesperPixel 2 yuyv , 3 rgb24 
       *pic =
           (unsigned char *) realloc((unsigned char *) *pic,
                                  (size_t) intwidth * (intheight +
                                                    8) * 2);
    }


    switch (dscans[0].hv) {
    case 0x22:
       mcusx = *width >> 4;
       mcusy = *height >> 4;
       enc411 = 1;
       break;
    case 0x21:
       mcusx = *width >> 4;
       mcusy = *height >> 3;
       enc411 = 0;
       break;
    default:
       err = ERR_NOT_YCBCR_221111;
       goto error;
       break;
    }


    idctqtab(quant[dscans[0].tq], decdata->dquant[0]);
    idctqtab(quant[dscans[1].tq], decdata->dquant[1]);
    idctqtab(quant[dscans[2].tq], decdata->dquant[2]);
    //initcol(decdata->dquant);
    setinput(&in, datap);


    dec_initscans();

    dscans[0].next = 6 - 4;
    dscans[1].next = 6 - 4 - 1;
    dscans[2].next = 6 - 4 - 1 - 1;       /* 411 encoding */
    for (my = 0; my < mcusy; my++) {
       for (mx = 0; mx < mcusx; mx++) {
           if (info.dri && !--info.nm)
              if (dec_checkmarker()) {
                  err = ERR_WRONG_MARKER;
                  goto error;
              }
           if (enc411) {
              decode_mcus(&in, decdata->dcts, 6, dscans, max);
              idct(decdata->dcts, decdata->out, decdata->dquant[0],
                   IFIX(128.5), max[0]);
              idct(decdata->dcts + 64, decdata->out + 64,
                   decdata->dquant[0], IFIX(128.5), max[1]);
              idct(decdata->dcts + 128, decdata->out + 128,
                   decdata->dquant[0], IFIX(128.5), max[2]);
              idct(decdata->dcts + 192, decdata->out + 192,
                   decdata->dquant[0], IFIX(128.5), max[3]);
              idct(decdata->dcts + 256, decdata->out + 256,
                   decdata->dquant[1], IFIX(0.5), max[4]);
              idct(decdata->dcts + 320, decdata->out + 320,
                   decdata->dquant[2], IFIX(0.5), max[5]);

              //col221111(decdata->out, *pic + (my * 16 * mcusx + mx) * 16 * 3, mcusx * 16 * 3);
              yuyvtopict(decdata->out,
                        *pic + (my * 16 * mcusx + mx) * 16 * 2,
                        mcusx * 16 * 2);
           } else {
              decode_mcus(&in, decdata->dcts, 4, dscans, max);
              idct(decdata->dcts, decdata->out, decdata->dquant[0],
                   IFIX(128.5), max[0]);
              idct(decdata->dcts + 64, decdata->out + 64,
                   decdata->dquant[0], IFIX(128.5), max[1]);
              idct(decdata->dcts + 128, decdata->out + 256,
                   decdata->dquant[1], IFIX(0.5), max[4]);
              idct(decdata->dcts + 192, decdata->out + 320,
                   decdata->dquant[2], IFIX(0.5), max[5]);

              //col221111(decdata->out, *pic + (my * 8 * mcusx + mx) * 16 * 3, mcusx * 16 * 3);
              yuyvtopict(decdata->out,
                        *pic + (my * 8 * mcusx + mx) * 16 * 2,
                        mcusx * 16 * 2);
           }

       }
    }

    m = dec_readmarker(&in);
    if (m != M_EOI) {
       err = ERR_NO_EOI;
       goto error;
    }
    if (decdata)
       free(decdata);
    return 0;
  error:
    if (decdata)
       free(decdata);
    return err;
}

/****************************************************************/
/**************       huffman decoder             ***************/
/****************************************************************/

static int fillbits __P((struct in *, int, unsigned int));
static int dec_rec2
__P((struct in *, struct dec_hufftbl *, int *, int, int));

static void setinput(in, p)
struct in *in;
unsigned char *p;
{
    in->p = p;
    in->left = 0;
    in->bits = 0;
    in->marker = 0;
}

static int fillbits(in, le, bi)
struct in *in;
int le;
unsigned int bi;
{
    int b, m;

    if (in->marker) {
       if (le <= 16)
           in->bits = bi << 16, le += 16;
       return le;
    }
    while (le <= 24) {
       b = *in->p++;
       if (b == 0xff && (m = *in->p++) != 0) {