/*

 * This is the gpsd driver for GeoStar Navigation receivers

 * operating in binary mode.

 *

 * Tested with GeoS-1M GPS/GLONASS receiver.

 *

 * By Viktar Palstsiuk, viktar.palstsiuk@promwad.com

 *

 * 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 "../include/bits.h"

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

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

#include "../include/timespec.h"

#ifdef GEOSTAR_ENABLE

#define GEOSTAR_CHANNELS   24

#define JAN_2008           0x47798280  // 1199145600 = 2008 - 1970 in seconds

#define OFFSET(n)          ((n)*4+4)

static int decode_channel_id (uint32_t ch_id) {

        int num = 0;

        num = (int)(ch_id & 0x1F);              // SV ID

        if (0 == (ch_id & (1<<30))) {

            num += GLONASS_PRN_OFFSET;          // GLONASS SV

        } else if (0 == num) {

            num = 32;                           // GPS SV

        }

        return num;

}

/* write to geostar

 *

 * id - message is

 * data - 32 bit words of message content

 * len - number of 32 bit words in data

 *

 * return: 0 == OK

 *         negative on error

 */

static int geostar_write(struct gps_device_t *session,

                         unsigned int id, unsigned char *data, size_t len)

{

    int i;

    unsigned long cs = 0;

    putbyte(session->msgbuf, 0, 'P');

    putbyte(session->msgbuf, 1, 'S');

    putbyte(session->msgbuf, 2, 'G');

    putbyte(session->msgbuf, 3, 'G');

    putbe16(session->msgbuf, 4, id);

    putbe16(session->msgbuf, 6, len);

    // Copy content

    memcpy(session->msgbuf + 8, data, len * 4);

    len += 2;      // PSGG + id + len

    // Calculate checksum

    for (i = 0; (size_t)i < len; i++) {

        cs ^= getleu32(session->msgbuf, i * 4);

    }

    putle32(session->msgbuf, len * 4, cs);

    len += 1;    // Checksum

    session->msgbuflen = len * 4;

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

             "Sent GeoStar packet id 0x%x\n", id);

    if (gpsd_write(session, session->msgbuf, session->msgbuflen) !=

        (ssize_t)session->msgbuflen)

        return -1;

    return 0;

}

/* geostar_detect()

 *

 * see if it looks like a GeoStar device is listening and

 * return 1 if found, 0 if not

 */

static bool geostar_detect(struct gps_device_t *session)

{

    unsigned char buf[1 * 4];

    bool ret = false;

    int myfd;

    myfd = session->gpsdata.gps_fd;

    // request firmware revision and look for a valid response

    putbe32(buf, 0, 0);

    if (0 == geostar_write(session, 0xc1, buf, 1)) {

        unsigned int n;

        struct timespec to;

        // FIXME: this holds the main loop from running...

        for (n = 0; n < 3; n++) {

            // wait one second

            to.tv_sec = 1;

            to.tv_nsec = 0;

            if (!nanowait(myfd, &to)) {

                break;

            }

            if (0 <= packet_get1(session)) {

                if (session->lexer.type == GEOSTAR_PACKET) {

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

                             "geostar_detect found\n");

                    ret = true;

                    break;

                }

            }

        }

    }

    return ret;

}

static gps_mask_t geostar_analyze(struct gps_device_t *session)

{

    int i, len;

    gps_mask_t mask = 0;

    unsigned int id;

    uint16_t uw1, uw2;

    uint32_t ul1, ul2, ul3, ul4, ul5;

    double d1, d2, d3, d4, d5;

    char buf[BUFSIZ];

    char buf2[BUFSIZ];

    if (session->lexer.type != GEOSTAR_PACKET) {

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

                 "geostar_analyze packet type %d\n",

                 session->lexer.type);

        return 0;

    }

    if (12 > session->lexer.outbuflen ||

        'P' != session->lexer.outbuffer[0]) {

        return 0;

    }

    // put data part of message in buf

    memset(buf, 0, sizeof(buf));

    memcpy(buf, session->lexer.outbuffer, session->lexer.outbuflen);

    buf2[len = 0] = '\0';

    for (i = 0; i < (int)session->lexer.outbuflen; i++) {

        str_appendf(buf2, sizeof(buf2),

                    "%02x", buf[len++] = session->lexer.outbuffer[i]);

    }

    id = (unsigned int)getleu16(session->lexer.outbuffer, OFFSET(0));

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

             "GeoStar packet id 0x%02x length %d: %s\n", id, len, buf2);

    session->cycle_end_reliable = true;

    switch (id) {

    case 0x10:

        GPSD_LOG(LOG_INF, &session->context->errout, "Raw measurements\n");

        break;

    case 0x11:

        GPSD_LOG(LOG_INF, &session->context->errout, "GPS sub-frame data\n");

        break;

    case 0x12:

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

                 "GLONASS sub-frame data\n");

        break;

    case 0x13:

        d1 = getled64(buf, OFFSET(1));

        d2 = getled64(buf, OFFSET(3));

        d3 = getled64(buf, OFFSET(5));

        d4 = getled64(buf, OFFSET(29));    // GPS time

        d5 = getled64(buf, OFFSET(31));    // GLONASS time

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

                 "ECEF coordinates %g %g %g %f %f\n", d1, d2, d3, d4, d5);

        break;

    case 0x20:

        d1 = getled64(buf, OFFSET(1)); /* time */

        DTOTS(&session->newdata.time, d1);

        session->newdata.time.tv_sec += JAN_2008;

        session->newdata.latitude = getled64(buf, OFFSET(3)) * RAD_2_DEG;

        session->newdata.longitude = getled64(buf, OFFSET(5)) * RAD_2_DEG;

        // altitude above ellipsoid

        session->newdata.altHAE = getled64(buf, OFFSET(7));

        session->newdata.geoid_sep = getled64(buf, OFFSET(9));

        session->gpsdata.satellites_used = (int)getles32(buf, OFFSET(11));

        session->gpsdata.dop.gdop = getled64(buf, OFFSET(13));

        session->gpsdata.dop.pdop = getled64(buf, OFFSET(15));

        session->gpsdata.dop.tdop = getled64(buf, OFFSET(17));

        session->gpsdata.dop.hdop = getled64(buf, OFFSET(19));

        session->gpsdata.dop.vdop = getled64(buf, OFFSET(21));

        session->newdata.speed = getled64(buf, OFFSET(31));

        session->newdata.track = getled64(buf, OFFSET(33)) * RAD_2_DEG;

        ul1 = getleu32(buf, OFFSET(29));   // status

        if (0 != ul1) {

            session->newdata.status = STATUS_UNK;

            mask |= STATUS_SET;

        } else if (STATUS_GPS > session->newdata.status) {

            // Don't step on previously set status

            session->newdata.status = STATUS_GPS;

            mask |= STATUS_SET;

        }

        mask |= TIME_SET | NTPTIME_IS | LATLON_SET | ALTITUDE_SET |

                SPEED_SET | STATUS_SET | TRACK_SET | DOP_SET | USED_IS |

                REPORT_IS;

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

                 "Geographic coordinates %f %g %g %g %g %g\n",

                 d1,

                 session->newdata.latitude,

                 session->newdata.longitude,

                 session->newdata.altHAE,

                 session->newdata.speed,

                 session->newdata.track);

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

                 "Dilution of precision %g %g %g %g %g\n",

                 session->gpsdata.dop.gdop,

                 session->gpsdata.dop.pdop,

                 session->gpsdata.dop.tdop,

                 session->gpsdata.dop.hdop,

                 session->gpsdata.dop.vdop);

        break;

    case 0x21:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

        uw1 = getleu16(buf, OFFSET(3));

        uw2 = getleu16(buf, OFFSET(3) + 2);

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

                 "Current receiver telemetry %x %d %d %d\n",

                 ul1, ul2, uw1, uw2);

        if (ul1 & (1<<3)) {

            session->newdata.mode = MODE_2D;

        } else {

            session->newdata.mode = MODE_3D;

        }

        if (ul1 & (1<<2)) {

            session->newdata.status = STATUS_GPS;

        } else {

            session->newdata.status = STATUS_UNK;

            session->newdata.mode = MODE_NO_FIX;

        }

        mask |= MODE_SET | STATUS_SET;

        break;

    case 0x22:

        ul1 = getleu32(buf, OFFSET(1));

        GPSD_LOG(LOG_INF, &session->context->errout, "SVs in view %d\n", ul1);

        session->gpsdata.satellites_visible = (int)ul1;

        if (ul1 > GEOSTAR_CHANNELS) {

            ul1 = GEOSTAR_CHANNELS;

        }

        for(i = 0; (uint32_t)i < ul1; i++) {

            int16_t s1, s2, s3;

            ul2 = getleu32(buf, OFFSET(2) + i * 3 * 4);

            s1 = getles16(buf, OFFSET(3) + i * 3 * 4);

            s2 = getles16(buf, OFFSET(3) + 2 + i * 3 * 4);

            s3 = getles16(buf, OFFSET(4) + 2 + i * 3 * 4);

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

                     "ID %d Az %g El %g SNR %g\n",

                     decode_channel_id(ul2), s1 * 0.001 * RAD_2_DEG,

                     s2 * 0.001 * RAD_2_DEG, s3 * 0.1);

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

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

                    (short)round((double)s1 * 0.001 * RAD_2_DEG);

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

                    (short)round((double)s2 * 0.001 * RAD_2_DEG);

            session->gpsdata.skyview[i].ss = (double)s3*0.1;

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

        }

        session->gpsdata.skyview_time.tv_sec = 0;

        session->gpsdata.skyview_time.tv_nsec = 0;

        mask |= SATELLITE_SET | USED_IS;

        break;

    case 0x3e:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

        ul3 = getleu32(buf, OFFSET(3));

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

                 "Receiver power-up message %d %d %d\n", ul1, ul2, ul3);

        break;

    case 0x3f:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

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

                 "Negative acknowledge %x %d\n", ul1, ul2);

        break;

    case 0x40:

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

                 "Response to Set initial parameters\n");

        break;

    case 0x41:

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

                 "Response to Set serial ports parameters\n");

        break;

    case 0x42:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

        ul3 = getleu32(buf, OFFSET(3));

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

                 "Response to Set receiver operation mode %d %d %d\n",

                 ul1, ul2, ul3);

        break;

    case 0x43:

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

                 "Response to Set navigation task solution parameters\n");

        break;

    case 0x44:

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

                 "Response to Set output data rate\n");

        break;

    case 0x46:

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

                 "Response to Assign data protocol to communication port\n");

        break;

    case 0x48:

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

                 "Response to Set GPS almanac\n");

        break;

    case 0x49:

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

                 "Response to Set GLONASS almanac\n");

        break;

    case 0x4a:

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

                 "Response to Set GPS ephemeris\n");

        break;

    case 0x4b:

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

                 "Response to Set GLONASS ephemeris\n");

        break;

    case 0x4c:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

        ul3 = getleu32(buf, OFFSET(3));

        ul4 = getleu32(buf, OFFSET(4));

        ul5 = getleu32(buf, OFFSET(5));

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

                 "Response to Set PPS parameters %d %d %d %d %d\n",

                 ul1, ul2, ul3, ul4, ul5);

        break;

    case 0x4d:

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

                 "Response to Enable/disable SV in position fix\n");

        break;

    case 0x4e:

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

                 "Response to Enable/disable NMEA messages\n");

        break;

    case 0x4f:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

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

                 "Response to Enable/disable binary messages %x %x\n",

                 ul1, ul2);

        break;

    case 0x80:

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

                 "Response to Query initial parameters\n");

        break;

    case 0x81:

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

                 "Response to Query serial ports parameters\n");

        break;

    case 0x82:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

        ul3 = getleu32(buf, OFFSET(3));

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

                 "Response to Query receiver operation mode %d %d %d\n",

                 ul1, ul2, ul3);

        break;

    case 0x83:

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

                 "Response to Query navigation task solution parameters\n");

        break;

    case 0x84:

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

                 "Response to Query output data rate\n");

        break;

    case 0x86:

        session->driver.geostar.physical_port =

            (unsigned int)getleu32(buf, OFFSET(1));

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

                 "Response to Query data protocol assignment to "

                 "communication port\n");

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

                 "Connected to physical port %d\n",

                 session->driver.geostar.physical_port);

        break;

    case 0x88:

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

                 "Response to Query GPS almanac\n");

        break;

    case 0x89:

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

                 "Response to Query GLONASS almanac\n");

        break;

    case 0x8a:

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

                 "Response to Query GPS ephemerides\n");

        break;

    case 0x8b:

        d1 = getled64(buf, OFFSET(23));

        d2 = getled64(buf, OFFSET(25));

        d3 = getled64(buf, OFFSET(27));

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

                 "Response to Query GLONASS ephemerides %g %g %g\n",

                 d1, d2, d3);

        break;

    case 0x8c:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

        ul3 = getleu32(buf, OFFSET(3));

        ul4 = getleu32(buf, OFFSET(4));

        ul5 = getleu32(buf, OFFSET(5));

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

                 "Response to Query PPS parameters %d %d %d %d %d\n",

                 ul1, ul2, ul3, ul4, ul5);

        break;

    case 0x8d:

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

                 "Response to Query enable/disable status of "

                 "the SV in position fix\n");

        break;

    case 0x8e:

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

                 "Response to Query enable NMEA messages\n");

        break;

    case 0x8f:

        ul1 = getleu32(buf, OFFSET(1));

        ul2 = getleu32(buf, OFFSET(2));

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

                 "Response to Query enable binary messages %x %x\n",

                 ul1, ul2);

        break;

    case 0xc0:

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

                 "Response to Change operation mode command\n");

        break;

    case 0xc1:

        ul4 = getleu32(buf, OFFSET(1));

        ul1 = getleu32(buf, OFFSET(2));

        ul2 = getleu32(buf, OFFSET(3));

        ul3 = getleu32(buf, OFFSET(4));

        (void)snprintf(session->subtype, sizeof(session->subtype),

                       "%u.%u %u.%u.%u %x %c-%u\n",

                       ul4>>16, ul4&0xFFFF, ul1>>9, (ul1>>5)&0xF,

                       ul1&0x1F, ul2, ul3>>24, ul3&0x00FFFFFF);

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

                 "Response to Request FW version command: %s\n",

                 session->subtype);

        mask |= DEVICEID_SET;

        break;

    case 0xc2:

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

                 "Response to Restart receiver command\n");

        break;

    case 0xc3:

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

                 "Response to Store parameters to Flash command\n");

        break;

    case 0xd0:

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

                 "Response to Erase Flash sector command\n");

        break;

    case 0xd1:

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

                 "Response to Write data to Flash command\n");

        break;

    case 0xd2:

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

                 "Response to Store Serial Number command\n");

        break;

    default:

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

                 "Unhandled GeoStar packet type 0x%02x\n", id);

        break;

    }

    return mask;

}

static gps_mask_t geostar_parse_input(struct gps_device_t *session)

{

    if (GEOSTAR_PACKET == session->lexer.type) {

        return geostar_analyze(session);

    }   // else

    return 0;

}

// not used by the daemon, it's for gpsctl and friends

static ssize_t geostar_control_send(struct gps_device_t *session,

                                 char *buf, size_t buflen)

{

    return (ssize_t) geostar_write(session,

                                   (unsigned int)buf[0],

                                   (unsigned char *)buf + 1, (buflen - 1)/4);

}

static void geostar_init_query(struct gps_device_t *session)

{

    unsigned char buf[4];

    // 0xC1 request content is ignored, init for Coverity

    memset(buf, 0, sizeof(buf));

    // Request Firmware Version

    (void)geostar_write(session, 0xc1, buf, 1);

}

static void geostar_event_hook(struct gps_device_t *session, event_t event)

{

    unsigned char buf[2 * 4];

    if (session->context->readonly ||

        session->context->passive) {

        return;

    }

    if (event == event_identified ||

        event == event_reactivate) {

        // Select binary packets

        putbe32(buf, 0, 0xffff0000);

        putbe32(buf, 4, 0);

        (void)geostar_write(session, 0x4f, buf, 2);

        // Poll Ports params

        putbe32(buf, 0, 1);

        (void)geostar_write(session, 0x81, buf, 1);

        putbe32(buf, 0, 0);

        (void)geostar_write(session, 0x81, buf, 1);

        // Poll Init params

        (void)geostar_write(session, 0x80, buf, 1);

        // Poll Mode

        (void)geostar_write(session, 0x82, buf, 1);

        // Poll Solution params

        (void)geostar_write(session, 0x83, buf, 1);

        // Poll Output rate

        (void)geostar_write(session, 0x84, buf, 1);

        // Poll Protocols assignment

        (void)geostar_write(session, 0x86, buf, 1);

        // Poll PPS params

        (void)geostar_write(session, 0x8c, buf, 1);

        // Poll NMEA packets selected

        (void)geostar_write(session, 0x8e, buf, 1);

        // Poll binary packets selected

        (void)geostar_write(session, 0x8f, buf, 1);

    }

    if (event_deactivate == event) {

        // Perform cold restart

        putbe32(buf, 0, 3);

        (void)geostar_write(session, 0xc2, buf, 1);

    }

}

static bool geostar_speed_switch(struct gps_device_t *session,

                                 speed_t speed, char parity, int stopbits)

{

    unsigned char buf[4 * 4];

    int iparity;

    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;

    }

    iparity = parity;

    putbe32(buf, 0, session->driver.geostar.physical_port);

    putbe32(buf, 4, speed);

    putbe32(buf, 8, stopbits);

    putbe32(buf, 12, iparity);

    (void)geostar_write(session, 0x41, buf, 4);

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

}

static void geostar_mode(struct gps_device_t *session, int mode)

{

    if (MODE_NMEA == mode) {

        unsigned char buf[1 * 4];

        // Switch to NMEA mode

        putbe32(buf, 0, 1);

        (void)geostar_write(session, 0x46, buf, 1);

    } else if (MODE_BINARY == mode) {

        // Switch to binary mode

        (void)nmea_send(session, "$GPSGG,SWPROT");

    } else {

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

                 "unknown mode %i requested\n", mode);

    }

}

static double geostar_time_offset(struct gps_device_t *session UNUSED)

{

    return 0.31;

}

// this is everything we export

// *INDENT-OFF*

const struct gps_type_t driver_geostar =

{

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

    .packet_type    = GEOSTAR_PACKET,        // associated lexer packet type

    .flags          = DRIVER_STICKY,         // remember this

    .trigger        = NULL,                  // no trigger

    .channels       = GEOSTAR_CHANNELS,      // consumer-grade GPS/GLONASS

    .probe_detect   = geostar_detect,        // probe for device

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

    .parse_packet   = geostar_parse_input,   // parse message packets

    .rtcm_writer    = NULL,                  // no DGPS corrections

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

    .event_hook     = geostar_event_hook,    // fire on various lifetime events

    .speed_switcher = geostar_speed_switch,  // change baud rate

    .mode_switcher  = geostar_mode,          // there is a mode switcher

    .rate_switcher  = NULL,                  // no rate switcher

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

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

    .control_send   = geostar_control_send,  // how to send commands

    .time_offset     = geostar_time_offset,

};

// *INDENT-ON*

#endif  // GEOSTAR_ENABLE

// vim: set expandtab shiftwidth=4