fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 05:53:34 +08:00
Code Issues 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-22 08:57:06 +02: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/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*/