Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Networking: use BinarySemaphore

This commit is contained in:
Andrew Tridgell 2023-12-26 17:23:16 +11:00
parent 1e7ca34531
commit 229a527fed
1 changed files with 32 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

186
libraries/AP_Networking/lwip_hal/arch/sys_arch.cpp
View File

@ -1,5 +1,6 @@
/*
port of lwip to ArduPilot AP_HAL
This is partly based on ChibiOS/os/various/lwip_bindings
*/
#include <AP_HAL/AP_HAL.h>
@ -12,7 +13,6 @@
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <pthread.h>
#include <errno.h>
#include <lwipopts.h>
@ -26,13 +26,6 @@ extern "C" {
#include "lwip/tcpip.h"
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <semaphore.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include "hal.h"
#include "../../../libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h"
#endif
extern const AP_HAL::HAL &hal;
unsigned int
@ -61,69 +54,35 @@ struct sys_mbox {
};
struct sys_sem {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
sem_t sem;
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
semaphore_t sem;
#else
#error "Need sys_sem implementation"
#endif
HAL_BinarySemaphore sem;
};
static struct sys_sem *sys_sem_new_internal(u8_t count);
static void sys_sem_free_internal(struct sys_sem *sem);
/* Threads */
struct thread_wrapper_data {
class ThreadWrapper {
public:
ThreadWrapper(lwip_thread_fn fn, void *_arg) :
function(fn),
arg(_arg)
{}
bool create(const char *name, int stacksize, int prio) {
return hal.scheduler->thread_create(
FUNCTOR_BIND_MEMBER(&ThreadWrapper::run, void), name, MAX(stacksize,2048), AP_HAL::Scheduler::PRIORITY_NET, prio);
}
private:
void run(void) {
function(arg);
}
lwip_thread_fn function;
void *arg;
};
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
static void *
thread_wrapper(void *arg)
{
auto *thread_data = (struct thread_wrapper_data *)arg;
thread_data->function(thread_data->arg);
return NULL;
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static void
thread_wrapper(void *arg)
{
auto *thread_data = (struct thread_wrapper_data *)arg;
thread_data->function(thread_data->arg);
}
#endif
sys_thread_t
sys_thread_new(const char *name, lwip_thread_fn function, void *arg, int stacksize, int prio)
{
sys_thread_t ret = nullptr;
struct thread_wrapper_data *thread_data;
thread_data = new thread_wrapper_data;
thread_data->arg = arg;
thread_data->function = function;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
pthread_t t;
if (pthread_create(&t, NULL, thread_wrapper, thread_data) == 0) {
pthread_setname_np(t, name);
ret = (void*)t;
auto *thread_data = new ThreadWrapper(function, arg);
if (!thread_data->create(name, stacksize, prio)) {
AP_HAL::panic("lwip: Failed to start thread %s", name);
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
ret = thread_create_alloc(THD_WORKING_AREA_SIZE(stacksize+1024),
name,
prio+60, // need to use HAL thread call
thread_wrapper,
thread_data);
#endif
if (ret == nullptr) {
AP_HAL::panic("Failed to create thread %s", name);
}
return ret;
return (sys_thread_t)thread_data;
}
void sys_lock_tcpip_core(void)
@ -163,9 +122,9 @@ sys_mbox_new(struct sys_mbox **mb, int size)
return ERR_MEM;
}
mbox->first = mbox->last = 0;
mbox->not_empty = sys_sem_new_internal(0);
mbox->not_full = sys_sem_new_internal(0);
mbox->mutex = sys_sem_new_internal(1);
sys_sem_new(&mbox->not_empty, 0);
sys_sem_new(&mbox->not_full, 0);
sys_sem_new(&mbox->mutex, 1);
mbox->wait_send = 0;
*mb = mbox;
@ -179,9 +138,9 @@ sys_mbox_free(struct sys_mbox **mb)
struct sys_mbox *mbox = *mb;
sys_arch_sem_wait(&mbox->mutex, 0);
sys_sem_free_internal(mbox->not_empty);
sys_sem_free_internal(mbox->not_full);
sys_sem_free_internal(mbox->mutex);
sys_sem_free(&mbox->not_empty);
sys_sem_free(&mbox->not_full);
sys_sem_free(&mbox->mutex);
mbox->not_empty = mbox->not_full = mbox->mutex = NULL;
/* LWIP_DEBUGF("sys_mbox_free: mbox 0x%lx\n", mbox); */
delete mbox;
@ -339,26 +298,11 @@ sys_arch_mbox_fetch(struct sys_mbox **mb, void **msg, u32_t timeout)
return 0;
}
/*-----------------------------------------------------------------------------------*/
/* Semaphore */
static struct sys_sem *
sys_sem_new_internal(u8_t count)
{
auto *ret = new sys_sem;
if (ret != nullptr) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
sem_init(&ret->sem, 0, count);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
chSemObjectInit(&ret->sem, (cnt_t)count);
#endif
}
return ret;
}
err_t
sys_sem_new(struct sys_sem **sem, u8_t count)
{
*sem = sys_sem_new_internal(count);
auto *s = new HAL_BinarySemaphore(count);
*sem = (struct sys_sem *)s;
if (*sem == NULL) {
return ERR_MEM;
}
@ -369,60 +313,23 @@ u32_t
sys_arch_sem_wait(struct sys_sem **s, u32_t timeout_ms)
{
struct sys_sem *sem = *s;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
if (timeout_ms == 0) {
sem_wait(&sem->sem);
return 0;
return sem->sem.wait_blocking()?0:SYS_ARCH_TIMEOUT;
}
struct timespec ts;
if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
return SYS_ARCH_TIMEOUT;
}
ts.tv_sec += timeout_ms/1000UL;
ts.tv_nsec += (timeout_ms % 1000U) * 1000000UL;
if (ts.tv_nsec >= 1000000000L) {
ts.tv_sec++;
ts.tv_nsec -= 1000000000L;
}
auto ret = sem_timedwait(&sem->sem, &ts);
if (ret != 0) {
return SYS_ARCH_TIMEOUT;
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
chSysLock();
sysinterval_t tmo = timeout_ms > 0 ? MIN(TIME_MAX_INTERVAL, TIME_MS2I((time_msecs_t)timeout_ms)) : TIME_INFINITE;
if (chSemWaitTimeoutS(&sem->sem, tmo) != MSG_OK) {
chSysUnlock();
return SYS_ARCH_TIMEOUT;
}
chSysUnlock();
#endif
return 0;
return sem->sem.wait(timeout_ms*1000U)?0:SYS_ARCH_TIMEOUT;
}
void
sys_sem_signal(struct sys_sem **s)
{
struct sys_sem *sem = *s;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
sem_post(&sem->sem);
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
chSemSignal(&sem->sem);
#endif
}
static void
sys_sem_free_internal(struct sys_sem *sem)
{
delete sem;
sem->sem.signal();
}
void
sys_sem_free(struct sys_sem **sem)
{
if ((sem != NULL) && (*sem != SYS_SEM_NULL)) {
sys_sem_free_internal(*sem);
}
delete *sem;
}
/*-----------------------------------------------------------------------------------*/
@ -472,9 +379,6 @@ sys_mutex_free(struct sys_mutex **mutex)
delete sem;
}
/*-----------------------------------------------------------------------------------*/
/* Time */
u32_t
sys_now(void)
{
@ -487,30 +391,11 @@ sys_jiffies(void)
return AP_HAL::micros();
}
/*-----------------------------------------------------------------------------------*/
/* Init */
void
sys_init(void)
{
}
/*-----------------------------------------------------------------------------------*/
/* Critical section */
/** sys_prot_t sys_arch_protect(void)
This optional function does a "fast" critical region protection and returns
the previous protection level. This function is only called during very short
critical regions. An embedded system which supports ISR-based drivers might
want to implement this function by disabling interrupts. Task-based systems
might want to implement this by using a mutex or disabling tasking. This
function should support recursive calls from the same task or interrupt. In
other words, sys_arch_protect() could be called while already protected. In
that case the return value indicates that it is already protected.
sys_arch_protect() is only required if your port is supporting an operating
system.
*/
sys_prot_t
sys_arch_protect(void)
{
@ -520,13 +405,6 @@ sys_arch_protect(void)
return 0;
}
/** void sys_arch_unprotect(sys_prot_t pval)
This optional function does a "fast" set of critical region protection to the
value specified by pval. See the documentation for sys_arch_protect() for
more information. This function is only required if your port is supporting
an operating system.
*/
void
sys_arch_unprotect(sys_prot_t pval)
{