driver_ubx.c

   1 /*
   2  * UBX driver
   3  *
   4  * This file is Copyright (c) 2010 by the GPSD project
   5  * BSD terms apply: see the file COPYING in the distribution root for details.
   6  *
   7  */
   8
   9 #include <sys/types.h>
  10 #include <stdio.h>
  11 #include <stdlib.h>
  12 #include <string.h>
  13 #include <math.h>
  14 #include <ctype.h>
  15 #ifndef S_SPLINT_S
  16 #include <unistd.h>
  17 #endif /* S_SPLINT_S */
  18 #include <time.h>
  19 #include <stdio.h>
  20 #include <assert.h>
  21
  22 #include "gpsd.h"
  23 #if defined(UBX_ENABLE) && defined(BINARY_ENABLE)
  24 #include "driver_ubx.h"
  25
  26 #include "bits.h"
  27
  28 /*
  29  * A ubx packet looks like this:
  30  * leader: 0xb5 0x62
  31  * message class: 1 byte
  32  * message type: 1 byte
  33  * length of payload: 2 bytes
  34  * payload: variable length
  35  * checksum: 2 bytes
  36  *
  37  * see also the FV25 and UBX documents on reference.html
  38  */
  39
  40 static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf,
  41                             size_t len);
  42 static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session,
  43                                   unsigned char *buf, size_t data_len);
  44 static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session,
  45                                   unsigned char *buf, size_t data_len);
  46 static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session,
  47                                       unsigned char *buf, size_t data_len);
  48 static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session,
  49                                      unsigned char *buf, size_t data_len);
  50 static void ubx_msg_sbas(struct gps_device_t *session, unsigned char *buf);
  51 static void ubx_msg_inf(unsigned char *buf, size_t data_len);
  52
  53 /**
  54  * Navigation solution message
  55  */
  56 static gps_mask_t
  57 ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf,
  58                 size_t data_len)
  59 {
  60     unsigned short gw;
  61     unsigned int tow, flags;
  62     double epx, epy, epz, evx, evy, evz;
  63     unsigned char navmode;
  64     gps_mask_t mask;
  65     double t;
  66
  67     if (data_len != 52)
  68         return 0;
  69
  70     flags = (unsigned int)getub(buf, 11);
  71     mask = 0;
  72     if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) {
  73         tow = (unsigned int)getleul(buf, 0);
  74         gw = (unsigned short)getlesw(buf, 8);
  75         session->context->gps_week = gw;
  76         session->context->gps_tow = tow / 1000.0;
  77
  78         t = gpstime_to_unix((int)session->context->gps_week,
  79                             session->context->gps_tow)
  80             - session->context->leap_seconds;
  81         session->newdata.time = t;
  82         mask |= TIME_IS;
  83     }
  84
  85     epx = (double)(getlesl(buf, 12) / 100.0);
  86     epy = (double)(getlesl(buf, 16) / 100.0);
  87     epz = (double)(getlesl(buf, 20) / 100.0);
  88     evx = (double)(getlesl(buf, 28) / 100.0);
  89     evy = (double)(getlesl(buf, 32) / 100.0);
  90     evz = (double)(getlesl(buf, 36) / 100.0);
  91     ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation,
  92                      epx, epy, epz, evx, evy, evz);
  93     mask |= LATLON_IS | ALTITUDE_IS | SPEED_IS | TRACK_IS | CLIMB_IS;
  94     session->newdata.epx = session->newdata.epy =
  95         (double)(getlesl(buf, 24) / 100.0) / sqrt(2);
  96     session->newdata.eps = (double)(getlesl(buf, 40) / 100.0);
  97     /* Better to have a single point of truth about DOPs */
  98     //session->gpsdata.dop.pdop = (double)(getleuw(buf, 44)/100.0);
  99     session->gpsdata.satellites_used = (int)getub(buf, 47);
 100
 101     navmode = (unsigned char)getub(buf, 10);
 102     switch (navmode) {
 103     case UBX_MODE_TMONLY:
 104     case UBX_MODE_3D:
 105         session->newdata.mode = MODE_3D;
 106         break;
 107     case UBX_MODE_2D:
 108     case UBX_MODE_DR:           /* consider this too as 2D */
 109     case UBX_MODE_GPSDR:        /* FIX-ME: DR-aided GPS may be valid 3D */
 110         session->newdata.mode = MODE_2D;
 111         break;
 112     default:
 113         session->newdata.mode = MODE_NO_FIX;
 114     }
 115
 116     if ((flags & UBX_SOL_FLAG_DGPS) != 0)
 117         session->gpsdata.status = STATUS_DGPS_FIX;
 118     else if (session->newdata.mode != MODE_NO_FIX)
 119         session->gpsdata.status = STATUS_FIX;
 120
 121     mask |= MODE_IS | STATUS_IS;
 122     gpsd_report(LOG_DATA,
 123                 "NAVSOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d mask=%s\n",
 124                 session->newdata.time,
 125                 session->newdata.latitude,
 126                 session->newdata.longitude,
 127                 session->newdata.altitude,
 128                 session->newdata.track,
 129                 session->newdata.speed,
 130                 session->newdata.climb,
 131                 session->newdata.mode,
 132                 session->gpsdata.status,
 133                 session->gpsdata.satellites_used, gpsd_maskdump(mask));
 134     return mask;
 135 }
 136
 137 /**
 138  * Dilution of precision message
 139  */
 140 static gps_mask_t
 141 ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf,
 142                 size_t data_len)
 143 {
 144     if (data_len != 18)
 145         return 0;
 146
 147     /*
 148      * We make a deliberate choice not to clear DOPs from the
 149      * last skyview here, but rather to treat this as a supplement
 150      * to our calculations from the visiniolity matrix, trusting
 151      * the firmware algorithms over ours.
 152      */
 153     session->gpsdata.dop.gdop = (double)(getleuw(buf, 4) / 100.0);
 154     session->gpsdata.dop.pdop = (double)(getleuw(buf, 6) / 100.0);
 155     session->gpsdata.dop.tdop = (double)(getleuw(buf, 8) / 100.0);
 156     session->gpsdata.dop.vdop = (double)(getleuw(buf, 10) / 100.0);
 157     session->gpsdata.dop.hdop = (double)(getleuw(buf, 12) / 100.0);
 158     gpsd_report(LOG_DATA, "NAVDOP: gdop=%.2f pdop=%.2f "
 159                 "hdop=%.2f vdop=%.2f tdop=%.2f mask={DOP}\n",
 160                 session->gpsdata.dop.gdop,
 161                 session->gpsdata.dop.hdop,
 162                 session->gpsdata.dop.vdop,
 163                 session->gpsdata.dop.pdop, session->gpsdata.dop.tdop);
 164     return DOP_IS;
 165 }
 166
 167 /**
 168  * GPS Leap Seconds
 169  */
 170 static gps_mask_t
 171 ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf,
 172                     size_t data_len)
 173 {
 174     unsigned int gw, tow, flags;
 175     double t;
 176
 177     if (data_len != 16)
 178         return 0;
 179
 180     tow = (unsigned int)getleul(buf, 0);
 181     gw = (unsigned int)getlesw(buf, 8);
 182     if (gw > session->context->gps_week)
 183         session->context->gps_week = (unsigned short)gw;
 184
 185     flags = (unsigned int)getub(buf, 11);
 186     if ((flags & 0x7) != 0)
 187         session->context->leap_seconds = (int)getub(buf, 10);
 188
 189     session->context->gps_tow = tow / 1000.0;
 190     t = gpstime_to_unix((int)session->context->gps_week,
 191                         session->context->gps_tow)
 192         - session->context->leap_seconds;
 193     session->newdata.time = t;
 194
 195     gpsd_report(LOG_DATA, "TIMEGPS: time=%.2f mask={TIME}\n",
 196                 session->newdata.time);
 197     return TIME_IS;
 198 }
 199
 200 /**
 201  * GPS Satellite Info
 202  */
 203 static gps_mask_t
 204 ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf,
 205                    size_t data_len)
 206 {
 207     unsigned int i, j, nchan, nsv, st;
 208
 209     if (data_len < 152) {
 210         gpsd_report(LOG_PROG, "runt svinfo (datalen=%zd)\n", data_len);
 211         return 0;
 212     }
 213     /*@ +charint @*/
 214     nchan = (unsigned int)getub(buf, 4);
 215     if (nchan > MAXCHANNELS) {
 216         gpsd_report(LOG_WARN,
 217                     "Invalid NAV SVINFO message, >%d reported visible",
 218                     MAXCHANNELS);
 219         return 0;
 220     }
 221     /*@ -charint @*/
 222     gpsd_zero_satellites(&session->gpsdata);
 223     nsv = 0;
 224     for (i = j = st = 0; i < nchan; i++) {
 225         unsigned int off = 8 + 12 * i;
 226         if ((int)getub(buf, off + 4) == 0)
 227             continue;           /* LEA-5H seems to have a bug reporting sats it does not see or hear */
 228         session->gpsdata.PRN[j] = (int)getub(buf, off + 1);
 229         session->gpsdata.ss[j] = (float)getub(buf, off + 4);
 230         session->gpsdata.elevation[j] = (int)getsb(buf, off + 5);
 231         session->gpsdata.azimuth[j] = (int)getlesw(buf, off + 6);
 232         if (session->gpsdata.PRN[j])
 233             st++;
 234         /*@ -predboolothers */
 235         if (getub(buf, off + 2) & 0x01)
 236             session->gpsdata.used[nsv++] = session->gpsdata.PRN[j];
 237         if (session->gpsdata.PRN[j] == (int)session->driver.ubx.sbas_in_use)
 238             session->gpsdata.used[nsv++] = session->gpsdata.PRN[j];
 239         /*@ +predboolothers */
 240         j++;
 241     }
 242     session->gpsdata.skyview_time = NAN;
 243     session->gpsdata.satellites_visible = (int)st;
 244     session->gpsdata.satellites_used = (int)nsv;
 245     gpsd_report(LOG_DATA,
 246                 "SVINFO: visible=%d used=%d mask={SATELLITE|USED}\n",
 247                 session->gpsdata.satellites_visible,
 248                 session->gpsdata.satellites_used);
 249     return SATELLITE_IS | USED_IS;
 250 }
 251
 252 /*
 253  * SBAS Info
 254  */
 255 static void ubx_msg_sbas(struct gps_device_t *session, unsigned char *buf)
 256 {
 257 #ifdef UBX_SBAS_DEBUG
 258     unsigned int i, nsv;
 259
 260     gpsd_report(LOG_WARN, "SBAS: %d %d %d %d %d\n",
 261                 (int)getub(buf, 4), (int)getub(buf, 5), (int)getub(buf, 6),
 262                 (int)getub(buf, 7), (int)getub(buf, 8));
 263
 264     nsv = (int)getub(buf, 8);
 265     for (i = 0; i < nsv; i++) {
 266         int off = 12 + 12 * i;
 267         gpsd_report(LOG_WARN, "SBAS info on SV: %d\n", (int)getub(buf, off));
 268     }
 269 #endif
 270 /* really 'in_use' depends on the sats info, EGNOS is still in test */
 271 /* In WAAS areas one might also check for the type of corrections indicated */
 272     session->driver.ubx.sbas_in_use = (unsigned char)getub(buf, 4);
 273 }
 274
 275 /*
 276  * Raw Subframes
 277  */
 278 static void ubx_msg_sfrb(struct gps_device_t *session, unsigned char *buf)
 279 {
 280     unsigned int i, words[10], chan, svid;
 281
 282     chan = (unsigned int)getub(buf, 0);
 283     svid = (unsigned int)getub(buf, 1);
 284     gpsd_report(LOG_PROG, "UBX_RXM_SFRB: %u %u\n", chan, svid);
 285
 286     /* UBX does all the parity checking, but still bad data gets through */
 287     for (i = 0; i < 10; i++) {
 288         words[i] = (unsigned int)getleul(buf, 4 * i + 2) & 0xffffff;
 289     }
 290
 291     gpsd_interpret_subframe(session, words);
 292 }
 293
 294 static void ubx_msg_inf(unsigned char *buf, size_t data_len)
 295 {
 296     unsigned short msgid;
 297     static char txtbuf[MAX_PACKET_LENGTH];
 298
 299     msgid = (unsigned short)((buf[2] << 8) | buf[3]);
 300     if (data_len > MAX_PACKET_LENGTH - 1)
 301         data_len = MAX_PACKET_LENGTH - 1;
 302
 303     (void)strlcpy(txtbuf, (char *)buf + 6, MAX_PACKET_LENGTH);
 304     txtbuf[data_len] = '\0';
 305     switch (msgid) {
 306     case UBX_INF_DEBUG:
 307         gpsd_report(LOG_PROG, "UBX_INF_DEBUG: %s\n", txtbuf);
 308         break;
 309     case UBX_INF_TEST:
 310         gpsd_report(LOG_PROG, "UBX_INF_TEST: %s\n", txtbuf);
 311         break;
 312     case UBX_INF_NOTICE:
 313         gpsd_report(LOG_INF, "UBX_INF_NOTICE: %s\n", txtbuf);
 314         break;
 315     case UBX_INF_WARNING:
 316         gpsd_report(LOG_WARN, "UBX_INF_WARNING: %s\n", txtbuf);
 317         break;
 318     case UBX_INF_ERROR:
 319         gpsd_report(LOG_WARN, "UBX_INF_ERROR: %s\n", txtbuf);
 320         break;
 321     default:
 322         break;
 323     }
 324     return;
 325 }
 326
 327 /*@ +charint @*/
 328 gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
 329                      size_t len)
 330 {
 331     size_t data_len;
 332     unsigned short msgid;
 333     gps_mask_t mask = 0;
 334     int i;
 335
 336     if (len < 6)                /* the packet at least contains a head of six bytes */
 337         return 0;
 338
 339     session->cycle_end_reliable = true;
 340
 341     /* extract message id and length */
 342     msgid = (buf[2] << 8) | buf[3];
 343     data_len = (size_t) getlesw(buf, 4);
 344     switch (msgid) {
 345     case UBX_NAV_POSECEF:
 346         gpsd_report(LOG_IO, "UBX_NAV_POSECEF\n");
 347         break;
 348     case UBX_NAV_POSLLH:
 349         gpsd_report(LOG_IO, "UBX_NAV_POSLLH\n");
 350         break;
 351     case UBX_NAV_STATUS:
 352         gpsd_report(LOG_IO, "UBX_NAV_STATUS\n");
 353         break;
 354     case UBX_NAV_DOP:
 355         gpsd_report(LOG_PROG, "UBX_NAV_DOP\n");
 356         mask = ubx_msg_nav_dop(session, &buf[6], data_len);
 357         break;
 358     case UBX_NAV_SOL:
 359         gpsd_report(LOG_PROG, "UBX_NAV_SOL\n");
 360         mask =
 361             ubx_msg_nav_sol(session, &buf[6],
 362                             data_len) | (CLEAR_IS | REPORT_IS);
 363         break;
 364     case UBX_NAV_POSUTM:
 365         gpsd_report(LOG_IO, "UBX_NAV_POSUTM\n");
 366         break;
 367     case UBX_NAV_VELECEF:
 368         gpsd_report(LOG_IO, "UBX_NAV_VELECEF\n");
 369         break;
 370     case UBX_NAV_VELNED:
 371         gpsd_report(LOG_IO, "UBX_NAV_VELNED\n");
 372         break;
 373     case UBX_NAV_TIMEGPS:
 374         gpsd_report(LOG_PROG, "UBX_NAV_TIMEGPS\n");
 375         mask = ubx_msg_nav_timegps(session, &buf[6], data_len);
 376         break;
 377     case UBX_NAV_TIMEUTC:
 378         gpsd_report(LOG_IO, "UBX_NAV_TIMEUTC\n");
 379         break;
 380     case UBX_NAV_CLOCK:
 381         gpsd_report(LOG_IO, "UBX_NAV_CLOCK\n");
 382         break;
 383     case UBX_NAV_SVINFO:
 384         gpsd_report(LOG_PROG, "UBX_NAV_SVINFO\n");
 385         mask = ubx_msg_nav_svinfo(session, &buf[6], data_len);
 386         break;
 387     case UBX_NAV_DGPS:
 388         gpsd_report(LOG_IO, "UBX_NAV_DGPS\n");
 389         break;
 390     case UBX_NAV_SBAS:
 391         gpsd_report(LOG_IO, "UBX_NAV_SBAS\n");
 392         ubx_msg_sbas(session, &buf[6]);
 393         break;
 394     case UBX_NAV_EKFSTATUS:
 395         gpsd_report(LOG_IO, "UBX_NAV_EKFSTATUS\n");
 396         break;
 397
 398     case UBX_RXM_RAW:
 399         gpsd_report(LOG_IO, "UBX_RXM_RAW\n");
 400         break;
 401     case UBX_RXM_SFRB:
 402         ubx_msg_sfrb(session, &buf[6]);
 403         break;
 404     case UBX_RXM_SVSI:
 405         gpsd_report(LOG_PROG, "UBX_RXM_SVSI\n");
 406         break;
 407     case UBX_RXM_ALM:
 408         gpsd_report(LOG_IO, "UBX_RXM_ALM\n");
 409         break;
 410     case UBX_RXM_EPH:
 411         gpsd_report(LOG_IO, "UBX_RXM_EPH\n");
 412         break;
 413     case UBX_RXM_POSREQ:
 414         gpsd_report(LOG_IO, "UBX_RXM_POSREQ\n");
 415         break;
 416
 417     case UBX_MON_SCHED:
 418         gpsd_report(LOG_IO, "UBX_MON_SCHED\n");
 419         break;
 420     case UBX_MON_IO:
 421         gpsd_report(LOG_IO, "UBX_MON_IO\n");
 422         break;
 423     case UBX_MON_IPC:
 424         gpsd_report(LOG_IO, "UBX_MON_IPC\n");
 425         break;
 426     case UBX_MON_VER:
 427         gpsd_report(LOG_IO, "UBX_MON_VER\n");
 428         break;
 429     case UBX_MON_EXCEPT:
 430         gpsd_report(LOG_IO, "UBX_MON_EXCEPT\n");
 431         break;
 432     case UBX_MON_MSGPP:
 433         gpsd_report(LOG_IO, "UBX_MON_MSGPP\n");
 434         break;
 435     case UBX_MON_RXBUF:
 436         gpsd_report(LOG_IO, "UBX_MON_RXBUF\n");
 437         break;
 438     case UBX_MON_TXBUF:
 439         gpsd_report(LOG_IO, "UBX_MON_TXBUF\n");
 440         break;
 441     case UBX_MON_HW:
 442         gpsd_report(LOG_IO, "UBX_MON_HW\n");
 443         break;
 444     case UBX_MON_USB:
 445         gpsd_report(LOG_IO, "UBX_MON_USB\n");
 446         break;
 447
 448     case UBX_INF_DEBUG:
 449         /* FALLTHROUGH */
 450     case UBX_INF_TEST:
 451         /* FALLTHROUGH */
 452     case UBX_INF_NOTICE:
 453         /* FALLTHROUGH */
 454     case UBX_INF_WARNING:
 455         /* FALLTHROUGH */
 456     case UBX_INF_ERROR:
 457         ubx_msg_inf(buf, data_len);
 458         break;
 459
 460     case UBX_TIM_TP:
 461         gpsd_report(LOG_IO, "UBX_TIM_TP\n");
 462         break;
 463     case UBX_TIM_TM:
 464         gpsd_report(LOG_IO, "UBX_TIM_TM\n");
 465         break;
 466     case UBX_TIM_TM2:
 467         gpsd_report(LOG_IO, "UBX_TIM_TM2\n");
 468         break;
 469     case UBX_TIM_SVIN:
 470         gpsd_report(LOG_IO, "UBX_TIM_SVIN\n");
 471         break;
 472
 473     case UBX_CFG_PRT:
 474         gpsd_report(LOG_IO, "UBX_CFG_PRT\n");
 475         for (i = 6; i < 26; i++)
 476             session->driver.ubx.original_port_settings[i - 6] = buf[i]; /* copy the original port settings */
 477         buf[14 + 6] &= ~0x02;   /* turn off NMEA output on this port */
 478         (void)ubx_write(session, 0x06, 0x00, &buf[6], 20);      /* send back with all other settings intact */
 479         session->driver.ubx.have_port_configuration = true;
 480         break;
 481
 482     case UBX_ACK_NAK:
 483         gpsd_report(LOG_IO, "UBX_ACK_NAK, class: %02x, id: %02x\n", buf[6],
 484                     buf[7]);
 485         break;
 486     case UBX_ACK_ACK:
 487         gpsd_report(LOG_IO, "UBX_ACK_ACK, class: %02x, id: %02x\n", buf[6],
 488                     buf[7]);
 489         break;
 490
 491     default:
 492         gpsd_report(LOG_WARN,
 493                     "UBX: unknown packet id 0x%04hx (length %zd) %s\n",
 494                     msgid, len, gpsd_hexdump_wrapper(buf, len, LOG_WARN));
 495     }
 496
 497     if (mask)
 498         (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag),
 499                        "0x%04hx", msgid);
 500
 501     return mask | ONLINE_IS;
 502 }
 503
 504 /*@ -charint @*/
 505
 506 static gps_mask_t parse_input(struct gps_device_t *session)
 507 {
 508     gps_mask_t st;
 509
 510     if (session->packet.type == UBX_PACKET) {
 511         st = ubx_parse(session, session->packet.outbuffer,
 512                        session->packet.outbuflen);
 513         session->gpsdata.dev.driver_mode = MODE_BINARY;
 514         return st;
 515 #ifdef NMEA_ENABLE
 516     } else if (session->packet.type == NMEA_PACKET) {
 517         st = nmea_parse((char *)session->packet.outbuffer, session);
 518         session->gpsdata.dev.driver_mode = MODE_NMEA;
 519         return st;
 520 #endif /* NMEA_ENABLE */
 521     } else
 522         return 0;
 523 }
 524
 525 bool ubx_write(struct gps_device_t * session,
 526                unsigned int msg_class, unsigned int msg_id,
 527                unsigned char *msg, unsigned short data_len)
 528 {
 529     unsigned char CK_A, CK_B;
 530     ssize_t i, count;
 531     bool ok;
 532
 533     /*@ -type @*/
 534     session->msgbuf[0] = 0xb5;
 535     session->msgbuf[1] = 0x62;
 536
 537     CK_A = CK_B = 0;
 538     session->msgbuf[2] = msg_class;
 539     session->msgbuf[3] = msg_id;
 540     session->msgbuf[4] = data_len & 0xff;
 541     session->msgbuf[5] = (data_len >> 8) & 0xff;
 542
 543     assert(msg != NULL || data_len == 0);
 544     if (msg != NULL)
 545         (void)memcpy(&session->msgbuf[6], msg, data_len);
 546
 547     /* calculate CRC */
 548     for (i = 2; i < 6; i++) {
 549         CK_A += session->msgbuf[i];
 550         CK_B += CK_A;
 551     }
 552     /*@ -nullderef @*/
 553     for (i = 0; i < data_len; i++) {
 554         CK_A += msg[i];
 555         CK_B += CK_A;
 556     }
 557
 558     session->msgbuf[6 + data_len] = CK_A;
 559     session->msgbuf[7 + data_len] = CK_B;
 560     session->msgbuflen = data_len + 8;
 561     /*@ +type @*/
 562
 563     gpsd_report(LOG_IO,
 564                 "=> GPS: UBX class: %02x, id: %02x, len: %d, data:%s, crc: %02x%02x\n",
 565                 msg_class, msg_id, data_len,
 566                 gpsd_hexdump_wrapper(msg, (size_t) data_len, LOG_IO),
 567                 CK_A, CK_B);
 568
 569     count = write(session->gpsdata.gps_fd,
 570                   session->msgbuf, session->msgbuflen);
 571     (void)tcdrain(session->gpsdata.gps_fd);
 572     ok = (count == (ssize_t) session->msgbuflen);
 573     /*@ +nullderef @*/
 574     return (ok);
 575 }
 576
 577 #ifdef ALLOW_CONTROLSEND
 578 static ssize_t ubx_control_send(struct gps_device_t *session, char *msg,
 579                                 size_t data_len)
 580 /* not used by gpsd, it's for gpsctl and friends */
 581 {
 582     return ubx_write(session, (unsigned int)msg[0], (unsigned int)msg[1],
 583                      (unsigned char *)msg + 2,
 584                      (unsigned short)(data_len - 2)) ? ((ssize_t) (data_len +
 585                                                                    7)) : -1;
 586 }
 587 #endif /* ALLOW_CONTROLSEND */
 588
 589 static void ubx_catch_model(struct gps_device_t *session, unsigned char *buf,
 590                             size_t len)
 591 {
 592     /*@ +charint */
 593     unsigned char *ip = &buf[19];
 594     unsigned char *op = (unsigned char *)session->subtype;
 595     size_t end = ((len - 19) < 63) ? (len - 19) : 63;
 596     size_t i;
 597
 598     for (i = 0; i < end; i++) {
 599         if ((*ip == 0x00) || (*ip == '*')) {
 600             *op = 0x00;
 601             break;
 602         }
 603         *(op++) = *(ip++);
 604     }
 605     /*@ -charint */
 606 }
 607
 608 static void ubx_event_hook(struct gps_device_t *session, event_t event)
 609 {
 610     if (event == event_triggermatch)
 611         ubx_catch_model(session,
 612                         session->packet.outbuffer, session->packet.outbuflen);
 613     else if (event == event_identified || event == event_reactivate) {
 614         unsigned char msg[32];
 615
 616         gpsd_report(LOG_IO, "UBX configure: %d\n", session->packet.counter);
 617
 618         (void)ubx_write(session, 0x06u, 0x00, NULL, 0); /* get this port's settings */
 619
 620         /*@ -type @*/
 621         msg[0] = 0x03;          /* SBAS mode enabled, accept testbed mode */
 622         msg[1] = 0x07;          /* SBAS usage: range, differential corrections and integrity */
 623         msg[2] = 0x03;          /* use the maximun search range: 3 channels */
 624         msg[3] = 0x00;          /* PRN numbers to search for all set to 0 => auto scan */
 625         msg[4] = 0x00;
 626         msg[5] = 0x00;
 627         msg[6] = 0x00;
 628         msg[7] = 0x00;
 629         (void)ubx_write(session, 0x06u, 0x16, msg, 8);
 630
 631         msg[0] = 0x01;          /* class */
 632         msg[1] = 0x04;          /* msg id  = UBX_NAV_DOP */
 633         msg[2] = 0x01;          /* rate */
 634         (void)ubx_write(session, 0x06u, 0x01, msg, 3);
 635         msg[0] = 0x01;          /* class */
 636         msg[1] = 0x06;          /* msg id  = NAV-SOL */
 637         msg[2] = 0x01;          /* rate */
 638         (void)ubx_write(session, 0x06u, 0x01, msg, 3);
 639         msg[0] = 0x01;          /* class */
 640         msg[1] = 0x20;          /* msg id  = UBX_NAV_TIMEGPS */
 641         msg[2] = 0x01;          /* rate */
 642         (void)ubx_write(session, 0x06u, 0x01, msg, 3);
 643         msg[0] = 0x01;          /* class */
 644         msg[1] = 0x30;          /* msg id  = NAV-SVINFO */
 645         msg[2] = 0x0a;          /* rate */
 646         (void)ubx_write(session, 0x06u, 0x01, msg, 3);
 647         msg[0] = 0x01;          /* class */
 648         msg[1] = 0x32;          /* msg id  = NAV-SBAS */
 649         msg[2] = 0x0a;          /* rate */
 650         (void)ubx_write(session, 0x06u, 0x01, msg, 3);
 651         /*@ +type @*/
 652     } else if (event == event_deactivate) {
 653         /*@ -type @*/
 654         unsigned char msg[4] = {
 655             0x00, 0x00,         /* hotstart */
 656             0x01,               /* controlled software reset */
 657             0x00
 658         };                      /* reserved */
 659         /*@ +type @*/
 660
 661         gpsd_report(LOG_IO, "UBX revert\n");
 662
 663         /* Reverting all in one fast and reliable reset */
 664         (void)ubx_write(session, 0x06, 0x04, msg, 4);   /* CFG-RST */
 665     }
 666 }
 667
 668 #ifdef ALLOW_RECONFIGURE
 669 static void ubx_nmea_mode(struct gps_device_t *session, int mode)
 670 {
 671     int i;
 672     unsigned char buf[sizeof(session->driver.ubx.original_port_settings)];
 673
 674     if (!session->driver.ubx.have_port_configuration)
 675         return;
 676
 677     /*@ +charint -usedef @*/
 678     for (i = 0; i < (int)sizeof(session->driver.ubx.original_port_settings);
 679          i++)
 680         buf[i] = session->driver.ubx.original_port_settings[i]; /* copy the original port settings */
 681     if (buf[0] == 0x01)         /* set baudrate on serial port only */
 682         putlelong(buf, 8, session->gpsdata.dev.baudrate);
 683
 684     if (mode == MODE_NMEA) {
 685         buf[14] &= ~0x01;       /* turn off UBX output on this port */
 686         buf[14] |= 0x02;        /* turn on NMEA output on this port */
 687     } else {                    /* MODE_BINARY */
 688         buf[14] &= ~0x02;       /* turn off NMEA output on this port */
 689         buf[14] |= 0x01;        /* turn on UBX output on this port */
 690     }
 691     /*@ -charint +usedef @*/
 692     (void)ubx_write(session, 0x06u, 0x00, &buf[6], 20); /* send back with all other settings intact */
 693 }
 694
 695 static bool ubx_speed(struct gps_device_t *session,
 696                       speed_t speed, char parity, int stopbits)
 697 {
 698     int i;
 699     unsigned char buf[sizeof(session->driver.ubx.original_port_settings)];
 700     unsigned long usart_mode;
 701
 702     /*@ +charint -usedef -compdef */
 703     for (i = 0; i < (int)sizeof(session->driver.ubx.original_port_settings);
 704          i++)
 705         buf[i] = session->driver.ubx.original_port_settings[i]; /* copy the original port settings */
 706     if ((!session->driver.ubx.have_port_configuration) || (buf[0] != 0x01))     /* set baudrate on serial port only */
 707         return false;
 708
 709     usart_mode = (unsigned long)getleul(buf, 4);
 710     usart_mode &= ~0xE00;       /* zero bits 11:9 */
 711     switch (parity) {
 712     case (int)'E':
 713     case 2:
 714         usart_mode |= 0x00;
 715         break;
 716     case (int)'O':
 717     case 1:
 718         usart_mode |= 0x01;
 719         break;
 720     case (int)'N':
 721     case 0:
 722     default:
 723         usart_mode |= 0x4;      /* 0x5 would work too */
 724         break;
 725     }
 726     usart_mode &= ~0x03000;     /* zero bits 13:12 */
 727     if (stopbits == 2)
 728         usart_mode |= 0x2000;   /* zero value means 1 stop bit */
 729     putlelong(buf, 4, usart_mode);
 730     putlelong(buf, 8, speed);
 731     (void)ubx_write(session, 0x06, 0x00, &buf[6], 20);  /* send back with all other settings intact */
 732     /*@ -charint +usedef +compdef */
 733     return true;
 734 }
 735
 736 static bool ubx_rate(struct gps_device_t *session, double cycletime)
 737 /* change the sample rate of the GPS */
 738 {
 739     unsigned short s;
 740     /*@ -type @*/
 741     unsigned char msg[6] = {
 742         0x00, 0x00,             /* U2: Measurement rate (ms) */
 743         0x00, 0x01,             /* U2: Navigation rate (cycles) */
 744         0x00, 0x00,             /* U2: Alignment to reference time: 0 = UTC, !0 = GPS */
 745     };
 746     /*@ +type @*/
 747
 748     /* clamp to cycle times that i know work on my receiver */
 749     if (cycletime > 1000.0)
 750         cycletime = 1000.0;
 751     if (cycletime < 200.0)
 752         cycletime = 200.0;
 753
 754     gpsd_report(LOG_IO, "UBX rate change, report every %f secs\n", cycletime);
 755     s = (unsigned short)cycletime;
 756     msg[0] = (unsigned char)(s >> 8);
 757     msg[1] = (unsigned char)(s & 0xff);
 758
 759     return ubx_write(session, 0x06, 0x08, msg, 6);      /* CFG-RATE */
 760 }
 761 #endif /* ALLOW_RECONFIGURE */
 762
 763 /* This is everything we export */
 764 /* *INDENT-OFF* */
 765 const struct gps_type_t ubx_binary = {
 766     .type_name        = "uBlox UBX binary",    /* Full name of type */
 767     .packet_type      = UBX_PACKET,     /* associated lexer packet type */
 768     .trigger          = "$GPTXT,01,01,02,MOD",
 769     .channels         = 50,             /* Number of satellite channels supported by the device */
 770     .probe_detect     = NULL,           /* Startup-time device detector */
 771     .get_packet       = generic_get,    /* Packet getter (using default routine) */
 772     .parse_packet     = parse_input,    /* Parse message packets */
 773     .rtcm_writer      = NULL,           /* RTCM handler (using default routine) */
 774     .event_hook       = ubx_event_hook, /* Fiew in variious lifetime events */
 775 #ifdef ALLOW_RECONFIGURE
 776     .speed_switcher   = ubx_speed,      /* Speed (baudrate) switch */
 777     .mode_switcher    = ubx_nmea_mode,  /* Switch to NMEA mode */
 778     .rate_switcher    = ubx_rate,       /* Message delivery rate switcher */
 779     .min_cycle        = 0.25,           /* Maximum 4Hz sample rate */
 780 #endif /* ALLOW_RECONFIGURE */
 781 #ifdef ALLOW_CONTROLSEND
 782     .control_send     = ubx_control_send,       /* no control sender yet */
 783 #endif /* ALLOW_CONTROLSEND */
 784 #ifdef NTPSHM_ENABLE
 785     .ntp_offset     = NULL,             /* no method for NTP fudge factor */
 786 #endif /* NTPSHM_ ENABLE */
 787 };
 788 /* *INDENT-ON* */
 789 #endif /* defined(UBX_ENABLE) && defined(BINARY_ENABLE) */