onur
516016efb9
Wait for every navigation-ready tracking channel in a GPS epoch, remove unlocked channels without a timeout, and keep late joins for the next epoch. Reset PLIC ownership during warm initialization and extend Linux synthetic, integrity, and fake-MMIO regression coverage.
476 lines
15 KiB
C
476 lines
15 KiB
C
#include "gps/gps_nav_epoch.h"
|
|
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#define SAMPLE_WINDOW 409200u
|
|
#define TOW_TOLERANCE 0.001
|
|
|
|
static int failures;
|
|
|
|
#define CHECK(condition, message) do { \
|
|
if(!(condition)) { \
|
|
fprintf(stderr, "FAIL: %s\n", message); \
|
|
failures++; \
|
|
} \
|
|
} while(0)
|
|
|
|
static gps_nav_epoch_selection_t select_epoch(
|
|
const gps_nav_epoch_key_t* candidates,
|
|
unsigned int count)
|
|
{
|
|
gps_nav_epoch_selection_t selection;
|
|
memset(&selection, 0, sizeof(selection));
|
|
CHECK(gps_nav_epoch_select(candidates, count, SAMPLE_WINDOW,
|
|
TOW_TOLERANCE, &selection),
|
|
"epoch selection call failed");
|
|
return selection;
|
|
}
|
|
|
|
static unsigned int mask_count(uint32_t 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 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_FUTURE,
|
|
"future epoch was mixed into an incomplete epoch");
|
|
CHECK(assembly.received_mask == 0x01u,
|
|
"rejected interrupts changed the received mask");
|
|
}
|
|
|
|
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");
|
|
}
|
|
}
|
|
|
|
static uint32_t deterministic_random(uint32_t* state)
|
|
{
|
|
uint32_t value = *state;
|
|
value ^= value << 13;
|
|
value ^= value >> 17;
|
|
value ^= value << 5;
|
|
*state = value;
|
|
return value;
|
|
}
|
|
|
|
static void randomized_interrupt_and_unlock_model(void)
|
|
{
|
|
uint32_t random_state = 0x6a09e667u;
|
|
for(unsigned int trial = 0u ; trial < 10000u ; trial++)
|
|
{
|
|
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;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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");
|
|
}
|
|
}
|
|
|
|
static void multi_epoch_hardware_interrupt_model(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++)
|
|
{
|
|
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;
|
|
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++)
|
|
{
|
|
if((epoch_ready_mask & (1u << channel)) != 0u)
|
|
{
|
|
order[order_count++] = channel;
|
|
}
|
|
}
|
|
for(unsigned int i = order_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_assembly_reset(&assembly);
|
|
bool completed = false;
|
|
for(unsigned int event_index = 0u ;
|
|
event_index < order_count ; event_index++)
|
|
{
|
|
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;
|
|
}
|
|
|
|
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");
|
|
}
|
|
}
|
|
|
|
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(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)
|
|
{
|
|
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
|
{2300, 604799.9998, UINT32_MAX - 100u},
|
|
{2301, 0.0001, 50u},
|
|
{2301, 0.0000, 1200u},
|
|
{2300, 604799.9999, UINT32_MAX - 2000u}
|
|
};
|
|
gps_nav_epoch_selection_t selection = select_epoch(candidates, 4u);
|
|
CHECK(selection.selected_count == 4u,
|
|
"sample/week wrap split a coherent epoch");
|
|
CHECK(selection.selected_mask == 0x0fu,
|
|
"sample/week wrap selected the wrong rows");
|
|
}
|
|
|
|
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}
|
|
};
|
|
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");
|
|
}
|
|
|
|
static void newest_sample_outlier_does_not_define_the_cluster(void)
|
|
{
|
|
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
|
{2300, 200.0000, 2000000u},
|
|
{2300, 200.0000, 2010000u},
|
|
{2300, 200.0000, 2020000u},
|
|
{2300, 200.0000, 2030000u},
|
|
{2300, 200.0000, 2040000u},
|
|
{2300, 200.0000, 2050000u},
|
|
{2300, 200.0000, 3000000u}
|
|
};
|
|
gps_nav_epoch_selection_t selection = select_epoch(candidates, 7u);
|
|
CHECK(selection.selected_count == 6u,
|
|
"newest raw-sample outlier displaced the largest cluster");
|
|
CHECK(selection.selected_mask == 0x3fu,
|
|
"newest raw-sample outlier was not excluded");
|
|
}
|
|
|
|
static void equal_clusters_prefer_newer_epoch(void)
|
|
{
|
|
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
|
{2300, 300.0000, 1000000u},
|
|
{2300, 300.0000, 1010000u},
|
|
{2300, 300.0000, 1020000u},
|
|
{2300, 300.0000, 1030000u},
|
|
{2300, 300.2000, 5000000u},
|
|
{2300, 300.2000, 5010000u},
|
|
{2300, 300.2000, 5020000u},
|
|
{2300, 300.2000, 5030000u}
|
|
};
|
|
gps_nav_epoch_selection_t selection = select_epoch(candidates, 8u);
|
|
CHECK(selection.selected_count == 4u,
|
|
"equal epoch clusters returned the wrong size");
|
|
CHECK(selection.selected_mask == 0xf0u,
|
|
"equal epoch clusters did not prefer the newer epoch");
|
|
}
|
|
|
|
static void nonfinite_candidate_is_excluded(void)
|
|
{
|
|
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
|
{2300, 400.0000, 1000000u},
|
|
{2300, 400.0000, 1010000u},
|
|
{2300, 400.0000, 1020000u},
|
|
{2300, 400.0000, 1030000u},
|
|
{2300, NAN, 1040000u}
|
|
};
|
|
gps_nav_epoch_selection_t selection = select_epoch(candidates, 5u);
|
|
CHECK(selection.selected_count == 4u,
|
|
"invalid TOW candidate blocked valid measurements");
|
|
CHECK(selection.selected_mask == 0x0fu,
|
|
"invalid TOW candidate was selected");
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
epoch_id_rounding_and_week_wrap();
|
|
assembly_waits_for_every_ready_channel();
|
|
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();
|
|
all_coherent_across_sample_wrap();
|
|
tow_outlier_does_not_block_good_cluster();
|
|
newest_sample_outlier_does_not_define_the_cluster();
|
|
equal_clusters_prefer_newer_epoch();
|
|
nonfinite_candidate_is_excluded();
|
|
|
|
if(failures == 0)
|
|
{
|
|
puts("gps_nav_epoch tests passed");
|
|
}
|
|
return failures == 0 ? 0 : 1;
|
|
}
|