utils.c

/*******************************************************************************
#                 luvcview: 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 "color.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>
#include "huffman.h"

#define ISHIFT 11

#define IFIX(a) ((int)((a) * (1 << ISHIFT) + .5))

#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 int huffman_init(void);
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 *));

inline static void idct(int *in, int *out, int *quant, long off, int max);

int is_huffman(unsigned char *buf);

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

static void yuv420pto422(int * out,unsigned char *pic,int width);
static void yuv422pto422(int * out,unsigned char *pic,int width);
static void yuv444pto422(int * out,unsigned char *pic,int width);
static void yuv400pto422(int * out,unsigned char *pic,int width);
typedef void (*ftopict) ( int *out, unsigned char *pic, int width) ;
/*********************************/

#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 *isDHT)
{
    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:
        //printf("find DQT \n");
            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:
        //printf("find DHT \n");
            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);
            }
            *isDHT= 1;
            break;

        case M_DRI:
        printf("find DRI \n");
            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 ypitch ,xpitch,bpp,pitch,x,y;
    int mb;
    int max[6];
    ftopict convert;
    int err = 0;
    int isInitHuffman = 0;
    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, &isInitHuffman)) {
        err = ERR_BAD_TABLES;
        goto error;
    }
    getword();
    i = getbyte();
    if (i != 8) {
        err = ERR_NOT_8BIT;
        goto error;
    }
    intheight = getword();
    intwidth = getword();

    if ((intheight & 7) || (intwidth & 7)) {
        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,&isInitHuffman)) {
        err = ERR_BAD_TABLES;
        goto error;
    }
    getword();
    info.ns = getbyte();
    if (!info.ns){
    printf("info ns %d/n",info.ns);
        err = ERR_NOT_YCBCR_221111;
        goto error;
    }
    for (i = 0; i < info.ns; 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) {
            printf("hmm FW error,not seq DCT ??\n");
    }
   // printf("ext huffman table %d \n",isInitHuffman);
    if(!isInitHuffman) {
            if(huffman_init() < 0)
                return -ERR_BAD_TABLES;
        }
/*
    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: // 411
            mb=6;
        mcusx = *width >> 4;
        mcusy = *height >> 4;
        bpp=2;
        xpitch = 16 * bpp;
        pitch = *width * bpp; // YUYV out
        ypitch = 16 * pitch;
        convert = yuv420pto422;
        break;
    case 0x21: //422
   // printf("find 422 %dx%d\n",*width,*height);
            mb=4;
        mcusx = *width >> 4;
        mcusy = *height >> 3;
        bpp=2;
        xpitch = 16 * bpp;
        pitch = *width * bpp; // YUYV out
        ypitch = 8 * pitch;
        convert = yuv422pto422;
        break;
    case 0x11: //444
        mcusx = *width >> 3;
        mcusy = *height >> 3;
        bpp=2;
        xpitch = 8 * bpp;
        pitch = *width * bpp; // YUYV out
        ypitch = 8 * pitch;
         if (info.ns==1) {
                    mb = 1;
                convert = yuv400pto422;
        } else {
                mb=3;
                convert = yuv444pto422;
        }
        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]);
    setinput(&in, datap);
    dec_initscans();

    dscans[0].next = 2;
    dscans[1].next = 1;
    dscans[2].next = 0;        /* 4xx encoding */
    for (my = 0,y=0; my < mcusy; my++,y+=ypitch) {
        for (mx = 0,x=0; mx < mcusx; mx++,x+=xpitch) {
            if (info.dri && !--info.nm)
                if (dec_checkmarker()) {
                    err = ERR_WRONG_MARKER;
                    goto error;
                }
        switch (mb){
            case 6: {
                decode_mcus(&in, decdata->dcts, mb, 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]);

            } break;
            case 4:
            {
                decode_mcus(&in, decdata->dcts, mb, 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]);

            }
            break;
            case 3:
                     decode_mcus(&in, decdata->dcts, mb, dscans, max);
                idct(decdata->dcts, decdata->out, decdata->dquant[0],
                     IFIX(128.5), max[0]);
                idct(decdata->dcts + 64, decdata->out + 256,
                     decdata->dquant[1], IFIX(0.5), max[4]);
                idct(decdata->dcts + 128, decdata->out + 320,
                     decdata->dquant[2], IFIX(0.5), max[5]);


            break;
            case 1:
                     decode_mcus(&in, decdata->dcts, mb, dscans, max);
                idct(decdata->dcts, decdata->out, decdata->dquant[0],
                     IFIX(128.5), max[0]);

            break;

        } // switch enc411
        convert(decdata->out,*pic+y+x,pitch);
        }
    }

    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 huffman_init(void)
{            int tc, th, tt;
         const unsigned char *ptr= JPEGHuffmanTable ;
        int i, j, l;
        l = JPG_HUFFMAN_TABLE_LENGTH ;
            while (l > 0) {
                int hufflen[16], k;
                unsigned char huffvals[256];

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

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) {
            if (m == M_EOF) {
                if (in->func && (m = in->func(in->data)) == 0)
                    continue;
            }
            in->marker = m;
            if (le <= 16)
                bi = bi << 16, le += 16;
            break;
        }
        bi = bi << 8 | b;
        le += 8;
    }
    in->bits = bi;                /* tmp... 2 return values needed */
    return le;
}

static int dec_readmarker(in)
struct in *in;
{
    int m;

    in->left = fillbits(in, in->left, in->bits);
    if ((m = in->marker) == 0)
        return 0;
    in->left = 0;
    in->marker = 0;
    return m;
}

#define LEBI_DCL        int le, bi
#define LEBI_GET(in)        (le = in->left, bi = in->bits)
#define LEBI_PUT(in)        (in->left = le, in->bits = bi)

#define GETBITS(in, n) (                                        \
  (le < (n) ? le = fillbits(in, le, bi), bi = in->bits : 0),        \
  (le -= (n)),                                                        \
  bi >> le & ((1 << (n)) - 1)                                        \
)

#define UNGETBITS(in, n) (        \
  le += (n)                        \
)


static int dec_rec2(in, hu, runp, c, i)
struct in *in;
struct dec_hufftbl *hu;
int *runp;
int c, i;
{
    LEBI_DCL;

    LEBI_GET(in);
    if (i) {
        UNGETBITS(in, i & 127);
        *runp = i >> 8 & 15;
        i >>= 16;
    } else {
        for (i = DECBITS;
             (c = ((c << 1) | GETBITS(in, 1))) >= (hu->maxcode[i]); i++);
        if (i >= 16) {
            in->marker = M_BADHUFF;
            return 0;
        }
        i = hu->vals[hu->valptr[i] + c - hu->maxcode[i - 1] * 2];
        *runp = i >> 4;
        i &= 15;
    }
    if (i == 0) {                /* sigh, 0xf0 is 11 bit */
        LEBI_PUT(in);
        return 0;
    }
    /* receive part */
    c = GETBITS(in, i);
    if (c < (1 << (i - 1)))
        c += (-1 << i) + 1;
    LEBI_PUT(in);
    return c;
}

#define DEC_REC(in, hu, r, i)         (        \
  r = GETBITS(in, DECBITS),                \
  i = hu->llvals[r],                        \
  i & 128 ?                                \
    (                                        \
      UNGETBITS(in, i & 127),                \
      r = i >> 8 & 15,                        \
      i >> 16                                \
    )                                        \
  :                                        \
    (                                        \
      LEBI_PUT(in),                        \
      i = dec_rec2(in, hu, &r, r, i),        \
      LEBI_GET(in),                        \
      i                                        \
    )                                        \
)

static void decode_mcus(in, dct, n, sc, maxp)
struct in *in;
int *dct;
int n;
struct scan *sc;
int *maxp;
{
    struct dec_hufftbl *hu;
    int i, r, t;
    LEBI_DCL;

    memset(dct, 0, n * 64 * sizeof(*dct));
    LEBI_GET(in);
    while (n-- > 0) {
        hu = sc->hudc.dhuff;
        *dct++ = (sc->dc += DEC_REC(in, hu, r, t));

        hu = sc->huac.dhuff;
        i = 63;
        while (i > 0) {
            t = DEC_REC(in, hu, r, t);
            if (t == 0 && r == 0) {
                dct += i;
                break;
            }
            dct += r;
            *dct++ = t;
            i -= r + 1;
        }
        *maxp++ = 64 - i;
        if (n == sc->next)
            sc++;
    }
    LEBI_PUT(in);
}

static void dec_makehuff(hu, hufflen, huffvals)
struct dec_hufftbl *hu;
int *hufflen;
unsigned char *huffvals;
{
    int code, k, i, j, d, x, c, v;
    for (i = 0; i < (1 << DECBITS); i++)
        hu->llvals[i] = 0;

/*
 * llvals layout:
 *
 * value v already known, run r, backup u bits:
 *  vvvvvvvvvvvvvvvv 0000 rrrr 1 uuuuuuu
 * value unknown, size b bits, run r, backup u bits:
 *  000000000000bbbb 0000 rrrr 0 uuuuuuu
 * value and size unknown:
 *  0000000000000000 0000 0000 0 0000000
 */
    code = 0;
    k = 0;
    for (i = 0; i < 16; i++, code <<= 1) {        /* sizes */
        hu->valptr[i] = k;
        for (j = 0; j < hufflen[i]; j++) {
            hu->vals[k] = *huffvals++;
            if (i < DECBITS) {
                c = code << (DECBITS - 1 - i);
                v = hu->vals[k] & 0x0f;        /* size */
                for (d = 1 << (DECBITS - 1 - i); --d >= 0;) {
                    if (v + i < DECBITS) {        /* both fit in table */
                        x = d >> (DECBITS - 1 - v - i);
                        if (v && x < (1 << (v - 1)))
                            x += (-1 << v) + 1;
                        x = x << 16 | (hu->vals[k] & 0xf0) << 4 |
                            (DECBITS - (i + 1 + v)) | 128;
                    } else
                        x = v << 16 | (hu->vals[k] & 0xf0) << 4 |
                            (DECBITS - (i + 1));
                    hu->llvals[c | d] = x;
                }
            }
            code++;
            k++;
        }
        hu->maxcode[i] = code;
    }
    hu->maxcode[16] = 0x20000;        /* always terminate decode */
}

/****************************************************************/
/**************             idct                  ***************/
/****************************************************************/


#define IMULT(a, b) (((a) * (b)) >> ISHIFT)
#define ITOINT(a) ((a) >> ISHIFT)

#define S22 ((PREC)IFIX(2 * 0.382683432))
#define C22 ((PREC)IFIX(2 * 0.923879532))
#define IC4 ((PREC)IFIX(1 / 0.707106781))

static unsigned char zig2[64] = {
    0, 2, 3, 9, 10, 20, 21, 35,
    14, 16, 25, 31, 39, 46, 50, 57,
    5, 7, 12, 18, 23, 33, 37, 48,
    27, 29, 41, 44, 52, 55, 59, 62,
    15, 26, 30, 40, 45, 51, 56, 58,
    1, 4, 8, 11, 19, 22, 34, 36,
    28, 42, 43, 53, 54, 60, 61, 63,
    6, 13, 17, 24, 32, 38, 47, 49
};

inline static void idct(int *in, int *out, int *quant, long off, int max)
{
    long t0, t1, t2, t3, t4, t5, t6, t7;        // t ;
    long tmp0, tmp1, tmp2, tmp3, tmp4, tmp5, tmp6;
    long tmp[64], *tmpp;
    int i, j, te;
    unsigned char *zig2p;

    t0 = off;
    if (max == 1) {
        t0 += in[0] * quant[0];
        for (i = 0; i < 64; i++)
            out[i] = ITOINT(t0);
        return;
    }
    zig2p = zig2;
    tmpp = tmp;
    for (i = 0; i < 8; i++) {
        j = *zig2p++;
        t0 += in[j] * (long) quant[j];
        j = *zig2p++;
        t5 = in[j] * (long) quant[j];
        j = *zig2p++;
        t2 = in[j] * (long) quant[j];
        j = *zig2p++;
        t7 = in[j] * (long) quant[j];
        j = *zig2p++;
        t1 = in[j] * (long) quant[j];
        j = *zig2p++;
        t4 = in[j] * (long) quant[j];
        j = *zig2p++;
        t3 = in[j] * (long) quant[j];
        j = *zig2p++;
        t6 = in[j] * (long) quant[j];


        if ((t1 | t2 | t3 | t4 | t5 | t6 | t7) == 0) {

            tmpp[0 * 8] = t0;
            tmpp[1 * 8] = t0;
            tmpp[2 * 8] = t0;
            tmpp[3 * 8] = t0;
            tmpp[4 * 8] = t0;
            tmpp[5 * 8] = t0;
            tmpp[6 * 8] = t0;
            tmpp[7 * 8] = t0;

            tmpp++;
            t0 = 0;
            continue;
        }
        //IDCT;
        tmp0 = t0 + t1;
        t1 = t0 - t1;
        tmp2 = t2 - t3;
        t3 = t2 + t3;
        tmp2 = IMULT(tmp2, IC4) - t3;
        tmp3 = tmp0 + t3;
        t3 = tmp0 - t3;
        tmp1 = t1 + tmp2;
        tmp2 = t1 - tmp2;
        tmp4 = t4 - t7;
        t7 = t4 + t7;
        tmp5 = t5 + t6;
        t6 = t5 - t6;
        tmp6 = tmp5 - t7;
        t7 = tmp5 + t7;
        tmp5 = IMULT(tmp6, IC4);
        tmp6 = IMULT((tmp4 + t6), S22);
        tmp4 = IMULT(tmp4, (C22 - S22)) + tmp6;
        t6 = IMULT(t6, (C22 + S22)) - tmp6;
        t6 = t6 - t7;
        t5 = tmp5 - t6;
        t4 = tmp4 - t5;

        tmpp[0 * 8] = tmp3 + t7;        //t0;
        tmpp[1 * 8] = tmp1 + t6;        //t1;
        tmpp[2 * 8] = tmp2 + t5;        //t2;
        tmpp[3 * 8] = t3 + t4;        //t3;
        tmpp[4 * 8] = t3 - t4;        //t4;
        tmpp[5 * 8] = tmp2 - t5;        //t5;
        tmpp[6 * 8] = tmp1 - t6;        //t6;
        tmpp[7 * 8] = tmp3 - t7;        //t7;
        tmpp++;
        t0 = 0;
    }
    for (i = 0, j = 0; i < 8; i++) {
        t0 = tmp[j + 0];
        t1 = tmp[j + 1];
        t2 = tmp[j + 2];
        t3 = tmp[j + 3];
        t4 = tmp[j + 4];
        t5 = tmp[j + 5];
        t6 = tmp[j + 6];
        t7 = tmp[j + 7];
        if ((t1 | t2 | t3 | t4 | t5 | t6 | t7) == 0) {
            te = ITOINT(t0);
            out[j + 0] = te;
            out[j + 1] = te;
            out[j + 2] = te;
            out[j + 3] = te;
            out[j + 4] = te;
            out[j + 5] = te;
            out[j + 6] = te;
            out[j + 7] = te;
            j += 8;
            continue;
        }
        //IDCT;
        tmp0 = t0 + t1;
        t1 = t0 - t1;
        tmp2 = t2 - t3;
        t3 = t2 + t3;
        tmp2 = IMULT(tmp2, IC4) - t3;
        tmp3 = tmp0 + t3;
        t3 = tmp0 - t3;
        tmp1 = t1 + tmp2;
        tmp2 = t1 - tmp2;
        tmp4 = t4 - t7;
        t7 = t4 + t7;
        tmp5 = t5 + t6;
        t6 = t5 - t6;
        tmp6 = tmp5 - t7;
        t7 = tmp5 + t7;
        tmp5 = IMULT(tmp6, IC4);
        tmp6 = IMULT((tmp4 + t6), S22);
        tmp4 = IMULT(tmp4, (C22 - S22)) + tmp6;
        t6 = IMULT(t6, (C22 + S22)) - tmp6;
        t6 = t6 - t7;
        t5 = tmp5 - t6;
        t4 = tmp4 - t5;

        out[j + 0] = ITOINT(tmp3 + t7);
        out[j + 1] = ITOINT(tmp1 + t6);
        out[j + 2] = ITOINT(tmp2 + t5);
        out[j + 3] = ITOINT(t3 + t4);
        out[j + 4] = ITOINT(t3 - t4);
        out[j + 5] = ITOINT(tmp2 - t5);
        out[j + 6] = ITOINT(tmp1 - t6);
        out[j + 7] = ITOINT(tmp3 - t7);
        j += 8;
    }

}

static unsigned char zig[64] = {
    0, 1, 5, 6, 14, 15, 27, 28,
    2, 4, 7, 13, 16, 26, 29, 42,
    3, 8, 12, 17, 25, 30, 41, 43,
    9, 11, 18, 24, 31, 40, 44, 53,
    10, 19, 23, 32, 39, 45, 52, 54,
    20, 22, 33, 38, 46, 51, 55, 60,
    21, 34, 37, 47, 50, 56, 59, 61,
    35, 36, 48, 49, 57, 58, 62, 63
};

static PREC aaidct[8] = {
    IFIX(0.3535533906), IFIX(0.4903926402),
    IFIX(0.4619397663), IFIX(0.4157348062),
    IFIX(0.3535533906), IFIX(0.2777851165),
    IFIX(0.1913417162), IFIX(0.0975451610)
};


static void idctqtab(qin, qout)
unsigned char *qin;
PREC *qout;
{
    int i, j;

    for (i = 0; i < 8; i++)
        for (j = 0; j < 8; j++)
            qout[zig[i * 8 + j]] = qin[zig[i * 8 + j]] *
                IMULT(aaidct[i], aaidct[j]);
}

#define  FOUR_TWO_TWO 2                //Y00 Cb Y01 Cr


/* translate YUV422Packed to rgb24 */

unsigned int
Pyuv422torgb24(unsigned char * input_ptr, unsigned char * output_ptr, unsigned int image_width, unsigned int image_height)
{
        unsigned int i, size;
        unsigned char Y, Y1, U, V;
        unsigned char *buff = input_ptr;
        unsigned char *output_pt = output_ptr;
        size = image_width * image_height /2;
  int index = 0;
        for (i = size; i > 0; i--) {
                /* bgr instead rgb ?? */
                Y = buff[0] ;
                U = buff[1] ;
                Y1 = buff[2];
                V = buff[3];
                buff += 4;




                *output_pt++ = R_FROMYV(Y,V);
                *output_pt++ = G_FROMYUV(Y,U,V); //b
                *output_pt++ = B_FROMYU(Y,U); //v

                *output_pt++ = R_FROMYV(Y1,V);
                *output_pt++ = G_FROMYUV(Y1,U,V); //b
                *output_pt++ = B_FROMYU(Y1,U); //v
        }

        return FOUR_TWO_TWO;
}

static void yuv420pto422(int * out,unsigned char *pic,int width)
{
    int j, k;
    unsigned char *pic0, *pic1;
    int *outy, *outu, *outv;
    int outy1 = 0;
    int outy2 = 8;


    pic0 = pic;
    pic1 = pic + width;
    outy = out;
    outu = out + 64 * 4;
    outv = out + 64 * 5;
        for (j = 0; j < 8; j++) {
            for (k = 0; k < 8; k++) {
            if( k == 4) {
                    outy1 += 56;
                    outy2 += 56;
            }
            *pic0++ = CLIP(outy[outy1]);
            *pic0++ = CLIP(128 + *outu);
            *pic0++ = CLIP(outy[outy1+1]);
            *pic0++ = CLIP(128 + *outv);
            *pic1++ = CLIP(outy[outy2]);
            *pic1++ = CLIP(128 + *outu);
            *pic1++ = CLIP(outy[outy2+1]);
            *pic1++ = CLIP(128 + *outv);
           outy1 +=2; outy2 += 2; outu++; outv++;
          }
          if(j==3) {
          outy = out + 128;
          } else {
          outy += 16;
          }
            outy1 = 0;
            outy2 = 8;
            pic0 += 2 * (width -16);
            pic1 += 2 * (width -16);

        }

}
static void yuv422pto422(int * out,unsigned char *pic,int width)
{
    int j, k;
    unsigned char *pic0, *pic1;
    int *outy, *outu, *outv;
    int outy1 = 0;
    int outy2 = 8;
    int outu1 = 0;
    int outv1 = 0;


    pic0 = pic;
    pic1 = pic + width;
    outy = out;
    outu = out + 64 * 4;
    outv = out + 64 * 5;
        for (j = 0; j < 4; j++) {
            for (k = 0; k < 8; k++) {
            if( k == 4) {
                    outy1 += 56;
                    outy2 += 56;
            }
            *pic0++ = CLIP(outy[outy1]);
            *pic0++ = CLIP(128 + outu[outu1]);
            *pic0++ = CLIP(outy[outy1+1]);
            *pic0++ = CLIP(128 + outv[outv1]);
            *pic1++ = CLIP(outy[outy2]);
            *pic1++ = CLIP(128 + outu[outu1+8]);
            *pic1++ = CLIP(outy[outy2+1]);
            *pic1++ = CLIP(128 + outv[outv1+8]);
            outv1 += 1; outu1 += 1;
            outy1 +=2; outy2 +=2;

          }

            outy += 16;outu +=8; outv +=8;
            outv1 = 0; outu1=0;
            outy1 = 0;
            outy2 = 8;
            pic0 += 2 * (width -16);
            pic1 += 2 * (width -16);

        }

}
static void yuv444pto422(int * out,unsigned char *pic,int width)
{
    int j, k;
    unsigned char *pic0, *pic1;
    int *outy, *outu, *outv;
    int outy1 = 0;
    int outy2 = 8;
    int outu1 = 0;
    int outv1 = 0;

    pic0 = pic;
    pic1 = pic + width;
    outy = out;
    outu = out + 64 * 4; // Ooops where did i invert ??
    outv = out + 64 * 5;
        for (j = 0; j < 4; j++) {
            for (k = 0; k < 4; k++) {

            *pic0++ =CLIP( outy[outy1]);
            *pic0++ =CLIP( 128 + outu[outu1]);
            *pic0++ =CLIP( outy[outy1+1]);
            *pic0++ =CLIP( 128 + outv[outv1]);
            *pic1++ =CLIP( outy[outy2]);
            *pic1++ =CLIP( 128 + outu[outu1+8]);
            *pic1++ =CLIP( outy[outy2+1]);
            *pic1++ =CLIP( 128 + outv[outv1+8]);
            outv1 += 2; outu1 += 2;
            outy1 +=2; outy2 +=2;
          }
            outy += 16;outu +=16; outv +=16;
            outv1 = 0; outu1=0;
            outy1 = 0;
            outy2 = 8;
            pic0 += 2 * (width -8);
            pic1 += 2 * (width -8);
        }

}
static void yuv400pto422(int * out,unsigned char *pic,int width)
{
    int j, k;
    unsigned char *pic0, *pic1;
    int *outy ;
    int outy1 = 0;
    int outy2 = 8;
    pic0 = pic;
    pic1 = pic + width;
    outy = out;

        for (j = 0; j < 4; j++) {
            for (k = 0; k < 4; k++) {
            *pic0++ = CLIP(outy[outy1]);
            *pic0++ = 128 ;
            *pic0++ = CLIP(outy[outy1+1]);
            *pic0++ = 128 ;
            *pic1++ = CLIP(outy[outy2]);
            *pic1++ = 128 ;
            *pic1++ = CLIP(outy[outy2+1]);
            *pic1++ = 128 ;
             outy1 +=2; outy2 +=2;
          }
            outy += 16;
            outy1 = 0;
            outy2 = 8;
            pic0 += 2 * (width -8);
            pic1 += 2 * (width -8);
        }

}

int
is_huffman(unsigned char *buf)
{
unsigned char *ptbuf;
int i = 0;
ptbuf = buf;
while (((ptbuf[0] << 8) | ptbuf[1]) != 0xffda){
        if(i++ > 2048)
                return 0;
        if(((ptbuf[0] << 8) | ptbuf[1]) == 0xffc4)
                return 1;
        ptbuf++;
}
return 0;
}
static void
 getPictureName (char *Picture, int fmt)
{
char temp[80];
  char *myext[] = { "pnm", "jpg" };
  int i;
  time_t curdate;
  struct tm *tdate;
  memset (temp, '\0', sizeof (temp));
  time (&curdate);
  tdate = localtime (&curdate);
  snprintf (temp, 26, "P-%02d:%02d:%04d-%02d:%02d:%02d.%s\0",
            tdate->tm_mon + 1, tdate->tm_mday, tdate->tm_year + 1900,
            tdate->tm_hour, tdate->tm_min, tdate->tm_sec, myext[fmt]);

  memcpy (Picture, temp, strlen (temp));
}
int get_picture(unsigned char *buf,int size)
{
  FILE *file;
  unsigned char *ptdeb,*ptcur = buf;
  int sizein;
  char *name = NULL;
  name = calloc(80,1);
  getPictureName (name, 1);
  printf("File %s\n", name);
  file = fopen(name, "wb");
  if (file != NULL) {
    if(!is_huffman(buf)){
      ptdeb = ptcur = buf;
      while (((ptcur[0] << 8) | ptcur[1]) != 0xffc0)
        ptcur++;
      sizein = ptcur-ptdeb;
      fwrite(buf,
          sizein, 1, file);
      fwrite(dht_data,
          DHT_SIZE, 1, file);
      fwrite(ptcur,size-sizein,1,file);
    } else {
      fwrite(ptcur,size,1,file); /* ptcur was uninit -wsr */
    }
    fclose(file);
  }
  if(name)
    free(name);
  return 0;
}

int
get_pictureYV2(unsigned char *buf,int width,int height)
{
FILE *foutpict;
unsigned char *picture = NULL;
char *name = NULL;
name = calloc(80,1);
getPictureName (name, 0);
picture = (unsigned char *)malloc(width*height*3*sizeof(char));
if(picture){
        Pyuv422torgb24(buf, picture, width, height);
}else{
        printf(" no room to take a picture \n");
        return 0;
}
if(name){
        foutpict = fopen (name, "wb");
        fprintf (foutpict, "P6\n%d %d\n255\n", width, height);
        fwrite (picture, sizeof (char), width * height * 3, foutpict);
        fclose (foutpict);
        free(name);
}
free(picture);
picture = NULL;
return 0;
}