void GetSentState(struct ubcsp_packet *sd_packet)
{
static uint8 i, j;
int iLength;
//char sTemp[24];
// Write data
DebugMessage(1,(_T("\r\nGetSentState: enter \r\n ")));
// Construct uBCSP packet header from PDU - ensures correct byte ordering
// Header
DebugMessage(1,(_T("GetSentState line = %d, giPDUIndex = %d\r\n"),__LINE__, giPDUIndex));
sd_packet->payload[0] = (uint8)gPDU[giPDUIndex].type;
DebugMessage(1,(_T("GetSentState line = %d\r\n"),__LINE__));
sd_packet->payload[1] = (uint8)(gPDU[giPDUIndex].type >> 8);
sd_packet->payload[2] = (uint8)gPDU[giPDUIndex].pdulen;
sd_packet->payload[3] = (uint8)(gPDU[giPDUIndex].pdulen >> 8);
sd_packet->payload[4] = (uint8)gPDU[giPDUIndex].seqno;
sd_packet->payload[5] = (uint8)(gPDU[giPDUIndex].seqno >> 8);
sd_packet->payload[6] = (uint8)gPDU[giPDUIndex].varid;
sd_packet->payload[7] = (uint8)(gPDU[giPDUIndex].varid >> 8);
sd_packet->payload[8] = (uint8)gPDU[giPDUIndex].status;
sd_packet->payload[9] = (uint8)(gPDU[giPDUIndex].status >> 8);
// PS specific
sd_packet->payload[10] = (uint8)gPDU[giPDUIndex].d.ps.id;
sd_packet->payload[11] = (uint8)(gPDU[giPDUIndex].d.ps.id >> 8);
sd_packet->payload[12] = (uint8)gPDU[giPDUIndex].d.ps.len;
sd_packet->payload[13] = (uint8)(gPDU[giPDUIndex].d.ps.len >> 8);
sd_packet->payload[14] = (uint8)gPDU[giPDUIndex].d.ps.stores;
sd_packet->payload[15] = (uint8)(gPDU[giPDUIndex].d.ps.stores >> 8);
// Data
j = 16;
for(i = 0; i < gPDU[giPDUIndex].d.ps.len; i++)
{
sd_packet->payload[i + j] = (uint8)gPDU[giPDUIndex].d.ps.psmem[i];
sd_packet->payload[i + j + 1] = (uint8)(gPDU[giPDUIndex].d.ps.psmem[i] >> 8);
j++;
}
// Packet length
iLength = 15 + (gPDU[giPDUIndex].d.ps.len * sizeof(uint16)) + 1;
// If we are sending warm_reset packet
DebugMessage(1,(_T("\n giPDUCount = %d \r\n"),giPDUCount));
if(giPDUCount -1 == giPDUIndex )
{
sd_packet->payload[16] = (uint8) 0x00;
sd_packet->payload[17] = (uint8) 0x00;
iLength = 18;
}
sd_packet->length = iLength;
// Set packet defaults
sd_packet->channel = BCCMD;
sd_packet->reliable = RELIABLE;
sd_packet->use_crc = TRUE;
// Configure uBCSP queue handler
uBCSP_TXQueue.NoOfRetries = 0;
uBCSP_TXQueue.Opcode[0] = sd_packet->payload[6];
uBCSP_TXQueue.Opcode[1] = sd_packet->payload[7];
uBCSP_TXQueue.CommandNumber = RECEIVED_STATE;
uBCSP_TXQueue.NoOfOpcodes = 1;
DebugMessage(1,(_T("\n BCCMD COMMAND SENT CMDID = 0x%x \r\n"),sd_packet->payload[6]));
// Send the Transmit Packet
ubcsp_send_packet(sd_packet);
giPDUIndex++; // Increase pdu index
if(giPDUIndex == giPDUCount) // Temp
giResetSent = 1;
// Some Debug Information
/* for (i = 0; i < sd_packet->length; i++)
{
sprintf(sTemp, "%02x ", sd_packet->payload[i]);
DebugMessage(1,(_T("++GetSentState++ %02x \r\n "),(wchar_t*)sTemp));
}
DebugMessage(1,(_T("\n")));*/
}
static int do_command(uint16_t command, uint16_t seqnum, uint16_t varid, uint8_t *value, uint16_t length)
{
unsigned char cp[254], rp[254];
uint8_t cmd[10];
uint16_t size;
uint8_t delay, activity = 0x00;
int timeout = 0, sent = 0;
size = (length < 8) ? 9 : ((length + 1) / 2) + 5;
cmd[0] = command & 0xff;
cmd[1] = command >> 8;
cmd[2] = size & 0xff;
cmd[3] = size >> 8;
cmd[4] = seqnum & 0xff;
cmd[5] = seqnum >> 8;
cmd[6] = varid & 0xff;
cmd[7] = varid >> 8;
cmd[8] = 0x00;
cmd[9] = 0x00;
memset(cp, 0, sizeof(cp));
cp[0] = 0x00;
cp[1] = 0xfc;
cp[2] = (size * 2) + 1;
cp[3] = 0xc2;
memcpy(cp + 4, cmd, sizeof(cmd));
memcpy(cp + 14, value, length);
receive_packet.length = 512;
ubcsp_receive_packet(&receive_packet);
send_packet.channel = 5;
send_packet.reliable = 1;
send_packet.length = (size * 2) + 4;
memcpy(send_packet.payload, cp, (size * 2) + 4);
ubcsp_send_packet(&send_packet);
while (1) {
delay = ubcsp_poll(&activity);
if (activity & UBCSP_PACKET_SENT) {
switch (varid) {
case CSR_VARID_COLD_RESET:
case CSR_VARID_WARM_RESET:
case CSR_VARID_COLD_HALT:
case CSR_VARID_WARM_HALT:
return 0;
}
sent = 1;
timeout = 0;
}
if (activity & UBCSP_PACKET_RECEIVED) {
if (sent && receive_packet.channel == 5 &&
receive_packet.payload[0] == 0xff) {
memcpy(rp, receive_packet.payload,
receive_packet.length);
break;
}
receive_packet.length = 512;
ubcsp_receive_packet(&receive_packet);
timeout = 0;
}
if (delay) {
usleep(delay * 100);
if (timeout++ > 5000) {
fprintf(stderr, "Operation timed out\n");
return -1;
}
}
}
if (rp[0] != 0xff || rp[2] != 0xc2) {
errno = EIO;
return -1;
}
if ((rp[11] + (rp[12] << 8)) != 0) {
errno = ENXIO;
return -1;
}
memcpy(value, rp + 13, length);
return 0;
}