onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(gps): harden navigation epoch recovery

Browse Source
This commit is contained in:
onur
2026-07-24 14:01:58 +03:00
parent ea055a36d2
commit fe0eb19dc5
15 changed files with 1007 additions and 773 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/tests/test_gps_nav_epoch.c
+174 -352
View File
@@ -5,8 +5,11 @@
#include <stdio.h>
#include <string.h>
#define SAMPLE_WINDOW 409200u
#define TOW_TOLERANCE 0.001
#define SAMPLE_WINDOW 409200u
#define TOW_TOLERANCE 0.001
#define CAPTURE_PERIOD_TICKS 25000000ULL
#define CAPTURE_WINDOW_TICKS 3125000ULL
#define HOLDOFF_LOCAL_TICKS 15000ULL
static int failures;
@@ -29,167 +32,17 @@ static gps_nav_epoch_selection_t select_epoch(
return selection;
}
static unsigned int mask_count(uint32_t mask)
static gps_nav_epoch_event_result_t push_capture(
gps_nav_epoch_assembly_t* assembly,
uint64_t capture_counter,
uint64_t local_ticks,
unsigned int channel,
uint32_t ready_mask)
{
unsigned int count = 0u;
while(mask != 0u)
{
count += mask & 1u;
mask >>= 1u;
}
return count;
}
static void epoch_id_rounding_and_week_wrap(void)
{
uint64_t before_wrap;
uint64_t after_wrap;
uint64_t same_epoch_a;
uint64_t same_epoch_b;
CHECK(gps_nav_epoch_id(2300, 604799.9999, 200u, &before_wrap),
"epoch id rejected a valid pre-wrap TOW");
CHECK(gps_nav_epoch_id(2301, 0.0001, 200u, &after_wrap),
"epoch id rejected a valid post-wrap TOW");
CHECK(before_wrap == after_wrap,
"week rollover split one rounded navigation epoch");
CHECK(gps_nav_epoch_id(2301, 123.3997, 200u, &same_epoch_a) &&
gps_nav_epoch_id(2301, 123.4003, 200u, &same_epoch_b) &&
(same_epoch_a == same_epoch_b),
"sub-millisecond TOW skew split one navigation epoch");
CHECK(!gps_nav_epoch_id(2301, NAN, 200u, &same_epoch_a),
"non-finite TOW produced an epoch id");
CHECK(!gps_nav_epoch_id(2301, 1.0, 333u, &same_epoch_a),
"non-integral epochs per GPS week were accepted");
}
static void assembly_waits_for_every_ready_channel(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint32_t ready_mask = 0xafu;
const unsigned int order[] = {7u, 0u, 5u, 3u, 2u, 1u};
for(unsigned int i = 0u ; i < sizeof(order) / sizeof(order[0]) ; i++)
{
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 1000u, order[i], ready_mask);
bool last = i + 1u == sizeof(order) / sizeof(order[0]);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
"assembly completed before every ready channel arrived");
}
CHECK(assembly.expected_mask == ready_mask,
"assembly changed its frozen ready mask");
CHECK(assembly.received_mask == ready_mask,
"assembly lost a ready-channel interrupt");
}
static void busy_unlocked_channel_does_not_block_epoch(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
/* Model the board failure: channels 0..8 are allocated and busy, but
* channel 4 has lost lock and therefore cannot produce this NAV epoch. */
const uint32_t allocated_busy_mask = 0x1ffu;
const uint32_t locked_mask = allocated_busy_mask & ~(1u << 4);
const uint32_t ready_mask = allocated_busy_mask & locked_mask;
for(unsigned int channel = 0u ; channel < 9u ; channel++)
{
if((ready_mask & (1u << channel)) == 0u)
{
continue;
}
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 1500u, channel, ready_mask);
bool last = channel == 8u;
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
"busy unlocked channel blocked a complete locked-channel epoch");
}
CHECK(assembly.expected_mask == 0x1efu,
"busy unlocked channel entered the ready mask");
CHECK(assembly.received_mask == assembly.expected_mask,
"locked channels did not complete the epoch");
}
static void assembly_handles_duplicates_and_epoch_order(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
CHECK(gps_nav_epoch_assembly_push(&assembly, 2000u, 0u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"first channel did not start assembly");
CHECK(gps_nav_epoch_assembly_push(&assembly, 2000u, 0u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_DUPLICATE,
"duplicate channel interrupt was not idempotent");
CHECK(gps_nav_epoch_assembly_push(&assembly, 1999u, 1u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_STALE,
"stale epoch interrupt was not rejected");
CHECK(gps_nav_epoch_assembly_push(&assembly, 2001u, 1u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_RESTARTED,
"newer epoch did not replace the incomplete epoch");
CHECK((assembly.epoch_id == 2001u) &&
(assembly.expected_mask == 0x0fu) &&
(assembly.received_mask == 0x02u),
"newer epoch restart retained measurements from the old epoch");
}
static void assembly_disconnect_and_reconnect(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 0u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"disconnect scenario did not start");
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 1u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"second interrupt unexpectedly completed");
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
"pending-channel disconnect completed too early");
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 2u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"post-disconnect interrupt was rejected");
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 3u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_COMPLETE,
"unlock did not release the removed channel from the epoch");
gps_nav_epoch_assembly_reset(&assembly);
CHECK(gps_nav_epoch_assembly_push(&assembly, 3001u, 0u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"reconnect epoch did not start");
for(unsigned int channel = 1u ; channel < 5u ; channel++)
{
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 3001u, channel, 0x1fu);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
(channel == 4u),
"reconnected channel was not required by the next epoch");
}
}
static void channel_join_does_not_expand_active_epoch(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
CHECK(gps_nav_epoch_assembly_push(&assembly, 4000u, 0u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"join scenario did not start");
CHECK(gps_nav_epoch_assembly_push(&assembly, 4000u, 4u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_INVALID,
"joining channel changed the active epoch");
CHECK(assembly.expected_mask == 0x0fu,
"active expected mask expanded after a channel joined");
for(unsigned int channel = 1u ; channel < 4u ; channel++)
{
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 4000u, channel, 0x1fu);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
(channel == 3u),
"original ready set did not complete after a join");
}
return gps_nav_epoch_assembly_push(
assembly, capture_counter, local_ticks, channel, ready_mask,
CAPTURE_PERIOD_TICKS, CAPTURE_WINDOW_TICKS,
HOLDOFF_LOCAL_TICKS);
}
static uint32_t deterministic_random(uint32_t* state)
@@ -202,122 +55,144 @@ static uint32_t deterministic_random(uint32_t* state)
return value;
}
static void randomized_interrupt_and_unlock_model(void)
static void local_capture_assembly_completes(void)
{
uint32_t random_state = 0x6a09e667u;
for(unsigned int trial = 0u ; trial < 10000u ; trial++)
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint32_t ready_mask = 0xafu;
const unsigned int order[] = {7u, 0u, 5u, 3u, 2u, 1u};
const uint64_t base = 810102900000ULL;
for(unsigned int i = 0u ; i < sizeof(order) / sizeof(order[0]) ; i++)
{
unsigned int channel_count =
4u + (deterministic_random(&random_state) % 6u);
uint32_t ready_mask = (1u << channel_count) - 1u;
unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
for(unsigned int i = 0u ; i < channel_count ; i++)
{
order[i] = i;
}
for(unsigned int i = channel_count ; i > 1u ; i--)
{
unsigned int swap =
deterministic_random(&random_state) % i;
unsigned int temporary = order[i - 1u];
order[i - 1u] = order[swap];
order[swap] = temporary;
}
gps_nav_epoch_event_result_t result =
push_capture(&assembly, base + (uint64_t)i * 250000u,
100000u + i, order[i], ready_mask);
bool last = i + 1u == sizeof(order) / sizeof(order[0]);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
"coherent local capture group completed at the wrong event");
}
CHECK(assembly.expected_mask == ready_mask,
"assembly changed its frozen ready mask");
CHECK(assembly.received_mask == ready_mask,
"assembly lost a ready-channel capture");
CHECK((assembly.maximum_capture_counter -
assembly.minimum_capture_counter) <= CAPTURE_WINDOW_TICKS,
"coherent group exceeded its capture span");
}
bool disconnect =
(deterministic_random(&random_state) & 3u) == 0u;
unsigned int dropped_channel = disconnect ?
(deterministic_random(&random_state) % channel_count) :
GPS_NAV_EPOCH_MAX_CANDIDATES;
uint32_t final_mask = ready_mask;
if(disconnect)
{
final_mask &= ~(1u << dropped_channel);
}
static void missing_channel_expires_as_usable_subset(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint32_t ready_mask = 0x3fu;
const uint64_t base = 900000000ULL;
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
bool complete = false;
for(unsigned int i = 0u ; i < channel_count ; i++)
{
unsigned int channel = order[i];
if(channel == dropped_channel)
{
complete = gps_nav_epoch_assembly_remove_channel(
&assembly, channel);
continue;
}
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 5000u + trial, channel, ready_mask);
if((deterministic_random(&random_state) & 7u) == 0u)
{
CHECK(gps_nav_epoch_assembly_push(
&assembly, 5000u + trial, channel,
ready_mask) ==
GPS_NAV_EPOCH_EVENT_DUPLICATE,
"random duplicate interrupt was not idempotent");
}
complete = result == GPS_NAV_EPOCH_EVENT_COMPLETE;
}
if(disconnect &&
((assembly.expected_mask & (1u << dropped_channel)) != 0u))
{
complete = gps_nav_epoch_assembly_remove_channel(
&assembly, dropped_channel);
}
CHECK(mask_count(final_mask) < 4u ||
(complete && gps_nav_epoch_assembly_complete(&assembly)),
"random interrupt/unlock sequence did not complete");
CHECK(assembly.expected_mask == final_mask,
"random unlock changed the wrong expected channel");
CHECK(assembly.received_mask == final_mask,
"random model lost or invented an interrupt");
for(unsigned int channel = 0u ; channel < 5u ; channel++)
{
CHECK(push_capture(&assembly,
base + (uint64_t)channel * 100000u,
200000u + channel,
channel, ready_mask) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"partial epoch rejected a coherent channel");
}
CHECK(!gps_nav_epoch_assembly_complete(&assembly),
"missing ready channel completed before holdoff");
CHECK(gps_nav_epoch_assembly_usable(&assembly, 4u),
"five-channel partial epoch was not usable");
CHECK(!gps_nav_epoch_assembly_expired(
&assembly, assembly.deadline_local_ticks - 1u),
"epoch expired before its deadline");
CHECK(gps_nav_epoch_assembly_expired(
&assembly, assembly.deadline_local_ticks),
"epoch did not expire at its deadline");
}
static void local_outlier_cannot_poison_epoch(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint32_t ready_mask = 0x1fu;
const uint64_t base = 1000000000ULL;
CHECK(push_capture(&assembly, base, 300000u, 0u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"outlier scenario did not start");
CHECK(push_capture(&assembly, base + 6000000u, 300001u,
1u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_OUTLIER,
"ambiguous local timestamp entered the active group");
CHECK((assembly.received_mask == 0x01u) &&
(assembly.minimum_capture_counter == base) &&
(assembly.maximum_capture_counter == base),
"local outlier mutated the active group");
CHECK(push_capture(&assembly, base - CAPTURE_PERIOD_TICKS,
300002u, 2u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_STALE,
"delayed previous epoch was not rejected");
CHECK(push_capture(&assembly, base + CAPTURE_PERIOD_TICKS,
300003u, 1u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_RESTARTED,
"new local epoch did not replace the incomplete group");
CHECK((assembly.first_capture_counter ==
base + CAPTURE_PERIOD_TICKS) &&
(assembly.received_mask == 0x02u),
"new epoch retained state from the old group");
}
static void duplicate_join_and_disconnect_are_bounded(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint64_t base = 1100000000ULL;
CHECK(push_capture(&assembly, base, 400000u, 0u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"membership scenario did not start");
CHECK(push_capture(&assembly, base, 400001u, 0u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_DUPLICATE,
"duplicate capture was not idempotent");
CHECK(push_capture(&assembly, base + 100000u, 400002u,
5u, 0x3fu) ==
GPS_NAV_EPOCH_EVENT_INVALID,
"joining channel expanded the active epoch");
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
"disconnect completed a one-channel group");
for(unsigned int channel = 1u ; channel < 4u ; channel++)
{
gps_nav_epoch_event_result_t result =
push_capture(&assembly,
base + (uint64_t)channel * 100000u,
400010u + channel, channel, 0x0fu);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
(channel == 3u),
"remaining frozen membership completed incorrectly");
}
}
static void multi_epoch_hardware_interrupt_model(void)
static void randomized_capture_liveness(void)
{
gps_nav_epoch_assembly_t assembly;
uint32_t random_state = 0xbb67ae85u;
uint32_t ready_mask = 0x7fu;
unsigned int completed_epochs = 0u;
unsigned int dropped_epochs = 0u;
unsigned int joined_epochs = 0u;
for(unsigned int epoch = 0u ; epoch < 2000u ; epoch++)
uint32_t random_state = 0x6a09e667u;
uint64_t base = 2000000000ULL;
for(unsigned int trial = 0u ; trial < 100000u ; trial++)
{
bool drop_before_interrupt = (epoch == 250u);
bool drop_after_interrupt = (epoch == 900u);
bool join_during_epoch = (epoch == 500u) || (epoch == 1200u);
unsigned int changing_channel =
(epoch == 1200u) ? 6u : 5u;
if(epoch == 400u)
{
ready_mask |= 1u << 2;
}
if(epoch == 1050u)
{
ready_mask |= 1u << 4;
}
if(join_during_epoch)
{
ready_mask &= ~(1u << changing_channel);
}
uint32_t epoch_ready_mask = ready_mask;
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
unsigned int ready_count =
4u + (deterministic_random(&random_state) % 6u);
uint32_t ready_mask = (1u << ready_count) - 1u;
unsigned int delivered_count =
4u + (deterministic_random(&random_state) %
(ready_count - 3u));
unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
unsigned int order_count = 0u;
for(unsigned int channel = 0u ;
channel < GPS_NAV_EPOCH_MAX_CANDIDATES ; channel++)
for(unsigned int i = 0u ; i < ready_count ; i++)
{
if((epoch_ready_mask & (1u << channel)) != 0u)
{
order[order_count++] = channel;
}
order[i] = i;
}
for(unsigned int i = order_count ; i > 1u ; i--)
for(unsigned int i = ready_count ; i > 1u ; i--)
{
unsigned int swap =
deterministic_random(&random_state) % i;
@@ -326,76 +201,28 @@ static void multi_epoch_hardware_interrupt_model(void)
order[swap] = temporary;
}
gps_nav_epoch_assembly_reset(&assembly);
bool completed = false;
for(unsigned int event_index = 0u ;
event_index < order_count ; event_index++)
for(unsigned int i = 0u ; i < delivered_count ; i++)
{
unsigned int channel = order[event_index];
if(drop_before_interrupt && (channel == 2u))
{
ready_mask &= ~(1u << channel);
(void)gps_nav_epoch_assembly_remove_channel(
&assembly, channel);
dropped_epochs++;
continue;
}
uint64_t offset =
deterministic_random(&random_state) %
(CAPTURE_WINDOW_TICKS + 1u);
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 100000u + epoch, channel,
epoch_ready_mask);
if(result == GPS_NAV_EPOCH_EVENT_COMPLETE)
{
completed = true;
}
if(drop_after_interrupt && (channel == 4u))
{
ready_mask &= ~(1u << channel);
if(gps_nav_epoch_assembly_remove_channel(
&assembly, channel))
{
completed = true;
}
dropped_epochs++;
}
if(join_during_epoch && (event_index == 0u))
{
ready_mask |= 1u << changing_channel;
CHECK(gps_nav_epoch_assembly_push(
&assembly, 100000u + epoch,
changing_channel, ready_mask) ==
GPS_NAV_EPOCH_EVENT_INVALID,
"mid-epoch connection expanded the frozen mask");
joined_epochs++;
}
if((event_index == 0u) && (epoch != 0u))
{
CHECK(gps_nav_epoch_assembly_push(
&assembly, 99999u + epoch,
channel, epoch_ready_mask) ==
GPS_NAV_EPOCH_EVENT_STALE,
"delayed previous-epoch interrupt was accepted");
}
push_capture(&assembly, base + offset,
500000u + i, order[i], ready_mask);
CHECK((result == GPS_NAV_EPOCH_EVENT_ACCEPTED) ||
(result == GPS_NAV_EPOCH_EVENT_COMPLETE),
"random coherent capture was rejected");
}
CHECK(completed &&
gps_nav_epoch_assembly_complete(&assembly),
"hardware stream did not complete a live epoch");
CHECK(assembly.received_mask == assembly.expected_mask,
"hardware stream completed with a missing interrupt");
completed_epochs++;
CHECK(gps_nav_epoch_assembly_usable(&assembly, 4u),
"random stream lost four-channel liveness");
if(delivered_count != ready_count)
{
CHECK(gps_nav_epoch_assembly_expired(
&assembly, assembly.deadline_local_ticks),
"partial random epoch did not expire");
}
base += CAPTURE_PERIOD_TICKS;
}
CHECK(completed_epochs == 2000u,
"hardware stream produced more or fewer than one completion per epoch");
CHECK(dropped_epochs == 2u,
"hardware stream did not exercise both unlock positions");
CHECK(joined_epochs == 2u,
"hardware stream did not exercise both reconnect positions");
}
static void all_coherent_across_sample_wrap(void)
@@ -416,19 +243,17 @@ static void all_coherent_across_sample_wrap(void)
static void tow_outlier_does_not_block_good_cluster(void)
{
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
{2300, 100.0000, 1000000u},
{2300, 100.0001, 1010000u},
{2300, 99.9999, 1020000u},
{2300, 100.0002, 1030000u},
{2300, 100.0000, 1040000u},
{2300, 100.0001, 1050000u},
{2300, 100.2000, 1060000u}
{380, 469367.4000, 2982422271u},
{380, 469367.4001, 2982428475u},
{381, 28831.6000, 2982490274u},
{380, 469367.3999, 2982550441u},
{380, 469367.4002, 2982661405u}
};
gps_nav_epoch_selection_t selection = select_epoch(candidates, 7u);
CHECK(selection.selected_count == 6u,
"single TOW outlier blocked the six-satellite cluster");
CHECK(selection.selected_mask == 0x3fu,
"TOW outlier was not excluded");
gps_nav_epoch_selection_t selection = select_epoch(candidates, 5u);
CHECK(selection.selected_count == 4u,
"live 45.6-hour TOW outlier blocked the good cluster");
CHECK(selection.selected_mask == 0x1bu,
"live TOW outlier was not excluded");
}
static void newest_sample_outlier_does_not_define_the_cluster(void)
@@ -486,14 +311,11 @@ static void nonfinite_candidate_is_excluded(void)
int main(void)
{
epoch_id_rounding_and_week_wrap();
assembly_waits_for_every_ready_channel();
busy_unlocked_channel_does_not_block_epoch();
assembly_handles_duplicates_and_epoch_order();
assembly_disconnect_and_reconnect();
channel_join_does_not_expand_active_epoch();
randomized_interrupt_and_unlock_model();
multi_epoch_hardware_interrupt_model();
local_capture_assembly_completes();
missing_channel_expires_as_usable_subset();
local_outlier_cannot_poison_epoch();
duplicate_join_and_disconnect_are_bounded();
randomized_capture_liveness();
all_coherent_across_sample_wrap();
tow_outlier_does_not_block_good_cluster();
newest_sample_outlier_does_not_define_the_cluster();