path: root/sirfmon.c

/*
 * SiRF packet monitor, originally by Rob Janssen, PE1CHL.
 * Heavily hacked by Eric S. Raymond for use with the gpsd project.
 *
 * Autobauds.  Takes a SiRF chip in NMEA mode to binary mode, if needed.
 * The autobauding code is fairly primitive and can sometimes fail to
 * sync properly.  If that happens, just kill and restart sirfmon.
 *
 * Useful commands:
 *	n -- switch device to NMEA at current speed and exit.
 *	l -- toggle packet logging
 *	a -- toggle receipt of 50BPS subframe data.
 *	b -- change baud rate.
 *	c -- set or clear static navigation mode
 *	s -- send hex bytes to device.
 *	t -- toggle navigation-parameter display mode
 *	q -- quit, leaving device in binary mode.
 *      Ctrl-S -- freeze display.
 *      Ctrl-Q -- unfreeze display.
 *
 * Note: one of the goals of sirfmon.c is *not* to use the gpsdata structure.  
 * sirfmon is intended to be an independent sanity check on SiRF decoding,
 * so it deliberately doesn't use much of the library. 
 */
#include <stdio.h>
#include <curses.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <unistd.h>
#include <time.h>
#include <termios.h>
#include <fcntl.h>	/* for O_RDWR */
#include <stdarg.h>
#include <stdbool.h>

#include "config.h"
#include "gps.h"	/* for DEFAULT_GPSD_PORT; brings in PI as well */

#define PUT_ORIGIN	-4
#include "bits.h"

#if defined(HAVE_SYS_TIME_H)
#include <sys/time.h>
#endif
#if defined (HAVE_SYS_SELECT_H)
#include <sys/select.h>
#endif

#ifdef S_SPLINT_S
extern struct tm *localtime_r(const time_t *,/*@out@*/struct tm *tp)/*@modifies tp@*/;
#endif /* S_SPLINT_S */

extern int netlib_connectsock(const char *, const char *, const char *);

#define BUFLEN		2048

#define START1		0xa0
#define START2		0xa2
#define END1		0xb0
#define END2		0xb3

#define RAD2DEG (180.0/PI)

/* how many characters to look at when trying to find baud rate lock */
#define SNIFF_RETRIES	1200

static int devicefd = -1, controlfd = -1;
static int nfix,fix[20];
static int gmt_offset;
static bool dispmode = false;
static bool serial, subframe_enabled = false;
static unsigned int stopbits, bps;

/*@ -nullassign @*/
static char *verbpat[] =
{
    "#Time:",
    "@R Time:",
    "CSTD: New almanac for",
    "NOTICE: DOP Q Boost",
    "RTC not set",
    "numOfSVs = 0",
    "rtcaj tow ",
    NULL
};
/*@ +nullassign @*/

static char *sbasvec[] =
{
    "None",
    "SBAS",
    "Serial",
    "Beacon",
    "Software",
};

static struct termios ttyset;
static WINDOW *mid2win, *mid4win, *mid6win, *mid7win, *mid9win, *mid13win;
static WINDOW *mid19win, *mid27win, *cmdwin, *debugwin;
static FILE *logfile;

#define NO_PACKET	0
#define SIRF_PACKET	1
#define NMEA_PACKET	2

#define display	(void)mvwprintw

/*****************************************************************************
 *
 * NMEA command composition
 *
 *****************************************************************************/

static void nmea_add_checksum(char *sentence)
/* add NMEA checksum to a possibly  *-terminated sentence */
{
    unsigned char sum = '\0';
    char c, *p = sentence;

    if (*p == '$') {
	p++;
	while ( ((c = *p) != '*') && (c != '\0')) {
	    sum ^= c;
	    p++;
	}
	*p++ = '*';
	(void)snprintf(p, 5, "%02X\r\n", (unsigned int)sum);
    }
}

static int nmea_send(int fd, const char *fmt, ... )
/* ship a command to the GPS, adding * and correct checksum */
{
    size_t status;
    char buf[BUFLEN];
    va_list ap;

    va_start(ap, fmt) ;
    (void)vsnprintf(buf, sizeof(buf)-5, fmt, ap);
    va_end(ap);
    strcat(buf, "*");
    nmea_add_checksum(buf);
    (void)fputs(buf, stderr);		/* so user can watch the baud hunt */
    status = (size_t)write(fd, buf, strlen(buf));
    if (status == strlen(buf)) {
	return (int)status;
    } else {
	perror("SiRF write");
	return -1;
    }
}

/*****************************************************************************
 *
 * SiRF packet-decoding routines
 *
 *****************************************************************************/

static void decode_time(int week, int tow)
{
    int day = tow / 8640000;
    int tod = tow % 8640000;
    int h = tod / 360000;
    int m = tod % 360000;
    int s = m % 6000;

    m = (m - s) / 6000;

    (void)wmove(mid2win, 3,7);
    (void)wprintw(mid2win, "%4d+%9.2f", week, (double)tow/100);
    (void)wmove(mid2win, 3, 29);
    (void)wprintw(mid2win, "%d %02d:%02d:%05.2f", day, h,m,(double)s/100);
    (void)wmove(mid2win, 4, 8);
    (void)wprintw(mid2win, "%f", timestamp()-gpstime_to_unix(week,tow/100.0));
    (void)wmove(mid2win, 4, 29);
    (void)wprintw(mid2win, "%d", gmt_offset);
}

static void decode_ecef(double x, double y, double z,
			double vx, double vy, double vz)
{
    const double a = 6378137;
    const double f = 1 / 298.257223563;
    const double b = a * (1 - f);
    const double e2 = (a*a - b*b) / (a*a);
    const double e_2 = (a*a - b*b) / (b*b);
    double lambda,p,theta,phi,n,h,vnorth,veast,vup,speed,heading;

    lambda = atan2(y,x);
    /*@ -evalorder @*/ 
    p = sqrt(pow(x,2) + pow(y,2));
    theta = atan2(z*a,p*b);
    phi = atan2(z + e_2*b*pow(sin(theta),3),p - e2*a*pow(cos(theta),3));
    n = a / sqrt(1.0 - e2*pow(sin(phi),2));
    h = p / cos(phi) - n;
    h -= wgs84_separation((double)(RAD2DEG*phi),(double)(RAD2DEG*lambda));
    vnorth = -vx*sin(phi)*cos(lambda)-vy*sin(phi)*sin(lambda)+vz*cos(phi);
    veast = -vx*sin(lambda)+vy*cos(lambda);
    vup = vx*cos(phi)*cos(lambda)+vy*cos(phi)*sin(lambda)+vz*sin(phi);
    speed = sqrt(pow(vnorth,2) + pow(veast,2));
    heading = atan2(veast,vnorth);
    /*@ +evalorder @*/
    if (heading < 0)
	heading += 2 * PI;

    (void)wmove(mid2win, 1,40);
    (void)wprintw(mid2win, "%9.5f %9.5f",(double)(RAD2DEG*phi),
				   (double)(RAD2DEG*lambda));
    (void)mvwaddch(mid2win, 1, 49, ACS_DEGREE);
    (void)mvwaddch(mid2win, 1, 59, ACS_DEGREE);
    (void)wmove(mid2win, 1,61);
    (void)wprintw(mid2win, "%8d",(int)h);

    (void)wmove(mid2win, 2,40);
    (void)wprintw(mid2win, "%9.1f %9.1f",vnorth,veast);
    (void)wmove(mid2win, 2,61);
    (void)wprintw(mid2win, "%8.1f",vup);

    (void)wmove(mid2win, 3,54);
    (void)wprintw(mid2win, "%5.1f",(double)(RAD2DEG*heading));
    (void)mvwaddch(mid2win, 3, 59, ACS_DEGREE);
    (void)wmove(mid2win, 3,61);
    (void)wprintw(mid2win, "%8.1f",speed);
}

static void decode_sirf(unsigned char buf[], int len)
{
    int i,j,ch,off,cn;

    switch (buf[0])
    {
    case 0x02:		/* Measured Navigation Data */
	(void)wmove(mid2win, 1,6);	/* ECEF position */
	(void)wprintw(mid2win, "%8d %8d %8d",getsl(buf, 1),getsl(buf, 5),getsl(buf, 9));
	(void)wmove(mid2win, 2,6);	/* ECEF velocity */
	(void)wprintw(mid2win, "%8.1f %8.1f %8.1f",
		(double)getsw(buf, 13)/8,(double)getsw(buf, 15)/8,(double)getsw(buf, 17)/8);
	decode_ecef((double)getsl(buf, 1),(double)getsl(buf, 5),(double)getsl(buf, 9),
		(double)getsw(buf, 13)/8,(double)getsw(buf, 15)/8,(double)getsw(buf, 17)/8);
	decode_time((int)getuw(buf, 22),getsl(buf, 24));
	/* line 4 */
	(void)wmove(mid2win, 4,49);
	(void)wprintw(mid2win, "%4.1f",(double)getub(buf, 20)/5);	/* HDOP */
	(void)wmove(mid2win, 4,58);
	(void)wprintw(mid2win, "%02x",getub(buf, 19));		/* Mode 1 */
	(void)wmove(mid2win, 4,70);
	(void)wprintw(mid2win, "%02x",getub(buf, 21));		/* Mode 2 */
	(void)wmove(mid2win, 5,7);
	nfix = (int)getub(buf, 28);
	(void)wprintw(mid2win, "%d = ",nfix);		/* SVs in fix */
	for (i = 0; i < SIRF_CHANNELS; i++) {	/* SV list */
	    if (i < nfix)
		(void)wprintw(mid2win, "%3d",fix[i] = (int)getub(buf, 29+i));
	    else
		(void)wprintw(mid2win, "   ");
	}
	(void)wprintw(debugwin, "MND 0x02=");
	break;

    case 0x04:		/* Measured Tracking Data */
	decode_time((int)getuw(buf, 1),getsl(buf, 3));
	ch = (int)getub(buf, 7);
	for (i = 0; i < ch; i++) {
	    int sv,st;
	    
	    off = 8 + 15 * i;
	    (void)wmove(mid4win, i+2, 3);
	    sv = (int)getub(buf, off);
	    (void)wprintw(mid4win, " %3d",sv);

	    (void)wprintw(mid4win, " %3d%3d %04x",((int)getub(buf, off+1)*3)/2,(int)getub(buf, off+2)/2,(int)getsw(buf, off+3));

	    st = ' ';
	    if ((int)getuw(buf, off+3) == 0xbf)
		st = 'T';
	    for (j = 0; j < nfix; j++)
		if (sv == fix[j]) {
		    st = 'N';
		    break;
		}

	    cn = 0;

	    for (j = 0; j < 10; j++)
		cn += (int)getub(buf, off+5+j);

	    (void)wprintw(mid4win, "%5.1f %c",(double)cn/10,st);

	    if (sv == 0)			/* not tracking? */
		(void)wprintw(mid4win, "   ");	/* clear other info */
	}
	(void)wprintw(debugwin, "MTD 0x04=");
    	break;

#ifdef __UNUSED__
    case 0x05:		/* raw track data */
	for (off = 1; off < len; off += 51) {
	    ch = getul(buf, off);
	    (void)wmove(mid4win, ch+2, 19);
	    cn = 0;

	    for (j = 0; j < 10; j++)
		cn += getub(buf, off+34+j);

	    printw("%5.1f",(double)cn/10);

	    printw("%9d%3d%5d",getul(buf, off+8),(int)getuw(buf, off+12),(int)getuw(buf, off+14));
	    printw("%8.5f %10.5f",
	    	(double)getul(buf, off+16)/65536,(double)getul(buf, off+20)/1024);
	}
	(void)wprintw(debugwin, "RTD 0x05=");
    	break;
#endif /* __UNUSED */

    case 0x06:		/* firmware version */
	display(mid6win, 1, 10, "%s",buf + 1);
	(void)wprintw(debugwin, "FV  0x06=");
    	break;

    case 0x07:		/* Response - Clock Status Data */
	decode_time((int)getuw(buf, 1),getsl(buf, 3));
	display(mid7win, 1, 5,  "%2d", getub(buf, 7));	/* SVs */
	display(mid7win, 1, 16, "%lu", getul(buf, 8));	/* Clock drift */
	display(mid7win, 1, 29, "%lu", getul(buf, 12));	/* Clock Bias */
	display(mid7win, 2, 21, "%lu", getul(buf, 16));	/* Estimated Time */
	(void)wprintw(debugwin, "CSD 0x07=");
	break;

    case 0x08:		/* 50 BPS data */
	ch = (int)getub(buf, 1);
	display(mid4win, ch, 27, "Y");
	(void)wprintw(debugwin, "50B 0x08=");
	subframe_enabled = true;
    	break;

    case 0x09:		/* Throughput */
	display(mid9win, 1, 6,  "%.3f",(double)getuw(buf, 1)/186);	/*SegStatMax*/
	display(mid9win, 1, 18, "%.3f",(double)getuw(buf, 3)/186);	/*SegStatLat*/
	display(mid9win, 1, 31, "%.3f",(double)getuw(buf, 5)/186);	/*SegStatTime*/
	display(mid9win, 1, 42, "%3d",(int)getuw(buf, 7));	/* Last Millisecond */
	(void)wprintw(debugwin, "THR 0x09=");
    	break;

    case 0x0b:		/* Command Acknowledgement */
	(void)wprintw(debugwin, "ACK 0x0b=");
    	break;

    case 0x0c:		/* Command NAcknowledgement */
	(void)wprintw(debugwin, "NAK 0x0c=");
    	break;

    case 0x0d:		/* Visible List */
	display(mid13win, 1, 6, "%d",getub(buf, 1));
	(void)wmove(mid13win, 1, 10);
	for (i = 0; i < SIRF_CHANNELS; i++) {
	    if (i < (int)getub(buf, 1))
		(void)wprintw(mid13win, " %2d",getub(buf, 2 + 5 * i));
	    else
		(void)wprintw(mid13win, "   ");

	}
	(void)wprintw(mid13win, "\n");
	(void)wprintw(debugwin, "VL  0x0d=");
    	break;

    case 0x13:
#define YESNO(n)	(((int)getub(buf, n) != 0)?'Y':'N')
	display(mid19win, 1, 20, "%d", getub(buf, 5));	/* Alt. hold mode */
	display(mid19win, 2, 20, "%d", getub(buf, 6));	/* Alt. hold source*/
	display(mid19win, 3, 20, "%dm", (int)getuw(buf, 7));	/* Alt. source input */
	display(mid19win, 4, 20, "%d", getub(buf, 9));	/* Degraded mode*/
	display(mid19win, 5, 20, "%dsec", getub(buf, 10));	/* Degraded timeout*/
	display(mid19win, 6, 20, "%dsec",getub(buf, 11));	/* DR timeout*/
	display(mid19win, 7, 20, "%c", YESNO(12));/* Track smooth mode*/
	display(mid19win, 8, 20, "%c", YESNO(13)); /* Static Nav.*/
	display(mid19win, 9, 20, "0x%x", getub(buf, 14));	/* 3SV Least Squares*/
	display(mid19win, 10,20, "0x%x", getub(buf, 19));	/* DOP Mask mode*/
	display(mid19win, 11,20, "0x%x", (int)getuw(buf, 20));	/* Nav. Elev. mask*/
	display(mid19win, 12,20, "0x%x", getub(buf, 22));	/* Nav. Power mask*/
	display(mid19win, 13,20, "0x%x", getub(buf, 27));	/* DGPS Source*/
	display(mid19win, 14,20, "0x%x", getub(buf, 28));	/* DGPS Mode*/
	display(mid19win, 15,20, "%dsec",getub(buf, 29));	/* DGPS Timeout*/
	display(mid19win, 1, 42, "%c", YESNO(34));/* LP Push-to-Fix */
	display(mid19win, 2, 42, "%dms", getul(buf, 35));	/* LP On Time */
	display(mid19win, 3, 42, "%d", getul(buf, 39));	/* LP Interval */
	display(mid19win, 4, 42, "%c", YESNO(43));/* User Tasks enabled */
	display(mid19win, 5, 42, "%d", getul(buf, 44));	/* User Task Interval */
	display(mid19win, 6, 42, "%c", YESNO(48));/* LP Power Cycling Enabled */
	display(mid19win, 7, 42, "%d", getul(buf, 49));/* LP Max Acq Search Time */
	display(mid19win, 8, 42, "%d", getul(buf, 53));/* LP Max Off Time */
	display(mid19win, 9, 42, "%c", YESNO(57));/* APM Enabled */
	display(mid19win,10, 42, "%d", (int)getuw(buf, 58));/* # of fixes */
	display(mid19win,11, 42, "%d", (int)getuw(buf, 60));/* Time Between fixes */
	display(mid19win,12, 42, "%d", getub(buf, 62));/* H/V Error Max */
	display(mid19win,13, 42, "%d", getub(buf, 63));/* Response Time Max */
	display(mid19win,14, 42, "%d", getub(buf, 64));/* Time/Accu & Duty Cycle Priority */
#undef YESNO
	dispmode = !dispmode;
	break;

    case 0x1b:
	/******************************************************************
	 Not actually documented in any published materials.
	 Here is what Chris Kuethe got from the SiRF folks:

	Start of message
	----------------
	Message ID          1 byte    27
	Correction Source   1 byte    0=None, 1=SBAS, 2=Serial, 3=Beacon,
	4=Software

	total:              2 bytes

	Middle part of message varies if using beacon or other:
	-------------------------------------------------------
	If Beacon:
	Receiver Freq Hz    4 bytes
	Bit rate BPS        1 byte
	Status bit map      1 byte    01=Signal Valid,
				     02=Auto frequency detect
				     04=Auto bit rate detect
	Signal Magnitude    4 bytes   Note: in internal units
	Signal Strength dB  2 bytes   derived from Signal Magnitude
	SNR  dB             2 bytes

	total:             14 bytes

	If Not Beacon:
	Correction Age[12]  1 byte x 12  Age in seconds in same order as follows
	Reserved            2 bytes

	total:             14 bytes

	End of Message
	--------------
	Repeated 12 times (pad with 0 if less than 12 SV corrections):
	SVID                1 byte
	Correction (m)      1 byte

	total               2 x 12 = 24 bytes
	******************************************************************/
	display(mid27win, 1, 14, "%d (%s)", getub(buf, 1), sbasvec[(int)getub(buf, 1)]);
	for (i = j = 0; i < 12; i++) {
	    if (/*@i1@*/getub(buf, 16+2*i) != '\0') {
		(void)wprintw(mid27win, "%d=%d ", getub(buf, 16+2*i), getub(buf, 16+2*i+1));
		j++;
	    }
	}
	display(mid27win, 1, 44, "%d", j);
	(void)wprintw(debugwin, "DST 0x1b=");
	break;

    case 0x1C:	/* NL Measurement Data */
    case 0x1D:	/* DGPS Data */
    case 0x1E:	/* SV State Data */
    case 0x1F:	/* NL Initialized Data */
	subframe_enabled = true;
	break;

#ifdef __UNUSED__
    case 0x62:
	attrset(A_BOLD);
	move(2,40);
	printw("%9.5f %9.5f",(double)(RAD2DEG*1e8*getsl(buf, 1)),
			     (double)(RAD2DEG*1e8*getsl(buf, 5)));
	move(2,63);
	printw("%8d",getsl(buf, 9)/1000);

	move(3,63);

	printw("%8.1f",(double)getsl(buf, 17)/1000);

	move(4,54);
	if (getul(buf, 13) > 50) {
	    double heading = RAD2DEG*1e8*getsl(buf, 21);
	    if (heading < 0)
		heading += 360;
	    printw("%5.1f",heading);
	} else
	    printw("  0.0");

	move(4,63);
	printw("%8.1f",(double)getsl(buf, 13)/1000);
	attrset(A_NORMAL);

	move(5,13);
	printw("%04d-%02d-%02d %02d:%02d:%02d.%02d",
		(int)getuw(buf, 26),getub(buf, 28),getub(buf, 29),getub(buf, 30),getub(buf, 31),
		(unsigned short)getuw(buf, 32)/1000,
		((unsigned short)getuw(buf, 32)%1000)/10);
	{
	    struct timeval clk,gps;
	    struct tm tm;

	    gettimeofday(&clk,NULL);

	    memset(&tm,0,sizeof(tm));
	    tm.tm_sec = (unsigned short)getuw(buf, 32)/1000;
	    tm.tm_min = (int)getub(buf, 31);
	    tm.tm_hour = (int)getub(buf, 30);
	    tm.tm_mday = (int)getub(buf, 29);
	    tm.tm_mon = (int)getub(buf, 28) - 1;
	    tm.tm_year = (int)getuw(buf, 26) - 1900;

	    gps.tv_sec = mkgmtime(&tm);
	    gps.tv_usec = (((unsigned short)getuw(buf, 32)%1000)/10) * 10000;

	    move(5,2);
	    printw("           ");
	    move(5,2);
#if 1	    
	    printw("%ld",(gps.tv_usec - clk.tv_usec) +
	    		 ((gps.tv_sec - clk.tv_sec) % 3600) * 1000000);
#else
	    printw("%ld %ld %ld %ld",gps.tv_sec % 3600,gps.tv_usec,
	    			     clk.tv_sec % 3600,clk.tv_usec);
#endif
	}
		(void)wprintw(debugwin, "??? 0x62=");
    	break;
#endif /* __UNUSED__ */

    case 0xff:		/* Development Data */
	/*@ +ignoresigns @*/
	while (len > 0 && buf[len-1] == '\n')
	    len--;
	while (len > 0 && buf[len-1] == ' ')
	    len--;
	/*@ -ignoresigns @*/
	buf[len] = '\0';
	j = 1;
	for (i = 0; verbpat[i] != NULL; i++)
	    if (strncmp((char *)(buf+1),verbpat[i],strlen(verbpat[i])) == 0) {
		j = 0;
		break;
	    }
	if (j != 0)
	    (void)wprintw(debugwin, "%s\n",buf+1);
	(void)wprintw(debugwin, "DD  0xff=");
	break;

    default:
	(void)wprintw(debugwin, "    0x%02x=", buf[0]);
	break;
    }

    (void)wprintw(debugwin, "(%d) ", len);
    for (i = 1; i < len; i++)
	(void)wprintw(debugwin, "%02x",buf[i]);
    (void)wprintw(debugwin, "\n");
}

/*****************************************************************************
 *
 * Serial-line handling
 *
 *****************************************************************************/

static unsigned int get_speed(struct termios* ttyctl)
{
    speed_t code = cfgetospeed(ttyctl);
    switch (code) {
    case B0:     return(0);
    case B300:   return(300);
    case B1200:  return(1200);
    case B2400:  return(2400);
    case B4800:  return(4800);
    case B9600:  return(9600);
    case B19200: return(19200);
    case B38400: return(38400);
    case B57600: return(57600);
    default: return(115200);
    }
}

static int set_speed(unsigned int speed, unsigned int stopbits)
{
    unsigned int	rate, count, state;
    int st;
    unsigned char	c;

    (void)tcflush(devicefd, TCIOFLUSH);	/* toss stale data */

    if (speed != 0) {
	/*@ +ignoresigns @*/
	if (speed < 300)
	    rate = 0;
	else if (speed < 1200)
	    rate =  B300;
	else if (speed < 2400)
	    rate =  B1200;
	else if (speed < 4800)
	    rate =  B2400;
	else if (speed < 9600)
	    rate =  B4800;
	else if (speed < 19200)
	    rate =  B9600;
	else if (speed < 38400)
	    rate =  B19200;
	else if (speed < 57600)
	    rate =  B38400;
	else
	    rate =  B57600;
	/*@ -ignoresigns @*/

	/*@ ignore @*/
	(void)cfsetispeed(&ttyset, (speed_t)rate);
	(void)cfsetospeed(&ttyset, (speed_t)rate);
	/*@ end @*/
    }
    ttyset.c_cflag &=~ CSIZE;
    ttyset.c_cflag |= (CSIZE & (stopbits==2 ? CS7 : CS8));
    if (tcsetattr(devicefd, TCSANOW, &ttyset) != 0)
	return NO_PACKET;
    (void)tcflush(devicefd, TCIOFLUSH);

    (void)fprintf(stderr, "Hunting at speed %u, %dN%u\n",
	    get_speed(&ttyset), 9-stopbits, stopbits);

    /* sniff for NMEA or SiRF packet */
    state = 0;
    for (count = 0; count < SNIFF_RETRIES; count++) {
	if ((st = (int)read(devicefd, &c, 1)) < 0)
	    return 0;
	else
	    count += st;
	/*@ +charint @*/
	if (state == 0) {
	    if (c == START1)
		state = 1;
	    else if (c == '$')
		state = 2;
	} else if (state == 1) {
	    if (c == START2)
		return SIRF_PACKET;
	    else if (c == '$')
		state = 2;
	    else
		state = 0;
	} else if (state == 2) {
	    if (c == 'G')
		state = 3;
	    else if (c == START1)
		state = 1;
	    else
		state = 0;
	} else if (state == 3) {
	    if (c == 'P')
		return NMEA_PACKET;
	    else if (c == START1)
		state = 1;
	    else
		state = 0;
	}
	/*@ -charint @*/
    }
    
    return NO_PACKET;
}

static unsigned int *ip, rates[] = {0, 4800, 9600, 19200, 38400, 57600};

static unsigned int hunt_open(unsigned int *pstopbits)
{
    unsigned int trystopbits;
    int st;
    /*
     * Tip from Chris Kuethe: the FTDI chip used in the Trip-Nav
     * 200 (and possibly other USB GPSes) gets completely hosed
     * in the presence of flow control.  Thus, turn off CRTSCTS.
     */
    ttyset.c_cflag &= ~(PARENB | CRTSCTS);
    ttyset.c_cflag |= CREAD | CLOCAL;
    ttyset.c_iflag = ttyset.c_oflag = ttyset.c_lflag = (tcflag_t) 0;
    ttyset.c_oflag = (ONLCR);

    for (trystopbits = 1; trystopbits <= 2; trystopbits++) {
	*pstopbits = trystopbits;
	for (ip = rates; ip < rates + sizeof(rates)/sizeof(rates[0]); ip++) {
	    if ((st = set_speed(*ip, trystopbits)) == SIRF_PACKET)
		return get_speed(&ttyset);
	    else if (st == NMEA_PACKET) {
		(void)fprintf(stderr, "Switching to SiRF mode...\n");
		(void)nmea_send(controlfd,"$PSRF100,0,%d,8,1,0", *ip);
		return *ip;
	    }
	}
    }
    return 0;
}

static void serial_initialize(char *device)
{
    if ((controlfd = devicefd = open(device,O_RDWR)) < 0) {
	perror(device);
	exit(1);
    }

    /* Save original terminal parameters */
    if (tcgetattr(devicefd, &ttyset) != 0 || (bps = hunt_open(&stopbits))==0) {
	(void)fputs("Can't sync up with device!\n", stderr);
	exit(1);
    }
}


/******************************************************************************
 *
 * Device-independent I/O routines
 *
 ******************************************************************************/

static int readbyte(void)
{
    /*@ -type -shiftnegative -compdef -nullpass @*/
    static int cnt = 0,pos = 0;
    static unsigned char inbuf[BUFLEN];
    struct timeval timeval;

    if (pos >= cnt) {
	fd_set select_set;

	FD_ZERO(&select_set);
	FD_SET(devicefd,&select_set);
	if (controlfd < -1)
	    FD_SET(controlfd,&select_set);
	timeval.tv_sec = 0;
	timeval.tv_usec = 500000;
	if (select(devicefd + 1,&select_set,NULL,NULL,&timeval) < 0)
	    return EOF;

	if (!FD_ISSET(devicefd,&select_set))
	    return EOF;

	(void)usleep(100000);

	if ((cnt = (int)read(devicefd,inbuf,BUFLEN)) <= 0)
	    return EOF;

	pos = 0;
    }
    /*@ +type +shiftnegative +compdef +nullpass @*/

    return (int)inbuf[pos++];
}

static int readword(void)
{
    int byte1,byte2;

    if ((byte1 = readbyte()) == EOF || (byte2 = readbyte()) == EOF)
	return EOF;

    /*@i@*/return (byte1 << 8) | byte2;
}

/*@ -globstate @*/
static int readpkt(unsigned char *buf)
{
    int byte,len,csum,cnt;
    unsigned char *cp = buf;

    do {
	while ((byte = readbyte()) != START1)
	    if (byte == EOF)
		return EOF;
    } while ((byte = readbyte()) != START2);

    if ((len = readword()) == EOF || len > BUFLEN)
	return EOF;

    csum = 0;
    cnt = len;

    while (cnt-- > 0) {
	if ((byte = readbyte()) == EOF)
	    return EOF;
	*cp++ = (unsigned char)byte;
	csum += byte;
    }

    csum &= 0x7fff;

    if (readword() != csum)
	return EOF;

    if (readbyte() != END1 || readbyte() != END2)
	return EOF;

    if (logfile != NULL) {
	/*@ -shiftimplementation @*/
	(void)fwrite("\xa0\xa2", (size_t)2,  sizeof(char), logfile);
	(void)fputc(len >> 8, logfile);
	(void)fputc(len & 0xff, logfile);
	(void)fwrite(buf, (size_t)len, sizeof(char), logfile);
	(void)fputc(csum >> 8, logfile);
	(void)fputc(csum & 0xff, logfile);
	(void)fwrite("\xb0\xb3", (size_t)2,  sizeof(char), logfile);
	/*@ +shiftimplementation @*/
    }
    return len;
}
/*@ +globstate @*/

static bool sendpkt(unsigned char *buf, size_t len, char *device)
{
    unsigned int csum;
    ssize_t st;
    size_t i;

    putbyte(buf, -4, START1);			/* start of packet */
    putbyte(buf, -3, START2);
    putword(buf, -2, len);			/* length */

    csum = 0;
    for (i = 0; i < len; i++)
	csum += (int)buf[4 + i];

    csum &= 0x7fff;
    putword(buf, len, csum);			/* checksum */
    putbyte(buf, len + 2,END1);			/* end of packet */
    putbyte(buf, len + 3,END2);
    len += 8;

    (void)wprintw(debugwin, ">>>");
    for (i = 0; i < len; i++)
	(void)wprintw(debugwin, " %02x",buf[i]);
    (void)wprintw(debugwin, "\n");

    if (controlfd == -1) 
	return false;
    else {
	if (!serial) {
	    (void)write(controlfd, "!", 1);
	    (void)write(controlfd, device, strlen(device));
	    (void)write(controlfd, "=", 1);
	}
	st = write(controlfd, buf,len);
	if (!serial)
	    (void)read(controlfd, buf, 8);	/* enough room for "ERROR\r\n\0" */
	return ((size_t)st == len);
    }
}

/*****************************************************************************
 *
 * Main sequence and display machinery
 *
 *****************************************************************************/

static long tzoffset(void)
{
    time_t now = time(NULL);
    struct tm tm;
    long res = 0;

    tzset();
#ifdef HAVE_TIMEZONE
    res = timezone;
#else
    res = localtime_r(&now, &tm)->tm_gmtoff;
#endif
#ifdef HAVE_DAYLIGHT
    if (daylight != 0 && localtime_r(&now, &tm)->tm_isdst != 0)
	res -= 3600;
#else
    if (localtime_r(&now, &tm)->tm_isdst)
	res -= 3600;
#endif
    return res;
}

/*@ -nullpass -globstate @*/
static void refresh_rightpanel1(void)
{
    (void)touchwin(mid6win);
    (void)touchwin(mid7win);
    (void)touchwin(mid9win);
    (void)touchwin(mid13win);
    (void)touchwin(mid27win);
    (void)wrefresh(mid6win);
    (void)wrefresh(mid7win);
    (void)wrefresh(mid9win);
    (void)wrefresh(mid13win);
    (void)wrefresh(mid27win);
}
/*@ +nullpass +globstate @*/

static void command(char buf[], size_t len, const char *fmt, ... )
/* assemble command in printf(3) style, use stderr or syslog */
{
    va_list ap;
    ssize_t n;

    va_start(ap, fmt) ;
    (void)vsnprintf(buf, len, fmt, ap);
    va_end(ap);

    (void)write(devicefd, buf, strlen(buf));
    n = read(devicefd, buf, len);
    if (n >= 0) {
	buf[n] = '\0';
	while (isspace(buf[strlen(buf)-1]))
	    buf[strlen(buf)-1] = '\0';
    }
}

int main (int argc, char **argv)
{
    unsigned int i, v;
    int len, option;
    char *p, *arg = NULL, *colon1 = NULL, *colon2 = NULL, *slash = NULL;
    char *server=NULL, *port = DEFAULT_GPSD_PORT, *device = NULL;
    char *controlsock = "/var/run/gpsd.sock";
    fd_set select_set;
    unsigned char buf[BUFLEN];
    char line[80];

    gmt_offset = (int)tzoffset();

    /*@ -branchstate @*/
    while ((option = getopt(argc, argv, "F:vh")) != -1) {
	switch (option) {
	case 'F':
	    controlsock = optarg;
	    break;
	case 'v':
	    (void)printf("sirfmon %s\n", VERSION);
	    exit(0);
	case 'h': case '?': default:
	    (void)fputs("usage:  sirfmon [-?hv] [-F controlsock] [server[:port:[device]]]\n", stderr);
	    exit(1);
	}
    }
    /*@ +branchstate @*/
    /*@ -nullpass -branchstate @*/
    if (optind < argc) {
	arg = strdup(argv[optind]);
	colon1 = strchr(arg, ':');
	slash = strchr(arg, '/');
	server = arg;
	if (colon1 != NULL) {
	    if (colon1 == arg)
		server = NULL;
	    else
		*colon1 = '\0';
	    port = colon1 + 1;
	    colon2 = strchr(port, ':');
	    if (colon2 != NULL) {
		if (colon2 == port)
		    port = NULL;
	        else
		    *colon2 = '\0';
		device = colon2 + 1;
	    }
	}
    }
    /*@ +nullpass +branchstate @*/

    /*@ -boolops */
    if (!arg || (arg && !slash) || (arg && colon1 && slash)) {	
	if (!server)
	    server = "localhost";
	devicefd = netlib_connectsock(server, port, "tcp");
	if (devicefd < 0) {
	    (void)fprintf(stderr, 
			  "%s: connection failure on %s:%s, error %d.\n", 
			  argv[0], server, port, devicefd);
	    exit(1);
	}
	controlfd = open(controlsock, O_RDWR);
	/*@ -compdef @*/
	if (device)
	    command((char *)buf, sizeof(buf), "F=%s\r\n", device);
	else
	    command((char *)buf, sizeof(buf), "O\r\n");	/* force device allocation */
	command((char *)buf, sizeof(buf), "F\r\n");
	device = strdup((char *)buf+7);
	command((char *)buf, sizeof(buf), "R=2\r\n");
	/*@ +compdef @*/
	serial = false;
    } else {
	serial_initialize(device = arg);
	serial = true;
    }
    /*@ +boolops */

    (void)initscr();
    (void)cbreak();
    (void)noecho();
    (void)intrflush(stdscr, FALSE);
    (void)keypad(stdscr, true);

    /*@ -onlytrans @*/
    mid2win   = newwin(7,  80,  0, 0);
    mid4win   = newwin(15, 30,  7, 0);
    mid6win   = newwin(3,  50,  7, 30);
    mid7win   = newwin(4,  50, 10, 30);
    mid9win   = newwin(3,  50, 14, 30);
    mid13win  = newwin(3,  50, 17, 30);
    mid19win  = newwin(17, 50,  7, 30);
    mid27win  = newwin(4,  50, 20, 30);
    cmdwin    = newwin(2,  30, 22, 0);
    debugwin  = newwin(0,   0, 24, 0);
    (void)scrollok(debugwin, true);
    (void)wsetscrreg(debugwin, 0, LINES-21);
    /*@ +onlytrans @*/

    /*@ -nullpass @*/
    (void)wborder(mid2win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid2win, A_BOLD);
    (void)wmove(mid2win, 0,1);
    display(mid2win, 0, 12, " X "); 
    display(mid2win, 0, 21, " Y "); 
    display(mid2win, 0, 30, " Z "); 
    display(mid2win, 0, 43, " North "); 
    display(mid2win, 0, 54, " East "); 
    display(mid2win, 0, 65, " Alt "); 

    (void)wmove(mid2win, 1,1);
    (void)wprintw(mid2win, "Pos:                            m                                    m");
    (void)wmove(mid2win, 2,1);
    (void)wprintw(mid2win, "Vel:                            m/s                                  climb m/s");
    (void)wmove(mid2win, 3,1);
    (void)wprintw(mid2win, "Time:                  UTC:                Heading:                  speed m/s");
    (void)wmove(mid2win, 4,1);
    (void)wprintw(mid2win, "Skew:                   TZ:                HDOP:      M1:        M2:    ");
    (void)wmove(mid2win, 5,1);
    (void)wprintw(mid2win, "Fix:");
    display(mid2win, 6, 24, " Packet type 2 (0x02) ");
    (void)wattrset(mid2win, A_NORMAL);

    (void)wborder(mid4win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid4win, A_BOLD);
    display(mid4win, 1, 1, " Ch SV  Az El Stat  C/N ? A");
    for (i = 0; i < SIRF_CHANNELS; i++) {
	display(mid4win, (int)(i+2), 1, "%2d",i);
    }
    display(mid4win, 14, 4, " Packet Type 4 (0x04) ");
    (void)wattrset(mid4win, A_NORMAL);

    (void)wborder(mid19win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid19win, A_BOLD);
    display(mid19win, 1, 1, "Alt. hold mode:");
    display(mid19win, 2, 1, "Alt. hold source:");
    display(mid19win, 3, 1, "Alt. source input:");
    display(mid19win, 4, 1, "Degraded mode:");
    display(mid19win, 5, 1, "Degraded timeout:");
    display(mid19win, 6, 1, "DR timeout:");
    display(mid19win, 7, 1, "Track smooth mode:");
    display(mid19win, 8, 1, "Static Navigation:");
    display(mid19win, 9, 1, "3SV Least Squares:");
    display(mid19win, 10,1, "DOP Mask mode:");
    display(mid19win, 11,1, "Nav. Elev. mask:");
    display(mid19win, 12,1, "Nav. Power mask:");
    display(mid19win, 13,1, "DGPS Source:");
    display(mid19win, 14,1, "DGPS Mode:");
    display(mid19win, 15,1, "DGPS Timeout:");
    display(mid19win, 1, 26,"LP Push-to-Fix:");
    display(mid19win, 2, 26,"LP On Time:");
    display(mid19win, 3, 26,"LP Interval:");
    display(mid19win, 4, 26,"U. Tasks Enab.:");
    display(mid19win, 5, 26,"U. Task Inter.:");
    display(mid19win, 6, 26,"LP Pwr Cyc En:");
    display(mid19win, 7, 26,"LP Max Acq Srch:");
    display(mid19win, 8, 26,"LP Max Off Time:");
    display(mid19win, 9, 26,"APM enabled:");
    display(mid19win,10, 26,"# of Fixes:");
    display(mid19win,11, 26,"Time btw Fixes:");
    display(mid19win,12, 26,"H/V Error Max:");
    display(mid19win,13, 26,"Rsp Time Max:");
    display(mid19win,14, 26,"Time/Accu:");

    display(mid19win, 16, 8, " Packet type 19 (0x13) ");
    (void)wattrset(mid19win, A_NORMAL);

    (void)wborder(mid6win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid6win, A_BOLD);
    display(mid6win, 1, 1, "Version:");
    display(mid6win, 2, 8, " Packet Type 6 (0x06) ");
    (void)wattrset(mid6win, A_NORMAL);

    (void)wborder(mid7win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid7win, A_BOLD);
    display(mid7win, 1, 1,  "SVs: ");
    display(mid7win, 1, 9,  "Drift: ");
    display(mid7win, 1, 23, "Bias: ");
    display(mid7win, 2, 1,  "Estimated GPS Time: ");
    display(mid7win, 3, 8, " Packet type 7 (0x07) ");
    (void)wattrset(mid7win, A_NORMAL);

    (void)wborder(mid9win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid9win, A_BOLD);
    display(mid9win, 1, 1,  "Max: ");
    display(mid9win, 1, 13, "Lat: ");
    display(mid9win, 1, 25, "Time: ");
    display(mid9win, 1, 39, "MS: ");
    display(mid9win, 2, 8, " Packet type 9 (0x09) ");
    (void)wattrset(mid9win, A_NORMAL);

    (void)wborder(mid13win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid13win, A_BOLD);
    display(mid13win, 1, 1, "SVs: ");
    display(mid13win, 1, 9, "=");
    display(mid13win, 2, 8, " Packet type 13 (0x0D) ");
    (void)wattrset(mid13win, A_NORMAL);

    (void)wborder(mid27win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid27win, A_BOLD);
    display(mid27win, 1, 1, "SBAS source: ");
    display(mid27win, 1, 31, "Corrections: ");
    display(mid27win, 3, 8, " Packet type 27 (0x1B) ");
    (void)wattrset(mid27win, A_NORMAL);

    (void)wattrset(cmdwin, A_BOLD);
    if (serial)
    	display(cmdwin, 1, 0, "%s %4d N %d", device, bps, stopbits);
    else
	display(cmdwin, 1, 0, "%s:%s:%s", server, port, device);
    (void)wattrset(cmdwin, A_NORMAL);

    (void)wmove(debugwin,0, 0);

    FD_ZERO(&select_set);

    /* probe for version */
    putbyte(buf, 0, 0x84);
    putbyte(buf, 1, 0x0);
    /*@ -compdef @*/
    (void)sendpkt(buf, 2, device);
    /*@ +compdef @*/

    for (;;) {
	(void)wmove(cmdwin, 0,0);
	(void)wprintw(cmdwin, "cmd> ");
	(void)wclrtoeol(cmdwin);
	(void)refresh();
	(void)wrefresh(mid2win);
	(void)wrefresh(mid4win);
	if (dispmode == 0) {
	    refresh_rightpanel1();
	} else {
	    (void)touchwin(mid19win);
	    (void)wrefresh(mid19win);
	}
	(void)wrefresh(debugwin);
	(void)wrefresh(cmdwin);

	FD_SET(0,&select_set);
	FD_SET(devicefd,&select_set);

	if (select(FD_SETSIZE, &select_set, NULL, NULL, NULL) < 0)
	    break;

	if (FD_ISSET(0,&select_set)) {
	    (void)wmove(cmdwin, 0,5);
	    (void)wrefresh(cmdwin);
	    (void)echo();
	    /*@ -usedef -compdef @*/
	    (void)wgetnstr(cmdwin, line, 80);
	    (void)noecho();
	    //(void)move(0,0);
	    //(void)clrtoeol();
	    //(void)refresh();
	    (void)wrefresh(mid2win);
	    (void)wrefresh(mid4win);
	    if (dispmode == 0) {
		refresh_rightpanel1();
	    } else {
		(void)touchwin(mid19win);
		(void)wrefresh(mid19win);
	    }
	    (void)wrefresh(mid19win);
	    (void)wrefresh(debugwin);
	    (void)wrefresh(cmdwin);

	    if ((p = strchr(line,'\r')) != NULL)
		*p = '\0';

	    if (line[0] == '\0')
		continue;
	    /*@ +usedef +compdef @*/

	    p = line;

	    while (*p != '\0' && !isspace(*p))
		p++;
	    while (*p != '\0' && isspace(*p))
		p++;

	    switch (line[0])
	    {
	    case 'a':		/* toggle 50bps subframe data */
		(void)memset(buf, '\0', sizeof(buf));
		putbyte(buf, 0, 0x80);
		putbyte(buf, 23, 12);
		putbyte(buf, 24, subframe_enabled ? 0x00 : 0x10);
		(void)sendpkt(buf, 25, device);
		break;

	    case 'b':
		if (serial) {
		    v = (unsigned)atoi(line+1);
		    for (ip=rates; ip<rates+sizeof(rates)/sizeof(rates[0]);ip++)
			if (v == *ip)
			    goto goodspeed;
		    break;
		goodspeed:
		    putbyte(buf, 0, 0x86);
		    putlong(buf, 1, v);		/* new baud rate */
		    putbyte(buf, 5, 8);		/* 8 data bits */
		    putbyte(buf, 6, stopbits);	/* 1 stop bit */
		    putbyte(buf, 7, 0);		/* no parity */
		    putbyte(buf, 8, 0);		/* reserved */
		    (void)sendpkt(buf, 9, device);
		    (void)usleep(50000);
		    (void)set_speed(bps = v, stopbits);
		    display(cmdwin, 1, 0, "%s %d N %d", device,bps,stopbits);
		} else {
		    line[0] = 'b';
		    (void)write(devicefd, line, strlen(line));
		    (void)read(devicefd, buf, sizeof(buf));	/* discard response */
		}
		break;

	    case 'c':				/* static navigation */
		putbyte(buf, 0,0x8f);			/* id */
		putbyte(buf, 1, atoi(line+1));
		(void)sendpkt(buf, 2, device);
		break;

	    case 'd':		/* MID 4 rate change -- not documented */
		v = (unsigned)atoi(line+1);
		if (v > 30)
		    break;
		putbyte(buf, 0,0xa6);
		putbyte(buf, 1,0);
		putbyte(buf, 2, 4);	/* satellite picture */
		putbyte(buf, 3, v);
		putbyte(buf, 4, 0);
		putbyte(buf, 5, 0);
		putbyte(buf, 6, 0);
		putbyte(buf, 7, 0);
		(void)sendpkt(buf, 8, device);
		break;

	    case 'l':				/* open logfile */
		if (logfile != NULL) {
		    (void)wprintw(debugwin, ">>> Logging to %s off", logfile);
		    (void)fclose(logfile);
		}

		logfile = fopen(line+1,"a");
		(void)wprintw(debugwin, ">>> Logging to %s on", logfile);
		break;

	    case 'n':				/* switch to NMEA */
		putbyte(buf, 0,0x81);			/* id */
		putbyte(buf, 1,0x02);			/* mode */
		putbyte(buf, 2,0x01);			/* GGA */
		putbyte(buf, 3,0x01);
		putbyte(buf, 4,0x01);			/* GLL */
		putbyte(buf, 5,0x01);
		putbyte(buf, 6,0x01);		  	/* GSA */
		putbyte(buf, 7,0x01);
		putbyte(buf, 8,0x05);			/* GSV */
		putbyte(buf, 9,0x01);
		putbyte(buf, 10,0x01);			/* RNC */
		putbyte(buf, 11,0x01);
		putbyte(buf, 12,0x01);			/* VTG */
		putbyte(buf, 13,0x01);
		putbyte(buf, 14,0x00);			/* unused fields */
		putbyte(buf, 15,0x01);
		putbyte(buf, 16,0x00);
		putbyte(buf, 17,0x01);
		putbyte(buf, 18,0x00);
		putbyte(buf, 19,0x01);
		putbyte(buf, 20,0x00);
		putbyte(buf, 21,0x01);
		putword(buf, 22,bps);
		(void)sendpkt(buf, 24, device);
		goto quit;

	    case 't':				/* poll navigation params */
		putbyte(buf, 0,0x98);
		putbyte(buf, 1,0x00);
		(void)sendpkt(buf, 2, device);
		break;

	    case 'q':
		goto quit;

	    case 's':
		len = 0;
		while (*p != '\0')
		{
		    /*@i1@*/(void)sscanf(p,"%x",&v);
		    putbyte(buf, len,v);
		    len++;
		    while (*p != '\0' && !isspace(*p))
			p++;
		    while (*p != '\0' && isspace(*p))
			p++;
		}

		(void)sendpkt(buf, (size_t)len, device);
		break;
	    }
	}

	if ((len = readpkt(buf)) != EOF) {
	    decode_sirf(buf,len);
	}
    }
    /*@ +nullpass @*/

 quit:
    if (logfile)
	(void)fclose(logfile);
    (void)endwin();
    exit(0);
}

/* sirfmon.c ends here */