Logo Search packages:      
Sourcecode: mas version File versions

mc.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 <string.h>
#include <math.h>
#ifdef linux
# include <sys/timex.h>
#endif
#include "mas_internal.h"
#include "assembler.h"
#include "mas_dpi.h"
#include "mc.h"

#define RATETOL 0.001

/* list of actions.  preserve the terminator */
static char* actions[] = { "mas_mc_update_rate", "mas_mc_add_clock", "mas_mc_delete_clock", "mas_get", "mas_set", "mas_mc_log_clock_values", "mas_mc_reset_clock", "mas_mc_add_fixed_clock", "" }; 

static char* _std_clk_names[] = { "system", "8 kHz", "11.025 kHz", "16 kHz", "22.05 kHz", "32 kHz", "44.1 kHz", "48 kHz", "96 kHz", "" };
static int   _std_clk_rates[] = { 1000000, 8000, 11025, 16000, 22050, 32000, 44100, 48000, 96000, 0 };

static struct mas_mc_clknode* mc_add_clock_stub( struct mas_mc_cb* cb, char* name, double nom_rate );
static struct mas_mc_clknode* mc_get_node( struct mas_mc_cb* cb, int32 id );
static void mc_sync_all( struct mas_mc_cb* cb, uint32 ts_us );
static uint32 mc_val( struct mas_mc_clknode* node, uint32 ts_us );
static uint32 mc_update_val( struct mas_mc_cb* cb, struct mas_mc_clknode* node );
static void mc_sync( struct mas_mc_clknode* node, uint32 ts_us );
static int32 mc_get( struct mas_mc_cb* cb, char* key, struct mas_package* arg, int send_req_info, struct mas_package* r_package );
static int32 mc_set( struct mas_mc_cb* cb, char* key, struct mas_package* arg );
static void mc_log_clock_values( struct mas_mc_cb* cb );
static void _show_sysclk_info( void );
static int32 mc_add_fixed_clock( struct mas_mc_cb* cb, char* name, double freq );
static int32 mc_set_clock_freq( struct mas_mc_cb* cb, struct mas_mc_clknode *n, double freq );


int32
mas_mc_init_std_clks( struct mas_mc_cb* cb )
{
    int i;
    struct mas_mc_clknode* node;
    uint32 ts;

    for (i=0; _std_clk_names[i][0] != 0; i++ )
    {
        node = mc_add_clock_stub( cb, _std_clk_names[i], 1E+6 / _std_clk_rates[i]  );
        if ( node == 0 ) return mas_error(MERR_MEMORY);
    }

    masc_get_short_usec_ts( &ts );
    mc_sync_all( cb, ts );
    
    return 0;
}

struct mas_mc_cb*
mas_mc_init( void )
{
    struct mas_mc_cb* cb;
    int32 err;

    masc_entering_log_level("mc: mas_mc_init");
    err = masc_init_timing();
    if ( err < 0 )
        masc_logerror( err, "mc: Timing routines not initialized.");
    
    cb = MAS_NEW( cb );
    err = mas_mc_init_std_clks( cb );
    if ( err < 0 )
        masc_logerror( err, "mc: Standard set of clocks not initialized.");

    _show_sysclk_info(); /* only supported on linux, for now */
    
    masc_exiting_log_level();
    return cb;
}

void
mas_mc_exit( void )
{
    masc_exit_timing();
    return;
}


int32
mc_delete_clknode( struct mas_mc_clknode* node )
{
    if (node)
    {
        /* connect nodes on either side in list */
        if (node->prev != 0)
            node->prev->next = node->next;
        if (node->next != 0)
            node->next->prev = node->prev;

        masc_stats_cleanup( &node->s );
        masc_stats_cleanup( &node->rc );

        if ( node->clkval ) masc_rtfree( node->clkval );
        masc_rtfree(node);
    }
    else return mas_error(MERR_MEMORY);

    return 0;
}

int32
mc_append_clknode(struct mas_mc_clknode* head, struct mas_mc_clknode* node)
{
    struct mas_mc_clknode* tail = head;
    
    while( tail->next != NULL ) tail = tail->next;

    tail->next = node;
    node->prev = tail;

    return 0;
}

struct mas_mc_clknode*
mc_new_clknode( void )
{
    struct mas_mc_clknode* node;

    node = MAS_NEW( node );
    if ( node == 0 ) return 0;

    node->clkval = MAS_NEW( node->clkval );
    node->p_clkval = MAS_NEW( node->p_clkval );

    masc_stats_init( &node->s, MC_RATE_WINDOW, MASC_STATS_ALL );
    masc_stats_init( &node->rc, MC_RC_WINDOW, MASC_STATS_ALL );
    
    return node;
}

/* Adds a simple clock to the clock list.  This stub can be used
   as-is for fixed-rate clocks.  If rate estimation is required,
   additional information and actions will be needed. */
struct mas_mc_clknode*
mc_add_clock_stub( struct mas_mc_cb* cb, char* name, double nom_rate )
{
    struct mas_mc_clknode* node;

    node = mc_new_clknode();
    if ( node == 0 ) return 0;

    /* initialize node */
    node->id = cb->next_clock_id++;
    node->clkval->id = node->id;
    strncpy( node->name, name, MAX_MC_CLK_NAME_LENGTH - 1 );
    node->rate = nom_rate;
    node->clkval->valid_sdev = MC_DEFAULT_VALID_SDEV;
    
    /* use the first clock's base timestamp as our own */
    if ( cb->clocks )
        node->base_ts_us = cb->clocks->base_ts_us;

    /* take care of first clock in list */
    if ( cb->clocks )
        mc_append_clknode( cb->clocks, node );
    else
        cb->clocks = node;
    
    return node;
}

int32
mc_add_clock( struct mas_mc_cb* cb, int32 device_instance, char* name )
{
    char buffer[1024];
    struct mas_package arg;
    struct mas_package predpack;
    struct mas_mc_clknode* node;
    void* predicate;
    int32 err;

    /* initialize stub */
    node = mc_add_clock_stub( cb, name, 0.0 );
    if ( node == 0 ) return mas_error(MERR_MEMORY);
    node->device_instance = device_instance;

    /* schedule mas_set mc_clock_addx on device_instance */
    /* First, create the arg package.  Can use static mem for this */
    masc_setup_package( &arg, buffer, sizeof buffer, MASC_PACKAGE_STATIC );
    masc_pushk_string( &arg, "clock", name );
    /*** this is a pointer to the clkval struct */
    masc_pushk_payload( &arg, "addx", (void*) &node->clkval, sizeof node->clkval );
    masc_finalize_package( &arg );
    /* Second, create the predicate package.  Have to use dynamic memory. */
    masc_setup_package( &predpack, NULL, 0, MASC_PACKAGE_NOFREE );
    masc_pushk_string( &predpack, "key", "mc_clock_addx" );
    masc_pushk_package( &predpack, "arg", &arg );
    masc_finalize_package( &predpack );
    
    err = mas_asm_schedule_event_simple(device_instance, "mas_set", predpack.contents);
    if ( err < 0 ) return err;
    masc_strike_package( &arg );
    masc_strike_package( &predpack );

    /* schedule mas_mc_update on device_instance */
    predicate = (void*) strdup( name );
    err = mas_asm_schedule_event(device_instance, "mas_mc_update", predicate, 0, 0, 0, MAS_PRIORITY_ROUNDTUIT, MC_UPDATE_RATE_PERIOD_MS * 1000, 0, 0 );
    if ( err < 0 ) return err;
    
    /* schedule mc_update_rate */
    predicate = masc_rtalloc( sizeof &node );
    *(struct mas_mc_clknode**)predicate = node;
    err = mas_asm_schedule_event(MAS_MC_INSTANCE, "mas_mc_update_rate", predicate, 0, 0, 0, MAS_PRIORITY_ROUNDTUIT, MC_UPDATE_RATE_PERIOD_MS * 1000, 0, 0 );
    if ( err < 0 ) return err;
    
    return 0;
}

/* adds a fixed-frequency clock.  "freq" is in Hz, and is *not* the
 * period. */
int32
mc_add_fixed_clock( struct mas_mc_cb* cb, char* name, double freq )
{
/*     uint32 ts; */
    struct mas_mc_clknode* node;

    node = mc_add_clock_stub( cb, name, 1.0E+6 / freq );
    if ( node == 0 ) return mas_error(MERR_MEMORY);

    mc_sync( node, mc_update_val(cb, cb->clocks) );

    /* set clock ahead to debug wraps  */
/*     if( !strcmp(name, "player") ) */
/*     { */
/*         masc_log_message(0, "yahooo"); */
/*         node->base_val = 4289675296; */
/*         masc_get_short_usec_ts( &ts ); */
/*         mc_sync_all( cb, ts ); */
/*     } */
    
    return node->id;
}

/* manually set a clock's frequency (in Hz, not period!) */
int32
mc_set_clock_freq( struct mas_mc_cb* cb, struct mas_mc_clknode *n, double freq )
{
    /* Set the base measurements of the clock to now, then set the new
     * rate. Only set the new rate AFTER we've updated the clock! */
    mc_sync( n, mc_update_val(cb, cb->clocks) );
    n->rate = 1.0E+6 / freq;
    n->clkval->expected_rate = 1.0E+6 / freq;

    return 0;
}

int32
mc_update_rate( struct mas_mc_cb* cb, struct mas_mc_clknode* n )
{
    /* TEMPORARY */
    masc_entering_log_level( "mc_update_rate" );

    if ( n->clkval == 0 )
        return mas_error(MERR_NULLPTR);

    /* If the rate hasn't been computed yet, set it to the expected
       rate */
    if ( fabs( n->rate ) < RATETOL )
        n->rate = n->clkval->expected_rate;
    
    /* Someone registered a veto of the measurement.  Clear the veto,
     * clear new, blow away the prior measurement and fail. */
    if ( n->clkval->veto )
    {
        n->clkval->veto = FALSE;
        n->clkval->newmeas = FALSE;

        /* The prior clock value is now void. */
        if ( n->p_clkval->val)
            memset( n->p_clkval, 0, sizeof *n->p_clkval );
        masc_log_message( MAS_VERBLVL_DEBUG+1, "mc: Measurement of clock '%s' vetoed.", n->name);
        goto fail;
    }
    
    /* Fail if we don't have a new measurement, but keep the prior value. */
    if ( ! n->clkval->newmeas )
    {
        masc_log_message( MAS_VERBLVL_DEBUG+1, "mc: No new measurement for estimation of clock '%s'.", n->name);
        goto fail;
    }

    /* The measurement is no longer new. */
    n->clkval->newmeas = FALSE;
    
    /* if there was no prior clock value, copy the current values in,
       and wait until next time for a rate update. */
    if ( n->p_clkval->val == 0 )
    {
        memcpy( n->p_clkval, n->clkval, sizeof *n->clkval );
        masc_log_message( MAS_VERBLVL_DEBUG+1, "mc: No prior clock value for clock '%s'; delaying rate computation.", n->name);
        goto fail;
    }

    /* skip it if there's no new value in the current clock */
    if ( (n->clkval->val == n->p_clkval->val) || (n->clkval->ts_us == n->p_clkval->ts_us) )
    {
        masc_log_message( MAS_VERBLVL_DEBUG+1, "mc: Value in clock '%s' has not been updated; delaying rate computation.", n->name);
        goto fail;
    }

    /*** update the statistics with the new measurement */
    masc_stats_update( &n->s, (double)( n->clkval->ts_us - n->p_clkval->ts_us ) / (double) ( n->clkval->val - n->p_clkval->val) );
    
    masc_log_message( MAS_VERBLVL_DEBUG+1, "mc: Clock '%s' has estimated rate %f us/tick: %f Hz.", n->name, n->s.win_mean, 1E+6 / n->s.win_mean );
    masc_exiting_log_level();

    /* Update the rate convergence statistics with the new rate */
    masc_stats_update( &n->rc, n->s.win_mean );

    /* Check to see if we need to watch the standard deviation of the
       clock's rate convergence to ensure valid estimates. */
    if ( n->clkval->valid_sdev < MC_MAX_VALID_SDEV )
    {
        masc_stats_compute_win_error( &n->rc );
        masc_log_message( MAS_VERBLVL_DEBUG, "mc: standard deviation of the estimates of clock %02d is %0.3f.", n->id, n->rc.win_std_dev );
        masc_log_message( MAS_VERBLVL_DEBUG, "mc: mean of the estimates is %0.3f.", n->rc.win_mean );

        if ( ! (( n->rc.valid & MASC_STATS_WIN_ERROR ) && ( n->rc.win_std_dev < n->clkval->valid_sdev )) )
        {
            masc_log_message( MAS_VERBLVL_DEBUG, "mc: standard deviation limit for estimates of clock %02d is %0.3f.", n->id, n->clkval->valid_sdev );
            if ( n->rc.valid & MASC_STATS_WIN_ERROR )
            {
                masc_log_message( MAS_VERBLVL_DEBUG, "mc: standard deviation of the estimates of clock %02d is %0.3f, not within the limit of %0.3f.", n->id, n->rc.win_std_dev, n->clkval->valid_sdev );
            }
            else
            {
                masc_log_message( MAS_VERBLVL_DEBUG, "mc: standard deviation of clock %02d can't be computed yet.", n->id );
            }
                
            masc_log_message( MAS_VERBLVL_DEBUG, "mc: can't use this estimate.");
            goto cleanup;
        }
    }
    
    if ( ! (n->s.valid & MASC_STATS_WIN_MEAN) )
    {
        masc_log_message( MAS_VERBLVL_DEBUG, "mc: recomputing windowed statistics for clock %d", n->id );
        masc_stats_recompute_window( & n->s );
    }

    /* Set the base measurements of the clock to now, then set the new
     * rate. Only set the new rate AFTER we've updated the clock! */
    mc_sync( n, mc_update_val(cb, cb->clocks) );
    n->rate = n->s.win_mean;
    
    /* log the first 5 estimates */
    if ( n->s.N > 0 && n->s.N < 6)
    {
        masc_log_message(MAS_VERBLVL_DEBUG, "mc: estimate #%d of clock %02d: '%10s', device %03d.  Rate: %10.2f", n->s.N, n->id, n->name, n->device_instance, 1E+6 / n->rate);
        if ( n->s.N == 5 )
            masc_log_message(MAS_VERBLVL_DEBUG, "mc: continuing to estimate, but will not report on this clock.");
    }

 cleanup:
    /* copy the current clock value to the prior */
    memcpy( n->p_clkval, n->clkval, sizeof *n->clkval );

    return 0;

 fail:
    masc_exiting_log_level();
    return 0;
}

/* Update the value of clock "node" to the current microsecond
   timestamp. */
uint32
mc_update_val( struct mas_mc_cb* cb, struct mas_mc_clknode* node )
{
    uint32 ts;

    masc_get_short_usec_ts( &ts );

/*     node->prev_val = node->val; */
    node->val = mc_val( node, ts );
    node->ts_us = ts;
    
    
    /* Catch usec_ts wraps. This function is called fairly often
     * (i.e. from the scheduler main loop via mas_mc_get_val() ).
     * Therefore, we can safely assume (because of the 2^31
     * re-syncing below) that any clock's base line time stamp has a
     * high enough value for this comparison by the time a wrap occurs
     * The idea is that the first time this is called after a usec_ts
     * wrap (regardless of the clock) it will cause a mc_sync_all(),
     * after which the condition will be false again until the next
     * wrap, or at least not true all too often.
     */ 
    if( ts < node->base_ts_us )
    {
        masc_entering_log_level( "mc_update_rate" );
        masc_log_message(MAS_VERBLVL_DEBUG, "mc: possible usec_ts wrap detected, syncing all clocks.");
        masc_log_message(MAS_VERBLVL_DEBUG, "mc: current clock: %d (%s), baseline %u, now_ts %u", node->id, node->name, node->base_ts_us, ts );
        
        masc_exiting_log_level();
        mc_sync_all( cb, ts );
    }

/*     if( node->val < node->prev_val ) */
/*     { */
/*         masc_entering_log_level( "mc_update_rate" ); */
/*         masc_log_message(MAS_VERBLVL_DEBUG, "mc: possible node value wrap detected, syncing all clocks."); */
/*         masc_log_message(MAS_VERBLVL_DEBUG, "mc: current clock: %d (%s), prev value %u, now %u", node->id, node->name, node->prev_val, node->val ); */
        
/*         masc_exiting_log_level(); */
/*         mc_sync_all( cb, ts ); */
/*     } */
    
    /* Re-establish the baseline measurements if we've used up 31
     * bits in our timestamp delta. */
    if ( ts - node->base_ts_us >= HALF_UINT32_MAX )
    {
        masc_entering_log_level( "mc_update_rate" );
        masc_log_message(MAS_VERBLVL_DEBUG, "mc: timestamp delta of 2^31 has been reached, syncing all clocks.");
        masc_exiting_log_level();
        mc_sync_all( cb, ts );
    }
    
    return node->val;
}

/* Reset the estimates for a clock.  Start over from the expected
   rate. */
int32
mc_reset( struct mas_mc_cb *cb, struct mas_mc_clknode *node )
{
    uint32 ts_us;

    /* This is a good idea. */
    masc_get_short_usec_ts( &ts_us );
    mc_sync_all( cb, ts_us );

    /* rate is now un-estimated */
    node->rate = 0.0;

    /* reset the statistics, but preserve the compute flags */
    masc_stats_reset( &node->s, FALSE );
    masc_stats_reset( &node->rc, FALSE );

    /* remove prior clock value */
    memset( node->p_clkval, 0, sizeof *node->p_clkval );

    /* keep current clock value intact, but null out the flags */
    node->clkval->newmeas = FALSE;
    node->clkval->veto = FALSE;
    
    return 0;
}

/* Given a microsecond timestamp, compute "node" clock's new value. */
uint32
mc_val( struct mas_mc_clknode* node, uint32 ts_us )
{
    /* system clock is a special case */
    if ( node->id == MAS_MC_SYSCLK )
    {
        return ts_us;
    }
    else
    {
        /* use rate and timestamp delta to compute new value */
        return (uint32)((ts_us - node->base_ts_us) / node->rate) + node->base_val;
    }
}

/* Synchronize the base_ts_us fields of all clocks to the same
 * count. */
void
mc_sync_all( struct mas_mc_cb* cb, uint32 ts_us )
{
    struct mas_mc_clknode* node;

    masc_entering_log_level( "mc: mc_sync_all");
    for ( node=cb->clocks; node; node=node->next )
    {
        mc_sync( node, ts_us );
        masc_log_message( MAS_VERBLVL_DEBUG, "mc: syncing clock '%s' (%d): %u, base %u us", node->name, node->id, node->val, node->base_ts_us );
    }
    masc_exiting_log_level();
}

void
mc_sync( struct mas_mc_clknode* node, uint32 ts_us )
{
    node->val = mc_val(node, ts_us);
    node->base_ts_us = ts_us;
    node->base_val = node->val;
}

/* Update the value of the specified clock and return it. */
int32
mas_mc_get_val( struct mas_mc_cb* cb, int32 id, uint32* retval )
{
    struct mas_mc_clknode* node;

    node = mc_get_node( cb, id );

    if ( node != 0 )
    {
        *retval = mc_update_val( cb, node );
        return 0;
    }
    else
    {
        return mas_error(MERR_INVALID);
    }
}

/* Get the id of a named clock. */
int32
mas_mc_get_id( struct mas_mc_cb* cb, char* name, int32 device_instance )
{
    struct mas_mc_clknode* node;

    node = cb->clocks;
    while (node)
    {
        if ( (strcmp( name, node->name ) == 0) && (node->device_instance == device_instance) )
            return node->id;

        node = node->next;
    }

    return mas_error(MERR_INVALID);
}

int32
mas_mc_get_rate( struct mas_mc_cb* cb, int id, double* retrate )
{
    struct mas_mc_clknode* node;

    node = mc_get_node( cb, id );

    if ( node != 0 )
    {
        *retrate = 1E+6 / node->rate;
        return 0;
    }
    else
    {
        return mas_error(MERR_INVALID);
    }
}

int32
mas_mc_match_rate( struct mas_mc_cb* cb, int rate )
{
    struct mas_mc_clknode* node;
    double node_rate;
    
    node = cb->clocks;
    while (node)
    {
        node_rate = 1E+6 / node->rate;
        
        if ( ( node_rate < (double)rate + RATETOL ) && ( node_rate > (double)rate - RATETOL ) )
            return node->id;

        node = node->next;
    }

    return mas_error(MERR_INVALID);
}

int32 
mas_mc_action_handler( struct mas_mc_cb* cb, struct mas_event* event )
{
    struct mas_package predpack;
    int32 retval = 0;
    void* predicate;
    int32 response;
    int16 n=0;
    int send_retval = TRUE;
    char *rkey = NULL;
    struct mas_package rpack;
    struct mas_data *data;
    
    predicate = event->predicate;
    response = event->response;
    
    if ( response )
        masc_setup_package( &rpack, NULL, 0, MASC_PACKAGE_NOFREE );
    
    /* make sure we have set this event's action index */
    if ( ! event->valid_action_index )
    {
        /* count the defined actions */
        while ( *actions[n] != 0 ) n++;
        
        event->action_index = masc_get_string_index(event->action_name, actions, n);
        event->valid_action_index = TRUE;
    }
        
    /**
     ** Be sure to add your new action to the "actions" array defined
     ** at the top of this file.
     **
     **/
    switch (event->action_index)
    {
    case 0: /*mas_mc_update_rate*/
        retval = mc_update_rate( cb, *(struct mas_mc_clknode**)predicate );
        break;
    case 1: /*mas_mc_add_clock*/
    {
/*         uint32 ts; */
        int32 device_instance;
        char* name;
        
        masc_setup_package( &predpack, predicate, 0, MASC_PACKAGE_STATIC|MASC_PACKAGE_EXTRACT);
        masc_pullk_int32( &predpack, "device_instance", &device_instance );
        masc_pullk_string( &predpack, "clock", &name, FALSE );
        retval = mc_add_clock( cb, device_instance, name );
        masc_strike_package( &predpack );

/*         masc_get_short_usec_ts( &ts ); */
/*         mc_sync_all( cb, ts ); */
    }
    break;
    case 2: /*mas_mc_delete_clock*/
    {
        int32 clkid;
        struct mas_mc_clknode *node;
        
        masc_setup_package( &predpack, predicate, 0, MASC_PACKAGE_STATIC|MASC_PACKAGE_EXTRACT);
        masc_pullk_int32( &predpack, "clkid", &clkid );
        node = mc_get_node( cb, clkid );
        if ( node == NULL )
            retval = mas_error(MERR_NOTDEF);
        retval = mc_delete_clknode( node );
        break;
    }
    case 3: /*mas_get*/
    {
        char* key;
        int32 portnum;
        struct mas_package argpack;
        struct mas_package retpack;
        struct mas_data* data;

        send_retval = FALSE;
        
        masc_setup_package( &predpack, predicate, 0, MASC_PACKAGE_STATIC|MASC_PACKAGE_EXTRACT);
      masc_pull_string( &predpack, &key, FALSE );
        argpack.contents = 0;
      if ( predpack.members > 1 )
            masc_pull_package( &predpack, &argpack, FALSE );

        /* pull out the return portnumber, if necessary.  default to
         * response port. */
        portnum = response;
        if ( masc_test_key( &predpack, "retport" ) > 0 )
            masc_pullk_int32( &predpack, "retport", &portnum );

        masc_setup_package( &retpack, NULL, 0, MASC_PACKAGE_NOFREE );

        /* don't send the request info if this goes back to the
           response port.  If it's going to a shared data channel,
           send it. */
        if ( portnum == response )
            retval = mc_get( cb, key, &argpack, FALSE, &retpack );
        else retval = mc_get( cb, key, &argpack, TRUE, &retpack );

        /* stuff data struct */
        data = MAS_NEW( data );
        if ( data == NULL ) return mas_error(MERR_MEMORY);
        data->length = retpack.size;
        data->allocated_length = retpack.allocated_size;
        data->segment = retpack.contents;
        masd_post_data( portnum, data ); /* and post response */
        masc_strike_package( &retpack );
        masc_strike_package( &predpack );
        if ( argpack.contents != NULL ) masc_strike_package( &argpack );
        break;
    }
    case 4: /*mas_set*/
    {
        char* key;
        struct mas_package argpack;

        send_retval = FALSE;
        
        masc_setup_package( &predpack, predicate, 0, MASC_PACKAGE_STATIC|MASC_PACKAGE_EXTRACT);
      masc_pull_string( &predpack, &key, FALSE );
        argpack.contents = 0;
      if ( predpack.members > 1 )
            masc_pull_package( &predpack, &argpack, FALSE );

        retval = mc_set( cb, key, &argpack );

        masc_strike_package( &predpack );
        if ( argpack.contents != NULL ) masc_strike_package( &argpack );
        break;
    }
    case 5: /* mas_mc_log_clock_values */
        mc_log_clock_values( cb );
        break;
    case 6: /* mas_mc_reset_clock */
    {
        int32 clkid;
        struct mas_mc_clknode *node;
        
        masc_setup_package( &predpack, predicate, 0, MASC_PACKAGE_STATIC|MASC_PACKAGE_EXTRACT);
        masc_pullk_int32( &predpack, "clkid", &clkid );
        node = mc_get_node( cb, clkid );
        if ( node == NULL )
            retval = mas_error(MERR_NOTDEF);
        retval = mc_reset( cb, node );
        masc_strike_package( &predpack );
        break;
    }
    case 7: /*mas_mc_add_fixed_clock*/
    {
        double freq;
        char* name;

        rkey = "clkid";
        masc_setup_package( &predpack, predicate, 0, MASC_PACKAGE_STATIC|MASC_PACKAGE_EXTRACT);
        masc_pullk_double( &predpack, "freq", &freq );
        masc_pullk_string( &predpack, "clock", &name, FALSE );
        retval = mc_add_fixed_clock( cb, name, freq );
        masc_strike_package( &predpack );
    }
    break;
    default:
        return mas_error(MERR_INVALID);
    }

 done:

    if ( response && send_retval )
    {
        if ( retval < 0 || rkey == NULL )
            rkey = "err";
        /* do response package */
        masc_pushk_int32( &rpack, rkey, retval );

        masc_finalize_package( &rpack );

        /* stuff data struct */
        data = MAS_NEW( data );
        data->length = rpack.size;
        data->allocated_length = rpack.allocated_size;
        data->segment = rpack.contents;
        masd_post_data( response, data );
    }

    return retval;
}

int32
mc_get( struct mas_mc_cb* cb, char* key, struct mas_package* arg, int send_req_info, struct mas_package* r_package )
{
    /* list of nuggets.  preserve the terminator */
    static char* nuggets[] = 
        { "list", "mc_rate", "" };
    int i;
    int16 n=0;

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

    i = masc_get_string_index(key, nuggets, n);

    if ( send_req_info )
    {
        masc_pushk_string( r_package, "req key", key );
        if ( arg != 0 )
            masc_pushk_package( r_package, "req arg", arg );
        masc_pushk_int8( r_package, "top", 0 );
    }
 
    switch (i)
    {
    case 0: /*list*/
        masc_push_strings( r_package, nuggets, n );
        break;
    case 1: /*mc_rate*/
    {
        int32 clkid;
        struct mas_mc_clknode* node;
        
        if ( arg->contents == NULL ) return mas_error( MERR_INVALID );
        masc_pull_int32(arg, &clkid);
        node = mc_get_node( cb, clkid );
        if ( ! node )
            return mas_error(MERR_INVALID);
        masc_pushk_double( r_package, "rate", 1E+6/node->rate );
        break;
    }
    default:
        return mas_error(MERR_INVALID);
        break;
    }

    masc_finalize_package( r_package );
                
    return 0;
}

int32
mc_set( struct mas_mc_cb* cb, char* key, struct mas_package* arg )
{
    /* list of nuggets.  preserve the terminator */
    static char* nuggets[] = 
        { "mc_rate", "" };
    int i;
    int16 n=0;

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

    i = masc_get_string_index(key, nuggets, n);

    switch (i)
    {
    case 0: /*mc_rate*/
    {
        int32 clkid;
        double freq;
        struct mas_mc_clknode* node;
        
        if ( arg->contents == NULL )
            return mas_error( MERR_INVALID );
        masc_pullk_int32(arg, "clkid", &clkid);
        masc_pullk_double( arg, "freq", &freq );
        node = mc_get_node( cb, clkid );
        if ( ! node )
            return mas_error(MERR_INVALID);
        mc_set_clock_freq( cb, node, freq );
        break;
    }
    default:
        return mas_error(MERR_INVALID);
        break;
    }

    return 0;
}

void
mas_mc_print_clocks( struct mas_mc_cb* cb )
{
    struct mas_mc_clknode* node;
    double rate;
    double sdev;
    double rc_sdev;
    
    masc_log_message(0, "clocks registered with master clock:");
    
    masc_log_message(0, "id:        name   dev      wcount  mean rate(Hz)     sdev(Hz)  rc sdev(Hz) ");
/*  masc_log_message(0, "XX: 'XXXXXXXXXX'  000  0123456789  0123456789.01  0123456.012  0123456.012"); */
    for ( node = cb->clocks; node; node = node->next )
    {
        masc_stats_recompute_window( & node->s );
        masc_stats_recompute_window( & node->rc );
        rate = 1E+6 / node->rate;
        sdev = fabs((1E+6 / node->rate) - 1E+6 / (node->rate - node->s.win_std_dev ));
        if ( node->rc.win_mean < 0.00001 )
            rc_sdev = 0.0;
        else
            rc_sdev = fabs((1E+6 / node->rc.win_mean) - 1E+6 / (node->rc.win_mean - node->rc.win_std_dev ));
        
        if ( node->rate < 0.0001 )
            masc_log_message(0, "%02d: '%10s'  %03d  % 10d        unknown      unknown      unknown", node->id, node->name, node->device_instance, node->s.count);
        else
            masc_log_message(0, "%02d: '%10s'  %03d  % 10d  % 13.2f  % 11.3f  % 11.3f", node->id, node->name, node->device_instance, node->s.count, rate, sdev, rc_sdev);
    }
}


int32
mas_mc_convert( struct mas_mc_cb* cb, int32 from_id, uint32 from_ts, int32 to_id, uint32* to_ts )
{
    struct mas_mc_clknode* fn;
    struct mas_mc_clknode* tn;

    /* test for trivial case */
    if ( from_id == to_id )
    {
        *to_ts = from_ts;
        return 0;
    }

    fn = mc_get_node( cb, from_id );
    tn = mc_get_node( cb, to_id );

    if ( fn == 0 || tn == 0 )
        return mas_error(MERR_NOTDEF);

    /* start by offsetting our result by the base value of the
       destination clock */

    *to_ts = tn->base_val;

    /* The second term accounts for the difference in base timestamps.
     * And, yes, this is funny.  If the destination timestamp is less
     * than the source timestamp, the contribution is positive.  If
     * not, it's negative.  Since these are all unsigned, we split 'em
     * up. */
    if ( fn->base_ts_us - tn->base_ts_us < HALF_UINT32_MAX )
    {
        *to_ts += (uint32)(( fn->base_ts_us - tn->base_ts_us ) / tn->rate);
    }
    else
    {
        *to_ts -= (uint32)(( tn->base_ts_us - fn->base_ts_us ) / tn->rate);
    }

    /* The last term adds the contribution from the actual clock
       offset from its base value */
    if ( from_ts - fn->base_val < HALF_UINT32_MAX )
    {
        *to_ts += (uint32)(( from_ts - fn->base_val ) * ( fn->rate/tn->rate ));
    }
    else
    {
        *to_ts -= (uint32)(( fn->base_val - from_ts ) * ( fn->rate/tn->rate ));
    }
    
    return 0;
}

struct mas_mc_clknode* 
mc_get_node( struct mas_mc_cb* cb, int32 id )
{
    struct mas_mc_clknode* node;

    /* we need a hash table for these id's -- this search could be
     * much faster! */
    node = cb->clocks;
    while (node)
    {
        if (id == node->id)
            return node;

        node = node->next;
    }

    return 0;
}

void
mc_log_clock_values( struct mas_mc_cb* cb )
{
    struct mas_mc_clknode* node;
    char tline[1024];
    char tstr[256];
    
    strcpy( tline, "mc: clock values: ");
    
    for ( node = cb->clocks; node != 0; node = node->next )
    {
        snprintf(tstr, sizeof tstr - 1, "%u ", mc_update_val( cb, node ) );
        strncat(tline, tstr, sizeof tline - strlen( tline ) - 1);
    }

    masc_log_message(0, tline);
}


char** 
mas_mc_actions( void )
{
    return actions;
}

/* this MAY help with debugging timing information on Linux.
 * -- rocko, 23 APR 2003 */
void
_show_sysclk_info( void )
{
#ifdef linux
    struct timex ti;
    int err;

    memset( &ti, 0, sizeof ti );
    err = adjtimex( &ti );

    masc_log_message( MAS_VERBLVL_DEBUG, "adjtimex() returned %d", err );
    masc_log_message( MAS_VERBLVL_DEBUG, "              modes: %d", ti.modes );
    masc_log_message( MAS_VERBLVL_DEBUG, "        time offset: %ld us", ti.offset );
    masc_log_message( MAS_VERBLVL_DEBUG, "   frequency offset: %ld ppm (scaled)", ti.freq );
    masc_log_message( MAS_VERBLVL_DEBUG, "      maximum error: %ld us", ti.maxerror );
    masc_log_message( MAS_VERBLVL_DEBUG, "    estimated error: %ld us", ti.esterror );
    masc_log_message( MAS_VERBLVL_DEBUG, "     command/status: %d", ti.status );
    masc_log_message( MAS_VERBLVL_DEBUG, "  pll time constant: %ld", ti.constant );
    masc_log_message( MAS_VERBLVL_DEBUG, "          precision: %ld us", ti.precision );
    masc_log_message( MAS_VERBLVL_DEBUG, "frequency tolerance: %ld ppm", ti.tolerance );
    masc_log_message( MAS_VERBLVL_DEBUG, " time between ticks: %ld us", ti.tick );
#else
    return;
#endif
}

Generated by  Doxygen 1.6.0   Back to index