move uavcan from modules to drivers

src/drivers/uavcan/uavcan_virtual_can_driver.hpp

@@ -0,0 +1,512 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *               Pavel Kirienko <pavel.kirienko@zubax.com>
+ *               David Sidrane <david_s5@nscdg.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <nuttx/config.h>
+
+#include <cstdlib>
+#include <cstdint>
+#include <cstring>
+#include <cstdio>
+#include <fcntl.h>
+#include <pthread.h>
+#include <semaphore.h>
+#include <debug.h>
+
+#include <uavcan/node/sub_node.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+
+/*
+ * General purpose wrapper around os's mutual exclusion
+ * mechanism.
+ *
+ * It supports the
+ *
+ * Note the naming of thier_mutex_ implies that the underlying
+ * mutex is owned by class using the Lock.
+ * and this wrapper provides service to initialize and de initialize
+ * the mutex.
+ */
+class Lock
+{
+	pthread_mutex_t &thier_mutex_;
+
+public:
+
+	Lock(pthread_mutex_t &m) :
+		thier_mutex_(m)
+	{
+		(void)pthread_mutex_lock(&m);
+	}
+
+	~Lock()
+	{
+		(void)pthread_mutex_unlock(&thier_mutex_);
+	}
+
+	static int init(pthread_mutex_t &thier_mutex_)
+	{
+		return pthread_mutex_init(&thier_mutex_, NULL);
+	}
+
+	static int deinit(pthread_mutex_t &thier_mutex_)
+	{
+		return pthread_mutex_destroy(&thier_mutex_);
+	}
+
+};
+
+/**
+ * Generic queue based on the linked list class defined in libuavcan.
+ * This class does not use heap memory.
+ */
+template <typename T>
+class Queue
+{
+	struct Item : public uavcan::LinkedListNode<Item> {
+		T payload;
+
+		template <typename... Args>
+		Item(Args... args) : payload(args...) { }
+	};
+
+	uavcan::LimitedPoolAllocator allocator_;
+	uavcan::LinkedListRoot<Item> list_;
+
+public:
+	Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) :
+		allocator_(arg_allocator, block_allocation_quota)
+	{
+		uavcan::IsDynamicallyAllocatable<Item>::check();
+	}
+
+	~Queue()
+	{
+		while (!isEmpty()) {
+			pop();
+		}
+	}
+
+	bool isEmpty() const { return list_.isEmpty(); }
+
+	/**
+	 * Creates one item in-place at the end of the list.
+	 * Returns true if the item was appended successfully, false if there's not enough memory.
+	 * Complexity is O(N) where N is queue length.
+	 */
+	template <typename... Args>
+	bool tryEmplace(Args... args)
+	{
+		// Allocating memory
+		void *const ptr = allocator_.allocate(sizeof(Item));
+
+		if (ptr == nullptr) {
+			return false;
+		}
+
+		// Constructing the new item
+		Item *const item = new (ptr) Item(args...);
+		assert(item != nullptr);
+
+		// Inserting the new item at the end of the list
+		Item *p = list_.get();
+
+		if (p == nullptr) {
+			list_.insert(item);
+
+		} else {
+			while (p->getNextListNode() != nullptr) {
+				p = p->getNextListNode();
+			}
+
+			assert(p->getNextListNode() == nullptr);
+			p->setNextListNode(item);
+			assert(p->getNextListNode()->getNextListNode() == nullptr);
+		}
+
+		return true;
+	}
+
+	/**
+	 * Accesses the first element.
+	 * Nullptr will be returned if the queue is empty.
+	 * Complexity is O(1).
+	 */
+	T       *peek()       { return isEmpty() ? nullptr : &list_.get()->payload; }
+	const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
+
+	/**
+	 * Removes the first element.
+	 * If the queue is empty, nothing will be done and assertion failure will be triggered.
+	 * Complexity is O(1).
+	 */
+	void pop()
+	{
+		Item *const item = list_.get();
+		assert(item != nullptr);
+
+		if (item != nullptr) {
+			list_.remove(item);
+			item->~Item();
+			allocator_.deallocate(item);
+		}
+	}
+};
+
+/**
+ * Objects of this class are owned by the sub-node thread.
+ * This class does not use heap memory.
+ */
+class VirtualCanIface : public uavcan::ICanIface,
+	uavcan::Noncopyable
+{
+	/**
+	 * This class re-defines uavcan::RxCanFrame with flags.
+	 * Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded,
+	 * rendering this class unusable with Queue<>.
+	 */
+	struct RxItem: public uavcan::CanFrame {
+		const uavcan::MonotonicTime ts_mono;
+		const uavcan::UtcTime ts_utc;
+		const uavcan::CanIOFlags flags;
+		const uint8_t iface_index;
+
+		RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) :
+			uavcan::CanFrame(arg_frame),
+			ts_mono(arg_frame.ts_mono),
+			ts_utc(arg_frame.ts_utc),
+			flags(arg_flags),
+			iface_index(arg_frame.iface_index)
+		{
+			// Making sure it will fit into a pool block with a pointer prefix
+			static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee");
+		}
+	};
+
+	pthread_mutex_t &common_driver_mutex_;
+
+	uavcan::CanTxQueue prioritized_tx_queue_;
+	Queue<RxItem> rx_queue_;
+
+	int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
+	{
+		Lock lock(common_driver_mutex_);
+		prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
+		return 1;
+	}
+
+	int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
+			uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override
+	{
+		Lock lock(common_driver_mutex_);
+
+		if (rx_queue_.isEmpty()) {
+			return 0;
+		}
+
+		const auto item = *rx_queue_.peek();
+		rx_queue_.pop();
+
+		out_frame = item;
+		out_ts_monotonic = item.ts_mono;
+		out_ts_utc = item.ts_utc;
+		out_flags = item.flags;
+
+		return 1;
+	}
+
+	int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; }
+	uint16_t getNumFilters() const override { return 0; }
+	uint64_t getErrorCount() const override { return 0; }
+
+public:
+	VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock,
+			pthread_mutex_t &arg_mutex, unsigned quota_per_queue) :
+		common_driver_mutex_(arg_mutex),
+		prioritized_tx_queue_(allocator, clock, quota_per_queue),
+		rx_queue_(allocator, quota_per_queue)
+	{
+	}
+
+	~VirtualCanIface()
+	{
+	}
+
+	/**
+	 * Note that RX queue overwrites oldest items when overflowed.
+	 * Call this from the main thread only.
+	 * No additional locking is required.
+	 */
+	void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags)
+	{
+		Lock lock(common_driver_mutex_);
+
+		if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) {
+			rx_queue_.pop();
+			(void)rx_queue_.tryEmplace(frame, flags);
+		}
+	}
+
+	/**
+	 * Call this from the main thread only.
+	 * No additional locking is required.
+	 */
+	void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index)
+	{
+		Lock lock(common_driver_mutex_);
+		const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
+
+		while (auto e = prioritized_tx_queue_.peek()) {
+			UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
+				     unsigned(iface_mask), e->toString().c_str());
+
+			const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
+								uavcan::CanTxQueue::Qos(e->qos), e->flags);
+
+			prioritized_tx_queue_.remove(e);
+
+			if (res <= 0) {
+				break;
+			}
+
+		}
+	}
+
+	/**
+	 * Call this from the sub-node thread only.
+	 * No additional locking is required.
+	 */
+	bool hasDataInRxQueue()
+	{
+		Lock lock(common_driver_mutex_);
+		return !rx_queue_.isEmpty();
+	}
+};
+
+/**
+ * This interface defines one method that will be called by the main node thread periodically in order to
+ * transfer contents of TX queue of the sub-node into the TX queue of the main node.
+ */
+class ITxQueueInjector
+{
+public:
+	virtual ~ITxQueueInjector() { }
+
+	/**
+	 * Flush contents of TX queues into the main node.
+	 * @param main_node         Reference to the main node.
+	 */
+	virtual void injectTxFramesInto(uavcan::INode &main_node) = 0;
+};
+
+/**
+ * Objects of this class are owned by the sub-node thread.
+ * This class does not use heap memory.
+ */
+class VirtualCanDriver : public uavcan::ICanDriver,
+	public uavcan::IRxFrameListener,
+	public ITxQueueInjector,
+	uavcan::Noncopyable
+{
+	class Event
+	{
+		FAR px4_sem_t sem;
+
+
+	public:
+
+		int init()
+		{
+			int rv = px4_sem_init(&sem, 0, 0);
+
+			if (rv == 0) {
+				px4_sem_setprotocol(&sem, SEM_PRIO_NONE);
+			}
+
+			return rv;
+		}
+
+		int deinit()
+		{
+			return px4_sem_destroy(&sem);
+		}
+
+
+		Event()
+		{
+		}
+
+		~Event()
+		{
+		}
+
+
+		/**
+		 */
+
+		void waitFor(uavcan::MonotonicDuration duration)
+		{
+			static const int NsPerSec = 1000000000;
+
+			if (duration.isPositive()) {
+				auto abstime = ::timespec();
+
+				if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) {
+					abstime.tv_nsec += duration.toUSec() * 1000;
+
+					if (abstime.tv_nsec >= NsPerSec) {
+						abstime.tv_sec++;
+						abstime.tv_nsec -= NsPerSec;
+					}
+
+					(void)px4_sem_timedwait(&sem, &abstime);
+				}
+			}
+		}
+
+		void signal()
+		{
+			int count;
+			int rv = px4_sem_getvalue(&sem, &count);
+
+			if (rv > 0 && count <= 0) {
+				px4_sem_post(&sem);
+			}
+		}
+	};
+
+	Event event_;                     ///< Used to unblock the select() call when IO happens.
+	pthread_mutex_t driver_mutex_;    ///< Shared across all ifaces
+	uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
+	const unsigned num_ifaces_;
+	uavcan::ISystemClock &clock_;
+
+	uavcan::ICanIface *getIface(uint8_t iface_index) override
+	{
+		return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr;
+	}
+
+	uint8_t getNumIfaces() const override { return num_ifaces_; }
+
+	/**
+	 * This and other methods of ICanDriver will be invoked by the sub-node thread.
+	 */
+	int16_t select(uavcan::CanSelectMasks &inout_masks,
+		       const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
+		       uavcan::MonotonicTime blocking_deadline) override
+	{
+		bool need_block = (inout_masks.write == 0);    // Write queue is infinite
+
+		for (unsigned i = 0; need_block && (i < num_ifaces_); i++) {
+			const bool need_read = inout_masks.read & (1U << i);
+
+			if (need_read && ifaces_[i]->hasDataInRxQueue()) {
+				need_block = false;
+			}
+		}
+
+		if (need_block) {
+			event_.waitFor(blocking_deadline - clock_.getMonotonic());
+		}
+
+		inout_masks = uavcan::CanSelectMasks();
+
+		for (unsigned i = 0; i < num_ifaces_; i++) {
+			const std::uint8_t iface_mask = 1U << i;
+			inout_masks.write |= iface_mask;           // Always ready to write
+
+			if (ifaces_[i]->hasDataInRxQueue()) {
+				inout_masks.read |= iface_mask;
+			}
+		}
+
+		return num_ifaces_;       // We're always ready to write, hence > 0.
+	}
+
+	/**
+	 * This handler will be invoked by the main node thread.
+	 */
+	void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
+	{
+		UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
+
+		if (frame.iface_index < num_ifaces_) {
+			ifaces_[frame.iface_index]->addRxFrame(frame, flags);
+			event_.signal();
+
+		}
+	}
+
+	/**
+	 * This method will be invoked by the main node thread.
+	 */
+	void injectTxFramesInto(uavcan::INode &main_node) override
+	{
+		for (unsigned i = 0; i < num_ifaces_; i++) {
+			ifaces_[i]->flushTxQueueTo(main_node, i);
+		}
+
+		event_.signal();
+	}
+
+public:
+	VirtualCanDriver(unsigned arg_num_ifaces,
+			 uavcan::ISystemClock &system_clock,
+			 uavcan::IPoolAllocator &allocator,
+			 unsigned virtual_iface_block_allocation_quota) :
+		num_ifaces_(arg_num_ifaces),
+		clock_(system_clock)
+	{
+		Lock::init(driver_mutex_);
+		event_.init();
+
+		assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
+
+		const unsigned quota_per_queue = virtual_iface_block_allocation_quota;             // 2x overcommit
+
+		for (unsigned i = 0; i < num_ifaces_; i++) {
+			ifaces_[i].construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &, pthread_mutex_t &,
+				unsigned>(allocator, clock_, driver_mutex_, quota_per_queue);
+		}
+	}
+
+	~VirtualCanDriver()
+	{
+		Lock::deinit(driver_mutex_);
+		event_.deinit();
+	}
+
+};