diff --git a/src/firmware/qurt/px4muorb.idl b/src/firmware/qurt/px4muorb.idl index 49e1305ffd..e87492fb3e 100644 --- a/src/firmware/qurt/px4muorb.idl +++ b/src/firmware/qurt/px4muorb.idl @@ -49,6 +49,28 @@ interface px4muorb{ * @param time_us: pointer to time in us */ AEEResult get_absolute_time(rout unsigned long long time_us); + + /** + * Interface to update param for krait. + * + * @param param: param index. + * @param value: param value. + */ + AEEResult param_update_to_shmem( in unsigned long param, in sequence value); + + /** + * Interface to update index for krait. + * @param data: param index array. + */ + AEEResult param_update_index_from_shmem( rout sequence data); + + /** + * Interface to get param value for krait. + * + * @param param: param index. + * @param value: param value. + */ + AEEResult param_update_value_from_shmem( in unsigned long param, rout sequence value); /** * Interface to add a subscriber to the identified topic diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp index efe8fde2d0..8ce0a50d36 100644 --- a/src/modules/muorb/adsp/px4muorb.cpp +++ b/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 __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; diff --git a/src/modules/muorb/adsp/px4muorb.hpp b/src/modules/muorb/adsp/px4muorb.hpp index 56154becfa..36d5f98ca5 100644 --- a/src/modules/muorb/adsp/px4muorb.hpp +++ b/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; diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp index b9fb8511e8..71a80e6969 100644 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp +++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp @@ -38,6 +38,7 @@ #include #include "px4muorb.h" #include "px4_log.h" +#include 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; } diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c index 5ff934b043..ea64b7e6ae 100644 --- a/src/modules/systemlib/param/param_shmem.c +++ b/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 } diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt index d80b6083ad..0e98f0bcb3 100644 --- a/src/platforms/posix/px4_layer/CMakeLists.txt +++ b/src/platforms/posix/px4_layer/CMakeLists.txt @@ -31,6 +31,11 @@ # ############################################################################ +include(hexagon_sdk) + +include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix) +include_directories(${HEXAGON_SDK_INCLUDES}) + if("${CONFIG_SHMEM}" STREQUAL "1") list(APPEND SHMEM_SRCS shmem_posix.c diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c index c9ec801ca9..8fbaece56f 100644 --- a/src/platforms/posix/px4_layer/shmem_posix.c +++ b/src/platforms/posix/px4_layer/shmem_posix.c @@ -51,236 +51,43 @@ #include "systemlib/param/param.h" #include +#include "px4muorb.h" //#define SHMEM_DEBUG - -int mem_fd; -unsigned char *map_base, *virt_addr; -struct shmem_info *shmem_info_p; -static void *map_memory(off_t target); - -int get_shmem_lock(const char *caller_file_name, int caller_line_number); -void release_shmem_lock(void); -void init_shared_memory(void); -void copy_params_to_shmem(struct param_info_s *); void update_to_shmem(param_t param, union param_value_u value); int update_from_shmem(param_t param, union param_value_u *value); void update_index_from_shmem(void); uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; -static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; +extern unsigned char *adsp_changed_index; struct param_wbuf_s { param_t param; union param_value_u val; bool unsaved; }; -extern struct param_wbuf_s *param_find_changed(param_t param); -#define MEMDEVICE "/dev/mem" - -static void *map_memory(off_t target) -{ - - if ((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1) { - PX4_ERR("Cannot open %s", MEMDEVICE); - exit(1); - } - - /* Map one page */ - map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, - MAP_SHARED, mem_fd, target & ~MAP_MASK); - - if (map_base == (void *) - 1) { - PX4_ERR("Cannot mmap /dev/atl_mem"); - exit(1); - } - - PX4_DEBUG("Initializing map memory: mem_fd: %d, 0x%X", mem_fd, map_base + (target & MAP_MASK) + LOCK_SIZE); - - return (map_base + (target & MAP_MASK) + LOCK_SIZE); - -} - -int get_shmem_lock(const char *caller_file_name, int caller_line_number) -{ - // TODO FIXME: just say this went through - return 0; - - int i = 0; - - /* TODO: make this comment so somebody can understand it: ioctl calls cmpxchg */ - while (ioctl(mem_fd, LOCK_MEM) != 0) { - - usleep(10000); //sleep for 10 msec - i++; - - if (i > 10) { - PX4_ERR("Could not get lock, file name: %s, line number: %d, errno: %d", - caller_file_name, caller_line_number, errno); - return -1; - } - } - - return 0; //got the lock -} - -void release_shmem_lock(void) -{ - // TODO FIXME: just say this went through - return; - - int ret = ioctl(mem_fd, UNLOCK_MEM); - - if (ret != 0) { - PX4_INFO("unlock failed, ret: %d, errno %d", ret, errno); - } -} - -void init_shared_memory(void) -{ - - virt_addr = map_memory(MAP_ADDRESS); //16K space - shmem_info_p = (struct shmem_info *) virt_addr; - - PX4_DEBUG("linux memory mapped"); -} - -void copy_params_to_shmem(struct param_info_s *param_info_base) -{ - param_t param; - unsigned int i; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); - return; - } - - PX4_DEBUG("%d krait params allocated", param_count()); - - for (param = 0; param < param_count(); param++) { - struct param_wbuf_s *s = param_find_changed(param); - - if (s == NULL) { - shmem_info_p->params_val[param] = param_info_base[param].val; - - } else { - shmem_info_p->params_val[param] = s->val; - } - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - { - PX4_INFO("%d: written %d for param %s to shared mem", - param, shmem_info_p->params_val[param].i, param_name(param)); - } - - } else if (param_type(param) == PARAM_TYPE_FLOAT) { - { - PX4_INFO("%d: written %f for param %s to shared mem", - param, (double)shmem_info_p->params_val[param].f, param_name(param)); - } - } - -#endif - } - - PX4_DEBUG("written %u params to shmem", param_count()); - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - shmem_info_p->krait_changed_index[i] = 0; - adsp_changed_index[i] = 0; - } - - release_shmem_lock(); -} /*update value and param's change bit in shared memory*/ void update_to_shmem(param_t param, union param_value_u value) { - unsigned int byte_changed, bit_changed; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); - return; - } - - shmem_info_p->params_val[param] = value; - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->krait_changed_index[byte_changed] |= bit_changed; - - PX4_DEBUG("set %d bit on krait changed index[%d] to %d", bit_changed, byte_changed, - shmem_info_p->krait_changed_index[byte_changed]); - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d", - value.i, param_name(param), byte_changed, bit_changed); - - } else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d", - (double)value.f, param_name(param), byte_changed, bit_changed); - } - -#endif - - release_shmem_lock(); - + if(px4muorb_param_update_to_shmem(param, (unsigned char*)&value, sizeof(value))) + PX4_ERR("krait update param %u failed", param); } void update_index_from_shmem(void) { - unsigned int i; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); + if(!adsp_changed_index) { + PX4_ERR("%s no param buffer", __FUNCTION__); return; } - PX4_DEBUG("Updating index from shmem"); - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - adsp_changed_index[i] = shmem_info_p->adsp_changed_index[i]; - } - - release_shmem_lock(); + px4muorb_param_update_index_from_shmem(adsp_changed_index, PARAM_BUFFER_SIZE); } static void update_value_from_shmem(param_t param, union param_value_u *value) { - unsigned int byte_changed, bit_changed; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock"); - return; - } - - *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(); - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO( - "Got value %d for param %s from shmem, cleared adsp index %d:%d", - value->i, param_name(param), byte_changed, bit_changed); - - } else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO( - "Got value %f for param %s from shmem, cleared adsp index %d:%d", - (double)value->f, param_name(param), byte_changed, bit_changed); - } - -#endif + if(px4muorb_param_update_value_from_shmem(param, (unsigned char*)value, sizeof(union param_value_u))) + PX4_ERR("%s get param failed", __FUNCTION__); } int update_from_shmem(param_t param, union param_value_u *value) @@ -288,6 +95,11 @@ int update_from_shmem(param_t param, union param_value_u *value) unsigned int byte_changed, bit_changed; unsigned int retval = 0; + if(!adsp_changed_index) { + PX4_ERR("%s no param buffer", __FUNCTION__); + return 0; + } + update_from_shmem_current_time = hrt_absolute_time(); if ((update_from_shmem_current_time - update_from_shmem_prev_time) diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c index ca29ba77db..5aeabf520b 100644 --- a/src/platforms/qurt/px4_layer/shmem_qurt.c +++ b/src/platforms/qurt/px4_layer/shmem_qurt.c @@ -48,13 +48,14 @@ #include //#define SHMEM_DEBUG +//#define PARAM_LOCK_DEBUG int mem_fd; unsigned char *map_base, *virt_addr; struct shmem_info *shmem_info_p; static void *map_memory(off_t target); int get_shmem_lock(const char *caller_file_name, int caller_line_number); -void release_shmem_lock(void); +void release_shmem_lock(const char *caller_file_name, int caller_line_number); void init_shared_memory(void); void copy_params_to_shmem(struct param_info_s *); void update_to_shmem(param_t param, union param_value_u value); @@ -91,15 +92,14 @@ static void *map_memory(off_t target) int get_shmem_lock(const char *caller_file_name, int caller_line_number) { - // TODO: don't do this for now - return 0; - unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); unsigned int i = 0; +#ifdef PARAM_LOCK_DEBUG + PX4_INFO("lock value %d before get from %s, line: %d\n", *(unsigned int*)0xfbfc000, strrchr(caller_file_name, '/'), caller_line_number); +#endif + while (!atomic_compare_and_set(lock, 1, 0)) { - PX4_INFO("Could not get lock, file name: %s, line number: %d.\n", - caller_file_name, caller_line_number); i++; usleep(1000); @@ -107,6 +107,8 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number) } if (i > 100) { + PX4_INFO("Could not get lock, file name: %s, line number: %d.\n", + strrchr(caller_file_name, '/'), caller_line_number); return -1; } else { @@ -117,24 +119,39 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number) } -void release_shmem_lock(void) +void release_shmem_lock(const char *caller_file_name, int caller_line_number) { - // TODO: don't do this either - return; - unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); *lock = 1; + +#ifdef PARAM_LOCK_DEBUG + PX4_INFO("release lock, file name: %s, line number: %d.\n", + strrchr(caller_file_name, '/'), caller_line_number); +#endif + return; } void init_shared_memory(void) { //PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000); + int i; + + if(shmem_info_p) + return; virt_addr = map_memory(MAP_ADDRESS); shmem_info_p = (struct shmem_info *)virt_addr; + //init lock as 1 + unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); + *lock = 1; + + for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { + shmem_info_p->krait_changed_index[i] = 0; + } + //PX4_INFO("adsp memory mapped\n"); } @@ -175,7 +192,7 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) krait_changed_index[i] = 0; } - release_shmem_lock(); + release_shmem_lock(__FILE__, __LINE__); //PX4_INFO("Released lock\n"); } @@ -217,21 +234,20 @@ void update_to_shmem(param_t param, union param_value_u value) #endif - release_shmem_lock(); + release_shmem_lock(__FILE__, __LINE__); } void update_index_from_shmem(void) { unsigned int i; + param_t params[MAX_SHMEM_PARAMS / 8 + 1]; if (get_shmem_lock(__FILE__, __LINE__) != 0) { PX4_ERR("Could not get shmem lock\n"); return; } - PX4_DEBUG("Updating index from shmem\n"); - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { // Check if any param has been changed. if (krait_changed_index[i] != shmem_info_p->krait_changed_index[i]) { @@ -243,15 +259,21 @@ void update_index_from_shmem(void) // Update our krait_changed_index as well. krait_changed_index[i] = shmem_info_p->krait_changed_index[i]; - - // FIXME: this is a hack but it gets the param so that it gets added - // to the local list param_values in param_shmem.c. - int32_t dummy; - param_get(param_to_get, &dummy); + params[i] = param_to_get; + } else { + params[i] = 0xFFFF; } } + release_shmem_lock(__FILE__, __LINE__); - release_shmem_lock(); + // FIXME: this is a hack but it gets the param so that it gets added + // to the local list param_values in param_shmem.c. + for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { + if(params[i] != 0xFFFF){ + int32_t dummy; + param_get(params[i], &dummy); + } + } } @@ -271,7 +293,7 @@ static void update_value_from_shmem(param_t param, union param_value_u *value) bit_changed = 1 << param % 8; shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed; - release_shmem_lock(); + release_shmem_lock(__FILE__, __LINE__); #ifdef SHMEM_DEBUG diff --git a/src/platforms/shmem.h b/src/platforms/shmem.h index 45f26684fa..bed181c267 100644 --- a/src/platforms/shmem.h +++ b/src/platforms/shmem.h @@ -33,6 +33,8 @@ #define MAX_SHMEM_PARAMS 2000 //MAP_SIZE - (LOCK_SIZE - sizeof(struct shmem_info)) +#define PARAM_BUFFER_SIZE (MAX_SHMEM_PARAMS / 8 + 1) + struct shmem_info { union param_value_u params_val[MAX_SHMEM_PARAMS]; unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by krait*/