Subversion Repositories shark

/* store.c, picture output routines                                         */

/* Copyright (C) 1996, MPEG Software Simulation Group. All Rights Reserved. */

/*

 * Disclaimer of Warranty

 *

 * These software programs are available to the user without any license fee or

 * royalty on an "as is" basis.  The MPEG Software Simulation Group disclaims

 * any and all warranties, whether express, implied, or statuary, including any

 * implied warranties or merchantability or of fitness for a particular

 * purpose.  In no event shall the copyright-holder be liable for any

 * incidental, punitive, or consequential damages of any kind whatsoever

 * arising from the use of these programs.

 *

 * This disclaimer of warranty extends to the user of these programs and user's

 * customers, employees, agents, transferees, successors, and assigns.

 *

 * The MPEG Software Simulation Group does not represent or warrant that the

 * programs furnished hereunder are free of infringement of any third-party

 * patents.

 *

 * Commercial implementations of MPEG-1 and MPEG-2 video, including shareware,

 * are subject to royalty fees to patent holders.  Many of these patents are

 * general enough such that they are unavoidable regardless of implementation

 * design.

 *

 */

#include <stdio.h>

#include <stdlib.h>

#include <fcntl.h>

#include "config.h"

#include "global.h"

#include "semaphore.h"

#include "kernel/kern.h"

//#debug DEBUG_MAILBOX

static void conv422to444 _ANSI_ARGS_((unsigned char *src, unsigned char *dst));

static void conv420to422 _ANSI_ARGS_((unsigned char *src, unsigned char *dst));

__inline__ WORD down32to16(unsigned char r, unsigned char g, unsigned char b)

{

    return ((b&0xf8)>>3)|((g&0xfc)<<3)|((r&0xf8)<<8);

}

#if 0

void xWrite_Frame _ANSI_ARGS_((unsigned char *src[], int frame))

{

  int i, j;

  int y, u, v, r, g, b;

//  int rm=0,gm=0,bm=0;

  int crv, cbu, cgu, cgv;

  unsigned char *py, *pu, *pv;

  int height, width;

  struct framebuf_struct *f;

  cprintf("%d ",frame);

  width = Coded_Picture_Width ;

  height= Coded_Picture_Height;

  f = get_free_framebuf();

  dither(src);

      f->f[i*width+j] = down32to16(r,g,b);

  f->n = frame;

  insert_frame(f);

}

#endif

void Write_Frame _ANSI_ARGS_((unsigned char *src[], int frame))

{

  int i, j;

  int y, u, v, r, g, b;

//  int rm=0,gm=0,bm=0;

  int crv, cbu, cgu, cgv;

  unsigned char *py, *pu, *pv;

  int height, width, incr;

  static unsigned char *u422, *v422, *u444, *v444;

  struct framebuf_struct *f;

  cprintf("%d ",frame);

  incr = width = Coded_Picture_Width ;

  height= Coded_Picture_Height;

  if (chroma_format==CHROMA444)

  {

    u444 = src[1];

    v444 = src[2];

  }

  else

  {

    if (!u444)

    {

      if (chroma_format==CHROMA420)

      {

        if (!(u422 = (unsigned char *)malloc((Coded_Picture_Width>>1)

                                             *Coded_Picture_Height)))

          Error("malloc failed");

        if (!(v422 = (unsigned char *)malloc((Coded_Picture_Width>>1)

                                             *Coded_Picture_Height)))

          Error("malloc failed");

      }

      if (!(u444 = (unsigned char *)malloc(Coded_Picture_Width

                                           *Coded_Picture_Height)))

        Error("malloc failed");

      if (!(v444 = (unsigned char *)malloc(Coded_Picture_Width

                                           *Coded_Picture_Height)))

        Error("malloc failed");

    }

    if (chroma_format==CHROMA420)

    {

      conv420to422(src[1],u422);

      conv420to422(src[2],v422);

      conv422to444(u422,u444);

      conv422to444(v422,v444);

    }

    else

    {

      conv422to444(src[1],u444);

      conv422to444(src[2],v444);

    }

  }

  f = get_free_framebuf();

  /* matrix coefficients */

  crv = Inverse_Table_6_9[matrix_coefficients][0];

  cbu = Inverse_Table_6_9[matrix_coefficients][1];

  cgu = Inverse_Table_6_9[matrix_coefficients][2];

  cgv = Inverse_Table_6_9[matrix_coefficients][3];

  for (i=0; i<height; i++)

  {

    py = src[0] + incr*i;

    pu = u444 + incr*i;

    pv = v444 + incr*i;

    for (j=0; j<width; j++)

    {

      u = *pu++ - 128;

      v = *pv++ - 128;

      y = 76309 * (*py++ - 16); /* (255/219)*65536 */

      r = Clip[(y + crv*v + 32768)>>16];

      g = Clip[(y - cgu*u - cgv*v + 32768)>>16];

      b = Clip[(y + cbu*u + 32786)>>16];

//      rm = max(rm,r);gm=max(gm,g);bm=max(bm,b);

//      cprintf("(r%dg%db%d)",rm,gm,bm);

      f->f[i*width+j] = down32to16(r,g,b);

//        r=g=b=rand()%255;

//      f->f[i*width+j] = down32to16(r,g,b);

    }

  }

  f->n = frame;

  insert_frame(f);

}

/*

void Display_Image(Dithered_Image)

unsigned char *Dithered_Image;

{

  / * display dithered image */

//}

/* horizontal 1:2 interpolation filter */

static void conv422to444(src,dst)

unsigned char *src,*dst;

{

  int i, i2, w, j, im3, im2, im1, ip1, ip2, ip3;

  w = Coded_Picture_Width>>1;

  if (base.MPEG2_Flag)

  {

    for (j=0; j<Coded_Picture_Height; j++)

    {

      for (i=0; i<w; i++)

      {

        i2 = i<<1;

        im2 = (i<2) ? 0 : i-2;

        im1 = (i<1) ? 0 : i-1;

        ip1 = (i<w-1) ? i+1 : w-1;

        ip2 = (i<w-2) ? i+2 : w-1;

        ip3 = (i<w-3) ? i+3 : w-1;

        /* FIR filter coefficients (*256): 21 0 -52 0 159 256 159 0 -52 0 21 */

        /* even samples (0 0 256 0 0) */

        dst[i2] = src[i];

        /* odd samples (21 -52 159 159 -52 21) */

        dst[i2+1] = Clip[(int)(21*(src[im2]+src[ip3])

                        -52*(src[im1]+src[ip2])

                       +159*(src[i]+src[ip1])+128)>>8];

      }

      src+= w;

      dst+= Coded_Picture_Width;

    }

  }

  else

  {

    for (j=0; j<Coded_Picture_Height; j++)

    {

      for (i=0; i<w; i++)

      {

        i2 = i<<1;

        im3 = (i<3) ? 0 : i-3;

        im2 = (i<2) ? 0 : i-2;

        im1 = (i<1) ? 0 : i-1;

        ip1 = (i<w-1) ? i+1 : w-1;

        ip2 = (i<w-2) ? i+2 : w-1;

        ip3 = (i<w-3) ? i+3 : w-1;

        /* FIR filter coefficients (*256): 5 -21 70 228 -37 11 */

        dst[i2] =   Clip[(int)(  5*src[im3]

                         -21*src[im2]

                         +70*src[im1]

                        +228*src[i]

                         -37*src[ip1]

                         +11*src[ip2]+128)>>8];

       dst[i2+1] = Clip[(int)(  5*src[ip3]

                         -21*src[ip2]

                         +70*src[ip1]

                        +228*src[i]

                         -37*src[im1]

                         +11*src[im2]+128)>>8];

      }

      src+= w;

      dst+= Coded_Picture_Width;

    }

  }

}

/* vertical 1:2 interpolation filter */

static void conv420to422(src,dst)

unsigned char *src,*dst;

{

  int w, h, i, j, j2;

  int jm6, jm5, jm4, jm3, jm2, jm1, jp1, jp2, jp3, jp4, jp5, jp6, jp7;

  w = Coded_Picture_Width>>1;

  h = Coded_Picture_Height>>1;

  if (progressive_frame)

  {

    /* intra frame */

    for (i=0; i<w; i++)

    {

      for (j=0; j<h; j++)

      {

        j2 = j<<1;

        jm3 = (j<3) ? 0 : j-3;

        jm2 = (j<2) ? 0 : j-2;

        jm1 = (j<1) ? 0 : j-1;

        jp1 = (j<h-1) ? j+1 : h-1;

        jp2 = (j<h-2) ? j+2 : h-1;

        jp3 = (j<h-3) ? j+3 : h-1;

        /* FIR filter coefficients (*256): 5 -21 70 228 -37 11 */

        /* New FIR filter coefficients (*256): 3 -16 67 227 -32 7 */

        dst[w*j2] =     Clip[(int)(  3*src[w*jm3]

                             -16*src[w*jm2]

                             +67*src[w*jm1]

                            +227*src[w*j]

                             -32*src[w*jp1]

                             +7*src[w*jp2]+128)>>8];

        dst[w*(j2+1)] = Clip[(int)(  3*src[w*jp3]

                             -16*src[w*jp2]

                             +67*src[w*jp1]

                            +227*src[w*j]

                             -32*src[w*jm1]

                             +7*src[w*jm2]+128)>>8];

      }

      src++;

      dst++;

    }

  }

  else

  {

    /* intra field */

    for (i=0; i<w; i++)

    {

      for (j=0; j<h; j+=2)

      {

        j2 = j<<1;

        /* top field */

        jm6 = (j<6) ? 0 : j-6;

        jm4 = (j<4) ? 0 : j-4;

        jm2 = (j<2) ? 0 : j-2;

        jp2 = (j<h-2) ? j+2 : h-2;

        jp4 = (j<h-4) ? j+4 : h-2;

        jp6 = (j<h-6) ? j+6 : h-2;

        /* Polyphase FIR filter coefficients (*256): 2 -10 35 242 -18 5 */

        /* New polyphase FIR filter coefficients (*256): 1 -7 30 248 -21 5 */

        dst[w*j2] = Clip[(int)(  1*src[w*jm6]

                         -7*src[w*jm4]

                         +30*src[w*jm2]

                        +248*src[w*j]

                         -21*src[w*jp2]

                          +5*src[w*jp4]+128)>>8];

        /* Polyphase FIR filter coefficients (*256): 11 -38 192 113 -30 8 */

        /* New polyphase FIR filter coefficients (*256):7 -35 194 110 -24 4 */

        dst[w*(j2+2)] = Clip[(int)( 7*src[w*jm4]

                             -35*src[w*jm2]

                            +194*src[w*j]

                            +110*src[w*jp2]

                             -24*src[w*jp4]

                              +4*src[w*jp6]+128)>>8];

        /* bottom field */

        jm5 = (j<5) ? 1 : j-5;

        jm3 = (j<3) ? 1 : j-3;

        jm1 = (j<1) ? 1 : j-1;

        jp1 = (j<h-1) ? j+1 : h-1;

        jp3 = (j<h-3) ? j+3 : h-1;

        jp5 = (j<h-5) ? j+5 : h-1;

        jp7 = (j<h-7) ? j+7 : h-1;

        /* Polyphase FIR filter coefficients (*256): 11 -38 192 113 -30 8 */

        /* New polyphase FIR filter coefficients (*256):7 -35 194 110 -24 4 */

        dst[w*(j2+1)] = Clip[(int)( 7*src[w*jp5]

                             -35*src[w*jp3]

                            +194*src[w*jp1]

                            +110*src[w*jm1]

                             -24*src[w*jm3]

                              +4*src[w*jm5]+128)>>8];

        dst[w*(j2+3)] = Clip[(int)(  1*src[w*jp7]

                             -7*src[w*jp5]

                             +30*src[w*jp3]

                            +248*src[w*jp1]

                             -21*src[w*jm1]

                              +5*src[w*jm3]+128)>>8];

      }

      src++;

      dst++;

    }

  }

}

// Frame buffer for displaying pictures

struct framebuf_struct *get_free_framebuf();

void insert_frame(struct framebuf_struct *f);

struct framebuf_struct *remove_frame();

int framebuf_sz;

// a queue of framebufs

struct framebuf_struct *framebuf_first;

struct framebuf_struct *framebuf_last;

// free framebuf list

struct framebuf_struct *framebuf_free;

// some semaphores

sem_t framebuf_sem;

sem_t framebuf_count;

sem_t framebuf_freecount;

void allocate_framebuf()

{

  struct framebuf_struct *f;

  f = (struct framebuf_struct *)malloc(sizeof(struct framebuf_struct));

  if (!f) {

    cputs("Not enough memory!!!\n");

    sys_end();

  }

  f->f = (WORD *)malloc(framebuf_sz);

  if (!f->f) {

    cputs("Not enough memory for buffer!!!\n");

    sys_end();

  }

  f->next = framebuf_free;

  framebuf_free = f;

  cprintf("(Alloc %d %d)\n",f,f->f);

}

void Initialize_framebuf(int sz)

{

  int i;

  framebuf_first = NULL;

  framebuf_last = NULL;

  framebuf_free = NULL;

  framebuf_sz = sz;

  cprintf("Coded W=%d H=%d sz=%d\n",Coded_Picture_Width,Coded_Picture_Height,sz);

  for (i=0; i<MAX_FRAMEBUF; i++)

    allocate_framebuf();

  sem_init(&framebuf_sem,0,1);

  sem_init(&framebuf_count,0,0);

  sem_init(&framebuf_freecount,0,MAX_FRAMEBUF);

}

struct framebuf_struct *get_free_framebuf()

{

  struct framebuf_struct *f;

#ifdef DEBUG_MAILBOX

  cprintf("G");

#endif

  sem_wait(&framebuf_freecount);

  sem_wait(&framebuf_sem);

  //if (!framebuf_free) 

  //  allocate_framebuf();

  f = framebuf_free;

  framebuf_free = framebuf_free->next;

  sem_post(&framebuf_sem);

  return f;  

}

void insert_frame(struct framebuf_struct *f)

{

  struct framebuf_struct *p, *q;

  int n;

#ifdef DEBUG_MAILBOX

  cprintf("I");

#endif

  sem_wait(&framebuf_sem);

  p = NULL;

  q = framebuf_first;

  n = f->n;

  while ((q != NULL) && (n >= q->n)) {

    p = q;

    q = q->next;

  }

  if (p)

    p->next = f;

  else

    framebuf_first = f;

  if (q)

    q->prev = f;

  else

    framebuf_last = f;

  f->next = q;

  f->prev = p;

  sem_post(&framebuf_count);

  sem_post(&framebuf_sem);

}

struct framebuf_struct *remove_frame()

{

  struct framebuf_struct *f;

#ifdef DEBUG_MAILBOX

  cprintf("R");

#endif

  sem_wait(&framebuf_count);

  sem_wait(&framebuf_sem);

  // remove the first frame

  f = framebuf_first;

  if (!f)

    sys_abort(69);  // should never happen

  framebuf_first = framebuf_first->next;

  if (framebuf_first)

    framebuf_first->prev = NULL;

  else

    framebuf_last = NULL;

  sem_post(&framebuf_sem);

  return f;

}

void give_back_framebuf(struct framebuf_struct *f)

{

#ifdef DEBUG_MAILBOX

  cprintf("B");

#endif

  sem_wait(&framebuf_sem);

  f->next = framebuf_free;

  framebuf_free = f;

  sem_post(&framebuf_sem);

  sem_post(&framebuf_freecount);

}