/src/gpsd/gpsd-3.27.6~dev/drivers/driver_nmea2000.c

Source
/*
 * NMEA2000 over CAN.
 *
 * NMEA2000 is proprietary and the doc is not public.
 * Much of this code is reverse engineered or built from
 * the sparse doc some vendors provide on their interpretation
 * of the specification.
 *
 * Here is one good source of reverse engineered info:
 *     https://github.com/canboat/canboat
 *     https://canboat.github.io/canboat/canboat.html
 *
 * Message contents can be had from canboat/analyzer:
 *     analyzer -explain
 *
 * This file is Copyright by the GPSD project
 * SPDX-License-Identifier: BSD-2-clause
 */

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

#if defined(NMEA2000_ENABLE)

#include <ctype.h>
#include <errno.h>                  // for strerror(errno), errno)
#include <fcntl.h>
#include <linux/can.h>              // for  struct can_frame
#include <linux/can/error.h>        // for CAN_ERR_BUSOFF, etc.
#include <linux/can/raw.h>
#include <math.h>
#include <net/if.h>
#include <stdarg.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <time.h>
#include <unistd.h>

#include "../include/gpsd.h"
#include "../include/libgps.h"
#include "../include/driver_nmea2000.h"
#include "../include/bits.h"
#include "../include/timespec.h"


#define LOG_FILE 1
#define NMEA2000_NETS 4
/* NMEA 2000 unit is a byte, but 254 is "request for address claim"
 * and 255 is broadcast address.  So 253 is the number of units possible,
 * and the highest unit number. */
#define NMEA2000_UNITS 253
#define CAN_NAMELEN 32
#define MIN(a,b) ((a < b) ? a : b)

#define NMEA2000_DEBUG_AIS 0
#define NMEA2000_FAST_DEBUG 0

static struct gps_device_t *nmea2000_units[NMEA2000_NETS][NMEA2000_UNITS];
static char can_interface_name[NMEA2000_NETS][CAN_NAMELEN+1];

typedef struct PGN {
    unsigned int  pgn;
    unsigned int  fast;
    unsigned int  type;
    gps_mask_t    (* func)(unsigned char *bu, int len, struct PGN *pgn,
                           struct gps_device_t *session);
    const char    *name;
} PGN;


#if LOG_FILE
FILE *logFile = NULL;
#endif  // of if LOG_FILE

extern bool __attribute__ ((weak)) gpsd_add_device(const char *device_name,
                                                   bool flag_nowait);

#define SHIFT32 0x100000000l

static int scale_int(int32_t var, const int64_t factor)
{
    int64_t ret;

    ret   = var;
    ret  *= factor;
    ret >>= 32;

    return (int)ret;
}

static void print_data(struct gps_context_t *context,
                       unsigned char *buffer, int len, PGN *pgn)
{
    if (LOG_IO <= libgps_debuglevel) {
        int   l1;
        char  bu[128];

        int ptr = 0;
        int l2 = sprintf(&bu[ptr], "got data:%6u:%3d: ", pgn->pgn, len);
        ptr += l2;
        for (l1 = 0; l1 < len; l1++) {
            if (((l1 % 20) == 0) && (l1 != 0)) {
                GPSD_LOG(LOG_IO, &context->errout, "%s\n", bu);
                ptr = 0;
                l2 = sprintf(&bu[ptr], "                   : ");
                ptr += l2;
            }
            l2 = sprintf(&bu[ptr], "%02ux ", (unsigned int)buffer[l1]);
            ptr += l2;
        }
        GPSD_LOG(LOG_IO, &context->errout, "%s\n", bu);
    }
}

static gps_mask_t get_mode(struct gps_device_t *session)
{
    if (1 & session->driver.nmea2000.mode_valid) {
        session->newdata.mode = session->driver.nmea2000.mode;
    } else {
        session->newdata.mode = MODE_NOT_SEEN;
    }

    if (2 & session->driver.nmea2000.mode_valid) {
        return MODE_SET | USED_IS;
    } else {
        return MODE_SET;
    }
}


static int decode_ais_header(struct gps_context_t *context,
                             unsigned char *bu,
                             int len,
                             struct ais_t *ais,
                             unsigned int mask)
{
    if (4 < len) {
        ais->type   = (unsigned int) ( bu[0]       & 0x3f);
        ais->repeat = (unsigned int) ((bu[0] >> 6) & 0x03);
        ais->mmsi   = (unsigned int)  getleu32(bu, 1);
        ais->mmsi  &= mask;
        GPSD_LOG(LOG_INF, &context->errout,
                 "NMEA2000 AIS  message type %u, MMSI %09d:\n",
                 ais->type, ais->mmsi);
        return 1;
    } else {
        ais->type   =  0;
        ais->repeat =  0;
        ais->mmsi   =  0;
        GPSD_LOG(LOG_ERROR, &context->errout,
                 "NMEA2000 AIS  message type %u, too short message.\n",
                 ais->type);
    }
    return 0;
}


static void decode_ais_channel_info(unsigned char *bu,
                                    int len,
                                    unsigned int offset,
                                    struct gps_device_t *session)
{
    unsigned int pos, bpos;
    uint16_t x;

    pos = offset / 8;
    bpos = offset % 8;
    if (pos >= (unsigned int)len) {
        session->driver.aivdm.ais_channel = 'A';
        return;
    }
    x = getleu16(bu, pos);
    x = (uint16_t)((x >> bpos) & 0x1f);
    switch (x) {
    case 1:
        FALLTHROUGH
    case 3:
        session->driver.aivdm.ais_channel = 'B';
        break;
    default:
        session->driver.aivdm.ais_channel = 'A';
        break;
    }
    return;
}


static int ais_turn_rate(int rate)
{
    if (0 > rate) {
        return -ais_turn_rate(-rate);
    }
    return (int)(4.733 * sqrt(rate * RAD_2_DEG * .0001 * 60.0));
}


static double ais_direction(unsigned int val, double scale)
{
    if ((0xffff == val) &&
        (1.0 == scale)) {
        return 511.0;
    }
    return val * RAD_2_DEG * 0.0001 * scale;
}


/*
 *   PGN 59392: ISO  Acknowledgment
 */
static gps_mask_t hnd_059392(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 60928: ISO  Address Claim
 */
static gps_mask_t hnd_060928(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 126208: NMEA Command/Request/Acknowledge
 */
static gps_mask_t hnd_126208(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 126464: ISO Transmit/Receive PGN List
 */
static gps_mask_t hnd_126464(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 126996: ISO  Product Information
 */
static gps_mask_t hnd_126996(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 127258: GNSS Magnetic Variation
 *
 *   1 Sequence ID
 *   2 Variation Source
 *   3 Reserved Bits
 *   4 Age of Service (Date)
 *   5 Variation
 *   6 Reserved B
 */
static gps_mask_t hnd_127258(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    // FIXME?  Get magnetic variation
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 129025: GNSS Position Rapid Update
 */
static gps_mask_t hnd_129025(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    session->newdata.latitude = getles32(bu, 0) * 1e-7;
    session->newdata.longitude = getles32(bu, 4) * 1e-7;

    return LATLON_SET | get_mode(session);
}


/*
 *   PGN 129026: GNSS COG and SOG Rapid Update
 */
static gps_mask_t hnd_129026(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    session->driver.nmea2000.sid[0]  =  bu[0];

    session->newdata.track           =  getleu16(bu, 2) * 1e-4 * RAD_2_DEG;
    session->newdata.speed           =  getleu16(bu, 4) * 1e-2;

    return SPEED_SET | TRACK_SET | get_mode(session);
}


/*
 * PGN: 126992 / 00370020 / 1F010 - 8 - System Time
 *
 *  Field #1: SID
 *                  Bits: 8
 *                  Signed: false
 *  Field #2: Source
 *                  Bits: 4
 *                  Type: Lookup table
 *                  Signed: false
 *                  Lookup: 0=GPS
 *                  Lookup: 1=GLONASS
 *                  Lookup: 2=Radio Station
 *                  Lookup: 3=Local Cesium clock
 *                  Lookup: 4=Local Rubidium clock
 *                  Lookup: 5=Local Crystal clock
 *  Field #3: Reserved - Reserved
 *                  Bits: 4
 *                  Type: Binary data
 *                  Signed: false
 *  Field #4: Date - Days since January 1, 1970
 *                  Bits: 16
 *                  Units: days
 *                  Type: Date
 *                  Resolution: 1
 *                  Signed: false
 *  Field #5: Time - Seconds since midnight
 *                  Bits: 32
 *                  Units: s
 *                  Type: Time
 *                  Resolution: 0.0001
 *                  Signed: false
 *
 */
static gps_mask_t hnd_126992(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    // uint8_t        sid;
    // uint8_t        source;
    uint64_t usecs;       // time in us

    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    // sid        = bu[0];
    // source     = bu[1] & 0x0f;

    usecs = getleu32(bu, 4) * (uint64_t)100;
    USTOTS(&session->newdata.time, usecs);
    session->newdata.time.tv_sec += (time_t)(getleu16(bu, 2) * 24 * 60 * 60);

    return TIME_SET | get_mode(session);
}


static const int mode_tab[] = {MODE_NO_FIX, MODE_2D, MODE_3D, MODE_NO_FIX,
                               MODE_NO_FIX, MODE_NO_FIX, MODE_NO_FIX,
                               MODE_NO_FIX};

/*
 *   PGN 129539: GNSS DOPs
 */
static gps_mask_t hnd_129539(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    gps_mask_t mask;
    unsigned int req_mode;
    unsigned int act_mode;

    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    mask                             = 0;
    session->driver.nmea2000.sid[1]  = bu[0];

    session->driver.nmea2000.mode_valid |= 1;

    req_mode = (unsigned int)((bu[1] >> 0) & 0x07);
    act_mode = (unsigned int)((bu[1] >> 3) & 0x07);

    /* This is a workaround for some GARMIN plotter,
     * actual mode auto makes no sense for me! */
    if ((3 == act_mode) &&
        (3 != req_mode)) {
        act_mode = req_mode;
    }

    session->driver.nmea2000.mode    = mode_tab[act_mode];

    session->gpsdata.dop.hdop        = getleu16(bu, 2) * 1e-2;
    session->gpsdata.dop.vdop        = getleu16(bu, 4) * 1e-2;
    session->gpsdata.dop.tdop        = getleu16(bu, 6) * 1e-2;
    mask                            |= DOP_SET;

    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d): sid:%02x hdop:%5.2f vdop:%5.2f tdop:%5.2f\n",
             pgn->pgn,
             session->driver.nmea2000.unit,
             session->driver.nmea2000.sid[1],
             session->gpsdata.dop.hdop,
             session->gpsdata.dop.vdop,
             session->gpsdata.dop.tdop);

    return mask | get_mode(session);
}


/*
 *   PGN 129540: GNSS Satellites in View
 */
static gps_mask_t hnd_129540(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    int         l1;
    int         expected_len;

    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    session->driver.nmea2000.sid[2]           = bu[0];
    session->gpsdata.satellites_visible       = (int)bu[2];
    if (MAXCHANNELS <= session->gpsdata.satellites_visible) {
        // Handle a CVE for overrunning skyview[]
        GPSD_LOG(LOG_WARN, &session->context->errout,
                 "pgn %6d(%3d): Too many sats %d\n",
                 pgn->pgn, session->driver.nmea2000.unit,
                 session->gpsdata.satellites_visible);
        session->gpsdata.satellites_visible = MAXCHANNELS;
    }
    expected_len = 3 + (12 * session->gpsdata.satellites_visible);
    if (len != expected_len) {
        GPSD_LOG(LOG_WARN, &session->context->errout,
                 "pgn %6d(%3d): wrong length %d s/b %d\n",
                 pgn->pgn, session->driver.nmea2000.unit,
                 len, expected_len);
        return 0;
    }

    memset(session->gpsdata.skyview, '\0', sizeof(session->gpsdata.skyview));
    for (l1 = 0; l1 < session->gpsdata.satellites_visible; l1++) {
        int offset = 3 + (12 * l1);
        double elev  = getles16(bu, offset + 1) * 1e-4 * RAD_2_DEG;
        double azi   = getleu16(bu, offset + 3) * 1e-4 * RAD_2_DEG;
        double snr   = getles16(bu, offset + 5) * 1e-2;

        int svt   = (int)(bu[offset + 11] & 0x0f);

        session->gpsdata.skyview[l1].elevation  = elev;
        session->gpsdata.skyview[l1].azimuth    = azi;
        session->gpsdata.skyview[l1].ss         = snr;
        session->gpsdata.skyview[l1].PRN        = (int16_t)bu[offset];
        session->gpsdata.skyview[l1].used = false;
        if ((2 == svt) ||
            (5 == svt)) {
            session->gpsdata.skyview[l1].used = true;
        }
    }
    session->driver.nmea2000.mode_valid |= 2;
    return SATELLITE_SET | USED_IS;
}


/*
 * PGN: 129029 / 00374005 / 1F805 - 51 - GNSS Position Data
 *
 *      The last 3 fields repeat until the data is exhausted.
 *
 *   Field #1: SID
 *                   Bits: 8
 *                   Signed: false
 *   Field #2: Date - Days since January 1, 1970
 *                   Bits: 16
 *                   Units: days
 *                   Type: Date
 *                   Resolution: 1
 *                   Signed: false
 *   Field #3: Time - Seconds since midnight
 *                   Bits: 32
 *                   Units: s
 *                   Type: Time
 *                   Resolution: 0.0001
 *                   Signed: false
 *   Field #4: Latitude
 *                   Bits: 64
 *                   Units: deg
 *                   Type: Latitude
 *                   Resolution: 0.0000000000000001
 *                   Signed: true
 *   Field #5: Longitude
 *                   Bits: 64
 *                   Units: deg
 *                   Type: Longitude
 *                   Resolution: 0.0000000000000001
 *                   Signed: true
 *   Field #6: Altitude - Altitude referenced to WGS-84
 *                   Bits: 64
 *                   Units: m
 *                   Resolution: 1e-06
 *                   Signed: true
 *   Field #7: GNSS type
 *                   Bits: 4
 *                   Type: Lookup table
 *                   Signed: false
 *                   Lookup: 0=GPS
 *                   Lookup: 1=GLONASS
 *                   Lookup: 2=GPS+GLONASS
 *                   Lookup: 3=GPS+SBAS/WAAS
 *                   Lookup: 4=GPS+SBAS/WAAS+GLONASS
 *                   Lookup: 5=Chayka
 *                   Lookup: 6=integrated
 *                   Lookup: 7=surveyed
 *                   Lookup: 8=Galileo
 *   Field #8: Method
 *                   Bits: 4
 *                   Type: Lookup table
 *                   Signed: false
 *                   Lookup: 0=no GNSS
 *                   Lookup: 1=GNSS fix
 *                   Lookup: 2=DGNSS fix
 *                   Lookup: 3=Precise GNSS
 *                   Lookup: 4=RTK Fixed Integer
 *                   Lookup: 5=RTK float
 *                   Lookup: 6=Estimated (DR) mode
 *                   Lookup: 7=Manual Input
 *                   Lookup: 8=Simulate mode
 *   Field #9: Integrity
 *                   Bits: 2
 *                   Type: Lookup table
 *                   Signed: false
 *                   Lookup: 0=No integrity checking
 *                   Lookup: 1=Safe
 *                   Lookup: 2=Caution
 *   Field #10: Reserved - Reserved
 *                   Bits: 6
 *                   Type: Binary data
 *                   Signed: false
 *   Field #11: Number of SVs - Number of satellites used in solution
 *                   Bits: 8
 *                   Signed: false
 *   Field #12: HDOP - Horizontal dilution of precision
 *                   Bits: 16
 *                   Resolution: 0.01
 *                   Signed: true
 *   Field #13: PDOP - Probable dilution of precision
 *                   Bits: 16
 *                   Resolution: 0.01
 *                   Signed: true
 *   Field #14: Geoidal Separation - Geoidal Separation
 *                   Bits: 32
 *                   Units: m
 *                   Resolution: 0.01
 *                   Signed: true
 *   Field #15: Reference Stations - Number of reference stations
 *                   Bits: 8
 *                   Signed: false
 *   Field #16: Reference Station Type
 *                   Bits: 4
 *                   Type: Lookup table
 *                   Signed: false
 *                   Lookup: 0=GPS
 *                   Lookup: 1=GLONASS
 *                   Lookup: 2=GPS+GLONASS
 *                   Lookup: 3=GPS+SBAS/WAAS
 *                   Lookup: 4=GPS+SBAS/WAAS+GLONASS
 *                   Lookup: 5=Chayka
 *                   Lookup: 6=integrated
 *                   Lookup: 7=surveyed
 *                   Lookup: 8=Galileo
 *   Field #17: Reference Station ID
 *                   Bits: 12
 *                   Units:
 *                   Signed: false
 *   Field #18: Age of DGNSS Corrections
 *                   Bits: 16
 *                   Units: s
 *                   Resolution: 0.01
 *                   Signed: false
 *
 */
static gps_mask_t hnd_129029(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    gps_mask_t mask;
    uint64_t usecs;    // time in us

    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    mask = 0;
    session->driver.nmea2000.sid[3]  = bu[0];

    // field 3 is time in 0.1 ms
    usecs = getleu32(bu, 3) * (uint64_t)100;
    USTOTS(&session->newdata.time, usecs);
    // add in the date from field 2
    session->newdata.time.tv_sec += (time_t)(getleu16(bu,1) * 24 * 60 * 60);
    mask |= TIME_SET;

    session->newdata.latitude = getles64(bu, 7) * 1e-16;
    session->newdata.longitude = getles64(bu, 15) * 1e-16;
    mask |= LATLON_SET;

    session->newdata.altHAE = getles64(bu, 23) * 1e-6;
    mask |= ALTITUDE_SET;

//  printf("mode %x %x\n", (bu[31] >> 4) & 0x0f, bu[31]);
    switch ((bu[31] >> 4) & 0x0f) {
    case 0:
        session->newdata.status = STATUS_UNK;
        break;
    case 1:
        session->newdata.status = STATUS_GPS;
        break;
    case 2:
        session->newdata.status = STATUS_DGPS;
        break;
    case 3:
        session->newdata.status = STATUS_PPS_FIX;
        break;
    case 4:
        session->newdata.status = STATUS_RTK_FIX;
        break;
    case 5:
        session->newdata.status = STATUS_RTK_FLT;
        break;
    case 6:
        session->newdata.status = STATUS_DR;
        break;
    case 7:
        session->newdata.status = STATUS_TIME;
        break;
    case 8:
        session->newdata.status = STATUS_SIM;
        break;
    default:
        session->newdata.status = STATUS_UNK;
        break;
    }
    mask |= STATUS_SET;

    session->newdata.geoid_sep = getles32(bu, 38) / 100.0;

    session->gpsdata.satellites_used = (int)bu[33];

    session->gpsdata.dop.hdop = getleu16(bu, 34) * 1e-2;
    session->gpsdata.dop.pdop = getleu16(bu, 36) * 1e-2;
    mask |= DOP_SET;

    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d): sid:%02x hdop:%5.2f pdop:%5.2f\n",
             pgn->pgn,
             session->driver.nmea2000.unit,
             session->driver.nmea2000.sid[1],
             session->gpsdata.dop.hdop,
             session->gpsdata.dop.pdop);
    return mask | get_mode(session);
}


/*
 *   PGN 129038: AIS  Class A Position Report
 */
static gps_mask_t hnd_129038(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0xffffffffU)) {
        ais->type1.lon = (int)scale_int(getles32(bu, 5),
                                        (int64_t)(SHIFT32 *.06L));
        ais->type1.lat = (int)scale_int(getles32(bu, 9),
                                        (int64_t)(SHIFT32 *.06L));
        ais->type1.accuracy  = (bool)         ((bu[13] >> 0) & 0x01);
        ais->type1.raim      = (bool)         ((bu[13] >> 1) & 0x01);
        ais->type1.second    = (unsigned int) ((bu[13] >> 2) & 0x3f);
        ais->type1.course = (unsigned int)ais_direction(
                                       (unsigned int)getleu16(bu, 14), 10.0);
        ais->type1.speed = (unsigned int)(getleu16(bu, 16) *
                                          MPS_TO_KNOTS * 0.01 / 0.1);
        ais->type1.radio     = (unsigned int) (getleu32(bu, 18) & 0x7ffff);
        ais->type1.heading =
            (unsigned int)ais_direction((unsigned int)getleu16(bu, 21), 1.0);
        ais->type1.turn = ais_turn_rate((int)getles16(bu, 23));
        ais->type1.status    = (unsigned int) ((bu[25] >> 0) & 0x0f);
        ais->type1.maneuver  = 0;  // Not transmitted ????
        decode_ais_channel_info(bu, len, 163, session);

        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 129039: AIS  Class B Position Report
 */
static gps_mask_t hnd_129039(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0xffffffffU)) {
        ais->type18.lon = (int)scale_int(getles32(bu, 5),
                                         (int64_t)(SHIFT32 *.06L));
        ais->type18.lat = (int)scale_int(getles32(bu, 9),
                                         (int64_t)(SHIFT32 *.06L));
        ais->type18.accuracy = (bool)         ((bu[13] >> 0) & 0x01);
        ais->type18.raim     = (bool)         ((bu[13] >> 1) & 0x01);
        ais->type18.second   = (unsigned int) ((bu[13] >> 2) & 0x3f);
        ais->type18.course =
            (unsigned int)ais_direction((unsigned int) getleu16(bu, 14), 10.0);
        ais->type18.speed = (unsigned int)(getleu16(bu, 16) *
                                           MPS_TO_KNOTS * 0.01 / 0.1);
        ais->type18.radio    = (unsigned int) (getleu32(bu, 18) & 0x7ffff);
        ais->type18.heading =
            (unsigned int)ais_direction((unsigned int) getleu16(bu, 21), 1.0);
        ais->type18.reserved = 0;
        ais->type18.regional = (unsigned int) ((bu[24] >> 0) & 0x03);
        ais->type18.cs       = (bool)         ((bu[24] >> 2) & 0x01);
        ais->type18.display  = (bool)         ((bu[24] >> 3) & 0x01);
        ais->type18.dsc      = (bool)         ((bu[24] >> 4) & 0x01);
        ais->type18.band     = (bool)         ((bu[24] >> 5) & 0x01);
        ais->type18.msg22    = (bool)         ((bu[24] >> 6) & 0x01);
        ais->type18.assigned = (bool)         ((bu[24] >> 7) & 0x01);
        decode_ais_channel_info(bu, len, 163, session);

        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 129040: AIS Class B Extended Position Report
 *
 *  No test case for this message at the moment
 */
static gps_mask_t hnd_129040(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0xffffffffU)) {
        uint16_t length, beam, to_bow, to_starboard;
        int l;

        ais->type19.lon = (int)scale_int(getles32(bu, 5),
                                         (int64_t)(SHIFT32 *.06L));
        ais->type19.lat = (int)scale_int(getles32(bu, 9),
                                         (int64_t)(SHIFT32 *.06L));
        ais->type19.accuracy     = (bool)         ((bu[13] >> 0) & 0x01);
        ais->type19.raim         = (bool)         ((bu[13] >> 1) & 0x01);
        ais->type19.second       = (unsigned int) ((bu[13] >> 2) & 0x3f);
        ais->type19.course =
            (unsigned int)ais_direction((unsigned int)getleu16(bu, 14), 10.0);
        ais->type19.speed =
            (unsigned int)(getleu16(bu, 16) * MPS_TO_KNOTS * 0.01 / 0.1);
        ais->type19.reserved     = (unsigned int) ((bu[18] >> 0) & 0xff);
        ais->type19.regional     = (unsigned int) ((bu[19] >> 0) & 0x0f);
        ais->type19.shiptype     = (unsigned int) ((bu[20] >> 0) & 0xff);
        ais->type19.heading =
           (unsigned int)  ais_direction((unsigned int) getleu16(bu, 21), 1.0);
        length                   =                 getleu16(bu, 24);
        beam                     =                 getleu16(bu, 26);
        to_starboard             =                 getleu16(bu, 28);
        to_bow                   =                 getleu16(bu, 30);
        if ((length == 0xffff) || (to_bow       == 0xffff)) {
            length       = 0;
            to_bow       = 0;
        }
        if ((beam   == 0xffff) || (to_starboard == 0xffff)) {
            beam         = 0;
            to_starboard = 0;
        }
        ais->type19.to_bow       = (unsigned int) (to_bow/10);
        ais->type19.to_stern     = (unsigned int) ((length-to_bow)/10);
        ais->type19.to_port      = (unsigned int) ((beam-to_starboard)/10);
        ais->type19.to_starboard = (unsigned int) (to_starboard/10);
        ais->type19.epfd         = (unsigned int) ((bu[23] >> 4) & 0x0f);
        ais->type19.dte          = (unsigned int) ((bu[52] >> 0) & 0x01);
        ais->type19.assigned     = (bool)         ((bu[52] >> 1) & 0x01);
        for (l = 0; l < AIS_SHIPNAME_MAXLEN; l++) {
            ais->type19.shipname[l] = (char)bu[32+l];
        }
        ais->type19.shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;
        decode_ais_channel_info(bu, len, 422, session);

        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 129793: AIS UTC and Date Report
 */
static gps_mask_t hnd_129793(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0xffffffffU)) {
        uint32_t  time;
        uint32_t  date;
        time_t    date1;
        struct tm date2;

        ais->type4.lon = (int)scale_int(getles32(bu, 5),
                                        (int64_t)(SHIFT32 *.06L));
        ais->type4.lat = (int)scale_int(getles32(bu, 9),
                                        (int64_t)(SHIFT32 *.06L));
        ais->type4.accuracy     = (bool)         ((bu[13] >> 0) & 0x01);
        ais->type4.raim         = (bool)         ((bu[13] >> 1) & 0x01);

        time = getleu32(bu, 14);
        if (0xffffffff != time) {
            time                = time / 10000;
            ais->type4.second   = time % 60; time = time / 60;
            ais->type4.minute   = time % 60; time = time / 60;
            ais->type4.hour     = time % 24;
        } else {
            ais->type4.second   = AIS_SECOND_NOT_AVAILABLE;
            ais->type4.minute   = AIS_MINUTE_NOT_AVAILABLE;
            ais->type4.hour     = AIS_HOUR_NOT_AVAILABLE;
        }

        ais->type4.radio        = (unsigned int) (getleu32(bu, 18) & 0x7ffff);

        date = getleu16(bu, 21);
        if (0xffff != date) {
            date1 = (time_t)date * (24L *60L *60L);
            (void) gmtime_r(&date1, &date2);
            ais->type4.year     = (unsigned int) (date2.tm_year+1900);
            ais->type4.month    = (unsigned int) (date2.tm_mon+1);
            ais->type4.day      = (unsigned int) (date2.tm_mday);
        } else {
            ais->type4.day      = AIS_DAY_NOT_AVAILABLE;
            ais->type4.month    = AIS_MONTH_NOT_AVAILABLE;
            ais->type4.year     = AIS_YEAR_NOT_AVAILABLE;
        }

        ais->type4.epfd         = (unsigned int) ((bu[23] >> 4) & 0x0f);

        decode_ais_channel_info(bu, len, 163, session);

        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 129794: AIS Class A Static and Voyage Related Data
 */
static gps_mask_t hnd_129794(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0xffffffffU)) {
        uint16_t  length, beam, to_bow, to_starboard, date;
        int       l;
        uint32_t  time;
        time_t    date1;
        struct tm date2;
        int       cpy_stop;

        ais->type5.ais_version   = (unsigned int) ((bu[73] >> 0) & 0x03);
        ais->type5.imo           = (unsigned int)  getleu32(bu,  5);
        if (0xffffffffU == ais->type5.imo) {
            ais->type5.imo       = 0;
        }
        ais->type5.shiptype      = (unsigned int) ((bu[36] >> 0) & 0xff);
        length                   =                 getleu16(bu, 37);
        beam                     =                 getleu16(bu, 39);
        to_starboard             =                 getleu16(bu, 41);
        to_bow                   =                 getleu16(bu, 43);
        if ((0xffff == length) ||
            (0xffff == to_bow)) {
            length       = 0;
            to_bow       = 0;
        }
        if ((0xffff == beam) ||
            (0xffff == to_starboard)) {
            beam         = 0;
            to_starboard = 0;
        }
        ais->type5.to_bow        = (unsigned int) (to_bow/10);
        ais->type5.to_stern      = (unsigned int) ((length-to_bow)/10);
        ais->type5.to_port       = (unsigned int) ((beam-to_starboard)/10);
        ais->type5.to_starboard  = (unsigned int) (to_starboard/10);
        ais->type5.epfd          = (unsigned int) ((bu[73] >> 2) & 0x0f);
        date                     =                 getleu16(bu, 45);
        time                     =                 getleu32(bu, 47);
        date1                    = (time_t)       (date*24*60*60);
        (void) gmtime_r(&date1, &date2);
        ais->type5.month         = (unsigned int) (date2.tm_mon+1);
        ais->type5.day           = (unsigned int) (date2.tm_mday);
        ais->type5.minute        = (unsigned int) (time/(10000*60));
        ais->type5.hour          = (unsigned int) (ais->type5.minute/60);
        ais->type5.minute =
            (unsigned int)(ais->type5.minute-(ais->type5.hour * 60));

        ais->type5.draught       = (unsigned int) (getleu16(bu, 51)/10);
        ais->type5.dte           = (unsigned int) ((bu[73] >> 6) & 0x01);

        for (l = 0, cpy_stop = 0; l < 7; l++) {
            char next;

            next = (char) bu[9+l];
            if ((next < ' ') ||
                (next > 0x7e)) {
                cpy_stop = 1;
            }
            if (cpy_stop == 0) {
                ais->type5.callsign[l] = next;
            } else {
                ais->type5.callsign[l] = 0;
            }
        }
        ais->type5.callsign[7]   = (char) 0;

        for (l = 0, cpy_stop = 0; l < AIS_SHIPNAME_MAXLEN; l++) {
            char next;

            next = (char) bu[16+l];
            if ((next < ' ') ||
                (next > 0x7e)) {
                cpy_stop = 1;
            }
            if (cpy_stop == 0) {
                ais->type5.shipname[l] = next;
            } else {
                ais->type5.shipname[l] = 0;
            }
        }
        ais->type5.shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;

        for (l = 0, cpy_stop = 0; l < 20; l++) {
            char next;

            next = (char) bu[53+l];
            if ((next < ' ') ||
                (next > 0x7e)) {
                cpy_stop = 1;
            }
            if (cpy_stop == 0) {
                ais->type5.destination[l] = next;
            } else {
                ais->type5.destination[l] = 0;
            }
        }
        ais->type5.destination[20] = (char) 0;
#if NMEA2000_DEBUG_AIS
        printf("AIS: MMSI:  %09u\n",
               ais->mmsi);
        printf("AIS: name:  %-20.20s i:%8u c:%-8.8s b:%6u s:%6u p:%6u"
               "s:%6u dr:%4.1f\n",
               ais->type5.shipname,
               ais->type5.imo,
               ais->type5.callsign,
               ais->type5.to_bow,
               ais->type5.to_stern,
               ais->type5.to_port,
               ais->type5.to_starboard,
               ais->type5.draught/10.0);
        printf("AIS: arrival:%-20.20s at %02u-%02u-%04d %02u:%0u\n",
               ais->type5.destination,
               ais->type5.day,
               ais->type5.month,
               date2.tm_year+1900,
               ais->type5.hour,
               ais->type5.minute);
#endif  // end of #if NMEA2000_DEBUG_AIS
        decode_ais_channel_info(bu, len, 592, session);
        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 129798: AIS SAR Aircraft Position Report
 *
 * No test case for this message at the moment
 */
static gps_mask_t hnd_129798(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0xffffffffU)) {
        ais->type9.lon = (int)scale_int(getles32(bu, 5),
                                        (int64_t)(SHIFT32 *.06L));
        ais->type9.lat = (int)scale_int(getles32(bu, 9),
                                        (int64_t)(SHIFT32 *.06L));
        ais->type9.accuracy  = (bool)         ((bu[13] >> 0) & 0x01);
        ais->type9.raim      = (bool)         ((bu[13] >> 1) & 0x01);
        ais->type9.second    = (unsigned int) ((bu[13] >> 2) & 0x3f);
        ais->type9.course =
            (unsigned int)ais_direction((unsigned int)getleu16(bu, 14), 10.0);
        ais->type9.speed =
            (unsigned int)(getleu16(bu, 16) * MPS_TO_KNOTS * 0.01 / 0.1);
        ais->type9.radio     = (unsigned int) (getleu32(bu, 18) & 0x7ffff);
        ais->type9.alt       = (unsigned int) (getleu64(bu, 21)/1000000);
        ais->type9.regional  = (unsigned int) ((bu[29] >> 0) & 0xff);
        ais->type9.dte       = (unsigned int) ((bu[30] >> 0) & 0x01);
//      ais->type9.spare     = (bu[30] >> 1) & 0x7f;
        ais->type9.assigned  = 0;  // Not transmitted ????
        decode_ais_channel_info(bu, len, 163, session);

        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 129802: AIS Safety Related Broadcast Message
 *
 * No test case for this message at the moment
 */
static gps_mask_t hnd_129802(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0x3fffffff)) {
        int                   l;

//      ais->type14.channel = (bu[ 5] >> 0) & 0x1f;
        for (l = 0; l < 36; l++) {
            ais->type14.text[l] = (char) bu[6+l];
        }
        ais->type14.text[36] = (char) 0;
        decode_ais_channel_info(bu, len, 40, session);

        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 129809: AIS Class B CS Static Data Report, Part A
 */
static gps_mask_t hnd_129809(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0xffffffffU)) {
        int l;
        int index   = session->driver.aivdm.context[0].type24_queue.index;
        struct ais_type24a_t *saveptr =
            &session->driver.aivdm.context[0].type24_queue.ships[index];

        GPSD_LOG(LOG_PROG, &session->context->errout,
                 "NMEA2000: AIS message 24A from %09u stashed.\n",
                 ais->mmsi);

        for (l = 0; l < AIS_SHIPNAME_MAXLEN; l++) {
            ais->type24.shipname[l] = (char) bu[ 5+l];
            saveptr->shipname[l] = (char) bu[ 5+l];
        }
        ais->type24.shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;
        saveptr->shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;

        saveptr->mmsi = ais->mmsi;

        index += 1;
        index %= MAX_TYPE24_INTERLEAVE;
        session->driver.aivdm.context[0].type24_queue.index = index;

        decode_ais_channel_info(bu, len, 200, session);

        ais->type24.part = part_a;
        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 129810: AIS Class B CS Static Data Report, Part B
 */
static gps_mask_t hnd_129810(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    struct ais_t *ais;

    ais =  &session->gpsdata.ais;
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);

    if (0 != decode_ais_header(session->context, bu, len, ais, 0xffffffffU)) {
        int l, i;

        ais->type24.shiptype = (unsigned int) ((bu[ 5] >> 0) & 0xff);

        for (l = 0; l < 7; l++) {
            ais->type24.vendorid[l] = (char) bu[ 6+l];
        }
        ais->type24.vendorid[7] = (char) 0;

        for (l = 0; l < 7; l++) {
            ais->type24.callsign[l] = (char) bu[13+l];
        }
        ais->type24.callsign[7] = (char )0;

        ais->type24.model = 0;
        ais->type24.serial = 0;

        if (AIS_AUXILIARY_MMSI(ais->mmsi)) {
            ais->type24.mothership_mmsi   = (unsigned int) (getleu32(bu, 28));
        } else {
            uint16_t length, beam, to_bow, to_starboard;

            length                        =                 getleu16(bu, 20);
            beam                          =                 getleu16(bu, 22);
            to_starboard                  =                 getleu16(bu, 24);
            to_bow                        =                 getleu16(bu, 26);
            if ((length == 0xffff) || (to_bow       == 0xffff)) {
                length       = 0;
                to_bow       = 0;
            }
            if ((beam   == 0xffff) || (to_starboard == 0xffff)) {
                beam         = 0;
                to_starboard = 0;
            }
            ais->type24.dim.to_bow   = (unsigned int) (to_bow/10);
            ais->type24.dim.to_stern = (unsigned int) ((length-to_bow)/10);
            ais->type24.dim.to_port  = (unsigned int) ((beam-to_starboard)/10);
            ais->type24.dim.to_starboard  = (unsigned int) (to_starboard/10);
        }

        for (i = 0; i < MAX_TYPE24_INTERLEAVE; i++) {
            if (session->driver.aivdm.context[0].type24_queue.ships[i].mmsi ==
                ais->mmsi) {
                for (l = 0; l < AIS_SHIPNAME_MAXLEN; l++) {
                    ais->type24.shipname[l] =
  (char)(session->driver.aivdm.context[0].type24_queue.ships[i].shipname[l]);
                }
                ais->type24.shipname[AIS_SHIPNAME_MAXLEN] = (char) 0;

                GPSD_LOG(LOG_PROG, &session->context->errout,
                         "NMEA2000: AIS 24B from %09u matches a 24A.\n",
                            ais->mmsi);
                // prevent false match if a 24B is repeated
                session->driver.aivdm.context[0].type24_queue.ships[i].mmsi = 0;
#if NMEA2000_DEBUG_AIS
                printf("AIS: MMSI:  %09u\n", ais->mmsi);
                printf("AIS: name:  %-20.20s v:%-8.8s c:%-8.8s b:%6u "
                       "s:%6u p:%6u s:%6u\n",
                       ais->type24.shipname,
                       ais->type24.vendorid,
                       ais->type24.callsign,
                       ais->type24.dim.to_bow,
                       ais->type24.dim.to_stern,
                       ais->type24.dim.to_port,
                       ais->type24.dim.to_starboard);
#endif  // of #if NMEA2000_DEBUG_AIS

                decode_ais_channel_info(bu, len, 264, session);
                ais->type24.part = both;
                return ONLINE_SET | AIS_SET;
            }
        }
#if NMEA2000_DEBUG_AIS
        printf("AIS: MMSI  :  %09u\n", ais->mmsi);
        printf("AIS: vendor:  %-8.8s c:%-8.8s b:%6u s:%6u p:%6u s:%6u\n",
               ais->type24.vendorid,
               ais->type24.callsign,
               ais->type24.dim.to_bow,
               ais->type24.dim.to_stern,
               ais->type24.dim.to_port,
               ais->type24.dim.to_starboard);
#endif  // of #if NMEA2000_DEBUG_AIS
        decode_ais_channel_info(bu, len, 264, session);
        ais->type24.part = part_b;
        return ONLINE_SET | AIS_SET;
    }
    return 0;
}


/*
 *   PGN 127506: PWR DC Detailed Status
 */
static gps_mask_t hnd_127506(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 127508: PWR Battery Status
 */
static gps_mask_t hnd_127508(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 127513: PWR Battery Configuration Status
 */
static gps_mask_t hnd_127513(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 127245: NAV Rudder
 */
static gps_mask_t hnd_127245(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 127250: NAV Vessel Heading
 */
static gps_mask_t hnd_127250(unsigned char *bu, int len,
                             PGN *pgn, struct gps_device_t *session)
{
    int aux;

    print_data(session->context, bu, len, pgn);

    session->gpsdata.attitude.heading = getleu16(bu, 1) * RAD_2_DEG * 0.0001;
//  printf("ATT 0:%8.3f\n",session->gpsdata.attitude.heading);
    aux = getles16(bu, 3);
    if (0x07fff != aux) {
        session->gpsdata.attitude.heading += aux * RAD_2_DEG * 0.0001;
    }
//  printf("ATT 1:%8.3f %6x\n",session->gpsdata.attitude.heading, aux);
    aux = getles16(bu, 5);
    if (0x07fff != aux) {
        session->gpsdata.attitude.heading += aux * RAD_2_DEG * 0.0001;
    }
//  printf("ATT 2:%8.3f %6x\n",session->gpsdata.attitude.heading, aux);

    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return ONLINE_SET | ATTITUDE_SET;
}


/*
 *   PGN 128259: NAV Speed
 */
static gps_mask_t hnd_128259(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 128267: NAV Water Depth
 */
static gps_mask_t hnd_128267(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);

    session->gpsdata.attitude.depth = getleu32(bu, 1) *.01;

    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return ONLINE_SET | ATTITUDE_SET;
}


/*
 *   PGN 128275: NAV Distance Log
 */
static gps_mask_t hnd_128275(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 129283: NAV Cross Track Error
 */
static gps_mask_t hnd_129283(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 129284: NAV Navigation Data
 */
static gps_mask_t hnd_129284(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 129285: NAV Navigation - Route/WP Information
 */
static gps_mask_t hnd_129285(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 130306: NAV Wind Data
 */
static gps_mask_t hnd_130306(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 130310: NAV Water Temp., Outside Air Temp., Atmospheric Pressure
 */
static gps_mask_t hnd_130310(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


/*
 *   PGN 130311: NAV Environmental Parameters
 */
static gps_mask_t hnd_130311(unsigned char *bu, int len, PGN *pgn,
                             struct gps_device_t *session)
{
    print_data(session->context, bu, len, pgn);
    GPSD_LOG(LOG_DATA, &session->context->errout,
             "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit);
    return 0;
}


static const char msg_059392[] = {"ISO  Acknowledgment"};
static const char msg_060928[] = {"ISO  Address Claim"};
static const char msg_126208[] = {"NMEA Command/Request/Acknowledge"};
static const char msg_126464[] = {"ISO  Transmit/Receive PGN List"};
static const char msg_126992[] = {"GNSS System Time"};
static const char msg_126996[] = {"ISO  Product Information"};

static const char msg_127506[] = {"PWR DC Detailed Status"};
static const char msg_127508[] = {"PWR Battery Status"};
static const char msg_127513[] = {"PWR Battery Configuration Status"};

static const char msg_127258[] = {"GNSS Magnetic Variation"};
static const char msg_129025[] = {"GNSS Position Rapid Update"};
static const char msg_129026[] = {"GNSS COG and SOG Rapid Update"};
static const char msg_129029[] = {"GNSS Positition Data"};
static const char msg_129539[] = {"GNSS DOPs"};
static const char msg_129540[] = {"GNSS Satellites in View"};

static const char msg_129038[] = {"AIS  Class A Position Report"};
static const char msg_129039[] = {"AIS  Class B Position Report"};
static const char msg_129040[] = {"AIS  Class B Extended Position Report"};
static const char msg_129793[] = {"AIS  UTC and Date report"};
static const char msg_129794[] = {"AIS  Class A Static and Voyage Related Data"};
static const char msg_129798[] = {"AIS  SAR Aircraft Position Report"};
static const char msg_129802[] = {"AIS  Safety Related Broadcast Message"};
static const char msg_129809[] = {"AIS  Class B CS Static Data Report, Part A"};
static const char msg_129810[] = {"AIS  Class B CS Static Data Report, Part B"};

static const char msg_127245[] = {"NAV Rudder"};
static const char msg_127250[] = {"NAV Vessel Heading"};
static const char msg_128259[] = {"NAV Speed"};
static const char msg_128267[] = {"NAV Water Depth"};
static const char msg_128275[] = {"NAV Distance Log"};

static const char msg_129283[] = {"NAV Cross Track Error"};
static const char msg_129284[] = {"NAV Navigation Data"};
static const char msg_129285[] = {"NAV Navigation - Route/WP Information"};

static const char msg_130306[] = {"NAV Wind Data"};
static const char msg_130310[] = {"NAV Water Temp., Outside Air Temp.,"
                                  "Atmospheric Pressure"};
static const char msg_130311[] = {"NAV Environmental Parameters"};

static const char msg_error [] = {"**error**"};

static PGN gpspgn[] = {{ 59392, 0, 0, hnd_059392, &msg_059392[0]},
                       { 60928, 0, 0, hnd_060928, &msg_060928[0]},
                       {126208, 0, 0, hnd_126208, &msg_126208[0]},
                       {126464, 1, 0, hnd_126464, &msg_126464[0]},
                       {126992, 0, 0, hnd_126992, &msg_126992[0]},
                       {126996, 1, 0, hnd_126996, &msg_126996[0]},
                       {127258, 0, 0, hnd_127258, &msg_127258[0]},
                       {129025, 0, 1, hnd_129025, &msg_129025[0]},
                       {129026, 0, 1, hnd_129026, &msg_129026[0]},
                       {129029, 1, 1, hnd_129029, &msg_129029[0]},
                       {129283, 0, 0, hnd_129283, &msg_129283[0]},
                       {129284, 1, 0, hnd_129284, &msg_129284[0]},
                       {129285, 1, 0, hnd_129285, &msg_129285[0]},
                       {129539, 0, 1, hnd_129539, &msg_129539[0]},
                       {129540, 1, 1, hnd_129540, &msg_129540[0]},
                       {0     , 0, 0, NULL,       &msg_error [0]}};

static PGN aispgn[] = {{ 59392, 0, 0, hnd_059392, &msg_059392[0]},
                       { 60928, 0, 0, hnd_060928, &msg_060928[0]},
                       {126208, 0, 0, hnd_126208, &msg_126208[0]},
                       {126464, 1, 0, hnd_126464, &msg_126464[0]},
                       {126992, 0, 0, hnd_126992, &msg_126992[0]},
                       {126996, 1, 0, hnd_126996, &msg_126996[0]},
                       {129038, 1, 2, hnd_129038, &msg_129038[0]},
                       {129039, 1, 2, hnd_129039, &msg_129039[0]},
                       {129040, 1, 2, hnd_129040, &msg_129040[0]},
                       {129793, 1, 2, hnd_129793, &msg_129793[0]},
                       {129794, 1, 2, hnd_129794, &msg_129794[0]},
                       {129798, 1, 2, hnd_129798, &msg_129798[0]},
                       {129802, 1, 2, hnd_129802, &msg_129802[0]},
                       {129809, 1, 2, hnd_129809, &msg_129809[0]},
                       {129810, 1, 2, hnd_129810, &msg_129810[0]},
                       {0     , 0, 0, NULL,       &msg_error [0]}};

static PGN pwrpgn[] = {{ 59392, 0, 0, hnd_059392, &msg_059392[0]},
                       { 60928, 0, 0, hnd_060928, &msg_060928[0]},
                       {126208, 0, 0, hnd_126208, &msg_126208[0]},
                       {126464, 1, 0, hnd_126464, &msg_126464[0]},
                       {126992, 0, 0, hnd_126992, &msg_126992[0]},
                       {126996, 1, 0, hnd_126996, &msg_126996[0]},
                       {127506, 1, 3, hnd_127506, &msg_127506[0]},
                       {127508, 1, 3, hnd_127508, &msg_127508[0]},
                       {127513, 1, 3, hnd_127513, &msg_127513[0]},
                       {0     , 0, 0, NULL,       &msg_error [0]}};

static PGN navpgn[] = {{ 59392, 0, 0, hnd_059392, &msg_059392[0]},
                       { 60928, 0, 0, hnd_060928, &msg_060928[0]},
                       {126208, 0, 0, hnd_126208, &msg_126208[0]},
                       {126464, 1, 0, hnd_126464, &msg_126464[0]},
                       {126992, 0, 0, hnd_126992, &msg_126992[0]},
                       {126996, 1, 0, hnd_126996, &msg_126996[0]},
                       {127245, 0, 4, hnd_127245, &msg_127245[0]},
                       {127250, 0, 4, hnd_127250, &msg_127250[0]},
                       {127258, 0, 0, hnd_127258, &msg_127258[0]},
                       {128259, 0, 4, hnd_128259, &msg_128259[0]},
                       {128267, 0, 4, hnd_128267, &msg_128267[0]},
                       {128275, 1, 4, hnd_128275, &msg_128275[0]},
                       {129283, 0, 0, hnd_129283, &msg_129283[0]},
                       {129284, 1, 0, hnd_129284, &msg_129284[0]},
                       {129285, 1, 0, hnd_129285, &msg_129285[0]},
                       {130306, 0, 4, hnd_130306, &msg_130306[0]},
                       {130310, 0, 4, hnd_130310, &msg_130310[0]},
                       {130311, 0, 4, hnd_130311, &msg_130311[0]},
                       {0     , 0, 0, NULL,       &msg_error [0]}};


static PGN *search_pgnlist(unsigned int pgn, PGN *pgnlist)
{
    int l1;
    PGN *work;

    l1 = 0;
    work = NULL;
    while (0 != pgnlist[l1].pgn) {
        if (pgnlist[l1].pgn == pgn) {
            work = &pgnlist[l1];
            break;
        } else {
            l1 = l1 + 1;
        }
    }
    return work;
}

static void find_pgn(struct can_frame *frame, struct gps_device_t *session)
{
    unsigned int can_net;

    GPSD_LOG(LOG_RAW, &session->context->errout,
             "NMEA2000 find_pgn()\n");
    session->driver.nmea2000.workpgn = NULL;
    can_net = session->driver.nmea2000.can_net;
    if ((NMEA2000_NETS - 1) < can_net) {
        GPSD_LOG(LOG_ERROR, &session->context->errout,
                 "NMEA2000 find_pgn: Invalid can network %d.\n", can_net);
        return;
    }

    if (frame->can_id & CAN_ERR_FLAG) {
        GPSD_LOG(LOG_ERROR, &session->context->errout,
                 "NMEA2000 CAN_ERR_FLAG set %d.\n", frame->can_id);
        return;
    }
    if (frame->can_id & CAN_EFF_FLAG) {
        unsigned int source_prio;
        unsigned int daddr;
        unsigned int source_pgn;
        unsigned int source_unit;

#if LOG_FILE
        if (logFile != NULL) {
            struct timespec  msgTime;

            clock_gettime(CLOCK_REALTIME, &msgTime);
            (void)fprintf(logFile,
                          "(%010lld.%06ld) can0 %08x#",
                          (long long)msgTime.tv_sec,
                          msgTime.tv_nsec / 1000,
                          frame->can_id & 0x1ffffff);
            if (0 < (frame->can_dlc & 0x0f)) {
                int l1;
                for (l1 = 0; l1 < (frame->can_dlc & 0x0f); l1++) {
                    (void)fprintf(logFile, "%02x", frame->data[l1]);
                }
            }
            (void)fprintf(logFile, "\n");
        }
#endif  // of if LOG_FILE
        source_unit = frame->can_id & 0x0ff;
        if (NMEA2000_UNITS < source_unit) {
            GPSD_LOG(LOG_PROG, &session->context->errout,
                     "NMEA2000 ignoring unit %d.\n", source_unit);
            return;
        }
        session->driver.nmea2000.can_msgcnt += 1;
        source_pgn = (frame->can_id >> 8) & 0x1ffff;
        source_prio = (frame->can_id >> 26) & 0x7;

        if (240 > ((source_pgn & 0x0ff00) >> 8)) {
            daddr  = source_pgn & 0x000ff;
            source_pgn  = source_pgn & 0x1ff00;
        } else {
            daddr = 0xff;
        }
        GPSD_LOG(LOG_DATA, &session->context->errout,
                 "nmea2000: source_prio %u daddr %u\n",
                 source_prio, daddr);

        if (!session->driver.nmea2000.unit_valid) {
            unsigned int l1, l2;

            for (l1 = 0; l1 < NMEA2000_NETS; l1++) {
                for (l2 = 0; l2 < NMEA2000_UNITS; l2++) {
                    if (session == nmea2000_units[l1][l2]) {
                        session->driver.nmea2000.unit = l2;
                        session->driver.nmea2000.unit_valid = true;
                        session->driver.nmea2000.can_net = l1;
                        can_net = l1;
                    }
                }
            }

            session->driver.nmea2000.unit = source_unit;
            session->driver.nmea2000.unit_valid = true;
            nmea2000_units[can_net][source_unit] = session;
        }

        if (source_unit == session->driver.nmea2000.unit) {
            PGN *work;
            if (NULL != session->driver.nmea2000.pgnlist) {
                work = search_pgnlist(source_pgn,
                                      session->driver.nmea2000.pgnlist);
            } else {
                PGN *pgnlist;

                pgnlist = &gpspgn[0];
                work = search_pgnlist(source_pgn, pgnlist);
                if (work == NULL) {
                    pgnlist = &aispgn[0];
                    work = search_pgnlist(source_pgn, pgnlist);
                }
                if (work == NULL) {
                    pgnlist = &pwrpgn[0];
                    work = search_pgnlist(source_pgn, pgnlist);
                }
                if (work == NULL) {
                    pgnlist = &navpgn[0];
                    work = search_pgnlist(source_pgn, pgnlist);
                }
                if ((work != NULL) && (work->type > 0)) {
                    session->driver.nmea2000.pgnlist = pgnlist;
                }
            }
            if (work != NULL) {
                if (work->fast == 0) {
                    size_t l2;

                    GPSD_LOG(LOG_DATA, &session->context->errout,
                             "pgn %6d:%s \n", work->pgn, work->name);
                    session->driver.nmea2000.workpgn = (void *) work;
                    session->lexer.outbuflen =  frame->can_dlc & 0x0f;
                    for (l2 = 0; l2 < session->lexer.outbuflen; l2++) {
                        session->lexer.outbuffer[l2]= frame->data[l2];
                    }
                } else if (0 == (frame->data[0] & 0x1f)) {
                    unsigned int l2;

                    session->driver.nmea2000.fast_packet_len = frame->data[1];
                    session->driver.nmea2000.idx = frame->data[0];
#if NMEA2000_FAST_DEBUG
                    GPSD_LOG(LOG_ERROR, &session->context->errout,
                             "Set idx    %2x    %2x %2x %6d\n",
                             frame->data[0],
                             session->driver.nmea2000.unit,
                             frame->data[1],
                             source_pgn);
#endif  // of #if NMEA2000_FAST_DEBUG
                    session->lexer.inbuflen = 0;
                    session->driver.nmea2000.idx += 1;
                    for (l2 = 2; l2 < 8; l2++) {
                        session->lexer.inbuffer[session->lexer.inbuflen++] =
                            frame->data[l2];
                    }
                    GPSD_LOG(LOG_DATA, &session->context->errout,
                             "pgn %6d:%s \n", work->pgn, work->name);
                } else if (frame->data[0] == session->driver.nmea2000.idx) {
                    unsigned int l2;

                    for (l2 = 1; l2 < 8; l2++) {
                        if (session->driver.nmea2000.fast_packet_len >
                            session->lexer.inbuflen) {
                            session->lexer.inbuffer[session->lexer.inbuflen++] =
                                frame->data[l2];
                        }
                    }
                    if (session->lexer.inbuflen ==
                        session->driver.nmea2000.fast_packet_len) {
#if NMEA2000_FAST_DEBUG
                        GPSD_LOG(LOG_ERROR, &session->context->errout,
                                 "Fast done  %2x %2x %2x %2x %6d\n",
                                 session->driver.nmea2000.idx,
                                 frame->data[0],
                                 session->driver.nmea2000.unit,
                                 (unsigned int)session->driver.nmea2000.fast_packet_len,
                                 source_pgn);
#endif  // of #if  NMEA2000_FAST_DEBUG
                        session->driver.nmea2000.workpgn = (void *) work;
                        session->lexer.outbuflen =
                            session->driver.nmea2000.fast_packet_len;
                        for (l2 = 0;
                             l2 < (unsigned int)session->lexer.outbuflen;
                             l2++) {
                            session->lexer.outbuffer[l2] =
                                session->lexer.inbuffer[l2];
                        }
                        session->driver.nmea2000.fast_packet_len = 0;
                    } else {
                        session->driver.nmea2000.idx += 1;
                    }
                } else {
                    GPSD_LOG(LOG_ERROR, &session->context->errout,
                         "Fast error %2x %2x %2x %2x %6d\n",
                         session->driver.nmea2000.idx,
                         frame->data[0],
                         session->driver.nmea2000.unit,
                         (unsigned int)session->driver.nmea2000.fast_packet_len,
                         source_pgn);
                }
            } else {
                GPSD_LOG(LOG_WARN, &session->context->errout,
                         "PGN not found %08d %08x \n",
                         source_pgn, source_pgn);
            }
        } else {
            // we got an unknown unit number
            if (NULL == nmea2000_units[can_net][source_unit]) {
                char buffer[55];

                (void) snprintf(buffer,
                                sizeof(buffer),
                                "nmea2000://%s:%u",
                                can_interface_name[can_net],
                                source_unit);
                if (NULL != gpsd_add_device) {
                    (void) gpsd_add_device(buffer, true);
                }
            }
        }
    } else {
        // we got RTR or 2.0A CAN frame, not used
    }
}


static ssize_t nmea2000_get(struct gps_device_t *session)
{
    struct can_frame frame;
    ssize_t          status;

    errno = 0;
    session->lexer.outbuflen = 0;
    // FIXME: read() into a struct is not guaranteed in C
    // sizeof(frame) === 16
    status = read(session->gpsdata.gps_fd, &frame, sizeof(frame));
    if ((ssize_t)sizeof(frame) == status) {
        session->lexer.type = NMEA2000_PACKET;
        find_pgn(&frame, session);

        return frame.can_dlc & 0x0f;
    }
    if (EAGAIN == status) {
        // nothing to read
        return 0;
    }
    GPSD_LOG(LOG_WARN, &session->context->errout,
             "NMEA2000 nmea2000_get() status %ld %s(%d) \n",
             status, strerror(errno), errno);
    return 0;
}

static gps_mask_t nmea2000_parse_input(struct gps_device_t *session)
{
    gps_mask_t mask;
    PGN *work;
    char buf[128];

//  printf("NMEA2000 parse_input called\n");
    GPSD_LOG(LOG_RAW, &session->context->errout,
             "NMEA2000 nmea2000_parse_input(%s)\n",
             gps_hexdump(buf, sizeof(buf),
                         session->lexer.outbuffer,
                         session->lexer.outbuflen));
    mask = 0;
    work = (PGN *)session->driver.nmea2000.workpgn;

    if (NULL != work) {
        mask = (work->func)(&session->lexer.outbuffer[0],
                            (int)session->lexer.outbuflen, work, session);
        session->driver.nmea2000.workpgn = NULL;
    }
    session->lexer.outbuflen = 0;

    return mask;
}


int nmea2000_open(struct gps_device_t *session)
{
    char interface_name[GPS_PATH_MAX];
    socket_t sock;
    int status;
    int unit_number;
    int can_net;
    unsigned int l;
    struct ifreq ifr;
    struct sockaddr_can addr;
    char *unit_ptr;
    can_err_mask_t err_mask;
    int rcvbuf_size = 1000000;  // requested receiver buffer size
    int curr_rcvbuf_size;
    socklen_t curr_rcvbuf_size_len = sizeof(curr_rcvbuf_size);


    INVALIDATE_SOCKET(session->gpsdata.gps_fd);

    session->driver.nmea2000.can_net = 0;
    can_net = -1;

    unit_number = -1;

    (void)strlcpy(interface_name, session->gpsdata.dev.path + 11,
                  sizeof(interface_name));
    unit_ptr = NULL;
    for (l = 0; l < strnlen(interface_name, sizeof(interface_name)); l++) {
        if (':' == interface_name[l]) {
            unit_ptr = &interface_name[l+1];
            interface_name[l] = 0;
            continue;
        }
        if (NULL != unit_ptr) {
            if (0 == isdigit(interface_name[l])) {
                GPSD_LOG(LOG_ERROR, &session->context->errout,
                         "NMEA2000 open: Invalid character in unit number.\n");
                return -1;
            }
        }
    }

    if (NULL != unit_ptr) {
        unit_number = atoi(unit_ptr);
        if ((0 > unit_number) ||
            ((NMEA2000_UNITS - 1) < unit_number)) {
            GPSD_LOG(LOG_ERROR, &session->context->errout,
                     "NMEA2000 open: Unit number out of range.\n");
            return -1;
        }
        for (l = 0; l < NMEA2000_NETS; l++) {
            if (strncmp(can_interface_name[l],
                        interface_name,
                        MIN(sizeof(interface_name),
                            sizeof(can_interface_name[l]))) == 0) {
                can_net = l;
                break;
            }
        }
        if (0 > can_net) {
            GPSD_LOG(LOG_ERROR, &session->context->errout,
                     "NMEA2000 open: CAN device not open: %s .\n",
                     interface_name);
            return -1;
        }
    } else {
        for (l = 0; l < NMEA2000_NETS; l++) {
            if (strncmp(can_interface_name[l],
                        interface_name,
                        MIN(sizeof(interface_name),
                            sizeof(can_interface_name[l]))) == 0) {
                GPSD_LOG(LOG_ERROR, &session->context->errout,
                         "NMEA2000 open: CAN device duplicate open: %s .\n",
                         interface_name);
                return -1;
            }
        }
        for (l = 0; l < NMEA2000_NETS; l++) {
            if (can_interface_name[l][0] == 0) {
                can_net = l;
                break;
            }
        }
        if (0 > can_net) {
            GPSD_LOG(LOG_ERROR, &session->context->errout,
                     "NMEA2000 open: Too many CAN networks open.\n");
            return -1;
        }
    }

    // Create the socket
    sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);

    if (BAD_SOCKET(sock)) {
        GPSD_LOG(LOG_ERROR, &session->context->errout,
                 "NMEA2000 open: socket(PF_CAN) %s(%d).\n",
                 strerror(errno), errno);
        return -1;
    }

    status = fcntl(sock, F_SETFL, O_NONBLOCK);
    if (0 != status) {
        GPSD_LOG(LOG_ERROR, &session->context->errout,
                 "NMEA2000 open: fcntl(O_NONBLOCK) %s(%d).\n",
                 strerror(errno), errno);
        close(sock);
        return -1;
    }

    // turn on CANBUS error reporting
    err_mask = CAN_ERR_ACK | CAN_ERR_BUSOFF | CAN_ERR_CRTL | CAN_ERR_LOSTARB |
               CAN_ERR_PROT | CAN_ERR_RESTARTED | CAN_ERR_TRX |
               CAN_ERR_TX_TIMEOUT;

    status = setsockopt(sock, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
                        &err_mask, sizeof(err_mask));
    if (0 != status) {
        GPSD_LOG(LOG_ERROR, &session->context->errout,
                 "NMEA2000 open: setsockopt() %s(%d)\n",
                 strerror(errno), errno);
    }

    /* enbiggen the receiver buffer size
     * try SO_RCVBUFFORCE first, if we run with CAP_NET_ADMIN */
    if (setsockopt(sock, SOL_SOCKET, SO_RCVBUFFORCE,
                   &rcvbuf_size, sizeof(rcvbuf_size)) < 0) {
            GPSD_LOG(LOG_ERROR, &session->context->errout,
                     "NMEA2000 open:SO_RCVBUFFORCE failed try RCVBUF. "
                     "%s(%d)\n",
                     strerror(errno), errno);
            if (0 > setsockopt(sock, SOL_SOCKET, SO_RCVBUF,
                               &rcvbuf_size, sizeof(rcvbuf_size))) {
                GPSD_LOG(LOG_ERROR, &session->context->errout,
                         "NMEA2000 open:setsockopt(SO_RCVBUF) %s(%d).\n",
                         strerror(errno), errno);
            }
    }
    if (0 > getsockopt(sock, SOL_SOCKET, SO_RCVBUF,
                   &curr_rcvbuf_size, &curr_rcvbuf_size_len)) {
            GPSD_LOG(LOG_ERROR, &session->context->errout,
                     "NMEA2000 open:getsockopt(SO_RCVBUF) %s(%d)\n",
                     strerror(errno), errno);
    } else {
            GPSD_LOG(LOG_ERROR, &session->context->errout,
                     "NMEA2000 open:getsockopt(SO_RCVBUF) =  %d\n",
                     curr_rcvbuf_size);
    }

    // Locate the interface you wish to use
    strlcpy(ifr.ifr_name, interface_name, sizeof(ifr.ifr_name));
    status = ioctl(sock, SIOCGIFINDEX, &ifr);  /* ifr.ifr_ifindex gets filled
                                                * with that device's index */

    if (0 != status) {
        GPSD_LOG(LOG_ERROR, &session->context->errout,
                 "NMEA2000 open: can not find CAN device.\n");
        close(sock);
        return -1;
    }

    // Select that CAN interface, and bind the socket to it.
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    status = bind(sock, (struct sockaddr*)&addr, sizeof(addr) );
    if (0 != status) {
        GPSD_LOG(LOG_ERROR, &session->context->errout,
                 "NMEA2000 open: bind failed.\n");
        close(sock);
        return -1;
    }

    gpsd_switch_driver(session, "NMEA2000");
    session->gpsdata.gps_fd = sock;
    session->sourcetype = SOURCE_CAN;
    session->servicetype = SERVICE_SENSOR;
    session->driver.nmea2000.can_net = can_net;

    if (NULL != unit_ptr) {
        nmea2000_units[can_net][unit_number] = session;
        session->driver.nmea2000.unit = unit_number;
        session->driver.nmea2000.unit_valid = true;
    } else {
        strlcpy(can_interface_name[can_net],
                interface_name,
                MIN(sizeof(can_interface_name[0]), sizeof(interface_name)));
        session->driver.nmea2000.unit_valid = false;
        for (l = 0; l < NMEA2000_UNITS; l++) {
            nmea2000_units[can_net][l] = NULL;
        }
    }

    session->gpsdata.dev.parity = 'N';
    session->gpsdata.dev.baudrate = 250000;
    session->gpsdata.dev.stopbits = 0;
    return session->gpsdata.gps_fd;
}

void nmea2000_close(struct gps_device_t *session)
{
    if (!BAD_SOCKET(session->gpsdata.gps_fd)) {
        // cast for 32-bit ints.
        GPSD_LOG(LOG_SPIN, &session->context->errout,
                 "close(%ld) in nmea2000_close(%s)\n",
                 (long)session->gpsdata.gps_fd, session->gpsdata.dev.path);
        (void)close(session->gpsdata.gps_fd);
        INVALIDATE_SOCKET(session->gpsdata.gps_fd);

        if (session->driver.nmea2000.unit_valid) {
            unsigned int l1, l2;

            for (l1 = 0; l1 < NMEA2000_NETS; l1++) {
                for (l2 = 0; l2 < NMEA2000_UNITS; l2++) {
                    if (session == nmea2000_units[l1][l2]) {
                        session->driver.nmea2000.unit_valid = false;
                        session->driver.nmea2000.unit = 0;
                        session->driver.nmea2000.can_net = 0;
                        nmea2000_units[l1][l2] = NULL;
                    }
                }
            }
        }
    }
}

// *INDENT-OFF*
const struct gps_type_t driver_nmea2000 = {
    .type_name      = "NMEA2000",       // full name of type
    .packet_type    = NMEA2000_PACKET,  // associated lexer packet type
    .flags          = DRIVER_STICKY,    // remember this
    .trigger        = NULL,             // detect their main sentence
    .channels       = 12,               // not an actual GPS at all
    .probe_detect   = NULL,
    .get_packet     = nmea2000_get,     // how to get a packet
    .parse_packet   = nmea2000_parse_input,     // how to interpret a packet
    .rtcm_writer    = NULL,             // Don't send RTCM to this
    .init_query     = NULL,             // non-perturbing query
    .event_hook     = NULL,
    .speed_switcher = NULL,             // no speed switcher
    .mode_switcher  = NULL,             // no 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   = NULL,             // how to send control strings
    .time_offset     = NULL,
};
// *INDENT-ON*

// end

#else   // of  defined(NMEA2000_ENABLE)
/* dummy variable to some old linkers do not complain about empty
 * object file */
int nmea2000_dummy = 1;
#endif  // of  defined(NMEA2000_ENABLE)

// vim: set expandtab shiftwidth=4

Coverage Report

Created: 2026-02-26 06:35