Logo Search packages:      
Sourcecode: mas version File versions

mas_srate_device.c

/*
 * Copyright (c) 2001-2003 Shiman Associates Inc. All Rights Reserved.
 * 
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy,
 * modify, merge, publish, distribute, sublicense, and/or sell copies
 * of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */


#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include "mas/mas_dpi.h"
#include "profile.h"


#define W 8   /* window size (W on both sides) */ 
#define NOT_YET_CONFIGURED 999
#define CLOCK_ADJUST_PERIOD 5000000
#define FRAG_SIZE 214 /* this number times 100 times the maximum */
                      /* supported sample rate (200.000Hz here)  */
                      /* must fit into an uint32                 */ 
#define MIN_SEGMENT_ALLOC 3072

#define clamp16(a) (((a) > 0)? min((a), INT16_MAX) : max((a), INT16_MIN))


/* FILE *outf; */

#define CIRCBUF_SIZE 20000
struct circular_buffer
{
    int16 buf[CIRCBUF_SIZE];
    int write_head; /* next free position */
    int read_head; /* start of unused data */
};


/************************************************************************
 * srate_state
 * State memory structure for this device instance.
 ************************************************************************/
struct srate_state
{
    int32   srate_sink;
    int32   srate_source;
    int32   reaction;
    uint32  sequence, outgoing_ts, expected_ts;
    int8    source_configured, sink_configured;
    uint8   clock_rate_adjusted;
    uint32  inrate, outrate;
    uint32  configured_inrate, configured_outrate;
    uint32  requested_inrate, requested_outrate;  /* this is getting out of hand... */
    int     ismono;    
    uint32  k, m, carry;
    struct  circular_buffer cb;

    int     has_scheduled_adjust;
    
    /* trying new approach to adjusting to clocks */
    int32   in_clock_to_adjust_to;
    int32   out_clock_to_adjust_to;
};



/**** local prototypes ****************************************************/
void   circular_buffer_init( struct circular_buffer *cb );
void   circular_buffer_add( struct circular_buffer *cb, int16 *data,int length );
int    circular_buffer_get_len( struct circular_buffer *cb );
uint32 large_expr1( uint32 in, uint32 out, uint32 c );
int32  large_expr2( uint32 a, uint32 b, uint32 c );
void   large_div( uint32 a, uint32 b, uint32 c, uint32 *div, uint32 *rem );
int    lcd(uint32 a, uint32 b);




/*************************************************************************
 * ACTIONS
 *************************************************************************/


/* standard actions ****************************************************/
int32 mas_dev_init_instance( int32 , void* );
int32 mas_dev_exit_instance( int32 , void* );
int32 mas_dev_terminate( int32 , void* );
int32 mas_dev_show_state( int32 device_instance, void* predicate );
int32 mas_dev_configure_port( int32 device_instance, void* predicate );
int32 mas_dev_disconnect_port( int32 device_instance, void* predicate );
int32 mas_get( int32 device_instance, void* predicate );
int32 mas_set( int32 device_instance, void* predicate );


/* device specific actions *********************************************/
int32 mas_srate_mono( int32 device_instance, void* predicate );
int32 mas_srate_stereo( int32 device_instance, void* predicate );
int32 mas_srate_clock_adjust( int32 device_instance, void* predicate );






/* these are stock circular buffer routines.. move into a library? */
void
circular_buffer_init( struct circular_buffer *cb )
{
    memset( cb->buf, 0, CIRCBUF_SIZE*sizeof(int16) );
    cb->write_head = 0;
    cb->read_head = 0;
}


void
circular_buffer_add( struct circular_buffer *cb, int16 *data, int length )
{
    int16 *buf;
    int remain;
    int fit;

    buf = cb->buf;
    
    remain = cb->write_head+length - CIRCBUF_SIZE;
    
    if( remain > 0 )
    {
        fit = CIRCBUF_SIZE - cb->write_head;

        if( fit>0 ) 
            memcpy( buf+cb->write_head, data, fit*sizeof(int16) );
        
        memcpy( buf, data+fit , remain*sizeof(int16) );

        cb->write_head = remain;
    }
    else
    {
        memcpy( buf+cb->write_head, data, length*sizeof(int16) );
        cb->write_head += length;
    }
}

int
circular_buffer_get_len( struct circular_buffer *cb )
{
    return ( (cb->write_head - cb->read_head + CIRCBUF_SIZE)%CIRCBUF_SIZE );
}



/***************************************************************************
 * mas_dev_init_instance - standard device action
 *
 *  predicate: unused
 *
 *  Initializes state structure.
 *  
 *  returns: error
 *
 ***************************************************************************/
int32 mas_dev_init_instance( int32 device_instance, void* predicate )
{
    struct srate_state*  state;
    
    /* Allocate state holder and cast it so we can work on it */
    state = masc_rtalloc(sizeof(struct srate_state));
    if ( state == 0 )
      return mas_error(MERR_MEMORY);
    
    
    state->m                       =  0;
    state->k                       =  0;
    state->carry                   =  0;
    state->expected_ts             =  0;
    state->outgoing_ts             =  0;
    state->clock_rate_adjusted     =  0;   
    state->in_clock_to_adjust_to   = -1;
    state->out_clock_to_adjust_to  = -1;
    state->has_scheduled_adjust    =  0;
    
    circular_buffer_init( &state->cb );
    
/*     outf = fopen("/home/silvio/MAS/out.raw", "w"); */
    
    state->source_configured = 0;
    state->sink_configured   = 0;

    state->ismono = NOT_YET_CONFIGURED;
    
    /* in case you wonder... the other state struct members are
       initialized in mas_dev_configure_port()                  */

    
    masd_set_state(device_instance, state);
    
    masd_get_port_by_name( device_instance, "sink",
                     &state->srate_sink );

    masd_get_port_by_name( device_instance, "source",
                     &state->srate_source );

    masd_get_port_by_name( device_instance, "reaction",
                     &state->reaction );

    return 0;
}

int32 mas_dev_terminate( int32 device_instance, void* predicate )
{
    return 0;
}

int32 mas_dev_exit_instance( int32 device_instance, void* predicate )
{
    struct srate_state*  state;
    
    masd_get_state( device_instance, (void**)&state );
    masc_rtfree( state );
    
    return 0;
}


/***************************************************************************
 * mas_dev_show_state - standard debugging action
 *
 *  predicate: unused
 *
 * Sends state information to stdout.
 *  
 * returns: error
 *
 ***************************************************************************/
int32 mas_dev_show_state( int32 device_instance, void* predicate )
{
    struct srate_state* s;
    masd_get_state(device_instance, (void**)&s);

    printf("*-- srate state ---------------------------------------------\n");

    printf(" input sample rate:");
    if (s->sink_configured) printf(" %d\n", s->inrate);
    else printf(" not yet configured\n");
    
    printf("output sample rate:");
    if (s->source_configured) printf(" %d\n", s->outrate);
    else printf(" not yet configured\n");
    
    if ((s->sink_configured)&&(s->source_configured))
    {
        printf("I am fully configured.\n"); 
    }
    else printf("Both ports need to be configured for operation.\n");
    
    return 0;
}


/***************************************************************************
 * mas_dev_configure_port
 *
 *  predicate: int32 portnum
 *  
 *  returns: 0 on success
 *
 ***************************************************************************/
int32 mas_dev_configure_port( int32 device_instance, void* predicate )
{
    struct srate_state*             state;
    struct mas_data_characteristic* dc;
    int32*                          dataflow_port_dependency;
    int                             sri; 
    int32                           err;
    
    err = masd_get_state(device_instance, (void**)&state);
    err = masd_get_data_characteristic( *(int32*)predicate, &dc );
    if ( err < 0 ) return err;
    
    
    /* relying on format, resolution to be as 
       requested in profile                                        */

    sri = masc_get_index_of_key(dc, "channels");
    if ( sri < 0 ) return mas_error(MERR_INVALID);

    if( atoi(dc->values[sri])==1 )
    {
        if( (state->ismono != NOT_YET_CONFIGURED) && (state->ismono !=1) )
            return mas_error(MERR_INVALID);
        else
        {
            state->ismono = 1;
        }
    }
    else
    {
        if( (state->ismono != NOT_YET_CONFIGURED) && (state->ismono !=0) )
            return mas_error(MERR_INVALID);
        else
        {
            state->ismono = 0;
        }   
    } 

    sri = masc_get_index_of_key(dc, "sampling rate");
    if ( sri < 0 ) return mas_error(MERR_INVALID);    
    
    
    masc_entering_log_level("srate: mas_dev_configure_port");
    
    /* set in- or output sampling rate, depending on which port we
       were called for                                             */
    if (*(int32*)predicate == state->srate_sink)
    {
        state->inrate = atoi(dc->values[sri]);
        state->configured_inrate = state->inrate;
        state->requested_inrate  = 100 * state->inrate;
        state->sink_configured = 1;
        masc_log_message(MAS_VERBLVL_DEBUG,
                         "srate: configuring sink for %lu %s", state->inrate,
                         (state->ismono)?"mono":"stereo");
    }
    
    
    if (*(int32*)predicate == state->srate_source)
    {
        state->outrate = atoi(dc->values[sri]);
        state->configured_outrate = state->outrate;
        state->requested_outrate = 100 * state->outrate;
        state->source_configured = 1;
        masc_log_message(MAS_VERBLVL_DEBUG,
                         "srate: configuring source for %lu %s",
                         state->outrate,
                         (state->ismono)?"mono":"stereo" );
    }

    masc_exiting_log_level();

    if((state->sink_configured)&&(state->source_configured))
    {
        
        dataflow_port_dependency = masc_rtalloc( sizeof (int32) );
        *dataflow_port_dependency = state->srate_sink;   

        
        if( !state->ismono )
        {
            err = masd_reaction_queue_action(state->reaction, device_instance, 
                                             "mas_srate_stereo",
                                             0, 0, 0, 0, 0,
                                             MAS_PRIORITY_DATAFLOW, 1, 1, 
                                             dataflow_port_dependency);   
            
        }
        else
        {
            err = masd_reaction_queue_action(state->reaction, device_instance, 
                                             "mas_srate_mono",
                                             0, 0, 0, 0, 0,
                                             MAS_PRIORITY_DATAFLOW, 1, 1, 
                                             dataflow_port_dependency);
        }        
    }
    return 0;
}


/***************************************************************************
 * mas_dev_disconnect_port
 *
 *  predicate: int32 portnum
 *
 * returns: error
 *
 ***************************************************************************/
int32 mas_dev_disconnect_port( int32 device_instance, void* predicate )
{
    return 0;
}



int32
mas_srate_clock_adjust( int32 device_instance, void* predicate )
{
    struct srate_state* state;
    double new_rate;
    
    masd_get_state(device_instance, (void**)&state);    

    if( state->in_clock_to_adjust_to >= 0 )
    {
        uint32 in, out, fact;

        masd_mc_rate( state->in_clock_to_adjust_to, &new_rate );
                    
        state->clock_rate_adjusted = 1;
        in  = 100 * new_rate;
        out = state->requested_outrate;
        state->requested_inrate = in;
        
        fact = (in>out) ? lcd(in,out) : lcd(out,in);

        in /= fact;
        out /= fact;

        state->inrate = in;
        state->outrate = out;
        masc_log_message( MAS_VERBLVL_DEBUG, "srate: new inrate: %d; new outrate: %d; (factored out %d)", state->inrate, state->outrate, fact );

    }
    
    if( state->out_clock_to_adjust_to >= 0 )
    {
        uint32 in, out, fact;
        
        masd_mc_rate( state->out_clock_to_adjust_to, &new_rate );
        
        state->clock_rate_adjusted = 1;
        out = 100 * new_rate;
        in  = state->requested_inrate;
        state->requested_outrate = out;
        
        fact = (in>out) ? lcd(in,out) : lcd(out,in);

        in /= fact;
        out /= fact;

        state->inrate = in;
        state->outrate = out;
        masc_log_message( MAS_VERBLVL_DEBUG, "srate: new inrate: %d; new outrate: %d; (factored out %d)", state->inrate, state->outrate, fact );
    }
    
    return 0;
}



int32
mas_get( int32 device_instance, void* predicate )
{
    struct srate_state*  state;
    int32 err;
    int32 retport;
    char* key;
    struct mas_package arg;
    struct mas_package r_package;
    /* list of nuggets.  preserve the terminator */
    static char* nuggets[] = 
        { "list", "inrate", "outrate", "mc_in_clkid", "mc_out_clkid", "" };
    int i, n=0;
    
    masd_get_state(device_instance, (void**)&state);

    /* Use the standard get_nugget wrapper. */
    err = masd_get_pre( predicate, &retport, &key, &arg );
    if ( err < 0 ) return err;

    /* construct our response */
    masc_setup_package( &r_package, NULL, 0, MASC_PACKAGE_NOFREE );
    
    /* count the defined nuggets */
    while ( *nuggets[n] != 0 ) n++;

    i = masc_get_string_index(key, nuggets, n);

    switch(i)
    {
    case 0: /*list*/
        masc_push_strings( &r_package, nuggets, n );
        break;
    case 1: /*inrate*/
        masc_pushk_uint32( &r_package, "inrate", state->inrate );
        break;
    case 2: /*outrate*/
        masc_pushk_uint32( &r_package, "outrate", state->outrate );
        break;
    case 3: /*mc_in_clkid*/
        masc_pushk_uint32( &r_package, "mc_in_clkid", state->in_clock_to_adjust_to );
        break;
    case 4: /*mc_out_clkid*/
        masc_pushk_uint32( &r_package, "mc_out_clkid", state->out_clock_to_adjust_to );
        break;
        
    default:
        break;
    }    
    
    masc_finalize_package( &r_package );
    
    /* post the response where it belongs and free the data structures
     * we abused */
    err = masd_get_post( state->reaction, retport, key, &arg, &r_package );

    return err;
}

     

int32
mas_set( int32 device_instance, void* predicate )
{
    struct srate_state*  state;
    int32 err;
    char* key;
    struct mas_package arg;
    /* list of nuggets.  preserve the terminator */
    static char* nuggets[] = 
        { "inrate", "outrate", "in_clock_rate", "out_clock_rate", "mc_in_clkid", "mc_out_clkid", "" };
    int i, n=0;
    uint32 getter;
    uint32 rate;
    
    
    masd_get_state(device_instance, (void**)&state);

    /* Use the standard get_nugget wrapper. */
    err = masd_set_pre( predicate, &key, &arg );
    if ( err < 0 ) return err;

    /* count the defined nuggets */
    while ( *nuggets[n] != 0 ) n++;

    i = masc_get_string_index(key, nuggets, n);

    switch( i )
    {
    case 0: /*inrate*/
        masc_pullk_uint32( &arg, "inrate", &getter );
        state->inrate = getter;
        state->requested_inrate = 100 * state->inrate;
        break;
        
    case 1: /*outrate*/
        masc_pullk_uint32( &arg, "outrate", &getter );
        state->outrate = getter;
        state->requested_outrate = 100 * state->outrate;
        break;
        
    case 2: /*in_clock_rate*/
    {
        uint32 in, out, fact;

        state->clock_rate_adjusted = 1;
        masc_pullk_uint32( &arg, "in_clock_rate", &rate );
        in  = rate;
        out = state->requested_outrate;
        state->requested_inrate = rate;
        
        fact = (in>out) ? lcd(in,out) : lcd(out,in);

        in /= fact;
        out /= fact;

        state->inrate = in;
        state->outrate = out;
        masc_log_message( MAS_VERBLVL_DEBUG, "srate: new inrate: %d; new outrate: %d; (factored out %d)", state->inrate, state->outrate, fact );
        
        break;
    }

    case 3: /*out_clock_rate*/
    {
        uint32 in, out, fact;
        
        state->clock_rate_adjusted = 1;
        masc_pullk_uint32( &arg, "out_clock_rate", &rate );
        out  = rate;
        in   = state->requested_inrate;
        state->requested_outrate = rate;
        
        fact = (in>out) ? lcd(in,out) : lcd(out,in);

        in /= fact;
        out /= fact;

        state->inrate = in;
        state->outrate = out;

        masc_log_message( MAS_VERBLVL_DEBUG, "srate: new inrate: %d; new outrate: %d; (factored out %d)", state->inrate, state->outrate, fact );
        
        break;
    }

    case 4: /*mc_in_clkid*/
    {
        masc_pullk_int32( &arg, "mc_in_clkid", &state->in_clock_to_adjust_to );
        masc_log_message( MAS_VERBLVL_DEBUG, "srate: mas_set(mc_in_clkid) set to clock %d", state->in_clock_to_adjust_to );

        if( ! state->has_scheduled_adjust )
            masd_reaction_queue_action( state->reaction,
                                        device_instance,
                                        "mas_srate_clock_adjust", 0, 0, 0, 0, 1,
                                        MAS_PRIORITY_ASAP,
                                        CLOCK_ADJUST_PERIOD, 0, 0 );

        state->has_scheduled_adjust = TRUE;
        
        break;
    }

    case 5: /*mc_out_clkid*/
    {
        masc_pullk_int32( &arg, "mc_out_clkid", &state->out_clock_to_adjust_to );
        masc_log_message( MAS_VERBLVL_DEBUG, "srate: mas_set(mc_out_clkid) set to clock %d", state->out_clock_to_adjust_to );

        if( ! state->has_scheduled_adjust )
            masd_reaction_queue_action( state->reaction,
                                        device_instance,
                                        "mas_srate_clock_adjust", 0, 0, 0, 0, 1,
                                        MAS_PRIORITY_ASAP,
                                        CLOCK_ADJUST_PERIOD, 0, 0 );

        state->has_scheduled_adjust = TRUE;
        
        break;
    }

    default:
        break;
    }

    /* cleanup after our mess */
    err = masd_set_post( key, &arg );
    
    return err;
}




/***************************************************************************
 * mas_srate_yourfoodsready -- No, _mono :(
 *
 * this has all the comments in it (stereo doesn't)
 **************************************************************************/
int32 mas_srate_mono( int32 device_instance, void* predicate )
{
    struct  srate_state* s;
    struct  mas_data     *data;
    struct  circular_buffer *cb;
    
    double  sinval, tmp, val, r, mr, sec;
    int     N, i, outsize;
    int16   *outbuf;
    int32   lo, up, k, err;
    uint32  mmax, m, a, in, out;
    uint32  timestamp, frag, siz;
    uint32  div, rem, j;
  
    
    masd_get_state(device_instance, (void**)&s);    
    err = masd_get_data(s->srate_sink, &data);
    if (data->length == 0) return mas_error(MERR_INVALID);
    
    
    in  = s->inrate;
    out = s->outrate;
    cb  = &(s->cb);
  
    N   = data->length /2;
    r   = (double)in / (double)out;
  
  
    circular_buffer_add( cb, (int16*)data->segment, N );


    /* this has to be here because we may have changed rates */
    if( s->m )
        s->m = large_expr1( in, out, s->k - W - 1 ); /* computes s->m = ( out*( s->k - W - 1) - 1 ) / in + 1; */
  

    /* trim int( k/in ) insample lengths off both arrays if possible */
    a = s->k / in;
    if( a )
    {
        if( s->k%in > (W+1) )
        {
            s->k = s->k%in;
            s->m = large_expr1( in, out, s->k - W - 1 );
            s->carry = (s->carry + a*in) % CIRCBUF_SIZE;
        }
    }  
  
    mmax =  large_expr1( in, out,  N + s->k - W - 1 );
  
    outsize = (mmax - s->m);
    if(outsize<0)
    {
        masc_log_message( MAS_VERBLVL_DEBUG, "srate: outsize = %d, but it must never be <0. I'd say you have a wrapping problem!", outsize );
    }

  
    /* "if shoe fits, wear it" */
    if( data->allocated_length >= 2*outsize )
    {
        outbuf = (int16*)data->segment;
        data->length = 2*outsize;
    }
    else
    {
        masc_rtfree( data->segment );
        data->length = 2*outsize;
        data->allocated_length = max( data->length, MIN_SEGMENT_ALLOC );
        data->segment = masc_rtalloc(  data->allocated_length );
        outbuf = (int16*)data->segment;
    }



  

    /* speed up: if in|out, sampling theorem says we can just drop samples */
    if( in%out )
    {
        i=0;
        /* we may have to "fragment" computation for this output packet;
           we don't want any double calculations (large_exprX) in the
           innermost loop, so we take them outside, but then must
           prevent uint32 wraps. See also the thread on sai-audio about
           srate optimization */ 
        for( frag=0; frag<outsize; frag+=FRAG_SIZE )
        {
            large_div( s->m+frag, in, out, &div, &rem );
          
            siz = frag+FRAG_SIZE;
            siz = (siz<outsize) ? siz : outsize;
          
            j=0;
            for( m=frag; m<siz; m++ )
            {
                val = 0;
                            
                /* if in*m|out then we will evaluate sin(x)/x at sin(x) zero
                 * crossings */
                if( (j+rem)%out )
                {
                    lo = div + j/out + (rem+j%out)/out - W;
                    up = lo + 2*W;
                  
                    mr = (m+s->m)*r;

                    /* value of the sin for all this m's convolution coefficients,
                     * except the sign */
                    sinval = sin( M_PI*(mr - lo) );
                  
                    /* do two loops here: one where the sign of the sin
                       is +1, the other where it is -1  [ saves computation of (1-2*(k%2)) ] */ 
                    for( k=lo; k<up; k+=2 )
                    {
                        tmp = M_PI * ( mr - k );
                        tmp = sinval / tmp;
                        val += tmp * cb->buf[ ( k + s->carry )%CIRCBUF_SIZE ];
                    }

                    sinval = -sinval;
                  
                    for( k=lo+1; k<up; k+=2 )
                    {
                        tmp = M_PI * ( mr - k );
                        tmp = sinval / tmp;
                        val += tmp * cb->buf[ ( k + s->carry )%CIRCBUF_SIZE ];
                    }
                }
                else
                {
                    val = cb->buf[ ( div + j/out + (rem+j%out)/out + s->carry )%CIRCBUF_SIZE ]; 
                }
                outbuf[ i++ ] = clamp16( val );
                j += in;
            }
        }
    }
    else
    {
        /* change this to not use large_expr() some day. The commented
           out code below doesn't work but I can't see right now why */
        i=0;
        for( m=s->m; m<mmax; m++ )
        {
            outbuf[ i++ ] = cb->buf[ ( large_expr2(in,m,out) + s->carry )%CIRCBUF_SIZE ];
        }

        /* ?!?! */
        /*       uint32 pos; */
        /*       printf("got here\n"); */
              
        /*       for( frag=0; frag<outsize; frag+=FRAG_SIZE ) */
        /*       { */
        /*           printf("  fraggin\n"); */
        /*           large_div( in, s->m+frag, out, &div, &rem ); */
                  
        /*           siz = frag+FRAG_SIZE; */
        /*           siz = (siz<outsize) ? siz : outsize; */
        
        /*           m=0; */
        /*           for( i=frag; i<siz; i++ ) */
        /*           { */
        /*               pos = div + m/out + (rem+m%out)/out; */
        /*               outbuf[i++] = cb->buf[ ( s->carry + pos )%CIRCBUF_SIZE ]; */
        /*               m+=in; */
        /*           } */
        /*       } */
    }
  
    s->m = mmax;
    s->k += N;

  
    /*   fwrite( outbuf, sizeof(int16), outsize, outf ); */
  

  
    /* One input packet corresponds to one output packet... so I am leaving
     * alone the sequence number and the mark flag */

    /* do we have rates that are multiplied by 100? */
    if( s->clock_rate_adjusted )
        sec = (double)data->header.media_timestamp / ( (double)out/100.0 );
    else
        sec = (double)data->header.media_timestamp / (double)out;
  

    
    /* new strategy for timestamp calculation: only disturb natural
     * order if needed */ 

    timestamp = s->expected_ts;
  
    if( data->header.media_timestamp != timestamp )
    {
        if( data->header.media_timestamp > timestamp )
        {
            i = data->header.media_timestamp - timestamp;
            i = large_expr2( i, out, in );
            s->outgoing_ts += i;
            masc_log_message( MAS_VERBLVL_DEBUG, "srate: It seems some data was dropped before it could get to me. Adjusting my timestamps." );
        }
        else
        {
            masc_log_message( MAS_VERBLVL_DEBUG, "srate: Incoming media time stamps are inconsistent! data->header.media_timestamp < s->expected_ts (%lu < %lu) but I ignore timestamps anyway...", data->header.media_timestamp, s->expected_ts  );
        }
    }
  
    s->expected_ts = data->header.media_timestamp + N;
  
  
    data->header.media_timestamp = s->outgoing_ts;
    s->outgoing_ts += data->length / 2;

  
  
    /* NTP calculation is an approximation */
    data->header.ntp_seconds = (uint32)floor(sec);
    data->header.ntp_fraction = (uint32)(4295000000.0 *
                                         (sec - (double)data->header.ntp_seconds));


    err = masd_post_data(s->srate_source, data);
    if ( err < 0 )
        return err;
  
    return 0;
}


/***************************************************************************
 * stereo routine
 *
 * this has the optimizations described in an sai-audio document in it
 * that resulted from profiling the code. The mono routine above does
 * not (yet?) have these optimizations. 
 **************************************************************************/
int32 mas_srate_stereo( int32 device_instance, void* predicate )
{
    struct srate_state     *s;
    struct mas_data        *data;
  
    int32                  err;
    double                 sec;
  
    uint32                 mmax, m, a;
    double                 sinval, tmp, lval, rval, r, mr;
    uint32                 div, rem, j;
    uint32                 in, out;
    struct circular_buffer *cb;
    int                    N;
    int                    i, outsize;
    int32                  lo, up, k;
    int16                  *outbuf;
    uint32                 timestamp;
    uint32                 frag, siz;
    uint32                 pos;
    int32                  ul, ll;
  
  
    masd_get_state(device_instance, (void**)&s);    
    err = masd_get_data(s->srate_sink, &data);
    if (data->length == 0) return mas_error(MERR_INVALID);  
  
  
    in  = s->inrate;
    out = s->outrate;
    cb  = &(s->cb);
  
    N   = data->length /4;
    r   = (double)in / (double)out;
  
  
    circular_buffer_add( cb, (int16*)data->segment, 2*N );

    if( s->m )
        s->m = large_expr1( in, out, s->k - W - 1 );

    a = s->k / in;
    if( a )
    {
        if( s->k%in > (W+1) )
        {
            s->k = s->k%in;
            s->m = large_expr1( in, out, s->k - W - 1 ); 
            s->carry = (s->carry + 2*a*in) % CIRCBUF_SIZE;
        }
    }
  
    mmax = large_expr1( in, out,  N + s->k - W - 1 );
  
    outsize = mmax - s->m;
    if( outsize<0 )
        printf("outsize<0 !!: %d\n", outsize);
  
    if( data->allocated_length >= 4*outsize )
    {
        outbuf = (int16*)data->segment;
        data->length = 4*outsize;
    }
    else
    {
        masc_rtfree( data->segment );
        data->length = 4*outsize;
        data->allocated_length = max( data->length, MIN_SEGMENT_ALLOC );
        data->segment = masc_rtalloc(  data->allocated_length );
        outbuf = (int16*)data->segment;
    }
  
    if( in%out )
    {
        i=0;
        for( frag=0; frag<outsize; frag+=FRAG_SIZE )
        {
            large_div( s->m+frag, in, out, &div, &rem );
          
            siz = frag+FRAG_SIZE;
            siz = (siz<outsize) ? siz : outsize;

            j=0;
            for( m=frag; m<siz; m++ )
            {
                lval = 0;
                rval = 0;

                if( (j+rem)%out )
                {
                    lo = div + j/out + (rem+j%out)/out - W;
                    up = lo + 2*W;
                  
                    mr = (m+s->m)*r;
                  
                    sinval = sin( M_PI* (mr - lo ) );
/*               sinval *= 1 - 2*(lo%2); */
                  
                    ul = (s->carry + (up<<1)) % CIRCBUF_SIZE;
                    ll = (s->carry + (lo<<1)) % CIRCBUF_SIZE;
                  
                    if( ul>ll )
                    {
                        /* we're not wrapping */
                        pos = ll;
                        for( k=lo; k<up; k+=2 )
                        {
                            tmp = M_PI * ( mr - k );
                            tmp = sinval / tmp;            
                          
                            lval += tmp * cb->buf[ pos++ ];
                            rval += tmp * cb->buf[ pos   ];
                            pos  += 3;
                        }
                      
                        sinval = -sinval;
                      
                        pos = ( ll + 2 )%CIRCBUF_SIZE;
                        for( k=lo+1; k<up; k+=2 )
                        {
                            tmp = M_PI * ( mr - k );
                            tmp = sinval / tmp;
                          
                            lval += tmp * cb->buf[ pos++ ];
                            rval += tmp * cb->buf[ pos   ];
                          
                            pos  += 3;
                        }    
                    }
                    else
                    {
                        pos = ll;
                        for( k=lo; k<up; k+=2 )
                        {
                            tmp = M_PI * ( mr - k );
                            tmp = sinval / tmp;
                          
                            pos %= CIRCBUF_SIZE;
                          
                            lval += tmp * cb->buf[ pos++ ];
                            rval += tmp * cb->buf[ pos   ];
                            pos  += 3;
                        }
                      
                        sinval = -sinval;
                      
                        pos = ll+2;
                        for( k=lo+1; k<up; k+=2 )
                        {
                            tmp = M_PI * ( mr - k );
                            tmp = sinval / tmp;
                          
                            pos %= CIRCBUF_SIZE;
                          
                            lval += tmp * cb->buf[ pos++ ];
                            rval += tmp * cb->buf[ pos   ];
                            pos  += 3;
                        }    
                    }
                }
                else
                {
                    pos = div + j/out + (rem+j%out)/out;
                    pos = ( s->carry + 2*pos )%CIRCBUF_SIZE;
                    lval = cb->buf[ pos++ ]; 
                    rval = cb->buf[ pos   ]; 
                }
              
                outbuf[ 2*i   ] = clamp16( lval );
                outbuf[ 2*i+1 ] = clamp16( rval );
                ++i;
                j+=in;
            }
        }
    }
    else
    {
        for( frag=0; frag<outsize; frag+=FRAG_SIZE )
        {
            large_div( s->m+frag, in, out, &div, &rem );

            siz = frag+FRAG_SIZE;
            siz = (siz<outsize) ? siz : outsize;

            m=0;
            for( i=frag; i<siz; i++ )
            {
                pos = div + m/out + (rem+m%out)/out;
                pos = ( s->carry + 2*pos )%CIRCBUF_SIZE;
                outbuf[2*i]   = cb->buf[ pos++ ];
                outbuf[2*i+1] = cb->buf[ pos   ];
                m+=in;
            }
        }
    }
  
    s->m = mmax;
    s->k += N;

  
/*   fwrite( outbuf, sizeof(int16), outsize, outf ); */


  

    if( s->clock_rate_adjusted )
        sec = (double)data->header.media_timestamp / ( (double)out/100.0 );
    else
        sec = (double)data->header.media_timestamp / (double)out;
  
    timestamp = s->expected_ts;
  
    if( data->header.media_timestamp != timestamp )
    {
        if( data->header.media_timestamp > timestamp )
        {
            masc_log_message(
                MAS_VERBLVL_DEBUG ,
                "srate: we seem to have dropouts... adjusting my timestamps");
      
            i = data->header.media_timestamp - timestamp;
            i = large_expr2( i, out, in );
            s->outgoing_ts += i;
        }
        else
        {
            masc_log_message( MAS_VERBLVL_DEBUG, "srate: data->header.media_timestamp < s->expected_ts (%lu < %lu), but I ignore timestamps anyway...",data->header.media_timestamp, s->expected_ts  );
        }
    }
  
    s->expected_ts = data->header.media_timestamp + N;
  
  
    data->header.media_timestamp = s->outgoing_ts;
    s->outgoing_ts += data->length / 4;
  
  
    /* NTP calculation is an approximation */
    data->header.ntp_seconds = (uint32)floor(sec);
    data->header.ntp_fraction = (uint32)(4295000000.0 *
                                         (sec - (double)data->header.ntp_seconds));
  
    err = masd_post_data(s->srate_source, data);
    if ( err < 0 ) return err;
  
  
    return 0;
}


/***************************************************************************
 * largest common denominator 
 ***************************************************************************/
int
lcd( uint32 a, uint32 b)
{
  return (a%b==0) ? b : lcd(b,a%b);
}



/* these are expressions of approximate type (a*b)/c, where a*b can be
   too big for a uint32. So we resort to using doubles intermittently
*/ 
uint32
large_expr1( uint32 in, uint32 out, uint32 c )
{
    double t;
    uint32 r;
    
    t  = (double)c * (double)out;
    t -= 1;
    t  = t / (double)in;
    r  = t + 1;

    return r;
}

/* (a*b)/c, gives int result and remainder */
void
large_div( uint32 a, uint32 b, uint32 c, uint32 *div, uint32 *rem )
{
    double t;
    
    t  = (double)a * (double)b;
    *div = t / (double)c;
    *rem = t - *div * (double)c;
}


/* (a*b)/c, int result only */
int32
large_expr2( uint32 a, uint32 b, uint32 c )
{
    double t;
    
    t = ((double)a * (double)b) / (double)c;
    return (int32)t;
}


Generated by  Doxygen 1.6.0   Back to index