/*

 * Handle the Rockwell binary packet format supported by the old Zodiac chipset

 *

 * Week counters are not limited to 10 bits. It's unknown what

 * the firmware is doing to disambiguate them, if anything; it might just

 * be adding a fixed offset based on a hidden epoch value, in which case

 * unhappy things will occur on the next rollover.

 *

 * This file is Copyright 2010 by the GPSD project

 * SPDX-License-Identifier: BSD-2-clause

 */

#include "../include/gpsd_config.h"  // must be before all includes

#include <math.h>

#include <stdbool.h>

#include <stdio.h>

#include <string.h>

#include <unistd.h>

#include "../include/gpsd.h"

#include "../include/bits.h"

#include "../include/strfuncs.h"

// Zodiac protocol description uses 1-origin indexing by little-endian word

#define get16z(buf, n)   ((buf[2*(n)-2]) | (buf[2*(n)-1] << 8))

#define getu16z(buf, n)  (uint16_t)((buf[2*(n)-2]) | (buf[2*(n)-1] << 8))

#define get32z(buf, n)   ((buf[2*(n)-2]) | (buf[2*(n)-1] << 8) | \

                          (buf[2*(n)+0] << 16) | (buf[2*(n)+1] << 24))

#define getu32z(buf, n)  (uint32_t)((buf[2*(n)-2]) | (buf[2*(n)-1] << 8) | \

                                    (buf[2*(n)+0] << 16) | (buf[2*(n)+1] << 24))

#define getstringz(to, from, s, e)                      \

    (void)memcpy(to, from+2*(s)-2, 2*((e)-(s)+1))

#ifdef ZODIAC_ENABLE

struct header

{

    unsigned short sync;

    unsigned short id;

    unsigned short ndata;

    unsigned short flags;

    unsigned short csum;

};

static unsigned short zodiac_checksum(unsigned short *w, int n)

{

    unsigned short csum = 0;

    while (n-- > 0)

        csum += *(w++);

    return -csum;

}

// write an array of shorts in little-endian format

static ssize_t end_write(int fd, void *d, size_t len)

{

    unsigned char buf[BUFSIZ];

    short *data = (short *)d;

    size_t n;

    for (n = 0; n < (size_t)(len/2); n++)

        putle16(buf, n*2, data[n]);

    return write(fd, (char*)buf, len);

}

/* zodiac_spew - Takes a message type, an array of data words, and a length

 * for the array, and prepends a 5 word header (including checksum).

 * The data words are expected to be checksummed.

 */

static ssize_t zodiac_spew(struct gps_device_t *session, unsigned short type,

                           unsigned short *dat, int dlen)

{

    struct header h;

    int i;

    char buf[BUFSIZ];

    h.sync = 0x81ff;

    h.id = (unsigned short)type;

    h.ndata = (unsigned short)(dlen - 1);

    h.flags = 0;

    h.csum = zodiac_checksum((unsigned short *)&h, 4);

    if (!BAD_SOCKET(session->gpsdata.gps_fd)) {

        size_t hlen, datlen;

        hlen = sizeof(h);

        datlen = sizeof(unsigned short) * dlen;

        if (end_write(session->gpsdata.gps_fd, &h, hlen) != (ssize_t) hlen ||

            end_write(session->gpsdata.gps_fd, dat,

                      datlen) != (ssize_t) datlen) {

            GPSD_LOG(LOG_INFO, &session->context->errout,

                     "ZODIAC: Reconfigure write failed\n");

            return -1;

        }

    }

    (void)snprintf(buf, sizeof(buf),

                   "%04x %04x %04x %04x %04x",

                   h.sync, h.id, h.ndata, h.flags, h.csum);

    for (i = 0; i < dlen; i++)

        str_appendf(buf, sizeof(buf), " %04x", dat[i]);

    GPSD_LOG(LOG_RAW, &session->context->errout,

             "ZODIAC: Sent Zodiac packet: %s\n", buf);

    return 0;

}

static void send_rtcm(struct gps_device_t *session,

                      const char *rtcmbuf, size_t rtcmbytes)

{

    unsigned short data[34];

    int n = 1 + (int)(rtcmbytes / 2 + rtcmbytes % 2);

    if (session->driver.zodiac.sn++ > 32767)

        session->driver.zodiac.sn = 0;

    memset(data, 0, sizeof(data));

    data[0] = session->driver.zodiac.sn;        // sequence number

    memcpy(&data[1], rtcmbuf, rtcmbytes);

    data[n] = zodiac_checksum(data, n);

    (void)zodiac_spew(session, 1351, data, n + 1);

}

static ssize_t zodiac_send_rtcm(struct gps_device_t *session,

                                const char *rtcmbuf, size_t rtcmbytes)

{

    while (rtcmbytes > 0) {

        size_t len = (size_t) (rtcmbytes > 64 ? 64 : rtcmbytes);

        send_rtcm(session, rtcmbuf, len);

        rtcmbytes -= len;

        rtcmbuf += len;

    }

    return 1;

}

#define getzword(n)     get16z(session->lexer.outbuffer, n)

#define getzu16(n)      getu16z(session->lexer.outbuffer, n)

#define getzlong(n)     get32z(session->lexer.outbuffer, n)

#define getzu32(n)      getu32z(session->lexer.outbuffer, n)

// time-position-velocity report

static gps_mask_t handle1000(struct gps_device_t *session)

{

    gps_mask_t mask;

    struct tm unpacked_date = {0};

    int datum;

    char ts_buf[TIMESPEC_LEN];

    // ticks                      = getzlong(6);

    // sequence                   = getzword(8);

    // measurement_sequence       = getzword(9);

    session->newdata.status = (getzword(10) & 0x1c) ? 0 : 1;

    if (0 != session->newdata.status) {

        session->newdata.mode = (getzword(10) & 1) ? MODE_2D : MODE_3D;

    } else {

        session->newdata.mode = MODE_NO_FIX;

    }

    // solution_type                 = getzword(11);

    session->gpsdata.satellites_used = (int)getzword(12);

    // polar_navigation              = getzword(13);

    session->context->gps_week = (unsigned short)getzword(14);

    // gps_seconds                   = getzlong(15);

    // gps_nanoseconds               = getzlong(17);

    unpacked_date.tm_mday = (int)getzword(19);

    unpacked_date.tm_mon = (int)getzword(20) - 1;

    unpacked_date.tm_year = (int)getzword(21) - 1900;

    unpacked_date.tm_hour = (int)getzword(22);

    unpacked_date.tm_min = (int)getzword(23);

    unpacked_date.tm_sec = (int)getzword(24);

    session->newdata.time.tv_sec = mkgmtime(&unpacked_date);

    session->newdata.time.tv_nsec = getzu32(25);

    session->newdata.latitude = ((long)getzlong(27)) * RAD_2_DEG * 1e-8;

    session->newdata.longitude = ((long)getzlong(29)) * RAD_2_DEG * 1e-8;

    /*

     * The Rockwell Jupiter TU30-D140 reports altitude as uncorrected height

     * above WGS84 geoid.  The Zodiac binary protocol manual does not

     * specify whether word 31 is geodetic or WGS 84.

     * Here we assume altitude is always wgs84.

     */

    session->newdata.altHAE = ((long)getzlong(31)) * 1e-2;

    session->newdata.geoid_sep = ((short)getzword(33)) * 1e-2;

    session->newdata.speed = (int)getzlong(34) * 1e-2;

    session->newdata.track = (int)getzword(36) * RAD_2_DEG * 1e-3;

    session->newdata.magnetic_var = ((short)getzword(37)) * RAD_2_DEG * 1e-4;

    session->newdata.climb = ((short)getzword(38)) * 1e-2;

    datum = getzword(39);

    datum_code_string(datum, session->newdata.datum,

                      sizeof(session->newdata.datum));

    /*

     * The manual says these are 1-sigma.  Device reports only eph, circular

     * error.  Let gpsd_model_error() do the rest

     */

    session->newdata.eph = (int)getzlong(40) * 1e-2 * GPSD_CONFIDENCE;

    session->newdata.epv = (int)getzlong(42) * 1e-2 * GPSD_CONFIDENCE;

    session->newdata.ept = (int)getzlong(44) * 1e-2 * GPSD_CONFIDENCE;

    session->newdata.eps = (int)getzword(46) * 1e-2 * GPSD_CONFIDENCE;

    // clock_bias                  = (int)getzlong(47) * 1e-2;

    // clock_bias_sd               = (int)getzlong(49) * 1e-2;

    // clock_drift                 = (int)getzlong(51) * 1e-2;

    // clock_drift_sd              = (int)getzlong(53) * 1e-2;

    mask = TIME_SET | NTPTIME_IS | LATLON_SET | ALTITUDE_SET | CLIMB_SET |

           SPEED_SET | TRACK_SET | STATUS_SET | MODE_SET |

           HERR_SET | SPEEDERR_SET | VERR_SET;

    GPSD_LOG(LOG_DATA, &session->context->errout,

             "ZODIAC: 1000: time=%s lat=%.2f lon=%.2f altHAE=%.2f track=%.2f "

             "speed=%.2f climb=%.2f mode=%d status=%d\n",

             timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),

             session->newdata.latitude,

             session->newdata.longitude, session->newdata.altHAE,

             session->newdata.track, session->newdata.speed,

             session->newdata.climb, session->newdata.mode,

             session->newdata.status);

    return mask | CLEAR_IS | REPORT_IS;

}

// Message 1002: Channel Summary Message

static gps_mask_t handle1002(struct gps_device_t *session)

{

    unsigned i;

    timespec_t ts_tow;

    // ticks                      = getzlong(6);

    // sequence                   = getzword(8);

    // measurement_sequence       = getzword(9);

    unsigned short gps_week = getzu16(10);

    time_t gps_seconds = (time_t)getzu32(11);

    unsigned long gps_nanoseconds = getzu32(13);

    char ts_buf[TIMESPEC_LEN];

    // Note: this week counter is not limited to 10 bits.

    session->context->gps_week = gps_week;

    session->gpsdata.satellites_used = 0;

    for (i = 0; i < ZODIAC_CHANNELS; i++) {

        int status, prn;

        session->driver.zodiac.Zv[i] = status = (int)getzword(15 + (3 * i));

        session->driver.zodiac.Zs[i] = prn = (int)getzword(16 + (3 * i));

        if (status & 1) {

            session->gpsdata.satellites_used++;

        }

        session->gpsdata.skyview[i].PRN = (short)prn;

        session->gpsdata.skyview[i].ss = (float)getzword(17 + (3 * i));

        session->gpsdata.skyview[i].used = (bool)(status & 1);

    }

    ts_tow.tv_sec = gps_seconds;

    ts_tow.tv_nsec = gps_nanoseconds;

    session->gpsdata.skyview_time = gpsd_gpstime_resolv(session, gps_week,

                                                        ts_tow);

    GPSD_LOG(LOG_DATA, &session->context->errout,

             "ZODIAC: 1002: visible=%d used=%d mask={SATELLITE|USED} "

             "ime %s\n",

             session->gpsdata.satellites_visible,

             session->gpsdata.satellites_used,

             timespec_str(&session->gpsdata.skyview_time, ts_buf,

                          sizeof(ts_buf)));

    return SATELLITE_SET | USED_IS;

}

// skyview report

static gps_mask_t handle1003(struct gps_device_t *session)

{

    unsigned i, n;

    gps_mask_t mask = 0;

    /* The Polaris (and probably the DAGR) emit some strange variant of

     * this message which causes gpsd to crash filtering on impossible

     * number of satellites avoids this */

    n = getzword(14);

    if (ZODIAC_CHANNELS < n) {

        return 0;

    }

    gpsd_zero_satellites(&session->gpsdata);

    // ticks              = getzlong(6);

    // sequence           = getzword(8);

    session->gpsdata.dop.gdop = (unsigned int)getzword(9) * 1e-2;

    session->gpsdata.dop.pdop = (unsigned int)getzword(10) * 1e-2;

    session->gpsdata.dop.hdop = (unsigned int)getzword(11) * 1e-2;

    session->gpsdata.dop.vdop = (unsigned int)getzword(12) * 1e-2;

    session->gpsdata.dop.tdop = (unsigned int)getzword(13) * 1e-2;

    mask |= DOP_SET;

    session->gpsdata.satellites_visible = n;

    for (i = 0; i < ZODIAC_CHANNELS; i++) {

        if (i < session->gpsdata.satellites_visible) {

            session->gpsdata.skyview[i].PRN = (short)getzword(15 + (3 * i));

            session->gpsdata.skyview[i].azimuth =

                (((double)getzword(16 + (3 * i))) * RAD_2_DEG * 1e-4);

            if (session->gpsdata.skyview[i].azimuth < 0)

                session->gpsdata.skyview[i].azimuth += 360;

            session->gpsdata.skyview[i].elevation =

                (((double)getzword(17 + (3 * i))) * RAD_2_DEG * 1e-4);

        } else {

            session->gpsdata.skyview[i].PRN = 0;

            session->gpsdata.skyview[i].azimuth = NAN;

            session->gpsdata.skyview[i].elevation = NAN;

            session->gpsdata.skyview[i].ss = NAN;

        }

    }

    session->gpsdata.skyview_time.tv_sec = 0;

    session->gpsdata.skyview_time.tv_nsec = 0;

    mask |= SATELLITE_SET;

    GPSD_LOG(LOG_DATA, &session->context->errout,

             "ZODIAC: NAVDOP: visible=%d gdop=%.2f pdop=%.2f "

             "hdop=%.2f vdop=%.2f tdop=%.2f mask={SATELLITE|DOP}\n",

             session->gpsdata.satellites_visible,

             session->gpsdata.dop.gdop,

             session->gpsdata.dop.hdop,

             session->gpsdata.dop.vdop,

             session->gpsdata.dop.pdop, session->gpsdata.dop.tdop);

    return mask;

}

// fix quality report

static gps_mask_t handle1005(struct gps_device_t *session UNUSED)

{

    // ticks              = getzlong(6);

    // sequence           = getzword(8);

    int numcorrections = (int)getzword(12);

    if (MODE_NO_FIX == session->newdata.mode) {

        session->newdata.status = STATUS_UNK;

    } else if (0 == numcorrections) {

        session->newdata.status = STATUS_GPS;

    } else {

        session->newdata.status = STATUS_DGPS;

    }

    return 0;

}

// version report

static gps_mask_t handle1011(struct gps_device_t *session)

{

    /*

     * This is UNTESTED -- but harmless if buggy.  Added to support

     * client querying of the ID with firmware version in 2006.

     * The Zodiac is supposed to send one of these messages on startup.

     */

    // software version field

    getstringz(session->subtype, session->lexer.outbuffer, 19, 28);

    GPSD_LOG(LOG_DATA, &session->context->errout,

             "ZODIAC: 1011: subtype=%s mask={DEVICEID}\n",

             session->subtype);

    return DEVICEID_SET;

}

// leap-second correction report

static gps_mask_t handle1108(struct gps_device_t *session)

{

    // ticks              = getzlong(6);

    // sequence           = getzword(8);

    // utc_week_seconds   = getzlong(14);

    // leap_nanoseconds   = getzlong(17);

    if ((getzword(19) & 3) == 3) {

        session->context->valid |= LEAP_SECOND_VALID;

        session->context->leap_seconds = (int)getzword(16);

    }

    return 0;

}

static gps_mask_t zodiac_analyze(struct gps_device_t *session)

{

    unsigned bad_len = 0;

    gps_mask_t mask = 0;

    unsigned id = 0;

    if (10 > session->lexer.outbuflen) {

        return 0;

    }

    id = getzu16(2);

    GPSD_LOG(LOG_PROG, &session->context->errout,

             "ZODIAC: %u: length %zd\n",

             id, session->lexer.outbuflen);

    GPSD_LOG(LOG_RAW, &session->context->errout,

             "ZODIAC: Raw Zodiac packet type %d length %zd: %s\n",

             id, session->lexer.outbuflen, gpsd_prettydump(session));

    /*

     * Normal cycle for these devices is 1001 1002.

     * We count 1001 as end of cycle because 1002 doesn't

     * carry fix information.

     */

    session->cycle_end_reliable = true;

    switch (id) {

    case 1000:

        if (110 > session->lexer.outbuflen) {

            bad_len = 110;

            break;

        }

        mask = handle1000(session);

        break;

    case 1002:

        if (102 > session->lexer.outbuflen) {

            bad_len = 102;

            break;

        }

        mask = handle1002(session);

        break;

    case 1003:

        if (102 > session->lexer.outbuflen) {

            bad_len = 102;

            break;

        }

        mask = handle1003(session);

        break;

    case 1005:

        if (50 > session->lexer.outbuflen) {

            bad_len = 50;

            break;

        }

        mask = handle1005(session);

        break;

    case 1011:

        if (118 > session->lexer.outbuflen) {

            bad_len = 118;

            break;

        }

        mask = handle1011(session);

        break;

    case 1108:

        if (40 > session->lexer.outbuflen) {

            bad_len = 40;

            break;

        }

        mask = handle1108(session);

        break;

    default:

        GPSD_LOG(LOG_WARN, &session->context->errout,

                 "ZODIAC: %u: unknwon message id, length %zd",

                 id, session->lexer.outbuflen);

        break;

    }

    if (bad_len) {

        GPSD_LOG(LOG_WARN, &session->context->errout,

                 "ZODIAC: %u: runt payload len %u s/b %zd",

                 id, bad_len, session->lexer.outbuflen);

    }

    return mask;

}

static ssize_t zodiac_control_send(struct gps_device_t *session,

                                   char *msg, size_t len)

{

    unsigned short shortwords[256];

    if (sizeof(shortwords) <= len) {

        return -1;

    }

#define min(x,y)        ((x) < (y) ? x : y)

    /*

     * We used to just cast msg to an unsigned short pointer.

     * This can fail on word-oriented architectures like a SPARC.

     */

    memcpy((char *)shortwords, msg, min(sizeof(shortwords), len));

    // and if len isn't even, it's your own fault

    return zodiac_spew(session, shortwords[0], shortwords + 1,

                       (int)(len / 2 - 1));

}

static bool zodiac_speed_switch(struct gps_device_t *session,

                                speed_t speed, char parity, int stopbits)

{

    unsigned short data[15];

    if (32767 < session->driver.zodiac.sn++) {

        session->driver.zodiac.sn = 0;

    }

    switch (parity) {

    case 'E':

    case 2:

        parity = (char)2;

        break;

    case 'O':

    case 1:

        parity = (char)1;

        break;

    case 'N':

    case 0:

    default:

        parity = (char)0;

        break;

    }

    memset(data, 0, sizeof(data));

    // data is the part of the message starting at word 6

    data[0] = session->driver.zodiac.sn;        // sequence number

    data[1] = 1;                                // port 1 data valid

    data[2] = (unsigned short)parity;   // port 1 character width (8 bits)

    // port 1 stop bits (1 stopbit)

    data[3] = (unsigned short)(stopbits - 1);

    data[4] = 0;                // port 1 parity (none)

    // port 1 speed

    data[5] = (unsigned short)(round(log((double)speed / 300) / GPS_LN2) + 1);

    data[14] = zodiac_checksum(data, 14);

    (void)zodiac_spew(session, 1330, data, 15);

    return true;                // it would be nice to error-check this

}

static double zodiac_time_offset(struct gps_device_t *session UNUSED)

{

    /* Removing/changing the magic number below is likely to disturb

     * the handling of the 1pps signal from the gps device. The regression

     * tests and simple gps applications do not detect this. A live test

     * with the 1pps signal active is required. */

    return 1.1;

}

// this is everything we export

// *INDENT-OFF*

const struct gps_type_t driver_zodiac =

{

    .type_name      = "Zodiac",            // full name of type

    .packet_type    = ZODIAC_PACKET,       // associated lexer packet type

    .flags          = DRIVER_STICKY,       // no flags set

    .trigger        = NULL,                // no trigger

    .channels       = 12,                  // consumer-grade GPS

    .probe_detect   = NULL,                // no probe

    .get_packet     = packet_get1,         // use the generic packet getter

    .parse_packet   = zodiac_analyze,      // parse message packets

    .rtcm_writer    = zodiac_send_rtcm,    // send DGPS correction

    .init_query     = NULL,                // non-perturbing initial query

    .event_hook     = NULL,                // no configuration

    .speed_switcher = zodiac_speed_switch, // we can change baud rate

    .mode_switcher  = NULL,                // no mode switcher

    .rate_switcher  = NULL,                // no sample-rate switcher

    .min_cycle.tv_sec  = 1,                // not relevant, no rate switch

    .min_cycle.tv_nsec = 0,                // not relevant, no rate switch

    .control_send   = zodiac_control_send, // for gpsctl and friends

    .time_offset     = zodiac_time_offset, // compute NTO fudge factor

};

// *INDENT-ON*

#endif  // ZODIAC_ENABLE

// vim: set expandtab shiftwidth=4