diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 3b9d56e253..68335db760 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -98,11 +98,10 @@ CDev::CDev(const char *name, // private _devname(devname), _registered(false), - _open_count(0) + _max_pollwaiters(0), + _open_count(0), + _pollset(nullptr) { - for (unsigned i = 0; i < _max_pollwaiters; i++) { - _pollset[i] = nullptr; - } } CDev::~CDev() @@ -110,6 +109,10 @@ CDev::~CDev() if (_registered) { unregister_driver(_devname); } + + if (_pollset) { + delete[](_pollset); + } } int @@ -385,7 +388,29 @@ CDev::store_poll_waiter(struct pollfd *fds) } } - return ENOMEM; + /* No free slot found. Resize the pollset */ + + if (_max_pollwaiters >= 256 / 2) { //_max_pollwaiters is uint8_t + return -ENOMEM; + } + + const uint8_t new_count = _max_pollwaiters > 0 ? _max_pollwaiters * 2 : 1; + pollfd **new_pollset = new pollfd*[new_count]; + + if (!new_pollset) { + return -ENOMEM; + } + + if (_max_pollwaiters > 0) { + memset(new_pollset + _max_pollwaiters, 0, sizeof(pollfd *) * (new_count - _max_pollwaiters)); + memcpy(new_pollset, _pollset, sizeof(pollfd *) * _max_pollwaiters); + delete[](_pollset); + } + + _pollset = new_pollset; + _pollset[_max_pollwaiters] = fds; + _max_pollwaiters = new_count; + return OK; } int diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h index 3a3e2d16be..6ea64d228c 100644 --- a/src/drivers/device/device_nuttx.h +++ b/src/drivers/device/device_nuttx.h @@ -451,13 +451,13 @@ protected: bool _pub_blocked; /**< true if publishing should be blocked */ private: - static const unsigned _max_pollwaiters = 8; const char *_devname; /**< device node name */ bool _registered; /**< true if device name was registered */ + uint8_t _max_pollwaiters; /**< size of the _pollset array */ uint16_t _open_count; /**< number of successful opens */ - struct pollfd *_pollset[_max_pollwaiters]; + struct pollfd **_pollset; /** * Store a pollwaiter in a slot where we can find it later. diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 810129ca33..37c4234ab8 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -84,13 +84,11 @@ VDev::VDev(const char *name, // private _devname(devname), _registered(false), - _open_count(0) + _max_pollwaiters(0), + _open_count(0), + _pollset(nullptr) { PX4_DEBUG("VDev::VDev"); - - for (unsigned i = 0; i < _max_pollwaiters; i++) { - _pollset[i] = nullptr; - } } VDev::~VDev() @@ -100,6 +98,10 @@ VDev::~VDev() if (_registered) { unregister_driver(_devname); } + + if (_pollset) { + delete[](_pollset); + } } int @@ -486,7 +488,29 @@ VDev::store_poll_waiter(px4_pollfd_struct_t *fds) } } - return -ENOMEM; + /* No free slot found. Resize the pollset */ + + if (_max_pollwaiters >= 256 / 2) { //_max_pollwaiters is uint8_t + return -ENOMEM; + } + + const uint8_t new_count = _max_pollwaiters > 0 ? _max_pollwaiters * 2 : 1; + px4_pollfd_struct_t **new_pollset = new px4_pollfd_struct_t *[new_count]; + + if (!new_pollset) { + return -ENOMEM; + } + + if (_max_pollwaiters > 0) { + memset(new_pollset + _max_pollwaiters, 0, sizeof(px4_pollfd_struct_t *) * (new_count - _max_pollwaiters)); + memcpy(new_pollset, _pollset, sizeof(px4_pollfd_struct_t *) * _max_pollwaiters); + delete[](_pollset); + } + + _pollset = new_pollset; + _pollset[_max_pollwaiters] = fds; + _max_pollwaiters = new_count; + return PX4_OK; } int diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index a5d76ce61a..571f296d15 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -425,13 +425,12 @@ protected: bool _pub_blocked; /**< true if publishing should be blocked */ private: - static const unsigned _max_pollwaiters = 8; - const char *_devname; /**< device node name */ bool _registered; /**< true if device name was registered */ + uint8_t _max_pollwaiters; /**< size of the _pollset array */ unsigned _open_count; /**< number of successful opens */ - px4_pollfd_struct_t *_pollset[_max_pollwaiters]; + px4_pollfd_struct_t **_pollset; /** * Store a pollwaiter in a slot where we can find it later.