Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:49:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
jwilson
2016-06-20 16:01:32 -07:00
committed by Julian Oes
parent 6739ae9dfc
commit a73ac821ab
9 changed files with 197 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/firmware/qurt/px4muorb.idl
+22
View File
@@ -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<octet> value);
/**
* Interface to update index for krait.
* @param data: param index array.
*/
AEEResult param_update_index_from_shmem( rout sequence<octet> 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<octet> value);
/**
* Interface to add a subscriber to the identified topic
src/modules/muorb/adsp/px4muorb.cpp
+78
View File
@@ -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
+6
View File
@@ -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
+19
View File
@@ -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
+8 -16
View File
@@ -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
}
src/platforms/posix/px4_layer/CMakeLists.txt
+5
View File
@@ -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
src/platforms/posix/px4_layer/shmem_posix.c
+14 -202
View File
@@ -51,236 +51,43 @@
#include "systemlib/param/param.h"
#include <shmem.h>
#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)
src/platforms/qurt/px4_layer/shmem_qurt.c
+43 -21
View File
@@ -48,13 +48,14 @@
#include <drivers/drv_hrt.h>
//#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
src/platforms/shmem.h
+2
View File
@@ -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*/