UAVCAN driver transformed to use global memory pool

src/modules/uavcan/uavcan_main.cpp

@@ -77,7 +77,8 @@
 UavcanNode *UavcanNode::_instance;
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
-	_node(can_driver, system_clock),
+	_pool_allocator(MemoryPoolBlockCapacitySoftLimit, MemoryPoolBlockCapacityHardLimit),
+	_node(can_driver, system_clock, _pool_allocator),
 	_node_mutex(),
 	_esc_controller(_node),
 	_time_sync_master(_node),

src/modules/uavcan/uavcan_main.hpp

@@ -36,6 +36,7 @@
 #include <px4_config.h>
 
 #include <uavcan_stm32/uavcan_stm32.hpp>
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
 #include <uavcan/protocol/global_time_sync_master.hpp>
 #include <uavcan/protocol/global_time_sync_slave.hpp>
 #include <uavcan/protocol/param/GetSet.hpp>
@@ -80,7 +81,6 @@ class UavcanNode : public device::CDev
 
 	static constexpr unsigned PollTimeoutMs      = 10;
 
-	static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
 	/*
 	 * This memory is reserved for uavcan to use for queuing CAN frames.
 	 * At 1Mbit there is approximately one CAN frame every 145 uS.
@@ -96,8 +96,10 @@ class UavcanNode : public device::CDev
 	static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
 	static constexpr unsigned StackSize          = 1800;
 
+	static constexpr unsigned MemoryPoolBlockCapacitySoftLimit = 250;
+	static constexpr unsigned MemoryPoolBlockCapacityHardLimit = 500;
+
 public:
-	typedef uavcan::Node<MemPoolSize> Node;
 	typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
 	enum eServerAction {None, Start, Stop, CheckFW , Busy};
 
@@ -109,7 +111,7 @@ public:
 
 	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
 
-	Node		&get_node() { return _node; }
+	uavcan::Node<>	&get_node() { return _node; }
 
 	// TODO: move the actuator mixing stuff into the ESC controller class
 	static int	control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
@@ -130,8 +132,8 @@ public:
 	int             set_param(int remote_node_id, const char *name, char *value);
 	int             get_param(int remote_node_id, const char *name);
 	int             reset_node(int remote_node_id);
-private:
 
+private:
 	void		fill_node_info();
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
@@ -167,7 +169,15 @@ private:
 
 	static UavcanNode	*_instance;			///< singleton pointer
 
-	Node			_node;				///< library instance
+	struct MemoryPoolSynchronizer
+	{
+		const ::irqstate_t state = ::irqsave();
+		~MemoryPoolSynchronizer() { ::irqrestore(state); }
+	};
+
+	uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, MemoryPoolSynchronizer> _pool_allocator;
+
+	uavcan::Node<>		_node;				///< library instance
 	pthread_mutex_t		_node_mutex;
 	px4_sem_t                   _server_command_sem;
 	UavcanEscController	_esc_controller;

src/modules/uavcan/uavcan_servers.cpp

@@ -85,8 +85,8 @@
 UavcanServers *UavcanServers::_instance;
 UavcanServers::UavcanServers(uavcan::INode &main_node) :
 	_subnode_thread(-1),
-	_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()),
-	_subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
+	_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
+	_subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()),
 	_main_node(main_node),
 	_tracer(),
 	_storage_backend(),

src/modules/uavcan/uavcan_servers.hpp

@@ -81,24 +81,10 @@ class UavcanServers
 {
 	static constexpr unsigned NumIfaces = 1;  // UAVCAN_STM32_NUM_IFACES
 
-	static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
-
-	static constexpr unsigned MaxCanFramesPerTransfer   =  63;
-
-	/**
-	 * This number is based on the worst case max number of frames per interface. With
-	 * MemPoolBlockSize set at 48 this is 6048 Bytes.
-	 *
-	 * The servers can be forced to use the primary interface only, this can be achieved simply by passing
-	 * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
-	 */
-	static constexpr unsigned QueuePoolSize =
-		(NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
-
 	static constexpr unsigned StackSize  = 6000;
 	static constexpr unsigned Priority  =  120;
 
-	typedef uavcan::SubNode<MemPoolSize> SubNode;
+	static constexpr unsigned VirtualIfaceBlockAllocationQuota =  80;
 
 public:
 	UavcanServers(uavcan::INode &main_node);
@@ -108,7 +94,7 @@ public:
 	static int      start(uavcan::INode &main_node);
 	static int      stop();
 
-	SubNode         &get_node() { return _subnode; }
+	uavcan::SubNode<> &get_node() { return _subnode; }
 
 	static UavcanServers *instance() { return _instance; }
 
@@ -131,12 +117,10 @@ private:
 
 	static UavcanServers	*_instance;            ///< singleton pointer
 
-	typedef VirtualCanDriver<QueuePoolSize> vCanDriver;
+	VirtualCanDriver _vdriver;
 
-	vCanDriver    _vdriver;
-
-	uavcan::SubNode<MemPoolSize>  _subnode;   ///< library instance
-	uavcan::INode                &_main_node; ///< library instance
+	uavcan::SubNode<>  _subnode;
+	uavcan::INode      &_main_node;
 
 	uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
 	uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;

src/modules/uavcan/uavcan_virtual_can_driver.hpp

@@ -329,11 +329,7 @@ public:
 /**
  * Objects of this class are owned by the sub-node thread.
  * This class does not use heap memory.
- * @tparam SharedMemoryPoolSize         Amount of memory, in bytes, that will be statically allocated for the
- *                                      memory pool that will be shared across all interfaces for RX/TX queues.
- *                                      Typically this value should be no less than 4K per interface.
  */
-template <unsigned SharedMemoryPoolSize>
 class VirtualCanDriver : public uavcan::ICanDriver,
 	public uavcan::IRxFrameListener,
 	public ITxQueueInjector,
@@ -402,7 +398,7 @@ class VirtualCanDriver : public uavcan::ICanDriver,
 
 	Event event_;               ///< Used to unblock the select() call when IO happens.
 	pthread_mutex_t driver_mutex_;    ///< Shared across all ifaces
-	uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_;   ///< Shared across all ifaces
+	uavcan::IPoolAllocator& allocator_;   ///< Shared across all ifaces
 	uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
 	const unsigned num_ifaces_;
 	uavcan::ISystemClock &clock_;
@@ -476,7 +472,11 @@ class VirtualCanDriver : public uavcan::ICanDriver,
 	}
 
 public:
-	VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) :
+	VirtualCanDriver(unsigned arg_num_ifaces,
+		         uavcan::ISystemClock &system_clock,
+		         uavcan::IPoolAllocator& allocator,
+		         unsigned virtual_iface_block_allocation_quota) :
+		allocator_(allocator),
 		num_ifaces_(arg_num_ifaces),
 		clock_(system_clock)
 	{
@@ -485,11 +485,7 @@ public:
 
 		assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
 
-		const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_;
-		const unsigned quota_per_queue = quota_per_iface;             // 2x overcommit
-
-		UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
-			     unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue));
+		const unsigned quota_per_queue = virtual_iface_block_allocation_quota;             // 2x overcommit
 
 		for (unsigned i = 0; i < num_ifaces_; i++) {
 			ifaces_[i].template construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &,