From 72773b75c302dfa5eec75b89e29216b338c55057 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 24 Jul 2018 19:53:17 +0200
Subject: [PATCH] CDev::poll: disable IRQs instead of using an expensive
 semaphore

poll() is one of the heavily used methods and thus needs to be optimized
as much as possible.

Test on Pixracer: uorb_tests latency_test
Before:
uORB note: ---------------- LATENCY TEST ------------------
INFO  [uorb_tests] mean:     40.4320 us
INFO  [uorb_tests] std dev:   1.3466 us
INFO  [uorb_tests] min:      39 us
INFO  [uorb_tests] max:      57 us
INFO  [uorb_tests] missed topic updates: 0
This Patch:
uORB note: ---------------- LATENCY TEST ------------------
INFO  [uorb_tests] mean:     31.3480 us
INFO  [uorb_tests] std dev:   1.4584 us
INFO  [uorb_tests] min:      30 us
INFO  [uorb_tests] max:      45 us
INFO  [uorb_tests] missed topic updates: 0
---
 src/lib/drivers/device/CDev.cpp | 96 ++++++++++++++++++++++-----------
 src/lib/drivers/device/CDev.hpp |  2 +-
 2 files changed, 65 insertions(+), 33 deletions(-)

diff --git a/src/lib/drivers/device/CDev.cpp b/src/lib/drivers/device/CDev.cpp
index 1f1827fc2d..c3693b22b3 100644
--- a/src/lib/drivers/device/CDev.cpp
+++ b/src/lib/drivers/device/CDev.cpp
@@ -271,11 +271,6 @@ CDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
 	DEVICE_DEBUG("CDev::Poll %s", setup ? "setup" : "teardown");
 	int ret = PX4_OK;
 
-	/*
-	 * Lock against pollnotify() (and possibly other callers)
-	 */
-	lock();
-
 	if (setup) {
 		/*
 		 * Save the file pointer in the pollfd for the subclass'
@@ -285,9 +280,66 @@ CDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
 		DEVICE_DEBUG("CDev::poll: fds->priv = %p", filep);
 
 		/*
-		 * Handle setup requests.
+		 * Lock against poll_notify() and possibly other callers.
 		 */
-		ret = store_poll_waiter(fds);
+		ATOMIC_ENTER;
+
+		/*
+		 * Try to store the fds for later use and handle array resizing.
+		 */
+		while ((ret = store_poll_waiter(fds)) == -ENFILE) {
+
+			// No free slot found. Resize the pollset. This is expensive, but it's only needed initially.
+
+			if (_max_pollwaiters >= 256 / 2) { //_max_pollwaiters is uint8_t
+				ret = -ENOMEM;
+				break;
+			}
+
+			const uint8_t new_count = _max_pollwaiters > 0 ? _max_pollwaiters * 2 : 1;
+			px4_pollfd_struct_t **prev_pollset = _pollset;
+
+#ifdef __PX4_NUTTX
+			// malloc uses a semaphore, we need to call it enabled IRQ's
+			px4_leave_critical_section(flags);
+#endif
+			px4_pollfd_struct_t **new_pollset = new px4_pollfd_struct_t *[new_count];
+
+#ifdef __PX4_NUTTX
+			flags = px4_enter_critical_section();
+#endif
+
+			if (prev_pollset == _pollset) {
+				// no one else updated the _pollset meanwhile, so we're good to go
+				if (!new_pollset) {
+					ret = -ENOMEM;
+					break;
+				}
+
+				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;
+
+				// Success
+				ret = PX4_OK;
+				break;
+			}
+
+#ifdef __PX4_NUTTX
+			px4_leave_critical_section(flags);
+#endif
+			// We have to retry
+			delete[] new_pollset;
+#ifdef __PX4_NUTTX
+			flags = px4_enter_critical_section();
+#endif
+		}
 
 		if (ret == PX4_OK) {
 
@@ -306,15 +358,17 @@ CDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
 			PX4_WARN("Store Poll Waiter error.");
 		}
 
+		ATOMIC_LEAVE;
+
 	} else {
+		ATOMIC_ENTER;
 		/*
 		 * Handle a teardown request.
 		 */
 		ret = remove_poll_waiter(fds);
+		ATOMIC_LEAVE;
 	}
 
-	unlock();
-
 	return ret;
 }
 
@@ -385,29 +439,7 @@ CDev::store_poll_waiter(px4_pollfd_struct_t *fds)
 		}
 	}
 
-	/* 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;
+	return -ENFILE;
 }
 
 int
diff --git a/src/lib/drivers/device/CDev.hpp b/src/lib/drivers/device/CDev.hpp
index 3496eebcbb..305f7e273a 100644
--- a/src/lib/drivers/device/CDev.hpp
+++ b/src/lib/drivers/device/CDev.hpp
@@ -289,7 +289,7 @@ private:
 	/**
 	 * Store a pollwaiter in a slot where we can find it later.
 	 *
-	 * Expands the pollset as required.  Must be called with the driver locked.
+	 * Must be called with the driver locked.
 	 *
 	 * @return		OK, or -errno on error.
 	 */