tomwalters@0: /* tomwalters@0: Copyright (c) Applied Psychology Unit, Medical Research Council. 1988, 1989 tomwalters@0: =========================================================================== tomwalters@0: tomwalters@0: Permission to use, copy, modify, and distribute this software without fee tomwalters@0: is hereby granted for research purposes, provided that this copyright tomwalters@0: notice appears in all copies and in all supporting documentation, and that tomwalters@0: the software is not redistributed for any fee (except for a nominal shipping tomwalters@0: charge). Anyone wanting to incorporate all or part of this software in a tomwalters@0: commercial product must obtain a license from the Medical Research Council. tomwalters@0: tomwalters@0: The MRC makes no representations about the suitability of this tomwalters@0: software for any purpose. It is provided "as is" without express or implied tomwalters@0: warranty. tomwalters@0: tomwalters@0: */ tomwalters@0: tomwalters@0: /* tomwalters@0: ======================================================== tomwalters@0: generic.c generic C code for recursive filter section. tomwalters@0: ======================================================== tomwalters@0: tomwalters@0: tomwalters@0: J. Holdsworth - 23rd February 1988. tomwalters@0: tomwalters@0: tomwalters@0: Copywright (c) Applied Psychology Unit, Medical Research Council. 1988. tomwalters@0: ======================================================================= tomwalters@0: tomwalters@0: tomwalters@0: Release 2: 5th July 1988. tomwalters@0: Rewrite : 21st Jan 1989. tomwalters@0: tomwalters@0: */ tomwalters@0: tomwalters@0: #ifndef _MATH_H_ tomwalters@0: #include tomwalters@0: #define _MATH_H_ tomwalters@0: #endif tomwalters@0: tomwalters@0: #ifndef _STITCH_H_ tomwalters@0: #include "stitch.h" tomwalters@0: #endif tomwalters@0: #ifndef _RECURSE_H_ tomwalters@0: #include "recurse.h" tomwalters@0: #endif tomwalters@0: #ifndef _PHASE_H_ tomwalters@0: #include "phase.h" tomwalters@0: #endif tomwalters@0: #ifndef _GAMMA_TONE_H_ tomwalters@0: #include "gamma_tone.h" tomwalters@0: #endif tomwalters@0: tomwalters@0: #ifndef FLOAT tomwalters@0: tomwalters@0: #define STATE_TYPE long tomwalters@0: tomwalters@0: #define I_SHIFT( _number ) ( ( _number ) << i_shift ) tomwalters@0: tomwalters@0: #ifdef vax tomwalters@0: #define K_SHIFT( _number ) ( ( _number ) << k_shift ) tomwalters@0: #define S_SHIFT( _number ) ( ( _number ) << s_shift ) tomwalters@0: #else tomwalters@0: #define K_SHIFT( _number ) ( ( _number ) >> k_shift ) tomwalters@0: #define S_SHIFT( _number ) ( ( _number ) >> s_shift ) tomwalters@0: #endif tomwalters@0: tomwalters@0: #else tomwalters@0: tomwalters@0: #define STATE_TYPE FLOAT tomwalters@0: tomwalters@0: #define I_SHIFT( _number ) ( _number ) tomwalters@0: #define K_SHIFT( _number ) ( _number ) tomwalters@0: #define S_SHIFT( _number ) ( _number ) tomwalters@0: tomwalters@0: #endif tomwalters@0: tomwalters@0: #ifndef INPUT_TYPE tomwalters@0: #define INPUT_TYPE short tomwalters@0: #endif tomwalters@0: tomwalters@0: #ifndef OUTPUT_TYPE tomwalters@0: #define OUTPUT_TYPE INPUT_TYPE tomwalters@0: #endif tomwalters@0: tomwalters@0: #ifndef MODULE_NAME tomwalters@0: #define MODULE_NAME DoFilterShortDataArray tomwalters@0: #define FILTER_NAME FilterShortDataArray tomwalters@0: #endif tomwalters@0: tomwalters@0: #ifdef DSP32 tomwalters@0: tomwalters@0: #define SIN( _PHI ) ( *( (float *) ( (char *) sin_table + istore ) ) ) tomwalters@0: #define COS( _PHI ) ( *( (float *) ( (char *) cos_table + istore ) ) ) tomwalters@0: tomwalters@0: #else tomwalters@0: tomwalters@0: #define SIN( _PHI ) ( sin_table[ HI( _PHI ) ] ) tomwalters@0: #define COS( _PHI ) ( cos_table[ HI( _PHI ) ] ) tomwalters@0: tomwalters@0: #endif tomwalters@0: tomwalters@0: /* Code that performs the actual filtering */ tomwalters@0: tomwalters@0: PointCount MODULE_NAME( filter_states, delays, input, output, points, channels ) tomwalters@0: RecursiveFilterState **filter_states ; tomwalters@0: int *delays ; tomwalters@0: INPUT_TYPE *input ; tomwalters@0: OUTPUT_TYPE *output ; tomwalters@0: int points, channels ; tomwalters@0: { tomwalters@0: extern char *bstart, *bend ; tomwalters@0: #ifdef DSP32 tomwalters@0: /* do not change the order of these definitions - the "asm()" calls depend on it */ tomwalters@0: register STATE_TYPE k, *states_ptr, *states_out, *states_end, *ptr ; tomwalters@0: #else tomwalters@0: register STATE_TYPE *states_ptr, *states_end, store, rstore, istore, k ; tomwalters@0: #endif tomwalters@0: register RecursiveFilterState *filter_state ; tomwalters@0: #ifndef FLOAT tomwalters@0: register int k_shift, s_shift, i_shift ; tomwalters@0: #endif tomwalters@0: #ifdef DSP32 tomwalters@0: register int istore ; tomwalters@0: #endif tomwalters@0: register Table sin_table, cos_table ; tomwalters@0: register STATE_TYPE output_store, input_store ; tomwalters@0: #ifdef COMPLEX tomwalters@0: register STATE_TYPE ioutput_store ; tomwalters@0: #endif tomwalters@0: register OUTPUT_TYPE *output_ptr, *end ; tomwalters@0: register INPUT_TYPE *input_ptr ; tomwalters@0: int loop_count, channel ; tomwalters@0: #ifdef DSP32 tomwalters@0: #ifndef ASM tomwalters@0: STATE_TYPE a0, a1 ; tomwalters@0: #endif tomwalters@0: #endif tomwalters@0: /* set up register pointers for speed of inner loop */ tomwalters@0: tomwalters@0: sin_table = filterSineTable ; tomwalters@0: cos_table = filterCosineTable ; tomwalters@0: tomwalters@0: end = output + points * channels ; tomwalters@0: tomwalters@0: /* for all channels */ tomwalters@0: tomwalters@0: for( channel=0 ; channel < channels ; channel++ ) { tomwalters@0: tomwalters@0: filter_state = filter_states[ channel ] ; tomwalters@0: tomwalters@0: #ifndef FLOAT tomwalters@0: i_shift = 16 - filter_state->input_bits ; tomwalters@0: k_shift = 16 - 2 ; tomwalters@0: tomwalters@0: s_shift = filterSineTableShift ; tomwalters@0: #endif tomwalters@0: tomwalters@0: /* if first time then perform implementation specific initialisation */ tomwalters@0: tomwalters@0: if( filter_state->states == (Pointer) 0 ) { tomwalters@0: tomwalters@0: filter_state->states = stitch_ralloc( (unsigned) ( filter_state->order + 1 ) * 2 * sizeof ( STATE_TYPE ), "generic.c for filter states" ) ; tomwalters@0: tomwalters@0: /* filter state stored in form of complex phasor for each cascade of filter */ tomwalters@0: /* real and imaginary state values interleaved in state array */ tomwalters@0: tomwalters@0: filter_state->states_end = (Pointer) ( (STATE_TYPE *) filter_state->states + ( filter_state->order + 1 ) * 2 ) ; tomwalters@0: tomwalters@0: stitch_bzero( filter_state->states, (int) ( filter_state->states_end - filter_state->states ) ) ; tomwalters@0: tomwalters@0: #ifdef FLOAT tomwalters@0: filter_state->output_scale /= ( 1l << filterSineTableShift ) ; tomwalters@0: #ifdef ENVELOPE tomwalters@0: filter_state->post_log = -log10( filter_state->output_scale ) * 2000. ; tomwalters@0: #else tomwalters@0: filter_state->output_scale /= ( 1l << filterSineTableShift ) ; tomwalters@0: #endif tomwalters@0: #else tomwalters@0: filter_state->ik = filter_state->k * ( 1l << k_shift ) + 0.5 ; tomwalters@0: filter_state->post_divisor = ( 1l << ( filterSineTableShift + i_shift ) ) / filter_state->output_scale + 0.5 ; tomwalters@0: #ifdef ENVELOPE tomwalters@0: filter_state->post_log = imB( (long) filter_state->post_divisor ) - imB( 1l << filterSineTableShift ) ; tomwalters@0: #endif tomwalters@0: #endif tomwalters@0: #ifdef ENVELOPE tomwalters@0: filter_state->post_log -= ( filter_state->over_sample - 1 ) * imB( 2l ) / 2 ; tomwalters@0: #endif tomwalters@0: } tomwalters@0: tomwalters@0: #ifdef FLOAT tomwalters@0: k = filter_state->k ; tomwalters@0: #else tomwalters@0: k = filter_state->ik ; tomwalters@0: #ifdef vax tomwalters@0: k_shift = -k_shift ; tomwalters@0: s_shift = -s_shift ; tomwalters@0: #endif tomwalters@0: #endif tomwalters@0: tomwalters@0: tomwalters@0: /* Eurgh - the price of compatability */ tomwalters@0: tomwalters@0: if( input == (INPUT_TYPE *) 0 ) { tomwalters@0: input_ptr = (INPUT_TYPE *) output ; tomwalters@0: tomwalters@0: stitch_bzero( input_ptr, ( end - output ) / channels * sizeof ( *input_ptr ) ) ; tomwalters@0: } tomwalters@0: else tomwalters@0: input_ptr = input - delays[ channel ] ; tomwalters@0: tomwalters@0: /* not required to test bank.c any more tomwalters@0: if( (char *) input_ptr < bstart || (char *) ( input_ptr + points ) > bend ) tomwalters@0: printf( "Filter bank error!! %x %x %x\n", bstart, input_ptr, bend ) ; tomwalters@0: */ tomwalters@0: states_end = (STATE_TYPE *) filter_state->states_end ; tomwalters@0: tomwalters@0: #ifdef DSP32 tomwalters@0: /* this section is optimised for DSP32 processor with more registers than instructions */ tomwalters@0: tomwalters@0: for( output_ptr = output + channel ; output_ptr < end ; output_ptr += channels ) { tomwalters@0: tomwalters@0: input_store = I_SHIFT( *input_ptr++ ) ; tomwalters@0: output_store = 0 ; tomwalters@0: #ifdef COMPLEX tomwalters@0: ioutput_store = 0 ; tomwalters@0: #endif tomwalters@0: tomwalters@0: loop_count = filter_state->over_sample ; tomwalters@0: tomwalters@0: do tomwalters@0: { tomwalters@0: /* multipy by complex phasor */ tomwalters@0: tomwalters@0: ptr = &input_store ; tomwalters@0: tomwalters@0: istore = HI( filter_state->phi ) << 1 << 1 ; tomwalters@0: tomwalters@0: #ifdef ASM tomwalters@0: asm( "r1e = r6 + r8" ) ; tomwalters@0: asm( "a0 = *r10 * *r1" ) ; tomwalters@0: asm( "r2e = r7 + r8" ) ; tomwalters@0: asm( "a1 = *r10 * *r2" ) ; tomwalters@0: #else tomwalters@0: a0 = *ptr * COS( filter_state->phi ) ; tomwalters@0: a1 = *ptr * SIN( filter_state->phi ) ; tomwalters@0: #endif tomwalters@0: tomwalters@0: /* low pass filter by simple alfa decay difference equation */ tomwalters@0: tomwalters@0: states_ptr = (STATE_TYPE *) filter_state->states ; tomwalters@0: states_out = states_ptr ; tomwalters@0: tomwalters@0: #ifdef ASM tomwalters@0: asm( "LOOP:" ) ; tomwalters@0: asm( "a0 = a0 - *r13++" ) ; tomwalters@0: asm( "a1 = a1 - *r13++" ) ; tomwalters@0: asm( "nop" ) ; tomwalters@0: asm( "*r12++ = a0 = *r12 + a3 * a0" ) ; tomwalters@0: asm( "r13e - r11" ) ; tomwalters@0: asm( "if(cc) goto EXIT" ); tomwalters@0: asm( "*r12++ = a1 = *r12 + a3 * a1" ) ; tomwalters@0: tomwalters@0: asm( " a2 = a0 - *r13++ " ) ; tomwalters@0: asm( " a1 = a1 - *r13++ " ) ; tomwalters@0: asm( " a0 = *r12 " ) ; tomwalters@0: asm( "*r12++ = a2 = *r12 + a3 * a2" ) ; tomwalters@0: asm( " a2 = a1 " ) ; tomwalters@0: asm( " a1 = *r12 " ) ; tomwalters@0: asm( "r13e - r11" ) ; tomwalters@0: asm( "if(cs) goto LOOP" ); tomwalters@0: asm( "*r12++ = a2 = *r12 + a3 * a2" ) ; tomwalters@0: asm( "EXIT:" ) ; tomwalters@0: #else tomwalters@0: do tomwalters@0: { tomwalters@0: a0 = a0 - *states_ptr++ ; tomwalters@0: a1 = a1 - *states_ptr++ ; tomwalters@0: a0 = *states_out++ += k * a0 ; tomwalters@0: a1 = *states_out++ += k * a1 ; tomwalters@0: } tomwalters@0: while( states_ptr < states_end ) ; tomwalters@0: /* multiply back up to carrier frequency */ tomwalters@0: #endif tomwalters@0: istore = HI( filter_state->output_phi ) << 1 << 1 ; tomwalters@0: tomwalters@0: #ifndef ENVELOPE tomwalters@0: #ifdef ASM tomwalters@0: asm( "r1e = r6 + r8" ) ; tomwalters@0: asm( "a2 = a2 + a0 * *r1" ) ; tomwalters@0: asm( "r2e = r7 + r8" ) ; tomwalters@0: asm( "a2 = a2 + a1 * *r2" ) ; tomwalters@0: #else tomwalters@0: output_store += a0 * COS( filter_state->output_phi ) ; tomwalters@0: output_store += a1 * SIN( filter_state->output_phi ) ; tomwalters@0: #endif tomwalters@0: #ifdef COMPLEX tomwalters@0: ptr = &ioutput_store ; tomwalters@0: #ifdef ASM tomwalters@0: asm( " a0 = *r10 + a0 * *r2" ) ; tomwalters@0: asm( "*r10 = a0 = a0 - a1 * *r1" ) ; tomwalters@0: #else tomwalters@0: ioutput_store += a0 * SIN( filter_state->output_phi ) ; tomwalters@0: ioutput_store -= a1 * COS( filter_state->output_phi ) ; tomwalters@0: #endif tomwalters@0: #endif tomwalters@0: #else ENVELOPE tomwalters@0: #ifdef ASM tomwalters@0: asm( "a2 = a2 + a1 * a1" ) ; tomwalters@0: asm( "a2 = a2 + a0 * a0" ) ; tomwalters@0: #else tomwalters@0: output_store += a0 * a0 ; tomwalters@0: output_store += a1 * a1 ; tomwalters@0: #endif tomwalters@0: #endif ENVELOPE tomwalters@0: tomwalters@0: ALL( filter_state->phi ) += filter_state->delta_phi ; tomwalters@0: } tomwalters@0: while( --loop_count > 0 ) ; tomwalters@0: /* do it again if over sampling */ tomwalters@0: tomwalters@0: #else tomwalters@0: /* this section optimised for generall purpose processor e.g. vax or 68000 */ tomwalters@0: tomwalters@0: for( output_ptr = output + channel ; output_ptr < end ; output_ptr += channels ) { tomwalters@0: tomwalters@0: input_store = I_SHIFT( *input_ptr++ ) ; tomwalters@0: tomwalters@0: output_store = 0 ; tomwalters@0: #ifdef COMPLEX tomwalters@0: ioutput_store = 0 ; tomwalters@0: #endif tomwalters@0: loop_count = filter_state->over_sample ; tomwalters@0: tomwalters@0: do tomwalters@0: { tomwalters@0: /* multipy by complex phasor */ tomwalters@0: tomwalters@0: states_ptr = (STATE_TYPE *) filter_state->states + 2 ; tomwalters@0: tomwalters@0: rstore = input_store * COS( filter_state->phi ) ; tomwalters@0: istore = input_store * SIN( filter_state->phi ) ; tomwalters@0: tomwalters@0: do tomwalters@0: { tomwalters@0: rstore = *states_ptr + k * K_SHIFT( rstore - *states_ptr ) ; tomwalters@0: *states_ptr++ = rstore ; tomwalters@0: tomwalters@0: istore = *states_ptr + k * K_SHIFT( istore - *states_ptr ) ; tomwalters@0: *states_ptr++ = istore ; tomwalters@0: tomwalters@0: if( states_ptr < states_end ) { tomwalters@0: store = k * K_SHIFT( rstore - *states_ptr ) ; tomwalters@0: rstore = *states_ptr ; tomwalters@0: *states_ptr++ = rstore + store ; tomwalters@0: tomwalters@0: store = k * K_SHIFT( istore - *states_ptr ) ; tomwalters@0: istore = *states_ptr ; tomwalters@0: *states_ptr++ = istore + store ; tomwalters@0: } tomwalters@0: } tomwalters@0: while( states_ptr < states_end ) ; tomwalters@0: /* tomwalters@0: *states_ptr++ = input_store * COS( filter_state->phi ) ; tomwalters@0: *states_ptr++ = input_store * SIN( filter_state->phi ) ; tomwalters@0: tomwalters@0: do tomwalters@0: { tomwalters@0: store = k * K_SHIFT( states_ptr[ -2 ] - *states_ptr ) ; tomwalters@0: *states_ptr++ += store ; tomwalters@0: store = k * K_SHIFT( states_ptr[ -2 ] - *states_ptr ) ; tomwalters@0: *states_ptr++ += store ; tomwalters@0: } tomwalters@0: while( states_ptr < states_end ) ; tomwalters@0: */ tomwalters@0: /* multiply back up to carrier frequency */ tomwalters@0: tomwalters@0: #ifdef ENVELOPE tomwalters@0: istore = S_SHIFT( istore ) ; tomwalters@0: output_store += istore * istore ; tomwalters@0: rstore = S_SHIFT( rstore ) ; tomwalters@0: output_store += rstore * rstore ; tomwalters@0: #else tomwalters@0: output_store += S_SHIFT( istore ) * SIN( filter_state->output_phi ) ; tomwalters@0: output_store += S_SHIFT( rstore ) * COS( filter_state->output_phi ) ; tomwalters@0: #ifdef COMPLEX tomwalters@0: ioutput_store -= S_SHIFT( istore ) * COS( filter_state->output_phi ) ; tomwalters@0: ioutput_store += S_SHIFT( rstore ) * SIN( filter_state->output_phi ) ; tomwalters@0: #endif tomwalters@0: #endif tomwalters@0: ALL( filter_state->phi ) += filter_state->delta_phi ; tomwalters@0: ALL( filter_state->phi ) &= filterSineTableMask ; tomwalters@0: } tomwalters@0: while( --loop_count > 0 ) ; tomwalters@0: /* do it again if over sampling */ tomwalters@0: #endif tomwalters@0: tomwalters@0: /* common tail end */ tomwalters@0: tomwalters@0: #ifdef ENVELOPE tomwalters@0: #ifdef FLOAT tomwalters@0: if( output_store < 1. ) tomwalters@0: *output_ptr = LOG_ZERO ; tomwalters@0: else tomwalters@0: *output_ptr = log10( output_store ) * 1000. - filter_state->post_log ; tomwalters@0: tomwalters@0: if( *output_ptr < 0. ) tomwalters@0: *output_ptr = 0. ; tomwalters@0: #else tomwalters@0: *output_ptr = ( imB( (long) output_store ) >> 1 ) - filter_state->post_log ; tomwalters@0: tomwalters@0: if( *output_ptr < 0 ) tomwalters@0: *output_ptr = 0 ; tomwalters@0: #endif tomwalters@0: #else tomwalters@0: /* for non-envelope output */ tomwalters@0: tomwalters@0: ALL( filter_state->output_phi ) += filter_state->output_delta_phi ; tomwalters@0: #ifndef DSP32 tomwalters@0: ALL( filter_state->output_phi ) &= filterSineTableMask ; tomwalters@0: #endif tomwalters@0: tomwalters@0: /* scale output_ptr back down for output_ptr array compensating for over sampling if ness. */ tomwalters@0: tomwalters@0: #ifdef COMPLEX tomwalters@0: #ifdef FLOAT tomwalters@0: output_ptr->real = output_store * filter_state->output_scale ; tomwalters@0: output_ptr->imag = ioutput_store * filter_state->output_scale ; tomwalters@0: #else tomwalters@0: output_ptr->real = output_store / filter_state->post_divisor ; tomwalters@0: output_ptr->imag = ioutput_store / filter_state->post_divisor ; tomwalters@0: #endif tomwalters@0: #else tomwalters@0: #ifdef FLOAT tomwalters@0: *output_ptr = output_store * filter_state->output_scale ; tomwalters@0: #else tomwalters@0: *output_ptr = output_store / filter_state->post_divisor ; tomwalters@0: #endif tomwalters@0: #endif tomwalters@0: #endif tomwalters@0: } tomwalters@0: } tomwalters@0: tomwalters@0: return( sizeof ( OUTPUT_TYPE ) ) ; tomwalters@0: } tomwalters@0: tomwalters@0: /* for compatability */ tomwalters@0: tomwalters@0: PointCount FILTER_NAME( compensator_state, input_array, output_array, npoints ) tomwalters@0: FilterChannelInfo *compensator_state ; tomwalters@0: INPUT_TYPE *input_array ; tomwalters@0: OUTPUT_TYPE *output_array ; tomwalters@0: PointCount npoints ; tomwalters@0: { tomwalters@0: ( (PhaseCompensatorState *) compensator_state )->filter_module = MODULE_NAME ; tomwalters@0: tomwalters@0: return( PhaseCompensateFilter( (PhaseCompensatorState *) compensator_state, ( Pointer ) input_array, ( Pointer ) output_array, npoints ) ) ; tomwalters@0: } tomwalters@0: tomwalters@0: tomwalters@0: /* tidy up defines for redefinition */ tomwalters@0: tomwalters@0: #undef STATE_TYPE tomwalters@0: #undef INPUT_TYPE tomwalters@0: #undef OUTPUT_TYPE tomwalters@0: #undef FILTER_NAME tomwalters@0: #undef MODULE_NAME tomwalters@0: #undef I_SHIFT tomwalters@0: #undef K_SHIFT tomwalters@0: #undef S_SHIFT tomwalters@0: tomwalters@0: #undef SIN tomwalters@0: #undef COS tomwalters@0: tomwalters@0: #ifdef FLOAT tomwalters@0: #undef FLOAT tomwalters@0: #endif tomwalters@0: tomwalters@0: #ifdef COMPLEX tomwalters@0: #undef COMPLEX tomwalters@0: #endif