static void bedumpall(void) { (void)printf("getsb: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) sb1, (uint64_t) sb2, (uint64_t) getsb(buf, 0), (uint64_t) getsb(buf, 8)); (void)printf("getub: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) ub1, (uint64_t) ub2, (uint64_t) getub(buf, 0), (uint64_t) getub(buf, 8)); (void)printf("getbes16: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) sw1, (uint64_t) sw2, (uint64_t) getbes16(buf, 0), (uint64_t) getbes16(buf, 8)); (void)printf("getbeu16: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) uw1, (uint64_t) uw2, (uint64_t) getbeu16(buf, 0), (uint64_t) getbeu16(buf, 8)); (void)printf("getbes32: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) sl1, (uint64_t) sl2, (uint64_t) getbes32(buf, 0), (uint64_t) getbes32(buf, 8)); (void)printf("getbeu32: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) ul1, (uint64_t) ul2, (uint64_t) getbeu32(buf, 0), (uint64_t) getbeu32(buf, 8)); (void)printf("getbes64: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) sL1, (uint64_t) sL2, (uint64_t) getbes64(buf, 0), (uint64_t) getbes64(buf, 8)); (void)printf("getbeu64: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) uL1, (uint64_t) uL2, (uint64_t) getbeu64(buf, 0), (uint64_t) getbeu64(buf, 8)); (void)printf("getbef32: %f %f\n", f1, getbef32((const char *)buf, 24)); (void)printf("getbed64: %.16f %.16f\n", d1, getbed64((const char *)buf, 16)); }
/*@ -duplicatequals +ignorequals @*/ int main(int argc, char *argv[]) { bool failures = false; bool quiet = (argc > 1) && (strcmp(argv[1], "--quiet") == 0); /*@ -observertrans -usereleased @*/ struct unsigned_test *up, unsigned_tests[] = { /* tests using the big buffer */ {buf, 0, 1, 0, false, "first bit of first byte"}, {buf, 0, 8, 0x01, false, "first 8 bits"}, {buf, 32, 7, 0x02, false, "first seven bits of fifth byte (0x05)"}, {buf, 56, 12, 0x8f, false, "12 bits crossing 7th to 8th bytes (0x08ff)"}, {buf, 78, 4, 0xb, false, "4 bits crossing 8th to 9th byte (0xfefd)"}, {buf, 0, 1, 0, true, "first bit of first byte"}, {buf, 0, 8, 0x80, true, "first 8 bits"}, {buf, 32, 7, 0x20, true, "first seven bits of fifth byte (0x05)"}, {buf, 56, 12, 0xf10,true, "12 bits crossing 7th to 8th bytes (0x08ff)"}, {buf, 78, 4, 0xd, true, "4 bits crossing 8th to 9th byte (0xfefd)"}, /* sporadic tests based on found bugs */ {(unsigned char *)"\x19\x23\f6", 7, 2, 2, false, "2 bits crossing 1st to 2nd byte (0x1923)"}, }; unsigned char *sp; memcpy(buf, "\x01\x02\x03\x04\x05\x06\x07\x08", 8); memcpy(buf + 8, "\xff\xfe\xfd\xfc\xfb\xfa\xf9\xf8", 8); memcpy(buf + 16, "\x40\x09\x21\xfb\x54\x44\x2d\x18", 8); memcpy(buf + 24, "\x40\x49\x0f\xdb", 4); /*@ +observertrans +usereleased @*/ /*@-type@*/ sb1 = getsb(buf, 0); sb2 = getsb(buf, 8); ub1 = getub(buf, 0); ub2 = getub(buf, 8); sw1 = getbes16(buf, 0); sw2 = getbes16(buf, 8); uw1 = getbeu16(buf, 0); uw2 = getbeu16(buf, 8); sl1 = getbes32(buf, 0); sl2 = getbes32(buf, 8); ul1 = getbeu32(buf, 0); ul2 = getbeu32(buf, 8); sL1 = getbes64(buf, 0); sL2 = getbes64(buf, 8); uL1 = getbeu64(buf, 0); uL2 = getbeu64(buf, 8); f1 = getbef32((const char *)buf, 24); d1 = getbed64((const char *)buf, 16); sb1 = getsb(buf, 0); sb2 = getsb(buf, 8); ub1 = getub(buf, 0); ub2 = getub(buf, 8); sw1 = getles16(buf, 0); sw2 = getles16(buf, 8); uw1 = getleu16(buf, 0); uw2 = getleu16(buf, 8); sl1 = getles32(buf, 0); sl2 = getles32(buf, 8); ul1 = getleu32(buf, 0); ul2 = getleu32(buf, 8); sL1 = getles64(buf, 0); sL2 = getles64(buf, 8); uL1 = getleu64(buf, 0); uL2 = getleu64(buf, 8); f1 = getlef32((const char *)buf, 24); d1 = getled64((const char *)buf, 16); /*@+type@*/ if (!quiet) { (void)fputs("Test data:", stdout); for (sp = buf; sp < buf + 28; sp++) (void)printf(" %02x", *sp); (void)putc('\n', stdout); /* big-endian test */ printf("Big-endian:\n"); bedumpall(); /* little-endian test */ printf("Little-endian:\n"); ledumpall(); } if (sb1 != 1) printf("getsb(buf, 0) FAILED\n"); if (sb2 != -1) printf("getsb(buf, 8) FAILED\n"); if (ub1 != 1) printf("getub(buf, 0) FAILED\n"); if (ub2 != 0xff) printf("getub(buf, 8) FAILED\n"); (void)printf("Testing bitfield extraction\n"); for (up = unsigned_tests; up < unsigned_tests + sizeof(unsigned_tests) / sizeof(unsigned_tests[0]); up++) { uint64_t res = ubits((unsigned char *)buf, up->start, up->width, up->le); bool success = (res == up->expected); if (!success) failures = true; if (!success || !quiet) (void)printf("ubits(%s, %d, %d, %s) %s should be %" PRIx64 ", is %" PRIx64 ": %s\n", hexdump(buf, strlen((char *)buf)), up->start, up->width, up->le ? "true" : "false", up->description, up->expected, res, success ? "succeeded" : "FAILED"); } exit(failures ? EXIT_FAILURE : EXIT_SUCCESS); }
/* * decode MID 0xDF, Nav status (PVT) * * 81 bytes */ static gps_mask_t sky_msg_DF(struct gps_device_t *session, unsigned char *buf, size_t len) { unsigned int iod; /* Issue of data 0 - 255 */ unsigned short navstat; unsigned int wn; /* week number 0 - 65535 */ double f_tow; /* receiver tow Sec */ double clock_bias; double clock_drift; gps_mask_t mask = 0; if ( 81 != len) return 0; iod = (unsigned int)getub(buf, 1); /* fix status is byte 2 */ navstat = (unsigned short)getub(buf, 2); session->gpsdata.status = STATUS_NO_FIX; session->newdata.mode = MODE_NO_FIX; switch ( navstat ) { case 1: /* fix prediction, ignore */ break; case 2: session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_2D; break; case 3: session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_3D; mask |= ALTITUDE_SET | CLIMB_SET; break; case 4: session->gpsdata.status = STATUS_DGPS_FIX; session->newdata.mode = MODE_3D; mask |= ALTITUDE_SET | CLIMB_SET; break; default: break; } wn = getbeu16(buf, 3); f_tow = getbed64((const char *)buf, 5); /* position/velocity is bytes 13-48, meters and m/s */ ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation, (double)getbed64((const char *)buf, 13), (double)getbed64((const char *)buf, 21), (double)getbed64((const char *)buf, 29), (double)getbef32((const char *)buf, 37), (double)getbef32((const char *)buf, 41), (double)getbef32((const char *)buf, 46)); clock_bias = getbed64((const char *)buf, 49); clock_drift = getbes32(buf, 57); session->gpsdata.dop.gdop = getbef32((const char *)buf, 61); session->gpsdata.dop.pdop = getbef32((const char *)buf, 65); session->gpsdata.dop.hdop = getbef32((const char *)buf, 69); session->gpsdata.dop.vdop = getbef32((const char *)buf, 73); session->gpsdata.dop.tdop = getbef32((const char *)buf, 77); session->newdata.time = gpsd_gpstime_resolve(session, wn, f_tow ); gpsd_log(&session->context->errout, LOG_DATA, "Skytraq: MID 0xDF: iod=%u, stat=%u, wn=%u, tow=%f, t=%.6f " "cb: %f, cd: %f " "gdop: %.2f, pdop: %.2f, hdop: %.2f, vdop: %.2f, tdop: %.2f\n", iod, navstat, wn, f_tow, session->newdata.time, clock_bias, clock_drift, session->gpsdata.dop.gdop, session->gpsdata.dop.pdop, session->gpsdata.dop.hdop, session->gpsdata.dop.vdop, session->gpsdata.dop.tdop); mask |= TIME_SET | LATLON_SET | TRACK_SET | SPEED_SET | STATUS_SET | MODE_SET | DOP_SET | CLEAR_IS | REPORT_IS; return mask; }