hrt: Add interface functions for latency counters

Add interface functions for fetching latency buckets and counters and use
those in perf_counter.cpp. This cleans up the usage of perf counters, when variables defined in hrt_drv are not referenced directly from perf.

This also enables implementing kernel-userspace interface for those for
nuttx protected/kernel build.

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c

@@ -988,4 +988,21 @@ hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
 	entry->deadline = hrt_absolute_time() + delay;
 }
 
+#if !defined(CONFIG_BUILD_FLAT)
+/* These functions are inlined in all but NuttX protected/kernel builds */
+
+latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx)
+{
+	latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]};
+	return ret;
+}
+
+void reset_latency_counters(void)
+{
+	for (int i = 0; i <= get_latency_bucket_count(); i++) {
+		latency_counters[i] = 0;
+	}
+}
+#endif
+
 #endif /* HRT_TIMER */

src/drivers/drv_hrt.h

@@ -84,6 +84,11 @@ extern const uint16_t latency_bucket_count;
 extern const uint16_t latency_buckets[LATENCY_BUCKET_COUNT];
 extern uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
 
+typedef struct latency_info {
+	uint16_t                bucket;
+	uint32_t                counter;
+} latency_info_t;
+
 /**
  * Get absolute time in [us] (does not wrap).
  */
@@ -199,8 +204,36 @@ static inline void px4_lockstep_progress(int component) { }
 static inline void px4_lockstep_wait_for_components(void) { }
 #endif /* defined(ENABLE_LOCKSTEP_SCHEDULER) */
 
-__END_DECLS
 
+/* Latency counter functions */
+
+static inline uint16_t get_latency_bucket_count(void) { return LATENCY_BUCKET_COUNT; }
+
+#if defined(CONFIG_BUILD_FLAT) || !defined(__PX4_NUTTX)
+
+static inline latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx)
+{
+	latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]};
+	return ret;
+}
+
+static inline void reset_latency_counters(void)
+{
+	for (int i = 0; i <= get_latency_bucket_count(); i++) {
+		latency_counters[i] = 0;
+	}
+}
+
+#else
+
+/* NuttX protected/kernel build interface functions */
+
+latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx);
+void reset_latency_counters(void);
+
+#endif
+
+__END_DECLS
 
 
 #ifdef	__cplusplus

src/lib/perf/perf_counter.cpp

@@ -610,15 +610,17 @@ perf_print_all(int fd)
 void
 perf_print_latency(int fd)
 {
+	latency_info_t latency;
 	dprintf(fd, "bucket [us] : events\n");
 
-	for (int i = 0; i < latency_bucket_count; i++) {
-		dprintf(fd, "       %4i : %li\n", latency_buckets[i], (long int)latency_counters[i]);
+	for (int i = 0; i < get_latency_bucket_count(); i++) {
+		latency = get_latency(i, i);
+		dprintf(fd, "       %4i : %li\n", latency.bucket, (long int)latency.counter);
 	}
 
 	// print the overflow bucket value
-	dprintf(fd, " >%4" PRIu16 " : %" PRIu32 "\n", latency_buckets[latency_bucket_count - 1],
-		latency_counters[latency_bucket_count]);
+	latency = get_latency(get_latency_bucket_count() - 1, get_latency_bucket_count());
+	dprintf(fd, " >%4" PRIu16 " : %" PRIu32 "\n", latency.bucket, latency.counter);
 }
 
 void
@@ -634,7 +636,5 @@ perf_reset_all(void)
 
 	pthread_mutex_unlock(&perf_counters_mutex);
 
-	for (int i = 0; i <= latency_bucket_count; i++) {
-		latency_counters[i] = 0;
-	}
+	reset_latency_counters();
 }