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:     THE MRC DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING 
tomwalters@0:     ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL THE
tomwalters@0:     A.P.U. BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY 
tomwalters@0:     DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN 
tomwalters@0:     AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 
tomwalters@0:     OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: 	 ===================================================
tomwalters@0: 	  new.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: #include "stitch.h"
tomwalters@0: #include "recurse.h"
tomwalters@0: #include "phase.h"
tomwalters@0: #include "gamma_tone.h"
tomwalters@0: #include "calc.h"
tomwalters@0: 
tomwalters@0: #ifdef  DSP32
tomwalters@0: #define ASM
tomwalters@0: #endif
tomwalters@0: 
tomwalters@0: #define STATE_TYPE float
tomwalters@0: 
tomwalters@0: #ifndef INPUT_TYPE
tomwalters@0: #define INPUT_TYPE DataType
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 DoNewFilterDataArray
tomwalters@0: #define FILTER_NAME NewFilterDataArray
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:     register STATE_TYPE  *start, *xpt, *ypt, *gpt ;
tomwalters@0: #ifndef ASM
tomwalters@0:     register double a0, a1, del1, del2 ;
tomwalters@0: #endif
tomwalters@0:     register RecursiveFilterState *filter_state ;
tomwalters@0:     register OUTPUT_TYPE *output_ptr, *end ;
tomwalters@0:     register INPUT_TYPE  *input_ptr ;
tomwalters@0:     register STATE_TYPE a3, a2 ;
tomwalters@0:     int channel ;
tomwalters@0: 
tomwalters@0:     /* set up register pointers for speed of inner loop */
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: 	/* 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\n" ) ;
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, filter_state->states_end - filter_state->states ) ;
tomwalters@0: 
tomwalters@0: 	}
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: 	start = ( STATE_TYPE * ) filter_state->states ;
tomwalters@0: 
tomwalters@0: 	a2 = filter_state->k1 ;
tomwalters@0: #ifndef ASM
tomwalters@0: 	a1 = a2 * 2 ;
tomwalters@0: #endif
tomwalters@0: 	a3 = filter_state->k2 ;
tomwalters@0: 
tomwalters@0: 	ypt = start + filter_state->order + 1 ;
tomwalters@0: 
tomwalters@0: 	gpt = &filter_state->gain ;
tomwalters@0: 
tomwalters@0: 	for( output_ptr = output + channel ; output_ptr < end ; output_ptr += channels ) {
tomwalters@0: 
tomwalters@0: #ifdef ASM
tomwalters@0: asm("     r12e = r13                        ") ;
tomwalters@0: 
tomwalters@0: asm("       a1 = a2 + a2                    ") ;
tomwalters@0: 
tomwalters@0: asm("*r12 = a0 = *r6++                      ") ;
tomwalters@0: 
tomwalters@0: asm("       a0 = a0 -          *r12++ * a2  ") ; /* xn-1 */
tomwalters@0: asm("       a0 = a0 + ( *r11 = *r12 ) * a1  ") ; /* yn-1 */
tomwalters@0: asm("*r12 = a0 = a0 -          *r11++ * a3  ") ; /* yn-2 */
tomwalters@0: 
tomwalters@0: asm("       a0 = a0 -          *r12++ * a2  ") ; /* xn-1 */
tomwalters@0: asm("       a0 = a0 + ( *r11 = *r12 ) * a1  ") ; /* yn-1 */
tomwalters@0: asm("*r12 = a0 = a0 -          *r11++ * a3  ") ; /* yn-2 */
tomwalters@0: 
tomwalters@0: asm("       a0 = a0 -          *r12++ * a2  ") ; /* xn-1 */
tomwalters@0: asm("       a0 = a0 + ( *r11 = *r12 ) * a1  ") ; /* yn-1 */
tomwalters@0: asm("*r12 = a0 = a0 -          *r11++ * a3  ") ; /* yn-2 */
tomwalters@0: 
tomwalters@0: asm("       a0 = a0 -          *r12++ * a2  ") ; /* xn-1 */
tomwalters@0: asm("       a0 = a0 + ( *r11 = *r12 ) * a1  ") ; /* yn-1 */
tomwalters@0: asm("*r12 = a0 = a0 -          *r11++ * a3  ") ; /* yn-2 */
tomwalters@0: 
tomwalters@0: asm("*r8  = a0 =               *r12++ * *r10") ;
tomwalters@0: #else
tomwalters@0: 	    xpt = start ;
tomwalters@0: 
tomwalters@0: 	    del1 = a0 = *input_ptr++ ;
tomwalters@0: 
tomwalters@0: 	    a0 = a0 -          *xpt++ * a2     ; /* xn-1 */ xpt[-1] = del1 ;
tomwalters@0: 	    a0 = a0 + ( del2 = *xpt ) * a1     ; /* yn-1 */
tomwalters@0:      del1 = a0 = a0 -          *ypt++ * a3     ; /* yn-2 */ ypt[-1] = del2 ;
tomwalters@0: 
tomwalters@0: 	    a0 = a0 -          *xpt++ * a2     ; /* xn-1 */ xpt[-1] = del1 ;
tomwalters@0: 	    a0 = a0 + ( del2 = *xpt ) * a1     ; /* yn-1 */
tomwalters@0:      del1 = a0 = a0 -          *ypt++ * a3     ; /* yn-2 */ ypt[-1] = del2 ;
tomwalters@0: 
tomwalters@0: 	    a0 = a0 -          *xpt++ * a2     ; /* xn-1 */ xpt[-1] = del1 ;
tomwalters@0: 	    a0 = a0 + ( del2 = *xpt ) * a1     ; /* yn-1 */
tomwalters@0:      del1 = a0 = a0 -          *ypt++ * a3     ; /* yn-2 */ ypt[-1] = del2 ;
tomwalters@0: 
tomwalters@0: 	    a0 = a0 -          *xpt++ * a2     ; /* xn-1 */ xpt[-1] = del1 ;
tomwalters@0: 	    a0 = a0 + ( del2 = *xpt ) * a1     ; /* yn-1 */
tomwalters@0:      del1 = a0 = a0 -          *ypt++ * a3     ; /* yn-2 */ ypt[-1] = del2 ;
tomwalters@0: 
tomwalters@0:    *output_ptr =               *gpt   * a0     ;            *xpt++  = del1 ;
tomwalters@0: #endif
tomwalters@0: 	    ypt = xpt ;
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