static int packet_rx(serioStuff *s, void *p, size_t size, int timeout)
{
if(!flags.hanmode){
return serio_read(s, p, size, timeout);
}
else{
int res;
u8 b[2];
int xfrcount = 0;
do{
res = serio_read(s, b, 1, timeout);
}
while((res == 1) && (b[0] != STX));
for(xfrcount = 0; (res == 1); xfrcount++){
res = serio_read(s, b, 1, timeout);
if(res != 1){
break;
}
if(b[0] == ETX){
break;
}
else if(b[0] == SUBST){
res = serio_read(s, b, 1, timeout);
if(res != 1){
break;
}
}
if(xfrcount < size)
((u8 *) p)[xfrcount] = b[0];
}
return (res < 0) ? res : xfrcount;
}
}
int serio_nb_line_readcr(serioStuffPtr_t serio)
{
char c;
int res;
if(!serio)
return -1;
do{
res = serio_read(serio, &c, 1);
if(serio->eof){
return TRUE;
}
else if(res < 0){
if((errno != EAGAIN) && (errno != EWOULDBLOCK)){
debug(DEBUG_UNEXPECTED, "Read error on fd %d: %s", serio->fd, strerror(errno));
serio->pos = 0;
return ERROR;
}
return FALSE;
}
else if(res == 1){
/* debug(DEBUG_ACTION,"Byte received"); */
if(c == '\r') /* Ignore return */
continue;
if(c != '\n'){
if(serio->pos < (SERIO_MAX_LINE - 1))
serio->line[serio->pos++] = c;
else
debug(DEBUG_UNEXPECTED,"End of line buffer reached!");
}
else{
debug(DEBUG_ACTION, "Line received");
serio->line[serio->pos] = 0;
serio->pos = 0;
return TRUE;
}
}
} while(TRUE);
return ERROR;
}
/*-----------------------------------------------------------------------
Receive a packet via the serial port.
Caller must put size of buffer in len before calling.
Packets larger than buffer will be silently ignored.
On failure, returns
ser_RES_BAD if *len looked bogus, or buf was NULL,
or packet was too big to fit in buffer, or 0 size.
ser_RES_EMPTY if no good packet was availible.
On success,
returns ser_RES_OK,
sets *len to the packet length,
Can only recieve from ser_HDL_YOU, so no source adr need be returned.
CRC errors cause ser_RES_EMPTY rather than ser_RES_BAD, since
ser_RES_BAD is reserved for caller brain damage errors.
Strategy:
Read ser_READSIZE bytes at a time from the serial port into ser->rbuf.
ser->len is number of bytes in ser->rbuf.
ser->head is the index of the next byte to get out of ser->rbuf.
Accumulate next good packet in ser->pkt.
ser->got is the number of good bytes gotten so far.
If anything goes wrong, just clear ser->got, and life keeps on humming.
-----------------------------------------------------------------------*/
ser_result_t ser_get(ser_t *ser, void *buf, size_t *len)
{
for (;;) {
int c;
assert((0 <= ser->got) && (ser->got < sizeof(ser->pkt)));
//DPRINT(("ser_get: top: got = %d\n", ser->got));
// Fill small input buffer if empty.
if (ser->head >= ser->len) {
size_t n_received;
serio_res_t err;
//DPRINT(("ser_get: calling serio_read\n"));
err = serio_read(&ser->serio, ser->rbuf, ser_READSIZE, &n_received);
if ((err != serio_RES_OK) && (err != serio_RES_EMPTY)) {
DPRINT(("ser_get: serio_read failed %d\n", err));
} else if ((n_received == 0) || (err == serio_RES_EMPTY)) {
DPRINT(("ser_get: returning EMPTY\n"));
return ser_RES_EMPTY;
}
ser->head = 0;
ser->len = n_received;
/* check if got a modem not connected signal */
{ unsigned char *p = &ser->rbuf[0];
int pos = ser->sigpos;
for ( ; n_received--; p++) {
if (*p == SIGNAL_HANGUP[pos]) {
DPRINT(("ser_get: matched SH[%d]=%c; pos now %d\n", pos, *p, pos+1));
pos++;
if (pos >= sizeof(SIGNAL_HANGUP)) {
DPRINT(("ser_get: returning NO_RESPONSE\n"));
return ser_RES_NO_RESPONSE;
}
} else if (pos) {
DPRINT(("ser_get: cleared pos\n"));
pos = 0;
if (*p == SIGNAL_HANGUP[pos]) {
DPRINT(("ser_get: matched SH[%d]=%c; pos now %d\n", pos, *p, pos+1));
pos++;
}
}
}
ser->sigpos = pos;
}
}
// Continue accumulating bytes of packet.
c = ser->rbuf[ser->head++];
((unsigned char *)&ser->pkt)[ser->got++] = c;
DPRINT(("ser_get: got char 0x%x, got now %d\n", c, ser->got));
// Do header processing.
if (ser->got == 1) {
// Does 1st byte of header match?
if (c != ser_HDR_FRAME0)
ser->got = 0; // no, just start over.
} else if (ser->got == 2) {
// Does 2nd byte of header match?
if (c != ser_HDR_FRAME1)
ser->got = 0; // no, just start over.
} else if (ser->got == 4) {
// Is header Checksum valid?
if (c != ((ser_HDR_FRAME0 + ser_HDR_FRAME1 + ser->pkt.hdr.bodylen) & 0xff))
ser->got = 0; // no, just start over.
} else if ((size_t)(ser->got) >= sizeof(ser_hdr_t) + ser->pkt.hdr.bodylen) {
int c = crc(ser->pkt.body, ser->pkt.hdr.bodylen);
ser->got = 0;
// Done with packet. Does crc match?
DPRINT(("ser_get: bodycrc %4x, len %d, first 2 bytes %02x %02x, last 5 bytes %02x %02x %02x %02x %02x\n",
ser->pkt.hdr.bodycrc,
ser->pkt.hdr.bodylen,
ser->pkt.body[0],
ser->pkt.body[1],
ser->pkt.body[ser->pkt.hdr.bodylen-5],
ser->pkt.body[ser->pkt.hdr.bodylen-4],
ser->pkt.body[ser->pkt.hdr.bodylen-3],
ser->pkt.body[ser->pkt.hdr.bodylen-2],
ser->pkt.body[ser->pkt.hdr.bodylen-1]));
if (c != ser->pkt.hdr.bodycrc) {
DPRINT(("ser_get: returning EMPTY; bad crc. last 4 bytes %02x %02x %02x %02x\n",
ser->pkt.body[ser->pkt.hdr.bodylen-4],
ser->pkt.body[ser->pkt.hdr.bodylen-3],
ser->pkt.body[ser->pkt.hdr.bodylen-2],
ser->pkt.body[ser->pkt.hdr.bodylen-1]));
return ser_RES_EMPTY;
}
// Is user buffer big enough to fit this packet?
if (ser->pkt.hdr.bodylen > *len) {
DPRINT(("ser_get: returning FULL\n"));
return ser_RES_FULL; // no, packet too big to fit, fail
} else {
// Yes. Copy packet to user, prepare for next packet.
memcpy(buf, ser->pkt.body, ser->pkt.hdr.bodylen);
*len = ser->pkt.hdr.bodylen;
DPRINT(("ser_get: returning OK\n"));
return ser_RES_OK;
}
}
}
DPRINT(("ser_get: bug: unreachable code reached\n"));
return ser_RES_EMPTY;
}
ser_result_t ser_get(ser_t *ser, void *buf, size_t *len)
{
DWORD dwErrorFlags;
DWORD dwReadLength;
BOOL bContinue = TRUE;
DWORD start_tmp;
while (bContinue) {
if (LOOKING_FOR_HDR == ser->state) {
if (RBUF_DATA(ser) >= sizeof(ser_hdr_t)) {
/* printbytes((void *)(&(ser->rbuf[ser->start])), "ser_get:hdr:", sizeof(ser_hdr_t)); */
if (ser_HDR_FRAME0 != ser->rbuf[ser->start]) {
RBUF_INCREMENT_START(ser, 1); continue;
}
RBUF_INCREMENT_START(ser, 1);
if (ser_HDR_FRAME1 != ser->rbuf[ser->start]) {
RBUF_INCREMENT_START(ser, 1); continue;
}
RBUF_INCREMENT_START(ser, 1);
ser->length = (ser->rbuf[RBUF_OFFSET(ser, 0)] & 0xff);
if (((ser_HDR_FRAME0 + ser_HDR_FRAME1 + ser->length) & 0xff) != ser->rbuf[RBUF_OFFSET(ser, 1)]) {
/* DPRINT(("ser_get:got ser->rbuf[RBUF_OFFSET(ser,1)] incorrect\n")); */
continue;
}
ser->crc = ser->rbuf[RBUF_OFFSET(ser, 2)];
ser->state = LOOKING_FOR_BODY;
}
else {
/* DPRINT(("ser_get: RBUF_DATA len:%d is smaller than %d\n", RBUF_DATA(ser), sizeof(ser_hdr_t))); */
bContinue = FALSE;
}
}
if (LOOKING_FOR_BODY == ser->state) {
if (RBUF_DATA(ser) >= ser->length + 3) {
start_tmp = ser->start; RBUF_INCREMENT_START(ser, 3);
if (RBUF_CRC(ser) == ser->crc) {
if (ser->length > *len) {
/* DPRINT(("ser_get: ser->length=%d > *len=%d\n", ser->length, *len)); */
*len = (ser->length & 0xff); return ser_RES_FULL;
} else {
*len = (ser->length & 0xff);
if (ser->start + ser->length <= ser->rbufsize) {
memcpy(buf, ser->rbuf + ser->start, ser->length);
} else {
memcpy(buf, ser->rbuf + ser->start, ser->rbufsize - ser->start);
memcpy((char *) buf + ser->rbufsize - ser->start, ser->rbuf, ser->length - (ser->rbufsize - ser->start));
}
/* printbytes(buf, "ser_get:buf:", ser->length); */
RBUF_INCREMENT_START(ser, ser->length);
ser->state = LOOKING_FOR_HDR;
return ser_RES_OK;
}
} else {
ser->start = start_tmp;
ser->state = LOOKING_FOR_HDR; continue;
}
} else {
/* DPRINT(("ser_get: RBUF_DATA len:%d is smaller than ser->len+3=%d\n", RBUF_DATA(ser), ser->length + 3)); */
bContinue = FALSE;
}
}
dwReadLength = RBUF_EMPTY(ser);
if (dwReadLength > 0) {
serio_res_t err;
err = serio_read(&ser->serio, &ser->rbuf[ser->end], dwReadLength, &dwReadLength);
if ((err != serio_RES_OK) && (err != serio_RES_EMPTY)) {
DPRINT(("ser_get: serio_read failed %d\n", err));
} else {
/* check if got a modem not connected signal */
{ DWORD i;
int pos;
unsigned char *p;
pos = ser->sigpos;
for (i = 1, p = &(ser->rbuf[ser->end]); i <= dwReadLength; i++, p++) {
if (*p == SIGNAL_HANGUP[pos]) {
pos++;
if (pos >= sizeof(SIGNAL_HANGUP))
return ser_RES_NO_RESPONSE;
} else if (pos) {
pos = 0;
if (*p == SIGNAL_HANGUP[pos])
pos++;
}
}
ser->sigpos = pos;
}
#if 0
DPRINT(("ser_get: read %d bytes\n", dwReadLength));
printbytes((void *)(&(ser->rbuf[ser->end])), "ser_get:readbuf:", dwReadLength);
#endif
RBUF_INCREMENT_END(ser, dwReadLength); bContinue = TRUE;
}
}
}
return ser_RES_EMPTY;
}
static int send_command(serioStuff *s, u8 cmd, u16 param, void *payload)
{
int retries, res;
int bytes_sent, bytes_received;
u8 ack,nak;
unsigned char resp = 0x55;
static u16 seqno = 0;
packet_init();
if(flags.hanmode){
packet.han.pkttype = HDC;
packet.han.addr = (u8) hannodeaddr;
packet.han.cmd = cmd;
packet.han.param = param;
packet.han.seq = seqno;
ack = HDC_ACK;
nak = HDC_NAK;
}
else{
packet.pbl.cmd = cmd;
packet.pbl.param = param;
packet.pbl.seq = seqno;
ack = ACK;
nak = NAK;
}
if(payload)
memcpy((flags.hanmode) ? packet.han.payload : packet.pbl.payload, payload, LOADER_PAYLOAD);
packet_finalize();
debug(DEBUG_ACTION,"Command: 0x%02X Sequence Number: %d, CRC: 0x%04X", cmd, seqno, (flags.hanmode)? packet.han.crc16 : packet.pbl.crc16);
if(!flags.handisrunning){ // Hand not running?
serio_flush_input(s);
for(retries = PACKET_RETRIES;;){
if((bytes_sent = packet_tx(s, packet.buffer, packet_size, 5000000)) < 0){
debug(DEBUG_ACTION, "Command Packet Write Error");
return FAIL;
}
debug(DEBUG_ACTION, "Bytes Sent: %d", bytes_sent);
if(bytes_sent != packet_size){
debug(DEBUG_ACTION, "Command Packet Write Incomplete");
return FAIL;
}
for(;;){
bytes_received = serio_read(s, &resp, 1, 5000000);
if(bytes_received < 0){
if(errno != EAGAIN){
debug(DEBUG_UNEXPECTED, "Response Read Error: %s", strerror(errno));
return FAIL;
}
else{
continue;
}
}
else{
break;
}
}
if(bytes_received != 1){
debug(DEBUG_ACTION, "Read Timeout Error");
return FAIL;
}
if((resp == ack)||(resp == nak)){
debug(DEBUG_ACTION, "Response: %s",(resp == ack)?"ACK":"NAK");
}
else{
debug(DEBUG_ACTION, "Response: 0x%02X", (unsigned int) resp);
}
if((cmd == BC_CHECK_APP) && (resp == STX)){
debug(DEBUG_UNEXPECTED, "Check App Failed");
return FAIL;
}
if((resp != ack)){
debug(DEBUG_ACTION, "Did not get ACK");
if((resp == nak) && (retries--)){
debug(DEBUG_UNEXPECTED, "***Retrying packet***");
usleep(100000);
continue;
}
return FAIL; // Retries used up or didn't get ACK or NAK. Fail.
}
break;
}
}
else{ // Hand is running
for(retries = 0; retries < PACKET_RETRIES; retries++){
client_command.cmd.raw.txlen = packet_format(client_command.cmd.raw.txbuffer, &packet.han, packet_size);
client_command.cmd.raw.rxexpectlen = 1;
client_command.cmd.raw.txtimeout = 100000;
client_command.cmd.raw.rxtimeout = 1000000;
client_command.request = HAN_CCMD_RAW_PACKET;
res = hanclient_send_command_return_res(&client_command);
bytes_received = 0;
if(!res)
bytes_received = client_command.cmd.raw.rxexpectlen;
if(bytes_received == 1){
resp = client_command.cmd.raw.rxbuffer[0];
if((resp == ack)||(resp == nak)){
debug(DEBUG_ACTION, "Response: %s",(resp == ack)?"ACK":"NAK");
}
else{
debug(DEBUG_ACTION, "Response: 0x%02X", (unsigned int) resp);
}
if((cmd == BC_CHECK_APP) && (resp == STX)){
debug(DEBUG_UNEXPECTED, "Check App Failed");
return FAIL;
}
if((resp != ack)){
debug(DEBUG_ACTION, "Did not get ACK");
if(resp == nak){
debug(DEBUG_UNEXPECTED, "***Retrying packet***, try = %d", retries);
usleep(100000);
continue;
}
}
else
break; // Success
}
debug(DEBUG_UNEXPECTED,"Received invalid or no response, try = %d", retries);
usleep(100000);
}
if(retries == PACKET_RETRIES){
debug(DEBUG_EXPECTED, "Too many packet retries!");
return FAIL;
}
}
seqno++;
return PASS;
}
