void
Diag(void *buf, size_t sz)
{
unsigned char *cbuf = (unsigned char *) buf;
while (sz--) {
GPS_Diag("%02x ", *cbuf++);
}
}
int32 GPS_Serial_Flush(gpsdevh *fd)
{
unsigned char u;
int n;
GPS_Diag("Flush:");
n=0;
while( (GPS_Serial_Chars_Ready(fd)) && (n < 1000))
{
GPS_Serial_Read(fd,&u,1);
GPS_Diag("%02x ", u);
n++;
}
GPS_Diag("\r\n");
return 1;
}
void
DiagS(void *buf, size_t sz)
{
unsigned char *cbuf = (unsigned char *) buf;
while (sz--) {
unsigned char c = *cbuf++;
GPS_Diag("%c", isalnum(c) ? c : '.');
}
}
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);
}
static
HANDLE * garmin_usb_start(HDEVINFO* hdevinfo, SP_DEVICE_INTERFACE_DATA *infodata)
{
DWORD size;
PSP_INTERFACE_DEVICE_DETAIL_DATA pdd = NULL;
SP_DEVINFO_DATA devinfo;
SetupDiGetDeviceInterfaceDetail(hdevinfo, infodata,
NULL, 0, &size, NULL);
pdd = malloc(size);
pdd->cbSize = sizeof(SP_INTERFACE_DEVICE_DETAIL_DATA);
devinfo.cbSize = sizeof(SP_DEVINFO_DATA);
if (!SetupDiGetDeviceInterfaceDetail(hdevinfo, infodata,
pdd, size, NULL, &devinfo)) {
GPS_Serial_Error("SetupDiGetDeviceInterfaceDetail");
return NULL;
}
/* Whew. All that just to get something we can open... */
GPS_Diag("Windows GUID for interface is \n\t%s\n",
pdd->DevicePath);
if (usb_handle != INVALID_HANDLE_VALUE) {
fatal("garmin_usb_start called while device already started.\n");
}
usb_handle = CreateFile(pdd->DevicePath, GENERIC_READ|GENERIC_WRITE,
0, NULL, OPEN_EXISTING, 0, NULL );
if (usb_handle == INVALID_HANDLE_VALUE) {
if (GetLastError() == ERROR_ACCESS_DENIED) {
// warning(
//"Exclusive access is denied. It's likely that something else such as\n"
//"Nroute, Spanner, Google Earth, or GPSGate already has control of the device\n");
}
GPS_Serial_Error("(usb) CreateFile on '%s' failed", pdd->DevicePath);
return NULL;
}
if(!DeviceIoControl(usb_handle, IOCTL_GARMIN_USB_BULK_OUT_PACKET_SIZE,
NULL, 0, &usb_tx_packet_size, GARMIN_USB_INTERRUPT_DATA_SIZE,
&size, NULL)) {
fatal("Couldn't get USB packet size.\n");
}
win_llops.max_tx_size = usb_tx_packet_size;
gusb_syncup();
return usb_handle;
}
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;
}
int32 GPS_Serial_On(const char *port, gpsdevh **dh)
{
DCB tio;
COMMTIMEOUTS timeout;
HANDLE comport;
#ifdef _UNICODE
wchar_t wport[80];
LPCWSTR cstr;
#endif
const char *xname = fix_win_serial_name(port);
win_serial_data *wsd = reinterpret_cast<win_serial_data *>(xcalloc(sizeof (win_serial_data), 1));
*dh = (gpsdevh*) wsd;
g_gps_devh = (gpsdevh*) wsd; // save a global copy
GPS_Diag("Opening %s\n", xname);
#ifdef _UNICODE
MultiByteToWideChar( 0, 0, xname, -1, wport, 80);
cstr = wport;
comport = CreateFile(cstr, GENERIC_READ|GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
#else
comport = CreateFile(xname, GENERIC_READ|GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
#endif
if (comport == INVALID_HANDLE_VALUE) {
GPS_Serial_Error("CreateFile failed");
gps_errno = SERIAL_ERROR;
return 0;
}
tio.DCBlength = sizeof(DCB);
GetCommState (comport, &tio);
tio.BaudRate = CBR_9600;
tio.fBinary = TRUE;
tio.fParity = TRUE;
tio.fOutxCtsFlow = FALSE;
tio.fOutxDsrFlow = FALSE;
tio.fDtrControl = DTR_CONTROL_ENABLE;
tio.fDsrSensitivity = FALSE;
tio.fTXContinueOnXoff = TRUE;
tio.fOutX = FALSE;
tio.fInX = FALSE;
tio.fErrorChar = FALSE;
tio.fNull = FALSE;
tio.fRtsControl = RTS_CONTROL_ENABLE;
tio.fAbortOnError = FALSE;
tio.ByteSize = 8;
tio.Parity = NOPARITY;
tio.StopBits = ONESTOPBIT;
if (!SetCommState (comport, &tio)) {
GPS_Serial_Error("SetCommState on port '%s' failed", port);
CloseHandle(comport);
comport = INVALID_HANDLE_VALUE;
gps_errno = SERIAL_ERROR;
return 0;
}
/*
* The timeouts are kind of fictional as we always end up doing
* single byte reads. At 9600bps (the default) the individual
* character time is 104Millisecs, so these are mostly "dead-man"
* (i.e. cable unplugged, unit not turned on) values.
*/
GetCommTimeouts (comport, &timeout);
timeout.ReadIntervalTimeout = 1000; /*like vtime. In MS. */
timeout.ReadTotalTimeoutMultiplier = 1000;
timeout.ReadTotalTimeoutConstant = 1000;
timeout.WriteTotalTimeoutMultiplier = 1000;
timeout.WriteTotalTimeoutConstant = 1000;
if (!SetCommTimeouts (comport, &timeout)) {
GPS_Serial_Error("SetCommTimeouts");
CloseHandle (comport);
comport = INVALID_HANDLE_VALUE;
gps_errno = SERIAL_ERROR;
return 0;
}
wsd->comport = comport;
return 1;
}
