annotate model/new.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.
 
    THE MRC DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING 
    ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL THE
    A.P.U. BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY 
    DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN 
    AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 
    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/

/*
	 ===================================================
	  new.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.

*/

#include "stitch.h"
#include "recurse.h"
#include "phase.h"
#include "gamma_tone.h"
#include "calc.h"

#ifdef  DSP32
#define ASM
#endif

#define STATE_TYPE float

#ifndef INPUT_TYPE
#define INPUT_TYPE DataType
#endif

#ifndef OUTPUT_TYPE
#define OUTPUT_TYPE INPUT_TYPE
#endif

#ifndef MODULE_NAME
#define MODULE_NAME DoNewFilterDataArray
#define FILTER_NAME NewFilterDataArray
#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 ;
{
    register STATE_TYPE  *start, *xpt, *ypt, *gpt ;
#ifndef ASM
    register double a0, a1, del1, del2 ;
#endif
    register RecursiveFilterState *filter_state ;
    register OUTPUT_TYPE *output_ptr, *end ;
    register INPUT_TYPE  *input_ptr ;
    register STATE_TYPE a3, a2 ;
    int channel ;

    /* set up register pointers for speed of inner loop */

    end = output + points * channels ;

    /* for all channels */

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

	filter_state = filter_states[ channel ] ;

	/* 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\n" ) ;
	    /* 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, filter_state->states_end - filter_state->states ) ;

	}

	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 ] ;

	start = ( STATE_TYPE * ) filter_state->states ;

	a2 = filter_state->k1 ;
#ifndef ASM
	a1 = a2 * 2 ;
#endif
	a3 = filter_state->k2 ;

	ypt = start + filter_state->order + 1 ;

	gpt = &filter_state->gain ;

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

#ifdef ASM
asm("     r12e = r13                        ") ;

asm("       a1 = a2 + a2                    ") ;

asm("*r12 = a0 = *r6++                      ") ;

asm("       a0 = a0 -          *r12++ * a2  ") ; /* xn-1 */
asm("       a0 = a0 + ( *r11 = *r12 ) * a1  ") ; /* yn-1 */
asm("*r12 = a0 = a0 -          *r11++ * a3  ") ; /* yn-2 */

asm("       a0 = a0 -          *r12++ * a2  ") ; /* xn-1 */
asm("       a0 = a0 + ( *r11 = *r12 ) * a1  ") ; /* yn-1 */
asm("*r12 = a0 = a0 -          *r11++ * a3  ") ; /* yn-2 */

asm("       a0 = a0 -          *r12++ * a2  ") ; /* xn-1 */
asm("       a0 = a0 + ( *r11 = *r12 ) * a1  ") ; /* yn-1 */
asm("*r12 = a0 = a0 -          *r11++ * a3  ") ; /* yn-2 */

asm("       a0 = a0 -          *r12++ * a2  ") ; /* xn-1 */
asm("       a0 = a0 + ( *r11 = *r12 ) * a1  ") ; /* yn-1 */
asm("*r12 = a0 = a0 -          *r11++ * a3  ") ; /* yn-2 */

asm("*r8  = a0 =               *r12++ * *r10") ;
#else
	    xpt = start ;

	    del1 = a0 = *input_ptr++ ;

	    a0 = a0 -          *xpt++ * a2     ; /* xn-1 */ xpt[-1] = del1 ;
	    a0 = a0 + ( del2 = *xpt ) * a1     ; /* yn-1 */
     del1 = a0 = a0 -          *ypt++ * a3     ; /* yn-2 */ ypt[-1] = del2 ;

	    a0 = a0 -          *xpt++ * a2     ; /* xn-1 */ xpt[-1] = del1 ;
	    a0 = a0 + ( del2 = *xpt ) * a1     ; /* yn-1 */
     del1 = a0 = a0 -          *ypt++ * a3     ; /* yn-2 */ ypt[-1] = del2 ;

	    a0 = a0 -          *xpt++ * a2     ; /* xn-1 */ xpt[-1] = del1 ;
	    a0 = a0 + ( del2 = *xpt ) * a1     ; /* yn-1 */
     del1 = a0 = a0 -          *ypt++ * a3     ; /* yn-2 */ ypt[-1] = del2 ;

	    a0 = a0 -          *xpt++ * a2     ; /* xn-1 */ xpt[-1] = del1 ;
	    a0 = a0 + ( del2 = *xpt ) * a1     ; /* yn-1 */
     del1 = a0 = a0 -          *ypt++ * a3     ; /* yn-2 */ ypt[-1] = del2 ;

   *output_ptr =               *gpt   * a0     ;            *xpt++  = del1 ;
#endif
	    ypt = xpt ;
	}
    }

    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