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; }