diff --git a/src/lib/cdev/CMakeLists.txt b/src/lib/cdev/CMakeLists.txt index c41989140f..650ebb49b1 100644 --- a/src/lib/cdev/CMakeLists.txt +++ b/src/lib/cdev/CMakeLists.txt @@ -39,7 +39,6 @@ if (${PX4_PLATFORM} STREQUAL "nuttx") else() list(APPEND SRCS_PLATFORM posix/cdev_platform.cpp - posix/vfile.cpp ) endif() diff --git a/src/lib/cdev/posix/cdev_platform.cpp b/src/lib/cdev/posix/cdev_platform.cpp index c6a9853f66..0cad37ad3a 100644 --- a/src/lib/cdev/posix/cdev_platform.cpp +++ b/src/lib/cdev/posix/cdev_platform.cpp @@ -34,68 +34,88 @@ #include "cdev_platform.hpp" -#include -#include - -#include "vfile.h" #include "../CDev.hpp" #include #include #include -using namespace std; - const cdev::px4_file_operations_t cdev::CDev::fops = {}; pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER; pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER; -#define PX4_MAX_FD 350 -static map devmap; -static cdev::file_t filemap[PX4_MAX_FD] = {}; +struct px4_dev_t { + char *name{nullptr}; + cdev::CDev *cdev{nullptr}; + + px4_dev_t(const char *n, cdev::CDev *c) : cdev(c) + { + name = strdup(n); + } + + ~px4_dev_t() + { + free(name); + } +private: + px4_dev_t() = default; +}; + +static px4_dev_t *devmap[128] {}; + +#define PX4_MAX_FD 128 +static cdev::file_t filemap[PX4_MAX_FD] {}; + +class VFile : public cdev::CDev +{ +public: + VFile(const char *fname, mode_t mode) : cdev::CDev(fname) {} + ~VFile() override = default; + + ssize_t write(cdev::file_t *handlep, const char *buffer, size_t buflen) override + { + // ignore what was written, but let pollers know something was written + poll_notify(POLLIN); + return buflen; + } +}; + +static cdev::CDev *getDev(const char *path) +{ + pthread_mutex_lock(&devmutex); + + for (const auto &dev : devmap) { + if (dev && (strcmp(dev->name, path) == 0)) { + pthread_mutex_unlock(&devmutex); + return dev->cdev; + } + } + + pthread_mutex_unlock(&devmutex); + + return nullptr; +} + +static cdev::CDev *getFile(int fd) +{ + pthread_mutex_lock(&filemutex); + cdev::CDev *dev = nullptr; + + if (fd < PX4_MAX_FD && fd >= 0) { + dev = filemap[fd].cdev; + } + + pthread_mutex_unlock(&filemutex); + return dev; +} extern "C" { - static cdev::CDev *getDev(const char *path) - { - PX4_DEBUG("CDev::getDev"); - - pthread_mutex_lock(&devmutex); - - auto item = devmap.find(path); - - if (item != devmap.end()) { - pthread_mutex_unlock(&devmutex); - return (cdev::CDev *)item->second; - } - - pthread_mutex_unlock(&devmutex); - - return nullptr; - } - - static cdev::CDev *get_vdev(int fd) - { - pthread_mutex_lock(&filemutex); - bool valid = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd].vdev); - cdev::CDev *dev; - - if (valid) { - dev = (cdev::CDev *)(filemap[fd].vdev); - - } else { - dev = nullptr; - } - - pthread_mutex_unlock(&filemutex); - return dev; - } - int register_driver(const char *name, const cdev::px4_file_operations_t *fops, cdev::mode_t mode, void *data) { PX4_DEBUG("CDev::register_driver %s", name); - int ret = 0; + int ret = -ENOSPC; if (name == nullptr || data == nullptr) { return -EINVAL; @@ -104,18 +124,28 @@ extern "C" { pthread_mutex_lock(&devmutex); // Make sure the device does not already exist - auto item = devmap.find(name); - - if (item != devmap.end()) { - pthread_mutex_unlock(&devmutex); - return -EEXIST; + for (const auto &dev : devmap) { + if (dev && (strcmp(dev->name, name) == 0)) { + pthread_mutex_unlock(&devmutex); + return -EEXIST; + } } - devmap[name] = (void *)data; - PX4_DEBUG("Registered DEV %s", name); + for (auto &dev : devmap) { + if (dev == nullptr) { + dev = new px4_dev_t(name, (cdev::CDev *)data); + PX4_DEBUG("Registered DEV %s", name); + ret = PX4_OK; + break; + } + } pthread_mutex_unlock(&devmutex); + if (ret != PX4_OK) { + PX4_ERR("No free devmap entries - increase devmap size"); + } + return ret; } @@ -130,9 +160,14 @@ extern "C" { pthread_mutex_lock(&devmutex); - if (devmap.erase(name) > 0) { - PX4_DEBUG("Unregistered DEV %s", name); - ret = 0; + for (auto &dev : devmap) { + if (dev && (strcmp(name, dev->name) == 0)) { + delete dev; + dev = nullptr; + PX4_DEBUG("Unregistered DEV %s", name); + ret = PX4_OK; + break; + } } pthread_mutex_unlock(&devmutex); @@ -158,15 +193,15 @@ extern "C" { // Create the file PX4_DEBUG("Creating virtual file %s", path); - dev = cdev::VFile::createFile(path, mode); + dev = new VFile(path, mode); + register_driver(path, nullptr, 0666, (void *)dev); } if (dev) { - pthread_mutex_lock(&filemutex); for (i = 0; i < PX4_MAX_FD; ++i) { - if (filemap[i].vdev == nullptr) { + if (filemap[i].cdev == nullptr) { filemap[i] = cdev::file_t(flags, dev); break; } @@ -180,10 +215,9 @@ extern "C" { } else { const unsigned NAMELEN = 32; - char thread_name[NAMELEN] = {}; + char thread_name[NAMELEN] {}; - PX4_WARN("%s: exceeded maximum number of file descriptors, accessing %s", - thread_name, path); + PX4_WARN("%s: exceeded maximum number of file descriptors, accessing %s", thread_name, path); #ifndef __PX4_QURT int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); @@ -213,13 +247,13 @@ extern "C" { { int ret; - cdev::CDev *dev = get_vdev(fd); + cdev::CDev *dev = getFile(fd); if (dev) { pthread_mutex_lock(&filemutex); ret = dev->close(&filemap[fd]); - filemap[fd].vdev = nullptr; + filemap[fd].cdev = nullptr; pthread_mutex_unlock(&filemutex); PX4_DEBUG("px4_close fd = %d", fd); @@ -239,7 +273,7 @@ extern "C" { { int ret; - cdev::CDev *dev = get_vdev(fd); + cdev::CDev *dev = getFile(fd); if (dev) { PX4_DEBUG("px4_read fd = %d", fd); @@ -260,7 +294,7 @@ extern "C" { { int ret; - cdev::CDev *dev = get_vdev(fd); + cdev::CDev *dev = getFile(fd); if (dev) { PX4_DEBUG("px4_write fd = %d", fd); @@ -282,7 +316,7 @@ extern "C" { PX4_DEBUG("px4_ioctl fd = %d", fd); int ret = 0; - cdev::CDev *dev = get_vdev(fd); + cdev::CDev *dev = getFile(fd); if (dev) { ret = dev->ioctl(&filemap[fd], cmd, arg); @@ -306,7 +340,7 @@ extern "C" { int ret = -1; const unsigned NAMELEN = 32; - char thread_name[NAMELEN] = {}; + char thread_name[NAMELEN] {}; #ifndef __PX4_QURT int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); @@ -332,7 +366,7 @@ extern "C" { fds[i].revents = 0; fds[i].priv = nullptr; - cdev::CDev *dev = get_vdev(fds[i].fd); + cdev::CDev *dev = getFile(fds[i].fd); // If fd is valid if (dev) { @@ -340,8 +374,7 @@ extern "C" { ret = dev->poll(&filemap[fds[i].fd], &fds[i], true); if (ret < 0) { - PX4_WARN("%s: px4_poll() error: %s", - thread_name, strerror(errno)); + PX4_WARN("%s: px4_poll() error: %s", thread_name, strerror(errno)); break; } @@ -355,7 +388,6 @@ extern "C" { // check for new data if (fd_pollable) { if (timeout > 0) { - // Get the current time struct timespec ts; // Note, we can't actually use CLOCK_MONOTONIC on macOS @@ -383,7 +415,7 @@ extern "C" { // go through all fds and count how many have data for (unsigned int i = 0; i < nfds; ++i) { - cdev::CDev *dev = get_vdev(fds[i].fd); + cdev::CDev *dev = getFile(fds[i].fd); // If fd is valid if (dev) { @@ -427,7 +459,9 @@ extern "C" { pthread_mutex_lock(&devmutex); for (const auto &dev : devmap) { - PX4_INFO_RAW(" %s\n", dev.first.c_str()); + if (dev) { + PX4_INFO_RAW(" %s\n", dev->name); + } } pthread_mutex_unlock(&devmutex); diff --git a/src/lib/cdev/posix/cdev_platform.hpp b/src/lib/cdev/posix/cdev_platform.hpp index c78d21c7a7..c9d0fd1591 100644 --- a/src/lib/cdev/posix/cdev_platform.hpp +++ b/src/lib/cdev/posix/cdev_platform.hpp @@ -17,13 +17,15 @@ struct file_operations { using px4_file_operations_t = struct file_operations; using mode_t = uint32_t; +class CDev; + struct file_t { int f_oflags{0}; void *f_priv{nullptr}; - void *vdev{nullptr}; + CDev *cdev{nullptr}; file_t() = default; - file_t(int f, void *c) : f_oflags(f), vdev(c) {} + file_t(int f, CDev *c) : f_oflags(f), cdev(c) {} }; } // namespace cdev diff --git a/src/lib/cdev/posix/vfile.cpp b/src/lib/cdev/posix/vfile.cpp deleted file mode 100644 index bd4e6c9df1..0000000000 --- a/src/lib/cdev/posix/vfile.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vdev_file.cpp - * Virtual file - * - * @author Mark Charlebois - */ - -#include "vfile.h" - -namespace cdev -{ - -VFile::VFile(const char *fname, mode_t mode) : - CDev(fname) -{ -} - -VFile *VFile::createFile(const char *fname, mode_t mode) -{ - VFile *me = new VFile(fname, mode); - px4_file_operations_t *file_ops = nullptr; - register_driver(fname, file_ops, 0666, (void *)me); - return me; -} - -ssize_t VFile::write(file_t *handlep, const char *buffer, size_t buflen) -{ - // ignore what was written, but let pollers know something was written - poll_notify(POLLIN); - - return buflen; -} - -} // namespace cdev diff --git a/src/lib/cdev/posix/vfile.h b/src/lib/cdev/posix/vfile.h deleted file mode 100644 index 0e7418915a..0000000000 --- a/src/lib/cdev/posix/vfile.h +++ /dev/null @@ -1,62 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vfile.cpp - * Virtual file - * - * @author Mark Charlebois - */ - -#pragma once - -#include "../CDev.hpp" - -namespace cdev -{ - -class VFile : public CDev -{ -public: - - static VFile *createFile(const char *fname, mode_t mode); - ~VFile() = default; - - ssize_t write(file_t *handlep, const char *buffer, size_t buflen) override; - -private: - VFile(const char *fname, mode_t mode); - VFile(const VFile &); -}; - -} // namespace cdev diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index a817a39163..f65f272835 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -42,18 +42,6 @@ #include "uORBCommunicator.hpp" #endif /* ORB_COMMUNICATOR */ -uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(cdev::file_t *filp) -{ -#ifndef __PX4_NUTTX - - if (!filp) { - return nullptr; - } - -#endif - return (SubscriberData *)(filp->f_priv); -} - uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, ORB_PRIO priority, uint8_t queue_size) : CDev(path), @@ -125,7 +113,7 @@ int uORB::DeviceNode::close(cdev::file_t *filp) { if (filp->f_oflags == PX4_F_RDONLY) { /* subscriber */ - SubscriberData *sd = filp_to_sd(filp); + SubscriberData *sd = static_cast(filp->f_priv); if (sd != nullptr) { remove_internal_subscriber(); @@ -196,7 +184,7 @@ uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen) return -EIO; } - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + SubscriberData *sd = static_cast(filp->f_priv); /* * Perform an atomic copy & state update @@ -282,12 +270,10 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) int uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) { - SubscriberData *sd = filp_to_sd(filp); - switch (cmd) { case ORBIOCUPDATED: { ATOMIC_ENTER; - *(bool *)arg = appears_updated(sd); + *(bool *)arg = appears_updated(filp); ATOMIC_LEAVE; return PX4_OK; } @@ -296,6 +282,8 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) int ret = PX4_OK; lock(); + SubscriberData *sd = static_cast(filp->f_priv); + if (arg == 0) { if (sd->update_interval) { delete (sd->update_interval); @@ -337,12 +325,15 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) return ret; } - case ORBIOCGETINTERVAL: - if (sd->update_interval) { - *(unsigned *)arg = sd->update_interval->interval; + case ORBIOCGETINTERVAL: { + SubscriberData *sd = static_cast(filp->f_priv); - } else { - *(unsigned *)arg = 0; + if (sd->update_interval) { + *(unsigned *)arg = sd->update_interval->interval; + + } else { + *(unsigned *)arg = 0; + } } return OK; @@ -459,12 +450,8 @@ int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta, ORB_PRIO px4_pollevent_t uORB::DeviceNode::poll_state(cdev::file_t *filp) { - SubscriberData *sd = filp_to_sd(filp); - - /* - * If the topic appears updated to the subscriber, say so. - */ - if (appears_updated(sd)) { + // If the topic appears updated to the subscriber, say so. + if (appears_updated(filp)) { return POLLIN; } @@ -474,24 +461,22 @@ uORB::DeviceNode::poll_state(cdev::file_t *filp) void uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) { - SubscriberData *sd = filp_to_sd((cdev::file_t *)fds->priv); - - /* - * If the topic looks updated to the subscriber, go ahead and notify them. - */ - if (appears_updated(sd)) { + // If the topic looks updated to the subscriber, go ahead and notify them. + if (appears_updated((cdev::file_t *)fds->priv)) { CDev::poll_notify_one(fds, events); } } bool -uORB::DeviceNode::appears_updated(SubscriberData *sd) +uORB::DeviceNode::appears_updated(cdev::file_t *filp) { // check if this topic has been published yet, if not bail out if (_data == nullptr) { return false; } + SubscriberData *sd = static_cast(filp->f_priv); + // if subscriber has interval check time since last update if (sd->update_interval != nullptr) { if (hrt_elapsed_time(&sd->update_interval->last_update) < sd->update_interval->interval) { diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 22337e5377..adf65758ff 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -269,8 +269,6 @@ private: uint8_t _queue_size; /**< maximum number of elements in the queue */ int8_t _subscriber_count{0}; - inline static SubscriberData *filp_to_sd(cdev::file_t *filp); - /** * Check whether a topic appears updated to a subscriber. * @@ -279,6 +277,6 @@ private: * @param sd The subscriber for whom to check. * @return True if the topic should appear updated to the subscriber */ - bool appears_updated(SubscriberData *sd); + bool appears_updated(cdev::file_t *filp); };