/*      $OpenBSD: tty_nmea.c,v 1.51 2022/04/02 22:45:18 mlarkin Exp $ */

/*
 * Copyright (c) 2006, 2007, 2008 Marc Balmer <mbalmer@openbsd.org>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

/*
 * A tty line discipline to decode NMEA 0183 data to get the time
 * and GPS position data
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/malloc.h>
#include <sys/sensors.h>
#include <sys/tty.h>
#include <sys/conf.h>
#include <sys/time.h>

#ifdef NMEA_DEBUG
#define DPRINTFN(n, x)  do { if (nmeadebug > (n)) printf x; } while (0)
int nmeadebug = 0;
#else
#define DPRINTFN(n, x)
#endif
#define DPRINTF(x)      DPRINTFN(0, x)

void    nmeaattach(int);

#define NMEAMAX         82
#define MAXFLDS         32
#define KNOTTOMS        (51444 / 100)
#ifdef NMEA_DEBUG
#define TRUSTTIME       30
#else
#define TRUSTTIME       (10 * 60)       /* 10 minutes */
#endif

int nmea_count, nmea_nxid;

struct nmea {
        char                    cbuf[NMEAMAX];  /* receive buffer */
        struct ksensor          time;           /* the timedelta sensor */
        struct ksensor          signal;         /* signal status */
        struct ksensor          latitude;
        struct ksensor          longitude;
        struct ksensor          altitude;
        struct ksensor          speed;
        struct ksensordev       timedev;
        struct timespec         ts;             /* current timestamp */
        struct timespec         lts;            /* timestamp of last '$' seen */
        struct timeout          nmea_tout;      /* invalidate sensor */
        int64_t                 gap;            /* gap between two sentences */
#ifdef NMEA_DEBUG
        int                     gapno;
#endif
        int64_t                 last;           /* last time rcvd */
        int                     sync;           /* if 1, waiting for '$' */
        int                     pos;            /* position in rcv buffer */
        int                     no_pps;         /* no PPS although requested */
        char                    mode;           /* GPS mode */
};

/* NMEA decoding */
void    nmea_scan(struct nmea *, struct tty *);
void    nmea_gprmc(struct nmea *, struct tty *, char *fld[], int fldcnt);
void    nmea_decode_gga(struct nmea *, struct tty *, char *fld[], int fldcnt);

/* date and time conversion */
int     nmea_date_to_nano(char *s, int64_t *nano);
int     nmea_time_to_nano(char *s, int64_t *nano);

/* longitude and latitude conversion */
int     nmea_degrees(int64_t *dst, char *src, int neg);
int     nmea_atoi(int64_t *dst, char *src);

/* degrade the timedelta sensor */
void    nmea_timeout(void *);

void
nmeaattach(int dummy)
{
        /* noop */
}

int
nmeaopen(dev_t dev, struct tty *tp, struct proc *p)
{
        struct nmea *np;
        int error;

        if (tp->t_line == NMEADISC)
                return (ENODEV);
        if ((error = suser(p)) != 0)
                return (error);
        np = malloc(sizeof(struct nmea), M_DEVBUF, M_WAITOK | M_ZERO);
        snprintf(np->timedev.xname, sizeof(np->timedev.xname), "nmea%d",
            nmea_nxid++);
        nmea_count++;
        np->time.status = SENSOR_S_UNKNOWN;
        np->time.type = SENSOR_TIMEDELTA;
        np->time.flags = SENSOR_FINVALID;
        sensor_attach(&np->timedev, &np->time);

        np->signal.type = SENSOR_INDICATOR;
        np->signal.status = SENSOR_S_UNKNOWN;
        np->signal.value = 0;
        strlcpy(np->signal.desc, "Signal", sizeof(np->signal.desc));
        sensor_attach(&np->timedev, &np->signal);

        np->latitude.type = SENSOR_ANGLE;
        np->latitude.status = SENSOR_S_UNKNOWN;
        np->latitude.flags = SENSOR_FINVALID;
        np->latitude.value = 0;
        strlcpy(np->latitude.desc, "Latitude", sizeof(np->latitude.desc));
        sensor_attach(&np->timedev, &np->latitude);

        np->longitude.type = SENSOR_ANGLE;
        np->longitude.status = SENSOR_S_UNKNOWN;
        np->longitude.flags = SENSOR_FINVALID;
        np->longitude.value = 0;
        strlcpy(np->longitude.desc, "Longitude", sizeof(np->longitude.desc));
        sensor_attach(&np->timedev, &np->longitude);

        np->altitude.type = SENSOR_DISTANCE;
        np->altitude.status = SENSOR_S_UNKNOWN;
        np->altitude.flags = SENSOR_FINVALID;
        np->altitude.value = 0;
        strlcpy(np->altitude.desc, "Altitude", sizeof(np->altitude.desc));
        sensor_attach(&np->timedev, &np->altitude);

        np->speed.type = SENSOR_VELOCITY;
        np->speed.status = SENSOR_S_UNKNOWN;
        np->speed.flags = SENSOR_FINVALID;
        np->speed.value = 0;
        strlcpy(np->speed.desc, "Ground speed", sizeof(np->speed.desc));
        sensor_attach(&np->timedev, &np->speed);

        np->sync = 1;
        tp->t_sc = (caddr_t)np;

        error = linesw[TTYDISC].l_open(dev, tp, p);
        if (error) {
                free(np, M_DEVBUF, sizeof(*np));
                tp->t_sc = NULL;
        } else {
                sensordev_install(&np->timedev);
                timeout_set(&np->nmea_tout, nmea_timeout, np);
        }
        return (error);
}

int
nmeaclose(struct tty *tp, int flags, struct proc *p)
{
        struct nmea *np = (struct nmea *)tp->t_sc;

        tp->t_line = TTYDISC;   /* switch back to termios */
        timeout_del(&np->nmea_tout);
        sensordev_deinstall(&np->timedev);
        free(np, M_DEVBUF, sizeof(*np));
        tp->t_sc = NULL;
        nmea_count--;
        if (nmea_count == 0)
                nmea_nxid = 0;
        return (linesw[TTYDISC].l_close(tp, flags, p));
}

/* Collect NMEA sentences from the tty. */
int
nmeainput(int c, struct tty *tp)
{
        struct nmea *np = (struct nmea *)tp->t_sc;
        struct timespec ts;
        int64_t gap;
        long tmin, tmax;

        switch (c) {
        case '$':
                nanotime(&ts);
                np->pos = np->sync = 0;
                gap = (ts.tv_sec * 1000000000LL + ts.tv_nsec) -
                    (np->lts.tv_sec * 1000000000LL + np->lts.tv_nsec);

                np->lts.tv_sec = ts.tv_sec;
                np->lts.tv_nsec = ts.tv_nsec;

                if (gap <= np->gap)
                        break;

                np->ts.tv_sec = ts.tv_sec;
                np->ts.tv_nsec = ts.tv_nsec;

#ifdef NMEA_DEBUG
                if (nmeadebug > 0) {
                        linesw[TTYDISC].l_rint('[', tp);
                        linesw[TTYDISC].l_rint('0' + np->gapno++, tp);
                        linesw[TTYDISC].l_rint(']', tp);
                }
#endif
                np->gap = gap;

                /*
                 * If a tty timestamp is available, make sure its value is
                 * reasonable by comparing against the timestamp just taken.
                 * If they differ by more than 2 seconds, assume no PPS signal
                 * is present, note the fact, and keep using the timestamp
                 * value.  When this happens, the sensor state is set to
                 * CRITICAL later when the GPRMC sentence is decoded.
                 */
                if (tp->t_flags & (TS_TSTAMPDCDSET | TS_TSTAMPDCDCLR |
                    TS_TSTAMPCTSSET | TS_TSTAMPCTSCLR)) {
                        tmax = lmax(np->ts.tv_sec, tp->t_tv.tv_sec);
                        tmin = lmin(np->ts.tv_sec, tp->t_tv.tv_sec);
                        if (tmax - tmin > 1)
                                np->no_pps = 1;
                        else {
                                np->ts.tv_sec = tp->t_tv.tv_sec;
                                np->ts.tv_nsec = tp->t_tv.tv_usec *
                                    1000L;
                                np->no_pps = 0;
                        }
                }
                break;
        case '\r':
        case '\n':
                if (!np->sync) {
                        np->cbuf[np->pos] = '\0';
                        nmea_scan(np, tp);
                        np->sync = 1;
                }
                break;
        default:
                if (!np->sync && np->pos < (NMEAMAX - 1))
                        np->cbuf[np->pos++] = c;
                break;
        }
        /* pass data to termios */
        return (linesw[TTYDISC].l_rint(c, tp));
}

/* Scan the NMEA sentence just received. */
void
nmea_scan(struct nmea *np, struct tty *tp)
{
        int fldcnt = 0, cksum = 0, msgcksum, n;
        char *fld[MAXFLDS], *cs;

        /* split into fields and calculate the checksum */
        fld[fldcnt++] = &np->cbuf[0];   /* message type */
        for (cs = NULL, n = 0; n < np->pos && cs == NULL; n++) {
                switch (np->cbuf[n]) {
                case '*':
                        np->cbuf[n] = '\0';
                        cs = &np->cbuf[n + 1];
                        break;
                case ',':
                        if (fldcnt < MAXFLDS) {
                                cksum ^= np->cbuf[n];
                                np->cbuf[n] = '\0';
                                fld[fldcnt++] = &np->cbuf[n + 1];
                        } else {
                                DPRINTF(("nr of fields in %s sentence exceeds "
                                    "maximum of %d\n", fld[0], MAXFLDS));
                                return;
                        }
                        break;
                default:
                        cksum ^= np->cbuf[n];
                }
        }

        /*
         * we only look at the messages coming from well-known sources or 'talkers',
         * distinguished by the two-chars prefix, the most common being:
         * GPS (GP)
         * Glonass (GL)
         * BeiDou (BD)
         * Galileo (GA)
         * 'Any kind/a mix of GNSS systems' (GN)
         */
        if (strncmp(fld[0], "BD", 2) &&
            strncmp(fld[0], "GA", 2) &&
            strncmp(fld[0], "GL", 2) &&
            strncmp(fld[0], "GN", 2) &&
            strncmp(fld[0], "GP", 2))
                return;

        /* we look for the RMC & GGA messages */
        if (strncmp(fld[0] + 2, "RMC", 3) &&
            strncmp(fld[0] + 2, "GGA", 3))
                return;

        /* if we have a checksum, verify it */
        if (cs != NULL) {
                msgcksum = 0;
                while (*cs) {
                        if ((*cs >= '0' && *cs <= '9') ||
                            (*cs >= 'A' && *cs <= 'F')) {
                                if (msgcksum)
                                        msgcksum <<= 4;
                                if (*cs >= '0' && *cs<= '9')
                                        msgcksum += *cs - '0';
                                else if (*cs >= 'A' && *cs <= 'F')
                                        msgcksum += 10 + *cs - 'A';
                                cs++;
                        } else {
                                DPRINTF(("bad char %c in checksum\n", *cs));
                                return;
                        }
                }
                if (msgcksum != cksum) {
                        DPRINTF(("checksum mismatch\n"));
                        return;
                }
        }
        if (strncmp(fld[0] + 2, "RMC", 3) == 0)
                nmea_gprmc(np, tp, fld, fldcnt);
        if (strncmp(fld[0] + 2, "GGA", 3) == 0)
                nmea_decode_gga(np, tp, fld, fldcnt);
}

/* Decode the recommended minimum specific GPS/TRANSIT data. */
void
nmea_gprmc(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
{
        int64_t date_nano, time_nano, nmea_now;
        int jumped = 0;

        if (fldcnt < 12 || fldcnt > 14) {
                DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt));
                return;
        }
        if (nmea_time_to_nano(fld[1], &time_nano)) {
                DPRINTF(("gprmc: illegal time, %s\n", fld[1]));
                return;
        }
        if (nmea_date_to_nano(fld[9], &date_nano)) {
                DPRINTF(("gprmc: illegal date, %s\n", fld[9]));
                return;
        }
        nmea_now = date_nano + time_nano;
        if (nmea_now <= np->last) {
                DPRINTF(("gprmc: time not monotonically increasing\n"));
                jumped = 1;
        }
        np->last = nmea_now;
        np->gap = 0LL;
#ifdef NMEA_DEBUG
        if (np->time.status == SENSOR_S_UNKNOWN) {
                np->time.status = SENSOR_S_OK;
                timeout_add_sec(&np->nmea_tout, TRUSTTIME);
        }
        np->gapno = 0;
        if (nmeadebug > 0) {
                linesw[TTYDISC].l_rint('[', tp);
                linesw[TTYDISC].l_rint('C', tp);
                linesw[TTYDISC].l_rint(']', tp);
        }
#endif

        np->time.value = np->ts.tv_sec * 1000000000LL +
            np->ts.tv_nsec - nmea_now;
        np->time.tv.tv_sec = np->ts.tv_sec;
        np->time.tv.tv_usec = np->ts.tv_nsec / 1000L;

        if (fldcnt < 13)
                strlcpy(np->time.desc, "GPS", sizeof(np->time.desc));
        else if (*fld[12] != np->mode) {
                np->mode = *fld[12];
                switch (np->mode) {
                case 'S':
                        strlcpy(np->time.desc, "GPS simulated",
                            sizeof(np->time.desc));
                        break;
                case 'E':
                        strlcpy(np->time.desc, "GPS estimated",
                            sizeof(np->time.desc));
                        break;
                case 'A':
                        strlcpy(np->time.desc, "GPS autonomous",
                            sizeof(np->time.desc));
                        break;
                case 'D':
                        strlcpy(np->time.desc, "GPS differential",
                            sizeof(np->time.desc));
                        break;
                case 'N':
                        strlcpy(np->time.desc, "GPS invalid",
                            sizeof(np->time.desc));
                        break;
                default:
                        strlcpy(np->time.desc, "GPS unknown",
                            sizeof(np->time.desc));
                        DPRINTF(("gprmc: unknown mode '%c'\n", np->mode));
                }
        }
        switch (*fld[2]) {
        case 'A':       /* The GPS has a fix, (re)arm the timeout. */
                        /* XXX is 'D' also a valid state? */
                np->time.status = SENSOR_S_OK;
                np->signal.value = 1;
                np->signal.status = SENSOR_S_OK;
                np->latitude.status = SENSOR_S_OK;
                np->longitude.status = SENSOR_S_OK;
                np->speed.status = SENSOR_S_OK;
                np->time.flags &= ~SENSOR_FINVALID;
                np->latitude.flags &= ~SENSOR_FINVALID;
                np->longitude.flags &= ~SENSOR_FINVALID;
                np->speed.flags &= ~SENSOR_FINVALID;
                break;
        case 'V':       /*
                         * The GPS indicates a warning status, do not add to
                         * the timeout, if the condition persist, the sensor
                         * will be degraded.  Signal the condition through
                         * the signal sensor.
                         */
                np->signal.value = 0;
                np->signal.status = SENSOR_S_CRIT;
                np->latitude.status = SENSOR_S_WARN;
                np->longitude.status = SENSOR_S_WARN;
                np->speed.status = SENSOR_S_WARN;
                break;
        }
        if (nmea_degrees(&np->latitude.value, fld[3], *fld[4] == 'S' ? 1 : 0))
                np->latitude.status = SENSOR_S_WARN;
        if (nmea_degrees(&np->longitude.value,fld[5], *fld[6] == 'W' ? 1 : 0))
                np->longitude.status = SENSOR_S_WARN;

        if (nmea_atoi(&np->speed.value, fld[7]))
                np->speed.status = SENSOR_S_WARN;
        /* convert from knot to um/s */
        np->speed.value *= KNOTTOMS;

        if (jumped)
                np->time.status = SENSOR_S_WARN;
        if (np->time.status == SENSOR_S_OK)
                timeout_add_sec(&np->nmea_tout, TRUSTTIME);
        /*
         * If tty timestamping is requested, but no PPS signal is present, set
         * the sensor state to CRITICAL.
         */
        if (np->no_pps)
                np->time.status = SENSOR_S_CRIT;
}

/* Decode the GPS fix data for altitude.
 * - field 9 is the altitude in meters
 * $GNGGA,085901.00,1234.5678,N,00987.12345,E,1,12,0.84,1040.9,M,47.4,M,,*4B
 */
void
nmea_decode_gga(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
{
        if (fldcnt != 15) {
                DPRINTF(("GGA: field count mismatch, %d\n", fldcnt));
                return;
        }
#ifdef NMEA_DEBUG
        if (nmeadebug > 0) {
                linesw[TTYDISC].l_rint('[', tp);
                linesw[TTYDISC].l_rint('C', tp);
                linesw[TTYDISC].l_rint(']', tp);
        }
#endif

        np->altitude.status = SENSOR_S_OK;
        if (nmea_atoi(&np->altitude.value, fld[9]))
                np->altitude.status = SENSOR_S_WARN;

        /* convert to uMeter */
        np->altitude.value *= 1000;
        np->altitude.flags &= ~SENSOR_FINVALID;
}

/*
 * Convert nmea integer/decimal values in the form of XXXX.Y to an integer value
 * if it's a meter/altitude value, will be returned as mm
 */
int
nmea_atoi(int64_t *dst, char *src)
{
        char *p;
        int i = 3; /* take 3 digits */
        *dst = 0;

        for (p = src; *p && *p != '.' && *p >= '0' && *p <= '9' ; )
                *dst = *dst * 10 + (*p++ - '0');

        /* *p should be '.' at that point */
        if (*p != '.')
                return -1;      /* no decimal point, or bogus value ? */
        p++;

        /* read digits after decimal point, stop at first non-digit */
        for (; *p && i > 0 && *p >= '0' && *p <= '9' ; i--)
                *dst = *dst * 10 + (*p++ - '0');

        for (; i > 0 ; i--)
                *dst *= 10;

        DPRINTFN(2,("%s -> %lld\n", src, *dst));
        return 0;
}

/*
 * Convert a nmea position in the form DDDMM.MMMM to an
 * angle sensor value (degrees*1000000)
 */
int
nmea_degrees(int64_t *dst, char *src, int neg)
{
        size_t ppos;
        int i, n;
        int64_t deg = 0, min = 0;
        char *p;

        while (*src == '0')
                ++src;  /* skip leading zeroes */

        for (p = src, ppos = 0; *p; ppos++)
                if (*p++ == '.')
                        break;

        if (*p == '\0')
                return (-1);    /* no decimal point */

        for (n = 0; *src && n + 2 < ppos; n++)
                deg = deg * 10 + (*src++ - '0');

        for (; *src && n < ppos; n++)
                min = min * 10 + (*src++ - '0');

        src++;          /* skip decimal point */

        for (; *src && n < (ppos + 4); n++)
                min = min * 10 + (*src++ - '0');

        for (i=0; i < 6 + ppos - n; i++)
                min *= 10;

        deg = deg * 1000000 + (min/60);

        *dst = neg ? -deg : deg;
        return (0);
}

/*
 * Convert a NMEA 0183 formatted date string to seconds since the epoch.
 * The string must be of the form DDMMYY.
 * Return 0 on success, -1 if illegal characters are encountered.
 */
int
nmea_date_to_nano(char *s, int64_t *nano)
{
        struct clock_ymdhms ymd;
        time_t secs;
        char *p;
        int n;

        /* make sure the input contains only numbers and is six digits long */
        for (n = 0, p = s; n < 6 && *p && *p >= '0' && *p <= '9'; n++, p++)
                ;
        if (n != 6 || (*p != '\0'))
                return (-1);

        ymd.dt_year = 2000 + (s[4] - '0') * 10 + (s[5] - '0');
        ymd.dt_mon = (s[2] - '0') * 10 + (s[3] - '0');
        ymd.dt_day = (s[0] - '0') * 10 + (s[1] - '0');
        ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0;

        secs = clock_ymdhms_to_secs(&ymd);
        *nano = secs * 1000000000LL;
        return (0);
}

/*
 * Convert NMEA 0183 formatted time string to nanoseconds since midnight.
 * The string must be of the form HHMMSS[.[sss]] (e.g. 143724 or 143723.615).
 * Return 0 on success, -1 if illegal characters are encountered.
 */
int
nmea_time_to_nano(char *s, int64_t *nano)
{
        long fac = 36000L, div = 6L, secs = 0L, frac = 0L;
        char ul = '2';
        int n;

        for (n = 0, secs = 0; fac && *s && *s >= '0' && *s <= ul; s++, n++) {
                secs += (*s - '0') * fac;
                div = 16 - div;
                fac /= div;
                switch (n) {
                case 0:
                        if (*s <= '1')
                                ul = '9';
                        else
                                ul = '3';
                        break;
                case 1:
                case 3:
                        ul = '5';
                        break;
                case 2:
                case 4:
                        ul = '9';
                        break;
                }
        }
        if (fac)
                return (-1);

        /* Handle the fractions of a second, up to a maximum of 6 digits. */
        div = 1L;
        if (*s == '.') {
                for (++s; div < 1000000 && *s && *s >= '0' && *s <= '9'; s++) {
                        frac *= 10;
                        frac += (*s - '0');
                        div *= 10;
                }
        }

        if (*s != '\0')
                return (-1);

        *nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div);
        return (0);
}

/*
 * Degrade the sensor state if we received no NMEA sentences for more than
 * TRUSTTIME seconds.
 */
void
nmea_timeout(void *xnp)
{
        struct nmea *np = xnp;

        np->signal.value = 0;
        np->signal.status = SENSOR_S_CRIT;
        if (np->time.status == SENSOR_S_OK) {
                np->time.status = SENSOR_S_WARN;
                np->latitude.status = SENSOR_S_WARN;
                np->longitude.status = SENSOR_S_WARN;
                np->altitude.status = SENSOR_S_WARN;
                np->speed.status = SENSOR_S_WARN;
                /*
                 * further degrade in TRUSTTIME seconds if no new valid NMEA
                 * sentences are received.
                 */
                timeout_add_sec(&np->nmea_tout, TRUSTTIME);
        } else {
                np->time.status = SENSOR_S_CRIT;
                np->latitude.status = SENSOR_S_CRIT;
                np->longitude.status = SENSOR_S_CRIT;
                np->altitude.status = SENSOR_S_CRIT;
                np->speed.status = SENSOR_S_CRIT;
        }
}