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

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/filter/generic.c	Fri May 20 15:19:45 2011 +0100
@@ -0,0 +1,481 @@
+/*
+    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