Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Networking: use HAL objects more directly

Browse Source 
and fix a ms/us bug
This commit is contained in:
Andrew Tridgell 2024-01-02 08:04:41 +11:00
parent f5bee94cba
commit 791a0a3230
2 changed files with 50 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -47,16 +47,12 @@ struct sys_mbox_msg {
struct sys_mbox { struct sys_mbox {
int first, last; int first, last;
void *msgs[SYS_MBOX_SIZE]; void *msgs[SYS_MBOX_SIZE];
struct sys_sem *not_empty; HAL_BinarySemaphore not_empty;
struct sys_sem *not_full; HAL_BinarySemaphore not_full;
struct sys_sem *mutex; HAL_BinarySemaphore mutex;
int wait_send; int wait_send;
}; };
struct sys_sem {
HAL_BinarySemaphore sem;
};
class ThreadWrapper { class ThreadWrapper {
public: public:
ThreadWrapper(lwip_thread_fn fn, void *_arg) : ThreadWrapper(lwip_thread_fn fn, void *_arg) :
@ -87,16 +83,12 @@ sys_thread_new(const char *name, lwip_thread_fn function, void *arg, int stacksi
void sys_lock_tcpip_core(void) void sys_lock_tcpip_core(void)
{ {
if (hal.scheduler != nullptr) { tcpip_mutex.take_blocking();
tcpip_mutex.take_blocking();
}
} }
void sys_unlock_tcpip_core(void) void sys_unlock_tcpip_core(void)
{ {
if (hal.scheduler != nullptr) { tcpip_mutex.give();
tcpip_mutex.give();
}
} }
void sys_mark_tcpip_thread(void) void sys_mark_tcpip_thread(void)
@ -122,9 +114,7 @@ sys_mbox_new(struct sys_mbox **mb, int size)
return ERR_MEM; return ERR_MEM;
} }
mbox->first = mbox->last = 0; mbox->first = mbox->last = 0;
sys_sem_new(&mbox->not_empty, 0); mbox->mutex.signal();
sys_sem_new(&mbox->not_full, 0);
sys_sem_new(&mbox->mutex, 1);
mbox->wait_send = 0; mbox->wait_send = 0;
*mb = mbox; *mb = mbox;
@ -136,13 +126,7 @@ sys_mbox_free(struct sys_mbox **mb)
{ {
if ((mb != NULL) && (*mb != SYS_MBOX_NULL)) { if ((mb != NULL) && (*mb != SYS_MBOX_NULL)) {
struct sys_mbox *mbox = *mb; struct sys_mbox *mbox = *mb;
sys_arch_sem_wait(&mbox->mutex, 0); mbox->mutex.wait_blocking();
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; delete mbox;
} }
} }
@ -155,13 +139,13 @@ sys_mbox_trypost(struct sys_mbox **mb, void *msg)
LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL)); LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL));
mbox = *mb; mbox = *mb;
sys_arch_sem_wait(&mbox->mutex, 0); mbox->mutex.wait_blocking();
LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_trypost: mbox %p msg %p\n", LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_trypost: mbox %p msg %p\n",
(void *)mbox, (void *)msg)); (void *)mbox, (void *)msg));
if ((mbox->last + 1) >= (mbox->first + SYS_MBOX_SIZE)) { if ((mbox->last + 1) >= (mbox->first + SYS_MBOX_SIZE)) {
sys_sem_signal(&mbox->mutex); mbox->mutex.signal();
return ERR_MEM; return ERR_MEM;
} }
@ -176,10 +160,10 @@ sys_mbox_trypost(struct sys_mbox **mb, void *msg)
mbox->last++; mbox->last++;
if (first) { if (first) {
sys_sem_signal(&mbox->not_empty); mbox->not_empty.signal();
} }
sys_sem_signal(&mbox->mutex); mbox->mutex.signal();
return ERR_OK; return ERR_OK;
} }
@ -192,15 +176,15 @@ sys_mbox_post(struct sys_mbox **mb, void *msg)
LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL)); LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL));
mbox = *mb; mbox = *mb;
sys_arch_sem_wait(&mbox->mutex, 0); mbox->mutex.wait_blocking();
LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_post: mbox %p msg %p\n", (void *)mbox, (void *)msg)); LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_post: mbox %p msg %p\n", (void *)mbox, (void *)msg));
while ((mbox->last + 1) >= (mbox->first + SYS_MBOX_SIZE)) { while ((mbox->last + 1) >= (mbox->first + SYS_MBOX_SIZE)) {
mbox->wait_send++; mbox->wait_send++;
sys_sem_signal(&mbox->mutex); mbox->mutex.signal();
sys_arch_sem_wait(&mbox->not_full, 0); mbox->not_full.wait_blocking();
sys_arch_sem_wait(&mbox->mutex, 0); mbox->mutex.wait_blocking();
mbox->wait_send--; mbox->wait_send--;
} }
@ -215,10 +199,10 @@ sys_mbox_post(struct sys_mbox **mb, void *msg)
mbox->last++; mbox->last++;
if (first) { if (first) {
sys_sem_signal(&mbox->not_empty); mbox->not_empty.signal();
} }
sys_sem_signal(&mbox->mutex); mbox->mutex.signal();
} }
u32_t u32_t
@ -226,10 +210,10 @@ sys_arch_mbox_tryfetch(struct sys_mbox **mb, void **msg)
{ {
struct sys_mbox *mbox = *mb; struct sys_mbox *mbox = *mb;
sys_arch_sem_wait(&mbox->mutex, 0); mbox->mutex.wait_blocking();
if (mbox->first == mbox->last) { if (mbox->first == mbox->last) {
sys_sem_signal(&mbox->mutex); mbox->mutex.signal();
return SYS_MBOX_EMPTY; return SYS_MBOX_EMPTY;
} }
@ -243,41 +227,37 @@ sys_arch_mbox_tryfetch(struct sys_mbox **mb, void **msg)
mbox->first++; mbox->first++;
if (mbox->wait_send) { if (mbox->wait_send) {
sys_sem_signal(&mbox->not_full); mbox->not_full.signal();
} }
sys_sem_signal(&mbox->mutex); mbox->mutex.signal();
return 0; return 0;
} }
u32_t u32_t
sys_arch_mbox_fetch(struct sys_mbox **mb, void **msg, u32_t timeout) sys_arch_mbox_fetch(struct sys_mbox **mb, void **msg, u32_t timeout_ms)
{ {
struct sys_mbox *mbox; struct sys_mbox *mbox;
LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL)); LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL));
mbox = *mb; mbox = *mb;
/* The mutex lock is quick so we don't bother with the timeout mbox->mutex.wait_blocking();
stuff here. */
sys_arch_sem_wait(&mbox->mutex, 0);
while (mbox->first == mbox->last) { while (mbox->first == mbox->last) {
sys_sem_signal(&mbox->mutex); mbox->mutex.signal();
/* We block while waiting for a mail to arrive in the mailbox. We /* We block while waiting for a mail to arrive in the mailbox. We
must be prepared to timeout. */ must be prepared to timeout. */
if (timeout != 0) { if (timeout_ms != 0) {
u32_t time_needed = sys_arch_sem_wait(&mbox->not_empty, timeout); if (!mbox->not_empty.wait(timeout_ms*1000U)) {
if (time_needed == SYS_ARCH_TIMEOUT) {
return SYS_ARCH_TIMEOUT; return SYS_ARCH_TIMEOUT;
} }
} else { } else {
sys_arch_sem_wait(&mbox->not_empty, 0); mbox->not_empty.wait_blocking();
} }
sys_arch_sem_wait(&mbox->mutex, 0); mbox->mutex.wait_blocking();
} }
if (msg != NULL) { if (msg != NULL) {
@ -290,19 +270,18 @@ sys_arch_mbox_fetch(struct sys_mbox **mb, void **msg, u32_t timeout)
mbox->first++; mbox->first++;
if (mbox->wait_send) { if (mbox->wait_send) {
sys_sem_signal(&mbox->not_full); mbox->not_full.signal();
} }
sys_sem_signal(&mbox->mutex); mbox->mutex.signal();
return 0; return 0;
} }
err_t err_t
sys_sem_new(struct sys_sem **sem, u8_t count) sys_sem_new(sys_sem_t *sem, u8_t count)
{ {
auto *s = new HAL_BinarySemaphore(count); *sem = (sys_sem_t)new HAL_BinarySemaphore(count);
*sem = (struct sys_sem *)s;
if (*sem == NULL) { if (*sem == NULL) {
return ERR_MEM; return ERR_MEM;
} }
@ -310,26 +289,26 @@ sys_sem_new(struct sys_sem **sem, u8_t count)
} }
u32_t u32_t
sys_arch_sem_wait(struct sys_sem **s, u32_t timeout_ms) sys_arch_sem_wait(sys_sem_t *s, u32_t timeout_ms)
{ {
struct sys_sem *sem = *s; HAL_BinarySemaphore *sem = (HAL_BinarySemaphore *)*s;
if (timeout_ms == 0) { if (timeout_ms == 0) {
return sem->sem.wait_blocking()?0:SYS_ARCH_TIMEOUT; return sem->wait_blocking()?0:SYS_ARCH_TIMEOUT;
} }
return sem->sem.wait(timeout_ms*1000U)?0:SYS_ARCH_TIMEOUT; return sem->wait(timeout_ms*1000U)?0:SYS_ARCH_TIMEOUT;
} }
void void
sys_sem_signal(struct sys_sem **s) sys_sem_signal(sys_sem_t *s)
{ {
struct sys_sem *sem = *s; HAL_BinarySemaphore *sem = (HAL_BinarySemaphore *)*s;
sem->sem.signal(); sem->signal();
} }
void void
sys_sem_free(struct sys_sem **sem) sys_sem_free(sys_sem_t *sem)
{ {
delete *sem; delete ((HAL_BinarySemaphore *)*sem);
} }
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
@ -338,45 +317,37 @@ sys_sem_free(struct sys_sem **sem)
* @param mutex pointer to the mutex to create * @param mutex pointer to the mutex to create
* @return a new mutex */ * @return a new mutex */
err_t err_t
sys_mutex_new(struct sys_mutex **mutex) sys_mutex_new(sys_mutex_t *mutex)
{ {
auto *sem = new HAL_Semaphore; *mutex = (sys_mutex_t)new HAL_Semaphore;
if (sem == nullptr) { if (*mutex == nullptr) {
return ERR_MEM; return ERR_MEM;
} }
*mutex = (struct sys_mutex *)sem;
return ERR_OK; return ERR_OK;
} }
/** Lock a mutex /** Lock a mutex
* @param mutex the mutex to lock */ * @param mutex the mutex to lock */
void void
sys_mutex_lock(struct sys_mutex **mutex) sys_mutex_lock(sys_mutex_t *mutex)
{ {
auto *sem = (HAL_Semaphore *)*mutex; ((HAL_Semaphore*)*mutex)->take_blocking();
if (hal.scheduler != nullptr) {
sem->take_blocking();
}
} }
/** Unlock a mutex /** Unlock a mutex
* @param mutex the mutex to unlock */ * @param mutex the mutex to unlock */
void void
sys_mutex_unlock(struct sys_mutex **mutex) sys_mutex_unlock(sys_mutex_t *mutex)
{ {
auto *sem = (HAL_Semaphore *)*mutex; ((HAL_Semaphore*)*mutex)->give();
if (hal.scheduler != nullptr) {
sem->give();
}
} }
/** Delete a mutex /** Delete a mutex
* @param mutex the mutex to delete */ * @param mutex the mutex to delete */
void void
sys_mutex_free(struct sys_mutex **mutex) sys_mutex_free(sys_mutex_t *mutex)
{ {
auto *sem = (HAL_Semaphore *)*mutex; delete (HAL_Semaphore*)*mutex;
delete sem;
} }
u32_t u32_t

2
libraries/AP_Networking/lwip_hal/include/arch/sys_arch.h
View File

@ -8,8 +8,6 @@ extern "C"
#define SYS_MBOX_NULL NULL #define SYS_MBOX_NULL NULL
#define SYS_SEM_NULL NULL #define SYS_SEM_NULL NULL
/*typedef u32_t sys_prot_t;*/
struct sys_sem; struct sys_sem;
typedef struct sys_sem * sys_sem_t; typedef struct sys_sem * sys_sem_t;
#define sys_sem_valid(sem) (((sem) != NULL) && (*(sem) != NULL)) #define sys_sem_valid(sem) (((sem) != NULL) && (*(sem) != NULL))