fix(gps): recover when tracking loss IRQ is missed

soc/software/gps_system/src/gps/gps_interrupt_handlers.c

@@ -48,6 +48,7 @@ typedef struct {
 } gps_tracking_shadow_t;
 
 static volatile bool tracking_allocated[GPS_TRCK_IP_COUNT];
+static unsigned char tracking_not_busy_polls[GPS_TRCK_IP_COUNT];
 static volatile gps_acq_engine_state_t acq_engine_state[GPS_ACQ_IP_COUNT];
 static volatile unsigned char acq_engine_prn[GPS_ACQ_IP_COUNT];
 static volatile uint64_t acq_engine_start_tick[GPS_ACQ_IP_COUNT];
@@ -81,6 +82,7 @@ static unsigned int gps_external_irq_id = 0u;
 static gps_external_irq_handler_t gps_external_irq_handler = NULL;
 
 #define GPS_PLIC_DRAIN_LIMIT (GPS_TRCK_IP_COUNT + GPS_ACQ_IP_COUNT + 4u)
+#define GPS_TRK_NOT_BUSY_CONFIRM_POLLS 2u
 #define GPS_NAV_CAPTURE_PERIOD_TICKS \
 	(((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u)
 #define GPS_NAV_CAPTURE_WINDOW_TICKS \
@@ -385,6 +387,7 @@ static void gps_runtime_state_init(void)
 	for(unsigned int i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
 	{
 		tracking_allocated[i] = false;
+		tracking_not_busy_polls[i] = 0u;
 		memset(&trk_shadow[i], 0, sizeof(trk_shadow[i]));
 		gps_clear_tracking_slot((unsigned char)i);
 	}
@@ -682,6 +685,7 @@ bool assign_tracking_from_result(const gps_acq_result_t* result)
 		   !trck_ip_exists(&tracking_node, i))
 		{
 			tracking_allocated[i] = true;
+			tracking_not_busy_polls[i] = 0u;
 			gps_clear_tracking_slot(i);
 			gps_ephemeris_t cached_ephemeris;
 			if(gps_ephemeris_cache_restore(
@@ -1030,6 +1034,7 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
 	}
 
 	tracking_allocated[trck_ip_no] = false;
+	tracking_not_busy_polls[trck_ip_no] = 0u;
 	if(trck_ip_exists(&tracking_node, trck_ip_no) &&
 	   !delete_by_ip_number(&tracking_node, trck_ip_no))
 	{
@@ -1731,6 +1736,73 @@ static void gps_mark_stale_measurements(void)
 	}
 }
 
+static unsigned int gps_prn_for_tracking_channel(
+	unsigned char trck_ip_no,
+	volatile gps_ephem_ll_t* node)
+{
+	if((node != NULL) && (node->data_p != NULL) &&
+	   gps_prn_valid(node->data_p->SVID))
+	{
+		return node->data_p->SVID;
+	}
+
+	unsigned int hardware_prn = trck_ips[trck_ip_no]->PRN_NUMBER;
+	if(gps_prn_valid(hardware_prn))
+	{
+		return hardware_prn;
+	}
+
+	for(unsigned int prn = 0u ; prn < GPS_PRN_COUNT ; prn++)
+	{
+		if(satellite_tracking_ip[prn] == trck_ip_no)
+		{
+			return prn;
+		}
+	}
+	return GPS_PRN_INVALID;
+}
+
+static void gps_reconcile_stopped_tracking_channels(void)
+{
+	for(unsigned char i = 0u ; i < GPS_TRCK_IP_COUNT ; i++)
+	{
+		volatile gps_ephem_ll_t* node =
+			get_element_by_ip_number(&tracking_node, i);
+		bool allocated = tracking_allocated[i] || (node != NULL);
+		uint32_t status = trck_ips[i]->STATUS_REG;
+		if(!allocated ||
+		   ((status & (1u << GPS_TRCK_BUSY_POS)) != 0u))
+		{
+			tracking_not_busy_polls[i] = 0u;
+			continue;
+		}
+
+		if(tracking_not_busy_polls[i] <
+		   GPS_TRK_NOT_BUSY_CONFIRM_POLLS)
+		{
+			tracking_not_busy_polls[i]++;
+		}
+		if(tracking_not_busy_polls[i] <
+		   GPS_TRK_NOT_BUSY_CONFIRM_POLLS)
+		{
+			continue;
+		}
+
+		gps_isr_event_t stopped_event;
+		memset(&stopped_event, 0, sizeof(stopped_event));
+		stopped_event.type = GPS_ISR_EVENT_TRK_STATUS;
+		stopped_event.index = i;
+		stopped_event.status = status;
+		stopped_event.control = trck_ips[i]->CONTROL_REG;
+		stopped_event.prn = gps_prn_for_tracking_channel(i, node);
+		stopped_event.tick = gps_get_systick();
+		GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK,
+		        "tracking stopped without loss IRQ: trk=%u prn=%u status=0x%x\r\n",
+		        i, stopped_event.prn, status);
+		gps_process_tracking_drop(&stopped_event);
+	}
+}
+
 void gps_background_task(void)
 {
 	gps_poll_acq_pending_completions();
@@ -1738,6 +1810,7 @@ void gps_background_task(void)
 	gps_process_isr_events();
 	if(gps_take_timer_maintenance_pending())
 	{
+		gps_reconcile_stopped_tracking_channels();
 		gps_mark_stale_measurements();
 		gps_check_acq_stuck_busy();
 		gps_log_acq_status();

soc/software/gps_system/tests/test_gps_receiver_hardware_sim.c

@@ -15,13 +15,17 @@ typedef struct {
 	gps_ephemeris_cache_t cache;
 	bool visible[GPS_ACQ_MANAGER_PRN_COUNT];
 	bool tracked[GPS_ACQ_MANAGER_PRN_COUNT];
+	bool hardware_busy[GPS_ACQ_MANAGER_PRN_COUNT];
 	uint64_t invisible_since[GPS_ACQ_MANAGER_PRN_COUNT];
+	unsigned int not_busy_polls[GPS_ACQ_MANAGER_PRN_COUNT];
 	bool engine_busy;
 	bool drop_completion_irq;
+	bool drop_loss_irq;
 	unsigned int engine_prn;
 	uint64_t engine_start;
 	uint64_t tick;
 	unsigned int polled_completions;
+	unsigned int polled_losses;
 	unsigned int cache_restores;
 } receiver_sim_t;
 
@@ -100,6 +104,8 @@ static void sim_complete_engine(receiver_sim_t* sim)
 		gps_acq_manager_mark_acquired(&sim->manager, prn);
 		gps_acq_manager_mark_tracking(&sim->manager, prn);
 		sim->tracked[prn] = true;
+		sim->hardware_busy[prn] = true;
+		sim->not_busy_polls[prn] = 0u;
 		sim->invisible_since[prn] = 0u;
 		gps_ephemeris_t restored;
 		if(gps_ephemeris_cache_restore(
@@ -119,6 +125,14 @@ static void sim_complete_engine(receiver_sim_t* sim)
 	}
 }
 
+static void sim_mark_tracking_lost(receiver_sim_t* sim, unsigned int prn)
+{
+	sim->tracked[prn] = false;
+	sim->not_busy_polls[prn] = 0u;
+	gps_acq_manager_mark_lost(
+		&sim->manager, prn, sim->tick);
+}
+
 static void sim_update_tracking(receiver_sim_t* sim)
 {
 	for(unsigned int prn = 0u;
@@ -140,9 +154,25 @@ static void sim_update_tracking(receiver_sim_t* sim)
 		else if((sim->tick - sim->invisible_since[prn]) >=
 		        SIM_UNLOCK_TICKS)
 		{
-			sim->tracked[prn] = false;
+			sim->hardware_busy[prn] = false;
-			gps_acq_manager_mark_lost(
+			if(!sim->drop_loss_irq)
-				&sim->manager, prn, sim->tick);
+			{
+				sim_mark_tracking_lost(sim, prn);
+			}
+		}
+
+		if(!sim->tracked[prn])
+		{
+			continue;
+		}
+		if(sim->hardware_busy[prn])
+		{
+			sim->not_busy_polls[prn] = 0u;
+		}
+		else if(++sim->not_busy_polls[prn] >= 2u)
+		{
+			sim->polled_losses++;
+			sim_mark_tracking_lost(sim, prn);
 		}
 	}
 }
@@ -307,6 +337,36 @@ static void test_missed_completion_interrupt_is_polled(void)
 	       GPS_ACQ_PRN_TRACKING);
 }
 
+static void test_missed_tracking_loss_interrupt_is_reconciled(void)
+{
+	receiver_sim_t sim;
+	sim_init(&sim);
+	sim.visible[0] = true;
+	sim_start_engine(&sim);
+	sim_run(&sim, SIM_SEARCH_TICKS);
+	assert(sim.tracked[0]);
+
+	sim.drop_loss_irq = true;
+	sim.visible[0] = false;
+	sim_run(&sim, SIM_UNLOCK_TICKS + 2u);
+	assert(!sim.tracked[0]);
+	assert(sim.polled_losses == 1u);
+	assert(gps_acq_manager_state(&sim.manager, 0u) ==
+	       GPS_ACQ_PRN_HOT);
+
+	sim.visible[0] = true;
+	sim.drop_loss_irq = false;
+	unsigned int return_tick = (unsigned int)sim.tick;
+	while(!sim.tracked[0] &&
+	      ((sim.tick - return_tick) <= (2u * SIM_SEARCH_TICKS)))
+	{
+		sim_step(&sim);
+	}
+	assert(sim.tracked[0]);
+	assert((sim.tick - return_tick) <= (2u * SIM_SEARCH_TICKS));
+	assert(sim.cache_restores == 1u);
+}
+
 int main(void)
 {
 	test_cold_boot_and_short_signal_gap();
@@ -314,6 +374,7 @@ int main(void)
 	test_long_outage_hot_reacquisition_and_cached_nav();
 	test_hot_failures_do_not_starve_cold_search();
 	test_missed_completion_interrupt_is_polled();
+	test_missed_tracking_loss_interrupt_is_reconciled();
 	puts("gps receiver hardware-simulation scenarios passed");
 	return 0;
 }