From bccb6c6b45e867a4aba184bffbe61b57ebbee48b Mon Sep 17 00:00:00 2001 From: likun17 Date: Tue, 30 Apr 2024 19:24:30 +0800 Subject: [PATCH] uorb:Added urob loop function module and supported epoll. Signed-off-by: likun17 --- system/uorb/CMakeLists.txt | 3 +- system/uorb/Kconfig | 4 + system/uorb/Makefile | 2 +- system/uorb/uORB/epoll.c | 185 ++++++++++++++++++++++++++++++++++++ system/uorb/uORB/internal.h | 49 ++++++++++ system/uorb/uORB/loop.c | 112 ++++++++++++++++++++++ system/uorb/uORB/uORB.h | 143 ++++++++++++++++++++++++++++ 7 files changed, 495 insertions(+), 3 deletions(-) create mode 100644 system/uorb/uORB/epoll.c create mode 100644 system/uorb/uORB/internal.h create mode 100644 system/uorb/uORB/loop.c diff --git a/system/uorb/CMakeLists.txt b/system/uorb/CMakeLists.txt index b890c0ae84..0242039c37 100644 --- a/system/uorb/CMakeLists.txt +++ b/system/uorb/CMakeLists.txt @@ -27,8 +27,7 @@ if(CONFIG_UORB) nuttx_add_library(uorb STATIC) - file(GLOB_RECURSE CSRCS "sensor/*.c") - list(APPEND CSRCS uORB/uORB.c) + file(GLOB_RECURSE CSRCS "sensor/*.c" "uORB/*.c") if(CONFIG_UORB_LISTENER) nuttx_add_application( diff --git a/system/uorb/Kconfig b/system/uorb/Kconfig index 4a9ab9712b..ba99d29078 100644 --- a/system/uorb/Kconfig +++ b/system/uorb/Kconfig @@ -26,6 +26,10 @@ config UORB_TESTS bool "uorb unit tests" default n +config UORB_LOOP_MAX_EVENTS + int "uorb loop max events" + default 16 + if UORB_TESTS config UORB_SRORAGE_DIR diff --git a/system/uorb/Makefile b/system/uorb/Makefile index e97bc87ec6..764ae9ac1f 100644 --- a/system/uorb/Makefile +++ b/system/uorb/Makefile @@ -20,7 +20,7 @@ include $(APPDIR)/Make.defs -CSRCS += uORB/uORB.c +CSRCS += $(wildcard uORB/*.c) CSRCS += $(wildcard sensor/*.c) ifneq ($(CONFIG_UORB_LISTENER),) diff --git a/system/uorb/uORB/epoll.c b/system/uorb/uORB/epoll.c new file mode 100644 index 0000000000..f2972fe847 --- /dev/null +++ b/system/uorb/uORB/epoll.c @@ -0,0 +1,185 @@ +/**************************************************************************** + * apps/system/uorb/uORB/epoll.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +#include "internal.h" + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int orb_loop_epoll_init(FAR struct orb_loop_s *loop); +static int orb_loop_epoll_run(FAR struct orb_loop_s *loop); +static int orb_loop_epoll_uninit(FAR struct orb_loop_s *loop); +static int orb_loop_epoll_enable(FAR struct orb_loop_s *loop, + FAR struct orb_handle_s *handle, bool en); + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +const struct orb_loop_ops_s g_orb_loop_epoll_ops = +{ + .init = orb_loop_epoll_init, + .run = orb_loop_epoll_run, + .uninit = orb_loop_epoll_uninit, + .enable = orb_loop_epoll_enable, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int orb_loop_epoll_init(FAR struct orb_loop_s *loop) +{ + loop->running = false; + loop->fd = epoll_create1(EPOLL_CLOEXEC); + if (loop->fd < 0) + { + return -errno; + } + + return OK; +} + +static int orb_loop_epoll_run(FAR struct orb_loop_s *loop) +{ + struct epoll_event et[CONFIG_UORB_LOOP_MAX_EVENTS]; + FAR struct orb_handle_s *handle; + int nfds; + int i; + + if (loop->running) + { + return -EBUSY; + } + + loop->running = true; + while (loop->running) + { + nfds = epoll_wait(loop->fd, et, CONFIG_UORB_LOOP_MAX_EVENTS, -1); + if (nfds == -1 && errno != EINTR) + { + return -errno; + } + + for (i = 0; i < nfds; i++) + { + handle = et[i].data.ptr; + if (handle == NULL) + { + continue; + } + + if (et[i].events & EPOLLIN) + { + if (handle->datain_cb != NULL) + { + handle->datain_cb(handle, handle->arg); + } + else + { + uorberr("epoll wait data in error! fd:%d", handle->fd); + } + } + else if (et[i].events & EPOLLOUT) + { + if (handle->dataout_cb != NULL) + { + handle->dataout_cb(handle, handle->arg); + } + else + { + uorberr("epoll wait data out error! fd:%d", handle->fd); + } + } + else if (et[i].events & EPOLLPRI) + { + if (handle->eventpri_cb != NULL) + { + handle->eventpri_cb(handle, handle->arg); + } + else + { + uorberr("epoll wait events pri error! fd:%d", handle->fd); + } + } + else if (et[i].events & EPOLLERR) + { + if (handle->eventerr_cb != NULL) + { + handle->eventerr_cb(handle, handle->arg); + } + else + { + uorberr("epoll wait events error! fd:%d", handle->fd); + } + } + } + } + + return OK; +} + +static int orb_loop_epoll_uninit(FAR struct orb_loop_s *loop) +{ + int ret; + + loop->running = false; + ret = close(loop->fd); + if (ret < 0) + { + return -errno; + } + + return ret; +} + +static int orb_loop_epoll_enable(FAR struct orb_loop_s *loop, + FAR struct orb_handle_s *handle, bool en) +{ + struct epoll_event ev; + int ret; + + if (en) + { + ev.events = handle->events; + ev.data.ptr = handle; + ret = epoll_ctl(loop->fd, EPOLL_CTL_ADD, handle->fd, &ev); + } + else + { + ret = epoll_ctl(loop->fd, EPOLL_CTL_DEL, handle->fd, NULL); + } + + if (ret < 0) + { + return -errno; + } + + return ret; +} diff --git a/system/uorb/uORB/internal.h b/system/uorb/uORB/internal.h new file mode 100644 index 0000000000..fc2e2f5201 --- /dev/null +++ b/system/uorb/uORB/internal.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * apps/system/uorb/uORB/internal.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __APP_SYSTEM_UORB_UORB_INTERNAL_H +#define __APP_SYSTEM_UORB_UORB_INTERNAL_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +extern const struct orb_loop_ops_s g_orb_loop_epoll_ops; + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct orb_loop_ops_s +{ + CODE int (*init)(FAR struct orb_loop_s *loop); + CODE int (*run)(FAR struct orb_loop_s *loop); + CODE int (*uninit)(FAR struct orb_loop_s *loop); + CODE int (*enable)(FAR struct orb_loop_s *loop, + FAR struct orb_handle_s *handle, bool en); +}; + +#endif /* __APP_SYSTEM_UORB_UORB_INTERNAL_H */ diff --git a/system/uorb/uORB/loop.c b/system/uorb/uORB/loop.c new file mode 100644 index 0000000000..62795f9bf6 --- /dev/null +++ b/system/uorb/uORB/loop.c @@ -0,0 +1,112 @@ +/**************************************************************************** + * apps/system/uorb/uORB/loop.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include "internal.h" + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int orb_loop_init(FAR struct orb_loop_s *loop, enum orb_loop_type_e type) +{ + int ret = -EINVAL; + + if (loop == NULL) + { + return ret; + } + + switch (type) + { + case ORB_EPOLL_TYPE: + loop->ops = &g_orb_loop_epoll_ops; + break; + + default: + uorberr("loop register type error! type:%d", type); + return ret; + } + + ret = loop->ops->init(loop); + if (ret < 0) + { + uorberr("loop init failed! ret:%d", ret); + loop->ops = NULL; + } + + return ret; +} + +int orb_loop_run(FAR struct orb_loop_s *loop) +{ + return loop->ops->run(loop); +} + +int orb_loop_deinit(FAR struct orb_loop_s *loop) +{ + int ret; + + ret = loop->ops->uninit(loop); + if (ret >= 0) + { + loop->ops = NULL; + } + + return ret; +} + +int orb_handle_init(FAR struct orb_handle_s *handle, int fd, int events, + FAR void *arg, orb_datain_cb_t datain_cb, + orb_dataout_cb_t dataout_cb, orb_eventpri_cb_t pri_cb, + orb_eventerr_cb_t err_cb) +{ + if (fd < 0) + { + return -EINVAL; + } + + handle->fd = fd; + handle->arg = arg; + handle->events = events; + handle->eventpri_cb = pri_cb; + handle->eventerr_cb = err_cb; + handle->datain_cb = datain_cb; + handle->dataout_cb = dataout_cb; + + return OK; +} + +int orb_handle_start(FAR struct orb_loop_s *loop, + FAR struct orb_handle_s *handle) +{ + return loop->ops->enable(loop, handle, true); +} + +int orb_handle_stop(FAR struct orb_loop_s *loop, + FAR struct orb_handle_s *handle) +{ + return loop->ops->enable(loop, handle, false); +} diff --git a/system/uorb/uORB/uORB.h b/system/uorb/uORB/uORB.h index 0655a1a866..0c99e6bacc 100644 --- a/system/uorb/uORB/uORB.h +++ b/system/uorb/uORB/uORB.h @@ -70,6 +70,41 @@ struct orb_object typedef uint64_t orb_abstime; +struct orb_handle_s; + +typedef CODE int (*orb_datain_cb_t)(FAR struct orb_handle_s *handle, + FAR void *arg); +typedef CODE int (*orb_dataout_cb_t)(FAR struct orb_handle_s *handle, + FAR void *arg); +typedef CODE int (*orb_eventpri_cb_t)(FAR struct orb_handle_s *handle, + FAR void *arg); +typedef CODE int (*orb_eventerr_cb_t)(FAR struct orb_handle_s *handle, + FAR void *arg); + +enum orb_loop_type_e +{ + ORB_EPOLL_TYPE = 0, +}; + +struct orb_loop_ops_s; +struct orb_loop_s +{ + FAR const struct orb_loop_ops_s *ops; /* Loop handle ops. */ + bool running; /* uORB loop is running flag. */ + int fd; /* Loop fd. */ +}; + +struct orb_handle_s +{ + int events; /* Events of interest. */ + int fd; /* Topic fd. */ + FAR void *arg; /* Callback parameter. */ + orb_datain_cb_t datain_cb; /* User EPOLLIN callback funtion. */ + orb_dataout_cb_t dataout_cb; /* User EPOLLOUT callback funtion. */ + orb_eventpri_cb_t eventpri_cb; /* User EPOLLPRI callback funtion. */ + orb_eventerr_cb_t eventerr_cb; /* User EPOLLERR callback funtion. */ +}; + /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ @@ -774,6 +809,114 @@ int orb_fprintf(FAR FILE *stream, FAR const char *format, FAR const void *data); #endif +/**************************************************************************** + * Name: orb_loop_init + * + * Description: + * Initialize orb loop, release it with orb_loop_deinit function. + * + * Input Parameters: + * loop orb loop contains multiple handles. + * type orb loop type. + * + * Returned Value: + * Returns the orb loop handle if successful, or NULL if an error occurs + ****************************************************************************/ + +int orb_loop_init(FAR struct orb_loop_s *loop, enum orb_loop_type_e type); + +/**************************************************************************** + * Name: orb_loop_run + * + * Description: + * Start the loop. Users can dynamically open new fd(orb_handle_start) + * and close fd(orb_handle_stop) that have been added to the loop after + * the loop is started. after starting it will be in a blocked state. + * + * Input Parameters: + * loop orb loop contains multiple handles. + * + * Returned Value: + * Zero (OK) or positive on success; a negated errno value on failure. + ****************************************************************************/ + +int orb_loop_run(FAR struct orb_loop_s *loop); + +/**************************************************************************** + * Name: orb_loop_deinit + * + * Description: + * Unregister the current loop. To use it again, you need to reinitialize + * it. The internally added handle needs to be closed by the user. + * + * Input Parameters: + * loop orb loop contains multiple handles. + * + * Returned Value: + * Zero (OK) or positive on success; a negated errno value on failure. + ****************************************************************************/ + +int orb_loop_deinit(FAR struct orb_loop_s *loop); + +/**************************************************************************** + * Name: orb_handle_init + * + * Description: + * Initialize the orb handle. + * + * Input Parameters: + * handle orb loop handle, need to be added to loop for use. + * fd orb fd, from orb_subscribe or orb_advertise. + * events Events of interest. + * arg Parameters passed in by the user. + * data_in_cb data in callback function. + * data_out_cb data out callback function. + * pri_cb pri callback function. + * err_cb err callback function. + * + * Returned Value: + * Zero (OK) or positive on success; a negated errno value on failure. + ****************************************************************************/ + +int orb_handle_init(FAR struct orb_handle_s *handle, int fd, int events, + FAR void *arg, orb_datain_cb_t datain_cb, + orb_dataout_cb_t dataout_cb, orb_eventpri_cb_t pri_cb, + orb_eventerr_cb_t err_cb); + +/**************************************************************************** + * Name: orb_handle_start + * + * Description: + * Start the handle from the loop. + * + * Input Parameters: + * loop orb loop contains multiple handles. + * handle orb loop handle, need to be added to loop for use. + * + * Returned Value: + * Zero (OK) or positive on success; a negated errno value on failure. + ****************************************************************************/ + +int orb_handle_start(FAR struct orb_loop_s *loop, + FAR struct orb_handle_s *handle); + +/**************************************************************************** + * Name: orb_handle_stop + * + * Description: + * Stop the handle from the loop. + * + * Input Parameters: + * loop orb loop contains multiple handles. + * handle orb loop handle, need to be added to loop for use. + * + * Returned Value: + * Zero (OK) or positive on success; a negated errno value on failure. + ****************************************************************************/ + +int orb_handle_stop(FAR struct orb_loop_s *loop, + FAR struct orb_handle_s *handle); + #ifdef __cplusplus } #endif