NuttX disable priority inheritance and adjust parameter save scheduling

src/lib/parameters/parameters.cpp

@@ -591,15 +591,12 @@ autosave_worker(void *arg)
 {
 	bool disabled = false;
 
-	if (!param_get_default_file()) {
-		// In case we save to FLASH, defer param writes until disarmed,
-		// as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7)
-		uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
+	// defer param writes until disarmed
+	uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
 
-		if (armed_sub.get().armed) {
-			work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(1_s));
-			return;
-		}
+	if (armed_sub.get().armed) {
+		work_queue(HPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(1_s));
+		return;
 	}
 
 	param_lock_writer();
@@ -647,7 +644,7 @@ param_autosave()
 	}
 
 	autosave_scheduled.store(true);
-	work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay));
+	work_queue(HPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay));
 }
 
 void
@@ -656,7 +653,7 @@ param_control_autosave(bool enable)
 	param_lock_writer();
 
 	if (!enable && autosave_scheduled.load()) {
-		work_cancel(LPWORK, &autosave_work);
+		work_cancel(HPWORK, &autosave_work);
 		autosave_scheduled.store(false);
 	}
 

src/lib/perf/perf_counter.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,6 @@
  * @brief Performance measuring tools.
  */
 
-#include <inttypes.h>
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
@@ -48,6 +47,8 @@
 
 #include "perf_counter.h"
 
+#include <px4_platform_common/sem.hpp>
+
 /**
  * Header common to all counters.
  */
@@ -97,20 +98,25 @@ struct perf_ctr_interval : public perf_ctr_header {
 static sq_queue_t	perf_counters = { nullptr, nullptr };
 
 /**
- * mutex protecting access to the perf_counters linked list (which is read from & written to by different threads)
+ * semaphore protecting access to the perf_counters linked list (which is read from & written to by different threads)
  */
-pthread_mutex_t perf_counters_mutex = PTHREAD_MUTEX_INITIALIZER;
-// FIXME: the mutex does **not** protect against access to/from the perf
+static px4_sem_t perf_counters_lock;
+
+// FIXME: the semaphore does **not** protect against access to/from the perf
 // counter's data. It can still happen that a counter is updated while it is
 // printed. This can lead to inconsistent output, or completely bogus values
 // (especially the 64bit values which are in general not atomically updated).
-// The same holds for shared perf counters (perf_alloc_once), that can be updated
-// concurrently (this affects the 'ctrl_latency' counter).
 
+void perf_init()
+{
+	px4_sem_init(&perf_counters_lock, 0, 1);
+}
 
 perf_counter_t
 perf_alloc(enum perf_counter_type type, const char *name)
 {
+	SmartLock smart_lock(perf_counters_lock);
+
 	perf_counter_t ctr = nullptr;
 
 	switch (type) {
@@ -133,43 +139,12 @@ perf_alloc(enum perf_counter_type type, const char *name)
 	if (ctr != nullptr) {
 		ctr->type = type;
 		ctr->name = name;
-		pthread_mutex_lock(&perf_counters_mutex);
 		sq_addfirst(&ctr->link, &perf_counters);
-		pthread_mutex_unlock(&perf_counters_mutex);
 	}
 
 	return ctr;
 }
 
-perf_counter_t
-perf_alloc_once(enum perf_counter_type type, const char *name)
-{
-	pthread_mutex_lock(&perf_counters_mutex);
-	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
-
-	while (handle != nullptr) {
-		if (!strcmp(handle->name, name)) {
-			if (type == handle->type) {
-				/* they are the same counter */
-				pthread_mutex_unlock(&perf_counters_mutex);
-				return handle;
-
-			} else {
-				/* same name but different type, assuming this is an error and not intended */
-				pthread_mutex_unlock(&perf_counters_mutex);
-				return nullptr;
-			}
-		}
-
-		handle = (perf_counter_t)sq_next(&handle->link);
-	}
-
-	pthread_mutex_unlock(&perf_counters_mutex);
-
-	/* if the execution reaches here, no existing counter of that name was found */
-	return perf_alloc(type, name);
-}
-
 void
 perf_free(perf_counter_t handle)
 {
@@ -177,9 +152,8 @@ perf_free(perf_counter_t handle)
 		return;
 	}
 
-	pthread_mutex_lock(&perf_counters_mutex);
+	SmartLock smart_lock(perf_counters_lock);
 	sq_rem(&handle->link, &perf_counters);
-	pthread_mutex_unlock(&perf_counters_mutex);
 
 	switch (handle->type) {
 	case PC_COUNT:
@@ -586,29 +560,25 @@ perf_mean(perf_counter_t handle)
 void
 perf_iterate_all(perf_callback cb, void *user)
 {
-	pthread_mutex_lock(&perf_counters_mutex);
+	SmartLock smart_lock(perf_counters_lock);
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
 
 	while (handle != nullptr) {
 		cb(handle, user);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
-
-	pthread_mutex_unlock(&perf_counters_mutex);
 }
 
 void
 perf_print_all(void)
 {
-	pthread_mutex_lock(&perf_counters_mutex);
+	SmartLock smart_lock(perf_counters_lock);
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
 
 	while (handle != nullptr) {
 		perf_print_counter(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
-
-	pthread_mutex_unlock(&perf_counters_mutex);
 }
 
 void
@@ -630,7 +600,7 @@ perf_print_latency(void)
 void
 perf_reset_all(void)
 {
-	pthread_mutex_lock(&perf_counters_mutex);
+	SmartLock smart_lock(perf_counters_lock);
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
 
 	while (handle != nullptr) {
@@ -638,7 +608,5 @@ perf_reset_all(void)
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
 
-	pthread_mutex_unlock(&perf_counters_mutex);
-
 	reset_latency_counters();
 }

src/lib/perf/perf_counter.h

@@ -69,14 +69,9 @@ __EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const cha
 #endif
 
 /**
- * Get the reference to an existing counter or create a new one if it does not exist.
- *
- * @param type			The type of the counter.
- * @param name			The counter name.
- * @return			Handle for the counter, or NULL if a counter
- *				could not be allocated.
+ * Initialize the perf backend. Call this on startup before calling any other methods.
  */
-__EXPORT extern perf_counter_t	perf_alloc_once(enum perf_counter_type type, const char *name);
+__EXPORT void			perf_init(void);
 
 /**
  * Free a counter.

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -275,6 +275,7 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
 		uORB::Manager::get_instance()->set_uorb_communicator(
 			uORB::ProtobufChannel::GetInstance());
 
+		perf_init();
 		param_init();
 
 		px4::WorkQueueManagerStart();