bool AS3935::readReg(uint8_t regAddress, uint8_t dataBitMask, uint8_t *data)
{
uint8_t tempVal;
if(!read(regAddress,&tempVal,1)) return false;
tempVal&=dataBitMask;
tempVal>>=getShiftValue(dataBitMask);
*data= tempVal;
return true;
}
bool AS3935::writeReg(uint8_t regAddress, uint8_t dataBitMask, uint8_t data)
{
uint8_t tempVal;
if(!read(regAddress,&tempVal,1)) return false;
tempVal&=~(dataBitMask);
tempVal|= data<<getShiftValue(dataBitMask);
return write(regAddress, &tempVal,1);
}
static __inline void scanFields(int num_fields, scan_field_t *fields, tap_state_t shiftState, int pause)
{
int i;
int j;
int k;
for (i = 0; i < num_fields; i++)
{
cyg_uint32 value;
static u8 *in_buff=NULL; /* pointer to buffer for scanned data */
static int in_buff_size=0;
u8 *inBuffer=NULL;
// figure out where to store the input data
int num_bits=fields[i].num_bits;
if (fields[i].in_value!=NULL)
{
inBuffer=fields[i].in_value;
} else if (fields[i].in_handler!=NULL)
{
if (in_buff_size*8<num_bits)
{
// we need more space
if (in_buff!=NULL)
free(in_buff);
in_buff=NULL;
in_buff_size=(num_bits+7)/8;
in_buff=malloc(in_buff_size);
if (in_buff==NULL)
{
LOG_ERROR("Out of memory");
jtag_error=ERROR_JTAG_QUEUE_FAILED;
return;
}
}
inBuffer=in_buff;
}
// here we shuffle N bits out/in
j=0;
while (j<num_bits)
{
tap_state_t pause_state;
int l;
k=num_bits-j;
pause_state=(shiftState==TAP_DRSHIFT)?TAP_DRSHIFT:TAP_IRSHIFT;
if (k>32)
{
k=32;
/* we have more to shift out */
} else if (pause&&(i == num_fields-1))
{
/* this was the last to shift out this time */
pause_state=(shiftState==TAP_DRSHIFT)?TAP_DRPAUSE:TAP_IRPAUSE;
}
// we have (num_bits+7)/8 bytes of bits to toggle out.
// bits are pushed out LSB to MSB
value=0;
if (fields[i].out_value!=NULL)
{
for (l=0; l<k; l+=8)
{
value|=fields[i].out_value[(j+l)/8]<<l;
}
}
/* mask away unused bits for easier debugging */
value&=~(((u32)0xffffffff)<<k);
shiftValueInner(shiftState, pause_state, k, value);
if (inBuffer!=NULL)
{
// data in, LSB to MSB
value=getShiftValue();
// we're shifting in data to MSB, shift data to be aligned for returning the value
value >>= 32-k;
for (l=0; l<k; l+=8)
{
inBuffer[(j+l)/8]=(value>>l)&0xff;
}
}
j+=k;
}
if (fields[i].in_handler!=NULL)
{
// invoke callback
int r=fields[i].in_handler(inBuffer, fields[i].in_handler_priv, fields+i);
if (r!=ERROR_OK)
{
/* this will cause jtag_execute_queue() to return an error */
jtag_error=r;
}
}
}