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;
 }