Fixes shared memory locking bug and eliminates the need for an AppsProm driver to reserve a shared memory region.

src/modules/muorb/adsp/px4muorb.cpp

@@ -42,9 +42,15 @@
 #include "px4_log.h"
 #include "uORB/topics/sensor_combined.h"
 #include "uORB.h"
+#include "systemlib/param/param.h"
+#include <shmem.h>
 
 __BEGIN_DECLS
 extern int dspal_main(int argc, char *argv[]);
+extern struct shmem_info *shmem_info_p;
+extern int get_shmem_lock(const char *caller_file_name, int caller_line_number);
+extern void release_shmem_lock(const char *caller_file_name, int caller_line_number);
+extern void init_shared_memory(void);
 __END_DECLS
 
 int px4muorb_orb_initialize()
@@ -76,6 +82,78 @@ int px4muorb_get_absolute_time(uint64_t *time_us)
 	return 0;
 }
 
+/*update value and param's change bit in shared memory*/
+int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes)
+{
+	unsigned int byte_changed, bit_changed;
+	union param_value_u *param_value = (union param_value_u *)value;
+
+	if(!shmem_info_p) {
+		init_shared_memory();
+	}
+
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
+		PX4_INFO("Could not get shmem lock\n");
+		return -1;
+	}
+
+	shmem_info_p->params_val[param] = *param_value;
+
+	byte_changed = param / 8;
+	bit_changed = 1 << param % 8;
+	shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
+
+	release_shmem_lock(__FILE__, __LINE__);
+
+	return 0;
+}
+
+int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes)
+{
+	unsigned int i;
+
+	if(!shmem_info_p)
+		return -1;
+
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
+		PX4_INFO("Could not get shmem lock\n");
+		return -1;
+	}
+
+	for (i = 0; i < data_len_in_bytes; i++) {
+		data[i] = shmem_info_p->adsp_changed_index[i];
+	}
+
+	release_shmem_lock(__FILE__, __LINE__);
+
+	return 0;
+}
+
+int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes)
+{
+	unsigned int byte_changed, bit_changed;
+	union param_value_u *param_value = (union param_value_u *)value;
+
+	if(!shmem_info_p)
+		return -1;
+
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
+		PX4_INFO("Could not get shmem lock\n");
+		return -1;
+	}
+
+	*param_value = shmem_info_p->params_val[param];
+
+	/*also clear the index since we are holding the lock*/
+	byte_changed = param / 8;
+	bit_changed = 1 << param % 8;
+	shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed;
+
+	release_shmem_lock(__FILE__, __LINE__);
+
+	return 0;
+}
+
 int px4muorb_add_subscriber(const char *name)
 {
 	int rc = 0;

src/modules/muorb/adsp/px4muorb.hpp

@@ -43,6 +43,12 @@ extern "C" {
 
 	int px4muorb_get_absolute_time(uint64_t *time_us) __EXPORT;
 
+	int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT;
+
+	int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) __EXPORT;
+
+	int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT;
+
 	int px4muorb_add_subscriber(const char *name) __EXPORT;
 
 	int px4muorb_remove_subscriber(const char *name) __EXPORT;

src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp

@@ -38,6 +38,7 @@
 #include <rpcmem.h>
 #include "px4muorb.h"
 #include "px4_log.h"
+#include <shmem.h>
 
 using namespace px4muorb;
 
@@ -63,6 +64,8 @@ static const uint32_t _MAX_TOPICS = 64;
 static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS;
 static uint8_t *_BulkTransferBuffer = 0;
 
+unsigned char *adsp_changed_index = 0;
+
 // The DSP timer can be read from this file.
 #define DSP_TIMER_FILE "/sys/kernel/boot_adsp/qdsp_qtimer"
 
@@ -195,6 +198,17 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 		PX4_DEBUG("%s rpcmem_alloc passed for data_buffer", __FUNCTION__);
 	}
 
+	adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
+					       MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t));
+
+	rc = (adsp_changed_index != NULL) ? true : false;
+
+	if (!rc) {
+		PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__);
+	} else {
+		memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t));
+	}
+
 	int32_t time_diff_us;
 
 	if (calc_timer_diff_to_dsp_us(&time_diff_us) != 0) {
@@ -242,6 +256,11 @@ bool px4muorb::KraitRpcWrapper::Terminate()
 		_DataBuffer = 0;
 	}
 
+	if(adsp_changed_index != NULL) {
+		rpcmem_free(adsp_changed_index);
+		adsp_changed_index = 0;
+	}
+
 	_Initialized = false;
 	return true;
 }

src/modules/systemlib/param/param_shmem.c

@@ -109,7 +109,7 @@ const int bits_per_allocation_unit  = (sizeof(*param_changed_storage) * 8);
 //#define ENABLE_SHMEM_DEBUG
 
 extern int get_shmem_lock(const char *caller_file_name, int caller_line_number);
-extern void release_shmem_lock(void);
+extern void release_shmem_lock(const char *caller_file_name, int caller_line_number);
 
 struct param_wbuf_s *param_find_changed(param_t param);
 
@@ -828,18 +828,9 @@ param_save_default(void)
 {
 	int res = OK;
 	int fd = -1;
-	bool is_locked = false;
 
 	const char *filename = param_get_default_file();
 
-	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
-		PX4_ERR("Could not get shmem lock");
-		res = ERROR;
-		goto exit;
-	}
-
-	is_locked = true;
-
 	fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
 
 	if (fd < 0) {
@@ -869,10 +860,6 @@ param_save_default(void)
 
 exit:
 
-	if (is_locked) {
-		release_shmem_lock();
-	}
-
 	if (fd >= 0) {
 		close(fd);
 	}
@@ -923,7 +910,9 @@ param_load_default_no_notify(void)
 	int fd_load = open(param_get_default_file(), O_RDONLY);
 
 	if (fd_load < 0) {
-		release_shmem_lock();
+#ifdef __PX4_QURT
+		release_shmem_lock(__FILE__, __LINE__);
+#endif
 
 		/* no parameter file is OK, otherwise this is an error */
 		if (errno != ENOENT) {
@@ -1241,8 +1230,10 @@ uint32_t param_hash_check(void)
 
 void init_params(void)
 {
+#ifdef __PX4_QURT
 	//copy params to shared memory
 	init_shared_memory();
+#endif
 
 	/*load params automatically*/
 #ifdef __PX4_POSIX
@@ -1250,6 +1241,7 @@ void init_params(void)
 #endif
 	param_import_done = 1;
 
+#ifdef __PX4_QURT
 	copy_params_to_shmem(param_info_base);
 
 
@@ -1260,6 +1252,6 @@ void init_params(void)
 		 (unsigned char *)&shmem_info_p->krait_changed_index - (unsigned char *)shmem_info_p,
 		 (unsigned char *)&shmem_info_p->adsp_changed_index - (unsigned char *)shmem_info_p);
 #endif
-
+#endif
 }