int
gusb_cmd_send(const garmin_usb_packet *opkt, size_t sz)
{
unsigned int rv, i;
unsigned char *obuf = (unsigned char *) &opkt->dbuf;
const char *m1, *m2;
rv = gusb_llops->llop_send(opkt, sz);
if (gps_show_bytes) {
const unsigned short pkttype = le_read16(&opkt->gusb_pkt.databuf[0]);
const unsigned short pkt_id = le_read16(&opkt->gusb_pkt.pkt_id);
GPS_Diag("TX [%d]:", sz);
for(i=0;i<sz;i++)
GPS_Diag("%02x ", obuf[i]);
for(i=0;i<sz;i++)
GPS_Diag("%c", isalnum(obuf[i])? obuf[i] : '.');
m1 = Get_Pkt_Type(pkt_id, pkttype, &m2);
GPS_Diag("(%-8s%s)\n", m1, m2 ? m2 : "");
}
/*
* Recursion, when used in a disciplined way, can be our friend.
*
* The Garmin protocol requires that packets that are exactly
* a multiple of the max tx size be followed by a zero length
* packet. Do that here so we can see it in debugging traces.
*/
if (sz && !(sz % gusb_llops->max_tx_size)) {
gusb_cmd_send(opkt, 0);
}
return (rv);
}
int32 GPS_Serial_Write_Packet(gpsdevh *fd, GPS_PPacket packet)
{
size_t ret;
const char *m1, *m2;
GPS_Serial_OPacket ser_pkt;
UC ser_pkt_data[MAX_GPS_PACKET_SIZE * sizeof(UC)];
US bytes;
if(!fd)
return 0;
if (packet->type >= 0xff || packet->n >= 0xff) {
GPS_Error("SEND: Unsupported packet type/size for serial protocol");
return 0;
}
ser_pkt.data = ser_pkt_data;
bytes = Build_Serial_Packet(packet, &ser_pkt);
GPS_Diag("Tx Data:");
Diag(&ser_pkt.dle, 3);
if((ret=GPS_Serial_Write(fd,(const void *) &ser_pkt.dle,(size_t)3)) == -1)
{
perror("write");
GPS_Error("SEND: Write to GPS failed");
return 0;
}
if(ret!=3)
{
GPS_Error("SEND: Incomplete write to GPS");
return 0;
}
Diag(ser_pkt.data, bytes);
if((ret=GPS_Serial_Write(fd,(const void *)ser_pkt.data,(size_t)bytes)) == -1)
{
perror("write");
GPS_Error("SEND: Write to GPS failed");
return 0;
}
if(ret!=bytes)
{
GPS_Error("SEND: Incomplete write to GPS");
return 0;
}
Diag(&ser_pkt.chk, 3);
GPS_Diag(": ");
DiagS(ser_pkt.data, bytes);
DiagS(&ser_pkt.chk, 3);
m1 = Get_Pkt_Type(ser_pkt.type, ser_pkt.data[0], &m2);
GPS_Diag("(%-8s%s)\n", m1, m2 ? m2 : "");
if((ret=GPS_Serial_Write(fd,(const void *)&ser_pkt.chk,(size_t)3)) == -1)
{
perror("write");
GPS_Error("SEND: Write to GPS failed");
return 0;
}
if(ret!=3)
{
GPS_Error("SEND: Incomplete write to GPS");
return 0;
}
return 1;
}
int32 GPS_Serial_Packet_Read(gpsdevh *fd, GPS_PPacket *packet)
{
time_t start;
int32 n;
int32 len;
UC u;
int32 isDLE;
UC *p;
uint32 i;
UC chk=0, chk_read;
const char *m1;
const char *m2;
len = 0;
isDLE = gpsFalse;
p = (*packet)->data;
start = GPS_Time_Now();
GPS_Diag("Rx Data:");
while(GPS_Time_Now() < start+GPS_TIME_OUT)
{
if((n=GPS_Serial_Chars_Ready(fd)))
{
if(GPS_Serial_Read(fd,&u,1)==-1)
{
perror("read");
GPS_Error("GPS_Packet_Read: Read error");
gps_errno = FRAMING_ERROR;
return 0;
}
GPS_Diag("%02x ", u);
if(!len)
{
if(u != DLE)
{
(void) fprintf(stderr,"GPS_Packet_Read: No DLE. Data received, but probably not a garmin packet.\n");
(void) fflush(stderr);
return 0;
}
++len;
continue;
}
if(len==1)
{
(*packet)->type = u;
++len;
continue;
}
if(u == DLE)
{
if(isDLE)
{
isDLE = gpsFalse;
continue;
}
isDLE = gpsTrue;
}
if(len == 2)
{
(*packet)->n = u;
len = -1;
continue;
}
if(u == ETX)
if(isDLE)
{
if(p-(*packet)->data-2 != (*packet)->n)
{
GPS_Error("GPS_Packet_Read: Bad count");
gps_errno = FRAMING_ERROR;
return 0;
}
chk_read = *(p-2);
for(i=0,p=(*packet)->data;i<(*packet)->n;++i)
chk -= *p++;
chk -= (*packet)->type;
chk -= (UC)((*packet)->n);
if(chk != chk_read)
{
GPS_Error("CHECKSUM: Read error\n");
gps_errno = FRAMING_ERROR;
return 0;
}
m1 = Get_Pkt_Type((*packet)->type, (*packet)->data[0], &m2);
if (gps_show_bytes) {
GPS_Diag(" ");
for (i = 0; i < (*packet)->n; i++) {
char c = (*packet)->data[i];
GPS_Diag("%c", c); //isalnum(c) ? c : '.');
}
GPS_Diag(" ");
}
GPS_Diag("(%-8s%s)\n", m1, m2 ? m2 : "");
return (*packet)->n;
}
if (p - (*packet)->data >= MAX_GPS_PACKET_SIZE)
{
GPS_Error("GPS_Serial_Packet_Read: Bad payload size/no ETX found");
gps_errno = FRAMING_ERROR;
return 0;
}
*p++ = u;
}
}
GPS_Error("GPS_Packet_Read: Timeout. No data received.");
gps_errno = SERIAL_ERROR;
return 0;
}
int
gusb_cmd_get(garmin_usb_packet *ibuf, size_t sz)
{
int rv = 0;
unsigned char *buf = (unsigned char *) &ibuf->dbuf;
int orig_receive_state;
unsigned short pkt_id;
top:
orig_receive_state = receive_state;
switch (receive_state) {
case rs_fromintr:
rv = gusb_llops->llop_get_intr(ibuf, sz);
break;
case rs_frombulk:
rv = gusb_llops->llop_get_bulk(ibuf, sz);
break;
default:
fatal("Unknown receiver state %d\n", receive_state);
}
pkt_id = le_read16(&ibuf->gusb_pkt.pkt_id);
if (gps_show_bytes) {
int i;
const char *m1, *m2;
unsigned short pkttype = le_read16(&ibuf->gusb_pkt.databuf[0]);
GPS_Diag("RX (%s) [%d]:",
receive_state == rs_fromintr ? "intr" : "bulk", rv);
for(i=0;i<rv;i++) {
/*dsr
if (DEBUG_THRESH) {
GPS_Diag("[...]");
break;
}
*/
GPS_Diag("%02x ", buf[i]);
}
for(i=0;i<rv;i++) {
/*dsr
if (DEBUG_THRESH) {
GPS_Diag("[...]");
break;
}
*/
GPS_Diag("%c", isalnum(buf[i])? buf[i] : '.');
}
m1 = Get_Pkt_Type(pkt_id, pkttype, &m2);
if ((rv == 0) && (receive_state == rs_frombulk) ) {m1= "RET2INTR";m2=NULL;};
GPS_Diag("(%-8s%s)\n", m1, m2 ? m2 : "");
}
/* Adjust internal state and retry the read */
if ((rv > 0) && (pkt_id == GUSB_REQUEST_BULK)) {
receive_state = rs_frombulk;
goto top;
}
/*
* If we were reading from the bulk pipe and we just got
* a zero request, adjust our internal state.
* It's tempting to retry the read here to hide this "stray"
* packet from our callers, but that only works when you know
* there's another packet coming. That works in every case
* except the A000 discovery sequence.
*/
if ((receive_state == rs_frombulk) && (rv <= 0)) {
receive_state = rs_fromintr;
}
return rv;
}
