@ -30,6 +30,7 @@
# include <stdint.h>
# include <assert.h>
# include <math.h>
# include <string.h>
# include "fclock.h"
# include "jtag.h"
@ -106,51 +107,314 @@ cable_free( cable_t *cable )
int
cable_init ( cable_t * cable )
{
cable - > delay = 0 ;
cable - > frequency = 0 ;
cable - > todo . max_items = 128 ;
cable - > todo . num_items = 0 ;
cable - > todo . next_item = 0 ;
cable - > todo . next_free = 0 ;
cable - > todo . data = malloc ( cable - > todo . max_items * sizeof ( cable_queue_t ) ) ;
cable - > done . max_items = 128 ;
cable - > done . num_items = 0 ;
cable - > done . next_item = 0 ;
cable - > done . next_free = 0 ;
cable - > done . data = malloc ( cable - > done . max_items * sizeof ( cable_queue_t ) ) ;
if ( cable - > todo . data = = NULL | | cable - > done . data = = NULL )
{
printf ( _ ( " Failed to allocate memory for cable activity queue. \n " ) ) ;
if ( cable - > todo . data ! = NULL ) free ( cable - > todo . data ) ;
if ( cable - > done . data ! = NULL ) free ( cable - > done . data ) ;
return 1 ;
} ;
return cable - > driver - > init ( cable ) ;
}
void
cable_flush ( cable_t * cable )
{
cable - > driver - > flush ( cable ) ;
}
void
cable_done ( cable_t * cable )
{
cable_flush ( cable ) ;
if ( cable - > todo . data ! = NULL )
{
free ( cable - > todo . data ) ;
free ( cable - > done . data ) ;
}
return cable - > driver - > done ( cable ) ;
}
int
cable_add_queue_item ( cable_t * cable , cable_queue_info_t * q )
{
int i , j ;
if ( q - > num_items > = q - > max_items ) /* queue full? */
{
/* TODO: try to alloc more space for queue */
// printf("add_queue_item to %p: %d\n", q, -1);
return - 1 ; /* report failure */
}
i = q - > next_free ;
j = i + 1 ;
if ( j > = q - > max_items ) j = 0 ;
q - > next_free = j ;
q - > num_items + + ;
// printf("add_queue_item to %p: %d\n", q, i);
return i ;
}
int
cable_get_queue_item ( cable_t * cable , cable_queue_info_t * q )
{
if ( q - > num_items > 0 )
{
int i = q - > next_item ;
int j = i + 1 ;
if ( j > = q - > max_items ) j = 0 ;
q - > next_item = j ;
q - > num_items - - ;
// printf("get_queue_item from %p: %d\n", q, i);
return i ;
}
// printf("get_queue_item from %p: %d\n", q, -1);
return - 1 ;
}
void
cable_purge_queue ( cable_queue_info_t * q , int io )
{
while ( q - > num_items > 0 )
{
int i = q - > next_item ;
if ( q - > data [ i ] . action = = CABLE_TRANSFER )
{
if ( io = = 0 ) /* todo queue */
{
if ( q - > data [ i ] . arg . transfer . in ! = NULL ) free ( q - > data [ i ] . arg . transfer . in ) ;
if ( q - > data [ i ] . arg . transfer . out ! = NULL ) free ( q - > data [ i ] . arg . transfer . out ) ;
}
else /* done queue */
{
if ( q - > data [ i ] . arg . xferred . out ! = NULL ) free ( q - > data [ i ] . arg . xferred . out ) ;
}
}
i + + ;
if ( i > = q - > max_items ) i = 0 ;
q - > num_items - - ;
} ;
q - > num_items = 0 ;
q - > next_item = 0 ;
q - > next_free = 0 ;
}
void
cable_clock ( cable_t * cable , int tms , int tdi , int n )
{
cable_flush ( cable ) ;
cable - > driver - > clock ( cable , tms , tdi , n ) ;
}
int
cable_defer_clock ( cable_t * cable , int tms , int tdi , int n )
{
int i = cable_add_queue_item ( cable , & ( cable - > todo ) ) ;
if ( i < 0 ) return 1 ; /* report failure */
cable - > todo . data [ i ] . action = CABLE_CLOCK ;
cable - > todo . data [ i ] . arg . clock . tms = tms ;
cable - > todo . data [ i ] . arg . clock . tdi = tdi ;
cable - > todo . data [ i ] . arg . clock . n = n ;
return 0 ; /* success */
}
int
cable_get_tdo ( cable_t * cable )
{
cable_flush ( cable ) ;
return cable - > driver - > get_tdo ( cable ) ;
}
int
cable_get_tdo_late ( cable_t * cable )
{
int i ;
cable_flush ( cable ) ;
i = cable_get_queue_item ( cable , & ( cable - > done ) ) ;
if ( i > = 0 )
{
if ( cable - > done . data [ i ] . action ! = CABLE_GET_TDO )
{
printf ( _ ( " Internal error: Got wrong type of result from queue (%d? %p.%d) \n " ) ,
cable - > done . data [ i ] . action , & ( cable - > done ) , i ) ;
cable_purge_queue ( & ( cable - > done ) , 1 ) ;
}
else
{
return cable - > done . data [ i ] . arg . value . tdo ;
}
} ;
return cable - > driver - > get_tdo ( cable ) ;
}
int
cable_defer_get_tdo ( cable_t * cable )
{
int i = cable_add_queue_item ( cable , & ( cable - > todo ) ) ;
if ( i < 0 ) return 1 ; /* report failure */
cable - > todo . data [ i ] . action = CABLE_GET_TDO ;
return 0 ; /* success */
}
int
cable_set_trst ( cable_t * cable , int trst )
{
cable_flush ( cable ) ;
return cable - > driver - > set_trst ( cable , trst ) ;
}
int
cable_defer_set_trst ( cable_t * cable , int trst )
{
int i = cable_add_queue_item ( cable , & ( cable - > todo ) ) ;
if ( i < 0 ) return 1 ; /* report failure */
cable - > todo . data [ i ] . action = CABLE_SET_TRST ;
cable - > todo . data [ i ] . arg . value . trst = trst ;
return 0 ; /* success */
}
int
cable_get_trst ( cable_t * cable )
{
cable_flush ( cable ) ;
return cable - > driver - > get_trst ( cable ) ;
}
int
cable_get_trst_late ( cable_t * cable )
{
int i ;
cable_flush ( cable ) ;
i = cable_get_queue_item ( cable , & ( cable - > done ) ) ;
if ( i > = 0 )
{
if ( cable - > done . data [ i ] . action ! = CABLE_GET_TRST )
{
printf ( _ ( " Internal error: Got wrong type of result from queue (%d? %p.%d) \n " ) ,
cable - > done . data [ i ] . action , & ( cable - > done ) , i ) ;
cable_purge_queue ( & ( cable - > done ) , 1 ) ;
}
else
{
return cable - > done . data [ i ] . arg . value . trst ;
}
} ;
return cable - > driver - > get_trst ( cable ) ;
}
static uint32_t delay = 0 ;
static uint32_t frequency = 0 ;
int
cable_defer_get_trst ( cable_t * cable )
{
int i = cable_add_queue_item ( cable , & ( cable - > todo ) ) ;
if ( i < 0 ) return 1 ; /* report failure */
cable - > todo . data [ i ] . action = CABLE_GET_TRST ;
return 0 ; /* success */
}
int
cable_transfer ( cable_t * cable , int len , char * in , char * out )
{
cable_flush ( cable ) ;
return cable - > driver - > transfer ( cable , len , in , out ) ;
}
int
cable_transfer_late ( cable_t * cable , char * out )
{
int i ;
cable_flush ( cable ) ;
i = cable_get_queue_item ( cable , & ( cable - > done ) ) ;
if ( i > = 0 & & cable - > done . data [ i ] . action = = CABLE_TRANSFER )
{
#if 0
printf ( " Got queue item (%p.%d) len=%d out=%p \n " ,
& ( cable - > done ) , i ,
cable - > done . data [ i ] . arg . xferred . len ,
cable - > done . data [ i ] . arg . xferred . out ) ;
# endif
if ( out ) memcpy ( out ,
cable - > done . data [ i ] . arg . xferred . out ,
cable - > done . data [ i ] . arg . xferred . len ) ;
free ( cable - > done . data [ i ] . arg . xferred . out ) ;
return cable - > done . data [ i ] . arg . xferred . res ;
} ;
if ( cable - > done . data [ i ] . action ! = CABLE_TRANSFER )
{
printf ( _ ( " Internal error: Got wrong type of result from queue (#%d %p.%d) \n " ) ,
cable - > done . data [ i ] . action , & ( cable - > done ) , i ) ;
cable_purge_queue ( & ( cable - > done ) , 1 ) ;
}
else
{
printf ( _ ( " Internal error: Wanted transfer result but none was queued \n " ) ) ;
}
return 0 ;
}
int
cable_defer_transfer ( cable_t * cable , int len , char * in , char * out )
{
char * dbuf ;
int i = cable_add_queue_item ( cable , & ( cable - > todo ) ) ;
if ( i < 0 ) return 1 ; /* report failure */
cable - > todo . data [ i ] . action = CABLE_TRANSFER ;
cable - > todo . data [ i ] . arg . transfer . len = len ;
dbuf = malloc ( len ) ;
if ( dbuf = = NULL ) return 1 ; /* report failure */
if ( in ) memcpy ( dbuf , in , len ) ;
cable - > todo . data [ i ] . arg . transfer . in = dbuf ;
if ( out )
{
dbuf = malloc ( len ) ;
if ( dbuf = = NULL )
{
free ( cable - > todo . data [ i ] . arg . transfer . in ) ;
return 1 ; /* report failure */
}
cable - > todo . data [ i ] . arg . transfer . out = dbuf ;
}
else
{
cable - > todo . data [ i ] . arg . transfer . out = NULL ;
}
return 0 ; /* success */
}
void
cable_set_frequency ( cable_t * cable , uint32_t new_frequency )
{
if ( new_frequency = = 0 ) {
delay = 0 ;
frequency = 0 ;
cable- > delay = 0 ;
cable- > frequency = 0 ;
} else {
const double tolerance = 0.1 ;
uint32_t loops ;
uint32_t delay = cable - > delay ;
uint32_t frequency = cable - > frequency ;
printf ( " requested frequency %u, now calibrating delay loop \n " , new_frequency ) ;
@ -201,29 +465,23 @@ cable_set_frequency( cable_t *cable, uint32_t new_frequency )
}
}
frequency = new_frequency ;
cable - > delay = delay ;
cable - > frequency = frequency ;
}
}
uint32_t
cable_get_frequency ( cable_t * cable )
{
return frequency ;
}
int
cable_transfer ( cable_t * cable , int len , char * in , char * out )
{
int r ;
r = cable - > driver - > transfer ( cable , len , in , out ) ;
return r ;
return cable - > frequency ;
}
void
cable_wait ( void )
cable_wait ( cable_t * cable )
{
int i ;
volatile int j ;
uint32_t delay = cable - > delay ;
if ( delay = = 0 )
return ;