Subversion Repositories shark

/* Project:     HARTIK 3.0                                      */

/* Description: Hard Real TIme Kernel for 386 & higher machines */

/* Author:      Paolo Gai <pgai@rsk.it>                         */

/*              Advanced Linux Sound Architecture (ALSA)        */

/*              Copyright (c) by Jaroslav Kysela <perex@jcu.cz> */

/*              Luca Abeni                                      */

/*              FFTW by M. Frigo and S. G. Johnson              */

/* Date:        08/09/1999                                      */

/* File:        fftplay.c                                       */

/* Revision:    1.00 (Kernel 0.1.4; Library 0.0.9; Util 0.0.4)  */

/*

FFTPlay 1.0

-----------

This application reads data from the audio microphone and then put it

to the screen in a graphical oscilloscope-like form (both standard Hartik

SB driver and Alsa driver can be used).

The application also calculate a FFT on the latest values, and displays

the power spectrum in tho ways, one like an equalizator, and the other in a

2D form.

A resolution of 1024x768, 64K colors is used.

The task set is composed by these tasks and functions:

Self buffering functions (SB only)

----------------------------------

this function takes the samples read by the mic and then makes a window

with the last WINDATA_NSAMPLES samples. The window is then put in

the CAB windata.

Task raw and task mixer (ALSA only)

-----------------------------------

These tasks are used with the Alsa driver; because it doesn't support the

self-buffering mode, we have to do a forever cycle in witch we have to read

all the data. Then the data are managed like the self-buffering functions of

the SB driver or sent to the mixer task (with a STREAM mailbox) which makes

the window.

(This approach is not good for realtime...)

Task wave

---------

This task read the last window and then put it on the screen in a wave form

on the top of the screen.

The task's period is set to 40 ms (25 fps).

Task fft

--------

This task read the last window and then it computes the FFT.

With the FFT data it computes the power spectrum, whitch is sent to the

CAB pwrdata.

The task's period is set to 10 ms (good for the 2D story task).

This task is the only Hard Task.

Task equ

--------

This task read the last power spectrum and displays it in a graphical

form, like a hi-fi equalizator.

The Histograms can be white or coloured like the equ2D task (see EQU_SHADE)

The task's period is set to 40 ms (25 fps).

Task equ2D

----------

This task read the lasf power spectrum and displays it in a graphical

one-line form. Each pixel is a power coefficient, and its colour shade

from black (no power) to red (high power) passing through green and blue.

The task display the last EQU2D_WIDTH power lines.

The task's period is set to 10 ms (good for the 2D story task).

****************************************************************************

TASK LOAD

****************************************************************************

                     period              wcet

task             tick     (ms)      us          %

--------------------------------------------------------

sound driver    24->3    12->1.5   200     0.016->0.133

wave              80       40     11500       0.2875

fft               20       10      3000       0.3000

equ               80       40      7000       0.1750

equ2D             20       10       500       0.0500

                                          -------------

                                              0.812 (last 4)

*/

#include <ll/ll.h>

#include <kernel/types.h>

#include <kernel/model.h>

#include <kernel/func.h>

#include <modules/cabs.h>

#include <string.h>

#include <stdlib.h>

#include <semaphore.h>

#include <drivers/keyb.h>

#include <drivers/crtwin.h>

#include <drivers/glib.h>

#include <drivers/sound.h>

#include <ports/rfftw.h>

/* now the load constants... */

#define WCET_WAVE   11500

#define WCET_FFT    3000

#define WCET_EQU    500

#define WCET_EQU2D  500

#define PERIOD_WAVE  40000

#define PERIOD_FFT   10000

#define PERIOD_EQU   40000

#define PERIOD_EQU2D 10000

/* define if shorts critical sections wanted */

#define SHORT_CRITICAL_SECTIONS(x) \

  if (!((x)%64))                   \

    {                              \

      sem_post(&mutex);            \

      sem_wait(&mutex);            \

    }

/* define if you want NRT or SOFT... */

#define TASK_TYPE SOFT

//#define TASK_TYPE NRT

/* Only 4 Debug... */

/*#define NO_GRX */

/* Samples are 16-bit signed integers */

typedef short SAMPLE;

#define MAX_SAMPLE 32768

/* Informations about the sampling rate and buffers */

WORD rawdata_nsamples;

WORD rawdata_buffer_size;

WORD rawdata_freq;

/* Numbers of samples of the sample window */

#define WINDATA_NSAMPLES  1024

/* task WAVE */

/* the point (wave_x,wave_y) is on the center left of the area... */

#define WAVE_NSAMPLES 1024

#define WAVE_X        0

#define WAVE_Y        130

#define WAVE_HEIGHT   80

/* task FFT */

#define FFT_NSAMPLES  WINDATA_NSAMPLES

#define PWR_NSAMPLES  (FFT_NSAMPLES/2+1)

/* task EQU */

/* the point (equ_x, equ_y) is the top right corner */

#define EQU_NSAMPLES  PWR_NSAMPLES

#define EQU_X         170

#define EQU_Y         255

#define EQU_HEIGHT    170

#define EQU_SHADE

/* task EQU2D */

/* the point (equ2d_x, equ2d_y) is the top left corner */

#define EQU2D_NSAMPLES  EQU_NSAMPLES

#define EQU2D_X         255

#define EQU2D_Y         EQU_Y

#define EQU2D_WIDTH     768

#define EQU2D_CLIP      255

/* scenario */

#define SCENARIO_NLABEL 16

/* Scale factors */

#define FFT_SCALE   (16384.0)

#define EQU_SCALE   (32.0)

#define EQU2D_SCALE (8.0)

//#define EQU_SCALE   (64.0)

//#define EQU2D_SCALE (16.0)

/* CAB ports... */

CAB cab_windata;   /* a window on the last WINDATA_DIM samples */

CAB cab_pwrdata;   /* the last power spectrum                  */

/* for the cab_windata */

typedef struct {

  int start;

  SAMPLE sample[WINDATA_NSAMPLES];

} window;

/* for the cab_pwrdata */

typedef struct {

  fftw_real p[PWR_NSAMPLES];

} power;

/* graphic mutex... */

sem_t mutex;

// win is global... because is used by raw_infun...

window win;

/* useful colors... */

int white;

int black;

int red;

static void version( void )

{

  cprintf( "Hartik FFT Play 1.0\n"     );

  cprintf( "-----------------------\n" );

  cprintf( "by Paolo Gai 1999\n"       );

  cprintf( "   <pj@hartik.sssup.it>\n" );

  cprintf( "-----------------------\n" );

}

void reverse(char s[])

{

  int c, i, j;

  for (i = 0, j = strlen(s)-1; i<j; i++, j--)

  {

    c = s[i];

    s[i] = s[j];

    s[j] = c;

  }

}

char * itoa(int n, char *s)

{

  int i, sign;

  if ((sign = n) < 0)

    n = -n;

  i = 0;

  do

  {

    s[i++] = n % 10 + '0';

  } while ((n /= 10) > 0);

  if (sign < 0)

    s[i++] = '-';

  s[i] = 0;

  reverse(s);

  return s;

}

/*

   This is the self-buffering function: read the samples and put their mean

   value in a CAB

*/

int raw_infun(void *b)

{

    int i;

    char *w;

    SAMPLE *audiobuf = (SAMPLE *)b;

    for (i=0; i<rawdata_nsamples/2; i++) {

      win.sample[win.start] = audiobuf[i];

      win.start = (win.start+1) % WINDATA_NSAMPLES;

    }

    w = cab_reserve(cab_windata);

    memcpy(w, &win, sizeof(window));

    cab_putmes(cab_windata,w);

  #if defined(NO_GRX)

    cprintf("X"); //"XXX%d\n",win.sample[win.start]);

  #endif

    return 0;

}

void init_rawdata()

{

  int i;

  char *w;

  win.start = 0;

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

    win.sample[i] = 0;

  w = cab_reserve(cab_windata);

  memcpy(w, &win, sizeof(window));

  cab_putmes(cab_windata,w);

}

TASK wave_task()

{

  window *p;

  int x,y;

  int s;

  while(1)

  {

    p = (window *)cab_getmes(cab_windata);

    /* let's print the wave */

    sem_wait(&mutex);

    for(x = WAVE_X, s = p->start;

        x < WAVE_X+WAVE_NSAMPLES;

        x++, s = (s+1)%WINDATA_NSAMPLES )

    {

      y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE;

      SHORT_CRITICAL_SECTIONS(x);

      grx_plot(x,y,white);

    }

    sem_post(&mutex);

    task_endcycle();

    /* let's erase the wave */

    sem_wait(&mutex);

    for(x = WAVE_X, s = p->start;

        x < WAVE_X+WAVE_NSAMPLES;

        x++, s = (s+1)%WINDATA_NSAMPLES )

    {

      y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE;

      SHORT_CRITICAL_SECTIONS(x);

      grx_plot(x,y,black);

    }

    sem_post(&mutex);

    cab_unget(cab_windata,(char *)p);

  }

}

rfftw_plan plan;

void fft_close(void *arg)

{

  rfftw_destroy_plan(plan);

}

TASK fft_task()

{

  fftw_real in[FFT_NSAMPLES], out[FFT_NSAMPLES];

  power power_spectrum;

  #if defined(NO_GRX)

    fftw_real max = 0.0;

  #endif

  char *m;

  int k, i;

  window *p;

  plan = rfftw_create_plan(FFT_NSAMPLES, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);

  sys_atrunlevel(fft_close, NULL, RUNLEVEL_BEFORE_EXIT);

  while(1)

  {

    /* Let's prepare the intput FFT data */

    p = (window *)cab_getmes(cab_windata);

    for (k = 0, i = p->start;

        k < FFT_NSAMPLES;

        k++, i = (i+1)%WINDATA_NSAMPLES)

      in[k] = p->sample[i]/FFT_SCALE;

    cab_unget(cab_windata,(char *)p);

    /* zero-padding if needed */

    for (k=WINDATA_NSAMPLES; k < FFT_NSAMPLES; k++)

      in[k] = 0.0;

    rfftw_one(plan, in, out);

    /* power spectrum computation */

    power_spectrum.p[0] = out[0]*out[0];  /* DC component */

    for (k = 1; k < PWR_NSAMPLES; ++k)  /* (k < N/2 rounded up) */

          power_spectrum.p[k] = out[k]*out[k] + out[FFT_NSAMPLES-k]*out[FFT_NSAMPLES-k];

    if (FFT_NSAMPLES % 2 == 0) /* N is even */

          power_spectrum.p[FFT_NSAMPLES/2] = out[FFT_NSAMPLES/2]*out[FFT_NSAMPLES/2];  /* Nyquist freq. */

    m = cab_reserve(cab_pwrdata);

    memcpy(m, &power_spectrum, sizeof(power));

    cab_putmes(cab_pwrdata,m);

    #if defined(NO_GRX)

    max = 0.0;

    for (k=0; k<PWR_NSAMPLES; k++)

      if (power_spectrum.p[k] > max)

        max = power_spectrum.p[k];

    //cprintf("%f %f\n",max,(max / EQU_SCALE) );

    #endif

    task_endcycle();

  }

}

/* structure is like the wave task... */

TASK equ_task()

{

  power *p;

  int x[PWR_NSAMPLES];

  int y;

  int s;

  int r,g,b;

  while(1)

  {

    p = (power *)cab_getmes(cab_pwrdata);

    /* print the lines */

    sem_wait(&mutex);

    for(y = EQU_Y, s = 0;

        s < EQU_NSAMPLES;

        y++, s++ )

    {

      x[s] = (int)(p->p[s] / EQU_SCALE);

      if (x[s] > EQU_HEIGHT)

        x[s] = EQU_HEIGHT;

      x[s] = EQU_X - x[s];

      #if defined(EQU_SHADE)

       /* like the task equ2d... */

       r = (int)(p->p[s] / EQU2D_SCALE);

       if (r > EQU2D_CLIP)

         r = EQU2D_CLIP;

            if (r< 64)  g = r * 4;

       else if (r<128)  g = (128-r) * 4;

       else             g = 0;

            if (r<128)  b = 0;

       else if (r<192)  b = (r-128) * 4;

       else             b = (256-r) * 4;

       SHORT_CRITICAL_SECTIONS(y);

       grx_line(EQU_X,y,x[s],y,rgb16(r,g,b));

      #else

       SHORT_CRITICAL_SECTIONS(y);

       grx_line(EQU_X,y,x[s],y,white);

      #endif

    }

    sem_post(&mutex);

    task_endcycle();

    /* erase the lines... */

    sem_wait(&mutex);

    for(y = EQU_Y, s = 0;

        s < EQU_NSAMPLES;

        y++, s++ )

    {

      SHORT_CRITICAL_SECTIONS(y);

      grx_line(EQU_X,y,x[s],y,black);

    }

    sem_post(&mutex);

    cab_unget(cab_pwrdata,(char *)p);

  }

}

TASK equ2d_task()

{

  power *p;

  int pwrint;

  int x = 0;

  int y,s;

  int r,g,b;

  while(1)

  {

    p = (power *)cab_getmes(cab_pwrdata);

    /* print the line */

    sem_wait(&mutex);

    for(y = EQU2D_Y, s = 0;

        s < EQU2D_NSAMPLES;

        y++, s++ )

    {

      pwrint = (int)(p->p[s] / EQU2D_SCALE);

      if (pwrint > EQU2D_CLIP)

        pwrint = EQU2D_CLIP;

      r = pwrint;

           if (pwrint< 64)  g = pwrint * 4;

      else if (pwrint<128)  g = (128-pwrint) * 4;

      else                  g = 0;

           if (pwrint<128)  b = 0;

      else if (pwrint<192)  b = (pwrint-128) * 4;

      else                  b = (256-pwrint) * 4;

      SHORT_CRITICAL_SECTIONS(y);

      grx_plot(EQU2D_X+x,y,rgb16(r,g,b));

    }

    x = (x+1) % EQU2D_WIDTH;

    grx_line(EQU2D_X+x,EQU2D_Y,EQU2D_X+x,EQU2D_Y+EQU2D_NSAMPLES,white);

    sem_post(&mutex);

    cab_unget(cab_pwrdata,(char *)p);

    task_endcycle();

  }

}

TASK prova_task()

{

  window *p;

  while(1)

  {

    p = (window *)cab_getmes(cab_windata);

    cprintf("%d %d %d\t", p->start /*sample[0]*/,p->sample[1],p->sample[2]);

    cab_unget(cab_windata,(char *)p);

    task_endcycle();

  }

}

void scenario(int f)

{

  int i,y;

  char s[6];

  grx_line(0,WAVE_Y-WAVE_HEIGHT-1,1023,WAVE_Y-WAVE_HEIGHT-1,red);

  grx_line(0,WAVE_Y+WAVE_HEIGHT+1,1023,WAVE_Y+WAVE_HEIGHT+1,red);

  grx_line(0,EQU_Y-11            ,1023,EQU_Y-11            ,red);

  /* lines near the frequencies */

  grx_line(EQU_X  +1,EQU_Y,EQU_X  +1,EQU_Y+EQU_NSAMPLES,red);

  grx_line(EQU2D_X-1,EQU_Y,EQU2D_X-1,EQU_Y+EQU_NSAMPLES,red);

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

  {

    y = (i*EQU_NSAMPLES)/(SCENARIO_NLABEL-1);

    if (i == SCENARIO_NLABEL-1) y--;

    grx_line(EQU_X  +1,EQU_Y+y,EQU_X  +10,EQU_Y+y,red);

    grx_line(EQU2D_X-1,EQU_Y+y,EQU2D_X-10,EQU_Y+y,red);

    itoa((i*f)/(SCENARIO_NLABEL-1),s);

    grx_text(s,EQU_X+20,EQU_Y+y-8,white,black);

  }

  grx_text("FFTPlay 1.0 - by Paolo Gai 1999 <pj@hartik.sssup.it>", 0,8, rgb16(0,255,0), black );

  grx_text("...press ENTER key to exit..."  , 0,24, rgb16(0,255,0), black );

  grx_text("FFT Power Spectrum", 0      , EQU_Y-21, rgb16(0,0,255), black);

  grx_text("FFT Power Story", EQU2D_X+16, EQU_Y-21, rgb16(0,0,255), black);

  grx_text("Waveform"   , 0, WAVE_Y-WAVE_HEIGHT-10, rgb16(0,0,255), black);

}

void compute_params(int *freq,WORD *nsamp, WORD *per)

{

  if (*freq< 2000)

  {

    cprintf("WARNING: frequency less than 2000Hz\n  ---> frequency set to 2000Hz\n");

    *freq = 2000;

  }

  if (*freq<= 8000) { *nsamp = 128; *per = 10; return; }

  if (*freq<=16000) { *nsamp = 256; *per = 10; return; }

  if (*freq<=24000) { *nsamp = 512; *per = 10; return; }

  if (*freq>48000)

  {

    cprintf("WARNING: frequency greather than 48000Hz\n  ---> frequency set to 48000Hz\n");

    *freq = 48000;

  }

  if (*freq<=48000) { *nsamp = 1024;*per = 10; return; }

}

void my_close(void *arg)

{

        grx_close();

        sys_status(3);

}

void endfun(KEY_EVT *k)

{

    cprintf("Ctrl-Brk pressed! Ending...\n");

    sys_end();

}

int main(int argc, char **argv)

{

    int modenum;

    int f;

    /* irq period... */

    WORD period;

    KEY_EVT k;

    SOFT_TASK_MODEL m3, m4, m5, m6;

    PID p3,p4,p5,p6;

    version();

    if (argc == 1)

    {

      cprintf("type x fftplay <freq>");

      return 0;

    }

    f = atoi(argv[1]);

    compute_params(&f,&rawdata_nsamples,&period);

    keyb_set_map(itaMap);

    k.flag = CNTR_BIT;

    k.scan = KEY_C;

    k.ascii = 'c';

    keyb_hook(k,endfun);

    k.flag = CNTL_BIT;

    k.scan = KEY_C;

    k.ascii = 'c';

    keyb_hook(k,endfun);

    cab_windata = cab_create("windata", sizeof(window), 4);

    cab_pwrdata = cab_create("pwr", sizeof(power), 4);

    /* Init the sound lib */

    sound_init((rawdata_nsamples * sizeof(SAMPLE)), NULL);

    sound_info();

    /* Init the data used by the raw_infun */

    init_rawdata();

    /* Start the self-buffering sampling operation */

    sound_setfun(raw_infun, (int (*)(void *))-1);

    sound_sample(NULL, f, 0, DMA_OP | PCM16 | MYFUN, NULL);

    cprintf("Press Enter...");

    while (keyb_getchar() != 13);

    sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT);

    #if !defined(NO_GRX)

    grx_init();

    modenum = grx_getmode(1024, 768, 16);

    grx_setmode(modenum);

    /* init the graphic mutex */

    sem_init(&mutex, 0, 1);

    /* useful colors ... */

    white = rgb16(255,255,255);

    black = rgb16(0,0,0);

    red   = rgb16(255,0,0);

    scenario(f/2);

    #endif

    #if !defined(NO_GRX)

    soft_task_default_model(m3);

    soft_task_def_period(m3, PERIOD_WAVE);

    soft_task_def_met(m3, WCET_WAVE);

    soft_task_def_group(m3, 1);

    p3 = task_create("wave", wave_task, &m3, NULL);

    if (p3 == -1) {

        perror("FFTPlay: Could not create task <wave>\n");

        sys_end();

    }

    #endif

    soft_task_default_model(m4);

    soft_task_def_period(m4, PERIOD_FFT);

    soft_task_def_met(m4, WCET_FFT);

    soft_task_def_group(m4, 1);

    soft_task_def_stack(m4,32*1024);

    soft_task_def_usemath(m4);

    p4 = task_create("fft", fft_task, &m4, NULL);

    if (p4 == -1) {

        perror("FFTPlay: Could not create task <fft>\n");

        sys_end();

    }

    #if !defined(NO_GRX)

    soft_task_default_model(m5);

    soft_task_def_period(m5, PERIOD_EQU);

    soft_task_def_met(m5, WCET_EQU);

    soft_task_def_group(m5, 1);

    soft_task_def_stack(m5,32*1024);

    soft_task_def_usemath(m5);

    p5 = task_create("equ", equ_task, &m5, NULL);

    if (p5 == -1) {

        perror("FFTPlay: Could not create task <equ>\n");

        sys_end();

    }

    #endif

    #if !defined(NO_GRX)

    soft_task_default_model(m6);

    soft_task_def_period(m6, PERIOD_EQU2D);

    soft_task_def_met(m6, WCET_EQU2D);

    soft_task_def_group(m6, 1);

    soft_task_def_stack(m6,32*1024);

    soft_task_def_usemath(m6);

    p6 = task_create("equ2D", equ2d_task, &m5, NULL);

    if (p6 == -1) {

        perror("FFTPlay: Could not create task <equ2D>\n");

        sys_end();

    }

    #else

    /* Start the prova task */

    //task_def_wcet(m6,1000);

    //task_activate(task_create("prova",prova_task,TASK_TYPE,PERIODIC,200,&m6));

    #endif

    group_activate(1);

    /* Wait until the user get bored */

    while (keyb_getchar() != 13);

    sys_end();

    return 0;

}