Skip to content

Commit 5473b43

Browse files
joe-saronicjoe-saronic
committed
introduced bounds checking functions and macro
1 parent 978cfb6 commit 5473b43

3 files changed

Lines changed: 161 additions & 102 deletions

File tree

‎src/rcv/adnav.c‎

Lines changed: 139 additions & 102 deletions
Original file line numberDiff line numberDiff line change
@@ -32,9 +32,19 @@
3232
// 9 // Reserved
3333
#define ANPP_SYS_NAV 10 // NavIC
3434

35-
/* packet 61 payload lengths */
36-
#define EPH_LEN_GPS 132 /* GPS ephemeris */
37-
#define EPH_LEN_GLO 94 /* GLONASS ephemeris */
35+
/* header size for all packets */
36+
#define HDR_LEN 5
37+
38+
/* packet 20 payload length */
39+
#define SYSSTATE_LEN 100 /* system state */
40+
41+
/* packet 21 payload length */
42+
#define UNIXTIME_LEN 8 /* unix time */
43+
44+
/* packet 60 sub-record sizes */
45+
#define OBS_HDR_LEN 16 /* per-epoch header (time + counters) */
46+
#define SAT_HDR_LEN 6 /* per-satellite header */
47+
#define FREQ_BLK_LEN 26 /* per-frequency block size */
3848

3949
/* tracking status bits (packet 60, per-frequency block, byte 1) */
4050
#define TRKS_CARRIER 0x01 /* carrier phase valid */
@@ -44,13 +54,49 @@
4454
#define TRKS_DOPPLER 0x10 /* Doppler valid */
4555
#define TRKS_SNR 0x20 /* SNR valid */
4656

57+
/* packet 61 payload lengths */
58+
#define EPH_LEN_GPS 132 /* GPS ephemeris */
59+
#define EPH_LEN_GLO 94 /* GLONASS ephemeris */
60+
4761
/* get fields (little-endian) ------------------------------------------------*/
48-
#define U1(p) (*((uint8_t *)(p)))
49-
#define I1(p) (*((int8_t *)(p)))
50-
static uint16_t U2(uint8_t *p) {uint16_t u; memcpy(&u,p,2); return u;}
51-
static uint32_t U4(uint8_t *p) {uint32_t u; memcpy(&u,p,4); return u;}
52-
static float R4(uint8_t *p) {float r; memcpy(&r,p,4); return r;}
53-
static double R8(uint8_t *p) {double r; memcpy(&r,p,8); return r;}
62+
static uint8_t U1(const raw_t *raw, size_t index) {
63+
RTKBOUNDSCHECK(raw->buff, sizeof(raw->buff), index);
64+
RTKBOUNDSCHECK(raw->buff, raw->len, index);
65+
return raw->buff[index];
66+
}
67+
static int8_t I1(const raw_t *raw, size_t index) {
68+
RTKBOUNDSCHECK(raw->buff, sizeof(raw->buff), index);
69+
RTKBOUNDSCHECK(raw->buff, raw->len, index);
70+
return (int8_t)raw->buff[index];
71+
}
72+
static uint16_t U2(const raw_t *raw, size_t index) {
73+
RTKBOUNDSCHECK(raw->buff, sizeof(raw->buff), index + 1);
74+
RTKBOUNDSCHECK(raw->buff, raw->len, index + 1);
75+
uint16_t u;
76+
memcpy(&u, raw->buff + index, 2);
77+
return u;
78+
}
79+
static uint32_t U4(const raw_t *raw, size_t index) {
80+
RTKBOUNDSCHECK(raw->buff, sizeof(raw->buff), index + 3);
81+
RTKBOUNDSCHECK(raw->buff, raw->len, index + 3);
82+
uint32_t u;
83+
memcpy(&u, raw->buff + index, 4);
84+
return u;
85+
}
86+
static float R4(const raw_t *raw, size_t index) {
87+
RTKBOUNDSCHECK(raw->buff, sizeof(raw->buff), index + 3);
88+
RTKBOUNDSCHECK(raw->buff, raw->len, index + 3);
89+
float r;
90+
memcpy(&r, raw->buff + index, 4);
91+
return r;
92+
}
93+
static double R8(const raw_t *raw, size_t index) {
94+
RTKBOUNDSCHECK(raw->buff, sizeof(raw->buff), index + 7);
95+
RTKBOUNDSCHECK(raw->buff, raw->len, index + 7);
96+
double r;
97+
memcpy(&r, raw->buff + index, 8);
98+
return r;
99+
}
54100

55101
/* receiver state ------------------------------------------------------------*/
56102
typedef struct {
@@ -233,28 +279,24 @@ static int obuf_slot(raw_t *raw, gtime_t time, int sat)
233279
static int decode_systemstate(raw_t *raw)
234280
{
235281
anpp_t *anpp = raw->rcv_data;
236-
uint8_t *p = raw->buff + 5;
237-
238-
if (raw->len < 5 + 100) {
282+
if (raw->len < HDR_LEN + SYSSTATE_LEN) {
239283
trace(2, "decode_systemstate: short packet len=%d\n", raw->len);
240284
return 0;
241285
}
242-
anpp->time.time = (time_t)U4(p + 4);
243-
anpp->time.sec = U4(p + 8) * 1e-6;
286+
anpp->time.time = (time_t)U4(raw, HDR_LEN + 4);
287+
anpp->time.sec = U4(raw, HDR_LEN + 8) * 1e-6;
244288
return 0;
245289
}
246290

247291
static int decode_unixtime(raw_t *raw)
248292
{
249293
anpp_t *anpp = (anpp_t *)raw->rcv_data;
250-
uint8_t *p = raw->buff + 5;
251-
252-
if (raw->len < 5 + 8) {
294+
if (raw->len < HDR_LEN + UNIXTIME_LEN) {
253295
trace(2, "decode_unixtime: short packet len=%d\n", raw->len);
254296
return 0;
255297
}
256-
anpp->time.time = (time_t)U4(p);
257-
anpp->time.sec = U4(p + 4) * 1e-6;
298+
anpp->time.time = (time_t)U4(raw, HDR_LEN);
299+
anpp->time.sec = U4(raw, HDR_LEN + 4) * 1e-6;
258300
return 0;
259301
}
260302

@@ -264,20 +306,16 @@ static int decode_rawsatdata(raw_t *raw)
264306
int ret = 0;
265307

266308
anpp_t *anpp = (anpp_t *)raw->rcv_data;
267-
uint8_t *p = raw->buff + 5;
268-
uint8_t *end = raw->buff + raw->len;
269-
270-
if (raw->len < 5 + 16) {
309+
if (raw->len < HDR_LEN + OBS_HDR_LEN) {
271310
trace(2, "decode_rawsatdata: short packet len=%d\n", raw->len);
272311
return 0;
273312
}
274-
275-
uint32_t unix_time = U4(p);
276-
uint32_t nanoseconds = U4(p + 4);
277-
uint8_t receiver_number = U1(p + 12);
278-
uint8_t packet_number = U1(p + 13);
279-
uint8_t total_packets = U1(p + 14);
280-
uint8_t nsats = U1(p + 15);
313+
uint32_t unix_time = U4(raw, HDR_LEN);
314+
uint32_t nanoseconds = U4(raw, HDR_LEN + 4);
315+
uint8_t receiver_number = U1(raw, HDR_LEN + 12);
316+
uint8_t packet_number = U1(raw, HDR_LEN + 13);
317+
uint8_t total_packets = U1(raw, HDR_LEN + 14);
318+
uint8_t nsats = U1(raw, HDR_LEN + 15);
281319

282320
/* select which receiver_number to emit: parse -RCVR<n> from raw->opt
283321
(default 0) */
@@ -308,44 +346,44 @@ static int decode_rawsatdata(raw_t *raw)
308346
anpp->epoch_active = 1;
309347
}
310348

311-
p += 16;
349+
size_t offset = HDR_LEN + OBS_HDR_LEN;
312350

313-
for (int i = 0; i < nsats && p + 6 <= end; i++) {
314-
uint8_t sys_id = U1(p);
351+
for (int i = 0; i < nsats && offset + SAT_HDR_LEN <= raw->len; i++) {
352+
uint8_t sys_id = U1(raw, offset);
315353
int sys = anpp2sys(sys_id);
316-
int sat = satno(sys, U1(p + 1));
317-
uint8_t nfreqs = U1(p + 5);
318-
p += 6;
354+
int sat = satno(sys, U1(raw, offset + 1));
355+
uint8_t nfreqs = U1(raw, offset + 5);
356+
offset += SAT_HDR_LEN;
319357

320358
if (sat == 0) {
321-
p += nfreqs * 26;
359+
offset += nfreqs * FREQ_BLK_LEN;
322360
continue;
323361
}
324362
int n = obuf_slot(raw, raw->time, sat);
325363
if (n < 0) {
326-
p += nfreqs * 26;
364+
offset += nfreqs * FREQ_BLK_LEN;
327365
continue;
328366
}
329367

330-
for (; nfreqs > 0 && p + 26 <= end; nfreqs--, p += 26) {
331-
uint8_t freq_id = U1(p);
332-
uint8_t trks = U1(p + 1);
368+
for (; nfreqs > 0 && offset + FREQ_BLK_LEN <= raw->len; nfreqs--, offset += FREQ_BLK_LEN) {
369+
uint8_t freq_id = U1(raw, offset);
370+
uint8_t trks = U1(raw, offset + 1);
333371
uint8_t code = anpp2code(sys_id, freq_id);
334372
if (code == CODE_NONE) continue;
335373
int idx = code2idx(sys, code);
336374
if (idx < 0 || idx >= NFREQ + NEXOBS) continue;
337375

338376
if (trks & TRKS_CARRIER) {
339-
raw->obuf.data[n].L[idx] = R8(p + 2);
377+
raw->obuf.data[n].L[idx] = R8(raw, offset + 2);
340378
raw->obuf.data[n].LLI[idx] = (trks & TRKS_SLIP) ? LLI_SLIP : 0;
341379
raw->obuf.data[n].LLI[idx] |= (trks & TRKS_HALFCYCLE) ? LLI_HALFC : 0;
342380
}
343381
if (trks & TRKS_PSEUDORANGE)
344-
raw->obuf.data[n].P[idx] = R8(p + 10);
382+
raw->obuf.data[n].P[idx] = R8(raw, offset + 10);
345383
if (trks & TRKS_DOPPLER)
346-
raw->obuf.data[n].D[idx] = R4(p + 18);
384+
raw->obuf.data[n].D[idx] = R4(raw, offset + 18);
347385
if (trks & TRKS_SNR)
348-
raw->obuf.data[n].SNR[idx] = R4(p + 22);
386+
raw->obuf.data[n].SNR[idx] = R4(raw, offset + 22);
349387

350388
raw->obuf.data[n].code[idx] = code;
351389
}
@@ -360,39 +398,39 @@ static int decode_rawsatdata(raw_t *raw)
360398
}
361399

362400
/* packet 61: raw satellite ephemeris ----------------------------------------*/
363-
static int decode_gps_eph(raw_t *raw, int sat, uint8_t *e)
401+
static int decode_gps_eph(raw_t *raw, int sat, size_t e)
364402
{
365403
eph_t eph = {0};
366404
double sqrtA;
367405

368406
eph.sat = sat;
369-
eph.iodc = (int)U2(e + 4);
370-
eph.iode = (int)U2(e + 6);
371-
eph.week = (int)U2(e + 116);
372-
eph.toes = (double)U4(e + 0);
407+
eph.iodc = (int)U2(raw, e + 4);
408+
eph.iode = (int)U2(raw, e + 6);
409+
eph.week = (int)U2(raw, e + 116);
410+
eph.toes = (double)U4(raw, e + 0);
373411
eph.toe = gpst2time(eph.week, eph.toes);
374412
eph.toc = eph.toe;
375-
eph.ttr = gpst2time(eph.week, (double)U4(e + 118));
376-
eph.f0 = (double)R4(e + 8);
377-
eph.f1 = (double)R4(e + 12);
378-
eph.f2 = (double)R4(e + 16);
379-
eph.crs = (double)R4(e + 20);
380-
eph.deln = (double)R4(e + 24);
381-
eph.M0 = R8(e + 28);
382-
eph.cuc = (double)R4(e + 36);
383-
eph.e = R8(e + 40);
384-
eph.cus = (double)R4(e + 48);
385-
sqrtA = R8(e + 52);
413+
eph.ttr = gpst2time(eph.week, (double)U4(raw, e + 118));
414+
eph.f0 = (double)R4(raw, e + 8);
415+
eph.f1 = (double)R4(raw, e + 12);
416+
eph.f2 = (double)R4(raw, e + 16);
417+
eph.crs = (double)R4(raw, e + 20);
418+
eph.deln = (double)R4(raw, e + 24);
419+
eph.M0 = R8(raw, e + 28);
420+
eph.cuc = (double)R4(raw, e + 36);
421+
eph.e = R8(raw, e + 40);
422+
eph.cus = (double)R4(raw, e + 48);
423+
sqrtA = R8(raw, e + 52);
386424
eph.A = sqrtA * sqrtA;
387-
eph.cic = (double)R4(e + 60);
388-
eph.OMG0 = R8(e + 64);
389-
eph.cis = (double)R4(e + 72);
390-
eph.i0 = R8(e + 76);
391-
eph.crc = (double)R4(e + 84);
392-
eph.omg = R8(e + 88);
393-
eph.OMGd = R8(e + 96);
394-
eph.idot = R8(e + 104);
395-
eph.tgd[0] = (double)R4(e + 112);
425+
eph.cic = (double)R4(raw, e + 60);
426+
eph.OMG0 = R8(raw, e + 64);
427+
eph.cis = (double)R4(raw, e + 72);
428+
eph.i0 = R8(raw, e + 76);
429+
eph.crc = (double)R4(raw, e + 84);
430+
eph.omg = R8(raw, e + 88);
431+
eph.OMGd = R8(raw, e + 96);
432+
eph.idot = R8(raw, e + 104);
433+
eph.tgd[0] = (double)R4(raw, e + 112);
396434
eph.sva = 0;
397435
eph.svh = 0;
398436
eph.code = 0;
@@ -409,27 +447,27 @@ static int decode_gps_eph(raw_t *raw, int sat, uint8_t *e)
409447
return 2;
410448
}
411449

412-
static int decode_glo_eph(raw_t *raw, int prn, uint8_t *e)
450+
static int decode_glo_eph(raw_t *raw, int prn, size_t e)
413451
{
414452
geph_t geph = {0};
415453
gtime_t utc;
416454

417455
geph.sat = satno(SYS_GLO, prn);
418-
geph.frq = (int)I1(e + 85);
419-
geph.svh = (int)U1(e + 86);
420-
geph.age = (int)U1(e + 84);
421-
geph.taun = (double)R4(e + 0);
422-
geph.gamn = (double)R4(e + 4);
423-
geph.pos[0] = R8(e + 8);
424-
geph.pos[1] = R8(e + 16);
425-
geph.pos[2] = R8(e + 24);
426-
geph.vel[0] = R8(e + 32);
427-
geph.vel[1] = R8(e + 40);
428-
geph.vel[2] = R8(e + 48);
429-
geph.acc[0] = R8(e + 56);
430-
geph.acc[1] = R8(e + 64);
431-
geph.acc[2] = R8(e + 72);
432-
utc.time = (time_t)U4(e + 80);
456+
geph.frq = (int)I1(raw, e + 85);
457+
geph.svh = (int)U1(raw, e + 86);
458+
geph.age = (int)U1(raw, e + 84);
459+
geph.taun = (double)R4(raw, e + 0);
460+
geph.gamn = (double)R4(raw, e + 4);
461+
geph.pos[0] = R8(raw, e + 8);
462+
geph.pos[1] = R8(raw, e + 16);
463+
geph.pos[2] = R8(raw, e + 24);
464+
geph.vel[0] = R8(raw, e + 32);
465+
geph.vel[1] = R8(raw, e + 40);
466+
geph.vel[2] = R8(raw, e + 48);
467+
geph.acc[0] = R8(raw, e + 56);
468+
geph.acc[1] = R8(raw, e + 64);
469+
geph.acc[2] = R8(raw, e + 72);
470+
utc.time = (time_t)U4(raw, e + 80);
433471
utc.sec = 0.0;
434472
geph.toe = utc2gpst(utc);
435473
geph.tof = geph.toe;
@@ -445,27 +483,26 @@ static int decode_glo_eph(raw_t *raw, int prn, uint8_t *e)
445483

446484
static int decode_rawsateph(raw_t *raw)
447485
{
448-
uint8_t *p = raw->buff + 5;
449-
uint8_t sys_id = U1(p + 4);
450-
int sys = anpp2sys(sys_id);
451-
int prn = (int)U1(p + 5);
452-
int sat = satno(sys, prn);
486+
uint8_t sys_id = U1(raw, HDR_LEN + 4);
487+
int sys = anpp2sys(sys_id);
488+
int prn = (int)U1(raw, HDR_LEN + 5);
489+
int sat = satno(sys, prn);
453490

454491
if (sat == 0) return 0;
455492

456493
switch (sys) {
457494
case SYS_GPS:
458-
if (raw->len < 5 + EPH_LEN_GPS) {
495+
if (raw->len < HDR_LEN + EPH_LEN_GPS) {
459496
trace(2, "decode_rawsateph GPS: short packet len=%d\n", raw->len);
460497
return 0;
461498
}
462-
return decode_gps_eph(raw, sat, p + 6);
499+
return decode_gps_eph(raw, sat, HDR_LEN + 6);
463500
case SYS_GLO:
464-
if (raw->len < 5 + EPH_LEN_GLO) {
501+
if (raw->len < HDR_LEN + EPH_LEN_GLO) {
465502
trace(2, "decode_rawsateph GLO: short packet len=%d\n", raw->len);
466503
return 0;
467504
}
468-
return decode_glo_eph(raw, prn, p + 6);
505+
return decode_glo_eph(raw, prn, HDR_LEN + 6);
469506
default:
470507
trace(2, "decode_rawsateph: unsupported sys=%d\n", sys_id);
471508
return 0;
@@ -492,7 +529,7 @@ static int valid_hdr(const uint8_t *p)
492529

493530
static int decode_anpp(raw_t *raw)
494531
{
495-
int id = (int)U1(raw->buff + 1);
532+
int id = (int)U1(raw, 1);
496533

497534
trace(4, "decode_anpp: id=%d len=%d\n", id, raw->len);
498535

@@ -510,22 +547,22 @@ extern int input_anpp(raw_t *raw, uint8_t data)
510547
if (raw->nbyte < MAXRAWLEN) raw->buff[raw->nbyte] = data;
511548
raw->nbyte++;
512549

513-
if (raw->nbyte < 5) return 0;
550+
if (raw->nbyte < HDR_LEN) return 0;
514551

515-
if (raw->nbyte == 5) {
552+
if (raw->nbyte == HDR_LEN) {
516553
if (!valid_hdr(raw->buff)) {
517-
memmove(raw->buff, raw->buff + 1, 4);
518-
raw->nbyte = 4;
554+
memmove(raw->buff, raw->buff + 1, HDR_LEN - 1);
555+
raw->nbyte = HDR_LEN - 1;
519556
return 0;
520557
}
521-
raw->len = 5 + (int)raw->buff[2];
558+
raw->len = HDR_LEN + (int)raw->buff[2];
522559
}
523560

524561
if (raw->nbyte < raw->len) return 0;
525562

526563
raw->nbyte = 0;
527564

528-
if (crc_ccitt(raw->buff + 5, raw->len - 5) != U2(raw->buff + 3)) {
565+
if (crc_ccitt(raw->buff + HDR_LEN, raw->len - HDR_LEN) != U2(raw, 3)) {
529566
trace(2, "input_anpp: CRC error id=%d\n", (int)raw->buff[1]);
530567
return 0;
531568
}

0 commit comments

Comments
 (0)