view filter/generic.c @ 0:5242703e91d3 tip

Initial checkin for AIM92 aimR8.2 (last updated May 1997).

/*
    Copyright (c) Applied Psychology Unit, Medical Research Council. 1988, 1989
    ===========================================================================

    Permission to use, copy, modify, and distribute this software without fee 
    is hereby granted for research purposes, provided that this copyright 
    notice appears in all copies and in all supporting documentation, and that 
    the software is not redistributed for any fee (except for a nominal shipping 
    charge). Anyone wanting to incorporate all or part of this software in a
    commercial product must obtain a license from the Medical Research Council.

    The MRC makes no representations about the suitability of this 
    software for any purpose.  It is provided "as is" without express or implied 
    warranty.
 
*/

/*
	 ========================================================
	  generic.c generic C code for recursive filter section.
	 ========================================================


		   J. Holdsworth - 23rd February 1988.


    Copywright (c) Applied Psychology Unit, Medical Research Council. 1988.
    =======================================================================


    Release 2:  5th July 1988.
    Rewrite  :  21st Jan 1989.

*/

#ifndef  _MATH_H_
#include <math.h>
#define  _MATH_H_
#endif

#ifndef  _STITCH_H_
#include "stitch.h"
#endif
#ifndef  _RECURSE_H_
#include "recurse.h"
#endif
#ifndef  _PHASE_H_
#include "phase.h"
#endif
#ifndef  _GAMMA_TONE_H_
#include "gamma_tone.h"
#endif

#ifndef  FLOAT

#define STATE_TYPE long

#define I_SHIFT( _number ) ( ( _number ) << i_shift )

#ifdef  vax
#define K_SHIFT( _number ) ( ( _number ) << k_shift )
#define S_SHIFT( _number ) ( ( _number ) << s_shift )
#else
#define K_SHIFT( _number ) ( ( _number ) >> k_shift )
#define S_SHIFT( _number ) ( ( _number ) >> s_shift )
#endif

#else

#define STATE_TYPE FLOAT

#define I_SHIFT( _number ) ( _number )
#define K_SHIFT( _number ) ( _number )
#define S_SHIFT( _number ) ( _number )

#endif

#ifndef INPUT_TYPE
#define INPUT_TYPE short
#endif

#ifndef OUTPUT_TYPE
#define OUTPUT_TYPE INPUT_TYPE
#endif

#ifndef MODULE_NAME
#define MODULE_NAME DoFilterShortDataArray
#define FILTER_NAME   FilterShortDataArray
#endif

#ifdef DSP32

#define SIN( _PHI ) ( *( (float *) ( (char *) sin_table + istore ) ) )
#define COS( _PHI ) ( *( (float *) ( (char *) cos_table + istore ) ) )

#else

#define SIN( _PHI ) ( sin_table[ HI( _PHI ) ] )
#define COS( _PHI ) ( cos_table[ HI( _PHI ) ] )

#endif

/* Code that performs the actual filtering */

PointCount MODULE_NAME( filter_states, delays, input, output, points, channels )
RecursiveFilterState **filter_states ;
int *delays ;
 INPUT_TYPE *input ;
OUTPUT_TYPE *output ;
int points, channels ;
{
extern char *bstart, *bend ;
#ifdef DSP32
    /* do not change the order of these definitions - the "asm()" calls depend on it */
    register STATE_TYPE k, *states_ptr, *states_out, *states_end, *ptr ;
#else
    register STATE_TYPE *states_ptr, *states_end, store, rstore, istore, k ;
#endif
    register RecursiveFilterState *filter_state ;
#ifndef FLOAT
    register int k_shift, s_shift, i_shift ;
#endif
#ifdef DSP32
    register int istore ;
#endif
    register Table sin_table, cos_table ;
    register STATE_TYPE output_store, input_store ;
#ifdef COMPLEX
    register STATE_TYPE ioutput_store ;
#endif
    register OUTPUT_TYPE *output_ptr, *end ;
    register INPUT_TYPE  *input_ptr ;
    int loop_count, channel ;
#ifdef DSP32
#ifndef ASM
    STATE_TYPE a0, a1 ;
#endif
#endif
    /* set up register pointers for speed of inner loop */

    sin_table = filterSineTable   ;
    cos_table = filterCosineTable ;

    end = output + points * channels ;

    /* for all channels */

    for( channel=0 ; channel < channels ; channel++ ) {

	filter_state = filter_states[ channel ] ;

#ifndef FLOAT
	i_shift = 16 - filter_state->input_bits ;
	k_shift = 16 - 2 ;

	s_shift = filterSineTableShift ;
#endif

	/* if first time then perform implementation specific initialisation */

	if( filter_state->states == (Pointer) 0 ) {

	    filter_state->states = stitch_ralloc( (unsigned) ( filter_state->order + 1 ) * 2 * sizeof ( STATE_TYPE ), "generic.c for filter states" ) ;

	    /* filter state stored in form of complex phasor for each cascade of filter */
	    /* real and imaginary state values interleaved in state array */

	    filter_state->states_end = (Pointer) ( (STATE_TYPE *) filter_state->states + ( filter_state->order + 1 ) * 2 ) ;

	    stitch_bzero( filter_state->states, (int) ( filter_state->states_end - filter_state->states ) ) ;

#ifdef  FLOAT
	    filter_state->output_scale /= ( 1l << filterSineTableShift ) ;
#ifdef  ENVELOPE
	    filter_state->post_log = -log10( filter_state->output_scale ) * 2000. ;
#else
	    filter_state->output_scale /= ( 1l << filterSineTableShift ) ;
#endif
#else
	    filter_state->ik = filter_state->k * ( 1l << k_shift ) + 0.5 ;
	    filter_state->post_divisor  = ( 1l << ( filterSineTableShift + i_shift ) ) / filter_state->output_scale + 0.5 ;
#ifdef  ENVELOPE
	    filter_state->post_log = imB( (long) filter_state->post_divisor ) - imB( 1l << filterSineTableShift ) ;
#endif
#endif
#ifdef  ENVELOPE
	    filter_state->post_log -= ( filter_state->over_sample - 1 ) * imB( 2l ) / 2 ;
#endif
	}

#ifdef  FLOAT
	k = filter_state->k  ;
#else
	k = filter_state->ik ;
#ifdef  vax
	k_shift = -k_shift ;
	s_shift = -s_shift ;
#endif
#endif


	/* Eurgh - the price of compatability */

	if( input    == (INPUT_TYPE *) 0 ) {
	    input_ptr = (INPUT_TYPE *) output ;

	    stitch_bzero( input_ptr, ( end - output ) / channels * sizeof ( *input_ptr ) ) ;
	}
	else
	    input_ptr = input - delays[ channel ] ;

/* not required to test bank.c any more
	if( (char *) input_ptr < bstart || (char *) ( input_ptr + points ) > bend )
	    printf( "Filter bank error!! %x %x %x\n", bstart, input_ptr, bend ) ;
*/
	states_end   = (STATE_TYPE *) filter_state->states_end ;

#ifdef DSP32
	/* this section is optimised for DSP32 processor with more registers than instructions */

	for( output_ptr = output + channel ; output_ptr < end ; output_ptr += channels ) {

	    input_store  = I_SHIFT( *input_ptr++ ) ;
	    output_store = 0 ;
#ifdef COMPLEX
	    ioutput_store = 0 ;
#endif

	    loop_count = filter_state->over_sample ;

	    do
	    {
		/* multipy by complex phasor */

		ptr = &input_store ;

		istore = HI( filter_state->phi ) << 1 << 1 ;

#ifdef ASM
		asm( "r1e =  r6  +  r8" ) ;
		asm( "a0  = *r10 * *r1" ) ;
		asm( "r2e =  r7  +  r8" ) ;
		asm( "a1  = *r10 * *r2" ) ;
#else
		a0 = *ptr * COS( filter_state->phi ) ;
		a1 = *ptr * SIN( filter_state->phi ) ;
#endif

		/* low pass filter by simple alfa decay difference equation */

		states_ptr  = (STATE_TYPE *) filter_state->states ;
		states_out  =  states_ptr ;

#ifdef ASM
		asm( "LOOP:" ) ;
		    asm( "a0 = a0 - *r13++" ) ;
		    asm( "a1 = a1 - *r13++" ) ;
		    asm( "nop" ) ;
		    asm( "*r12++ = a0 = *r12 + a3 * a0" ) ;
		asm( "r13e - r11" ) ;
		asm( "if(cc) goto EXIT" );
		    asm( "*r12++ = a1 = *r12 + a3 * a1" ) ;

		    asm( "         a2 =  a0  - *r13++ " ) ;
		    asm( "         a1 =  a1  - *r13++ " ) ;
		    asm( "         a0 = *r12          " ) ;
		    asm( "*r12++ = a2 = *r12 + a3 * a2" ) ;
		    asm( "         a2 =  a1           " ) ;
		    asm( "         a1 = *r12          " ) ;
		asm( "r13e - r11" ) ;
		asm( "if(cs) goto LOOP" );
		    asm( "*r12++ = a2 = *r12 + a3 * a2" ) ;
		asm( "EXIT:" ) ;
#else
		do
		{
		    a0 = a0 - *states_ptr++ ;
		    a1 = a1 - *states_ptr++ ;
		    a0 = *states_out++ += k * a0 ;
		    a1 = *states_out++ += k * a1 ;
		}
		while( states_ptr < states_end ) ;
		/* multiply back up to carrier frequency */
#endif
		istore = HI( filter_state->output_phi ) << 1 << 1 ;

#ifndef     ENVELOPE
#ifdef ASM
		asm( "r1e = r6 + r8" ) ;
		asm( "a2  = a2 + a0 * *r1" ) ;
		asm( "r2e = r7 + r8" ) ;
		asm( "a2  = a2 + a1 * *r2" ) ;
#else
		output_store += a0 * COS( filter_state->output_phi ) ;
		output_store += a1 * SIN( filter_state->output_phi ) ;
#endif
#ifdef COMPLEX
		ptr = &ioutput_store ;
#ifdef ASM
		asm( "       a0 = *r10 + a0 * *r2" ) ;
		asm( "*r10 = a0 =   a0 - a1 * *r1" ) ;
#else
	       ioutput_store += a0 * SIN( filter_state->output_phi ) ;
	       ioutput_store -= a1 * COS( filter_state->output_phi ) ;
#endif
#endif
#else       ENVELOPE
#ifdef ASM
		asm( "a2 = a2 + a1 * a1" ) ;
		asm( "a2 = a2 + a0 * a0" ) ;
#else
		output_store += a0 * a0 ;
		output_store += a1 * a1 ;
#endif
#endif      ENVELOPE

		ALL( filter_state->phi ) += filter_state->delta_phi ;
	    }
	    while( --loop_count > 0 ) ;
	    /* do it again if over sampling */

#else
	/* this section optimised for generall purpose processor e.g. vax or 68000 */

	for( output_ptr = output + channel ; output_ptr < end ; output_ptr += channels ) {

	    input_store  = I_SHIFT( *input_ptr++ ) ;

	    output_store = 0 ;
#ifdef COMPLEX
	    ioutput_store = 0 ;
#endif
	    loop_count = filter_state->over_sample ;

	    do
	    {
		/* multipy by complex phasor */

		states_ptr = (STATE_TYPE *) filter_state->states + 2 ;

		rstore = input_store * COS( filter_state->phi ) ;
		istore = input_store * SIN( filter_state->phi ) ;

		do
		{
		    rstore = *states_ptr + k * K_SHIFT( rstore - *states_ptr ) ;
		    *states_ptr++ = rstore ;

		    istore = *states_ptr + k * K_SHIFT( istore - *states_ptr ) ;
		    *states_ptr++ = istore ;

		    if( states_ptr < states_end ) {
			store = k * K_SHIFT( rstore - *states_ptr ) ;
			rstore = *states_ptr ;
			*states_ptr++ = rstore + store ;

			store = k * K_SHIFT( istore - *states_ptr ) ;
			istore = *states_ptr ;
			*states_ptr++ = istore + store ;
		    }
		}
		while( states_ptr < states_end ) ;
/*
		*states_ptr++ = input_store * COS( filter_state->phi ) ;
		*states_ptr++ = input_store * SIN( filter_state->phi ) ;

		do
		{
		    store = k * K_SHIFT( states_ptr[ -2 ] - *states_ptr ) ;
		    *states_ptr++ += store ;
		    store = k * K_SHIFT( states_ptr[ -2 ] - *states_ptr ) ;
		    *states_ptr++ += store ;
		}
		while( states_ptr < states_end ) ;
*/
		/* multiply back up to carrier frequency */

#ifdef     ENVELOPE
		istore = S_SHIFT( istore ) ;
		output_store  +=  istore * istore   ;
		rstore = S_SHIFT( rstore ) ;
		output_store  +=  rstore * rstore   ;
#else
		 output_store += S_SHIFT( istore ) * SIN( filter_state->output_phi ) ;
		 output_store += S_SHIFT( rstore ) * COS( filter_state->output_phi ) ;
#ifdef COMPLEX
		ioutput_store -= S_SHIFT( istore ) * COS( filter_state->output_phi ) ;
		ioutput_store += S_SHIFT( rstore ) * SIN( filter_state->output_phi ) ;
#endif
#endif
		ALL( filter_state->phi ) += filter_state->delta_phi ;
		ALL( filter_state->phi ) &= filterSineTableMask ;
	    }
	    while( --loop_count > 0 ) ;
	    /* do it again if over sampling */
#endif

/* common tail end */

#ifdef ENVELOPE
#ifdef  FLOAT
	    if( output_store < 1. )
		*output_ptr = LOG_ZERO ;
	    else
		*output_ptr = log10(      output_store ) * 1000. - filter_state->post_log ;

	    if( *output_ptr < 0. )
		*output_ptr = 0. ;
#else
	    *output_ptr = ( imB( (long)   output_store ) >> 1  ) - filter_state->post_log ;

	    if( *output_ptr < 0 )
		*output_ptr = 0 ;
#endif
#else
	    /* for non-envelope output */

	    ALL( filter_state->output_phi ) += filter_state->output_delta_phi ;
#ifndef DSP32
	    ALL( filter_state->output_phi ) &= filterSineTableMask ;
#endif

	    /* scale output_ptr back down for output_ptr array compensating for over sampling if ness. */

#ifdef COMPLEX
#ifdef  FLOAT
	    output_ptr->real =  output_store * filter_state->output_scale ;
	    output_ptr->imag = ioutput_store * filter_state->output_scale ;
#else
	    output_ptr->real =  output_store / filter_state->post_divisor ;
	    output_ptr->imag = ioutput_store / filter_state->post_divisor ;
#endif
#else
#ifdef  FLOAT
	   *output_ptr       =  output_store * filter_state->output_scale ;
#else
	   *output_ptr       =  output_store / filter_state->post_divisor ;
#endif
#endif
#endif
	}
    }

    return( sizeof ( OUTPUT_TYPE ) ) ;
}

/* for compatability */

PointCount FILTER_NAME( compensator_state, input_array, output_array, npoints )
FilterChannelInfo *compensator_state ;
INPUT_TYPE *input_array ;
OUTPUT_TYPE *output_array ;
PointCount npoints ;
{
    ( (PhaseCompensatorState *) compensator_state )->filter_module = MODULE_NAME ;

    return( PhaseCompensateFilter( (PhaseCompensatorState *) compensator_state, ( Pointer ) input_array, ( Pointer ) output_array, npoints ) ) ;
}


/* tidy up defines for redefinition */

#undef  STATE_TYPE
#undef  INPUT_TYPE
#undef OUTPUT_TYPE
#undef FILTER_NAME
#undef MODULE_NAME
#undef I_SHIFT
#undef K_SHIFT
#undef S_SHIFT

#undef SIN
#undef COS

#ifdef FLOAT
#undef FLOAT
#endif

#ifdef COMPLEX
#undef COMPLEX
#endif