uavcan_virtual_can_driver.hpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   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();
    }

};