/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2014 Pavel Kirienko
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

/*
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Code by Siddharth Bharat Purohit
 */

#include <hal.h>
#include "AP_HAL_ChibiOS.h"

#if HAL_NUM_CAN_IFACES
#include <cassert>
#include <cstring>
#include <AP_Math/AP_Math.h>
# include <hal.h>
#include <AP_CANManager/AP_CANManager.h>
#include <AP_Common/ExpandingString.h>

# if !defined(STM32H7XX) && !defined(STM32G4)
#include "CANIface.h"

/* STM32F3's only CAN inteface does not have a number. */
#if defined(STM32F3XX)
#define RCC_APB1ENR_CAN1EN     RCC_APB1ENR_CANEN
#define RCC_APB1RSTR_CAN1RST   RCC_APB1RSTR_CANRST
#define CAN1_TX_IRQn           CAN_TX_IRQn
#define CAN1_RX0_IRQn          CAN_RX0_IRQn
#define CAN1_RX1_IRQn          CAN_RX1_IRQn
#define CAN1_TX_IRQ_Handler      STM32_CAN1_TX_HANDLER
#define CAN1_RX0_IRQ_Handler     STM32_CAN1_RX0_HANDLER
#define CAN1_RX1_IRQ_Handler     STM32_CAN1_RX1_HANDLER
#else
#define CAN1_TX_IRQ_Handler      STM32_CAN1_TX_HANDLER
#define CAN1_RX0_IRQ_Handler     STM32_CAN1_RX0_HANDLER
#define CAN1_RX1_IRQ_Handler     STM32_CAN1_RX1_HANDLER
#define CAN2_TX_IRQ_Handler      STM32_CAN2_TX_HANDLER
#define CAN2_RX0_IRQ_Handler     STM32_CAN2_RX0_HANDLER
#define CAN2_RX1_IRQ_Handler     STM32_CAN2_RX1_HANDLER
#endif // #if defined(STM32F3XX)

#if HAL_CANMANAGER_ENABLED
#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANIface", fmt, ##args); } while (0)
#else
#define Debug(fmt, args...)
#endif

#if !defined(HAL_BOOTLOADER_BUILD)
#define PERF_STATS(x) (x++)
#else
#define PERF_STATS(x)
#endif


extern const AP_HAL::HAL& hal;

using namespace ChibiOS;

constexpr bxcan::CanType* const CANIface::Can[];
static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES];

uint8_t CANIface::next_interface;

// mapping from logical interface to physical. First physical is 0, first logical is 0
static constexpr uint8_t can_interfaces[HAL_NUM_CAN_IFACES] = { HAL_CAN_INTERFACE_LIST };

// mapping from physical interface back to logical. First physical is 0, first logical is 0
static constexpr int8_t can_iface_to_idx[3] = { HAL_CAN_INTERFACE_REV_LIST };

static inline void handleTxInterrupt(uint8_t phys_index)
{
    const int8_t iface_index = can_iface_to_idx[phys_index];
    if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) {
        return;
    }
    uint64_t precise_time = AP_HAL::micros64();
    if (precise_time > 0) {
        precise_time--;
    }
    if (can_ifaces[iface_index] != nullptr) {
        can_ifaces[iface_index]->handleTxInterrupt(precise_time);
    }
}

static inline void handleRxInterrupt(uint8_t phys_index, uint8_t fifo_index)
{
    const int8_t iface_index = can_iface_to_idx[phys_index];
    if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) {
        return;
    }
    uint64_t precise_time = AP_HAL::micros64();
    if (precise_time > 0) {
        precise_time--;
    }
    if (can_ifaces[iface_index] != nullptr) {
        can_ifaces[iface_index]->handleRxInterrupt(fifo_index, precise_time);
    }
}

/*
 * CANIface
 */
const uint32_t CANIface::TSR_ABRQx[CANIface::NumTxMailboxes] = {
    bxcan::TSR_ABRQ0,
    bxcan::TSR_ABRQ1,
    bxcan::TSR_ABRQ2
};


CANIface::CANIface(uint8_t index) :
    rx_bytebuffer_((uint8_t*)rx_buffer, sizeof(rx_buffer)),
    rx_queue_(&rx_bytebuffer_),
    self_index_(index)
{
    if (index >= HAL_NUM_CAN_IFACES) {
        AP_HAL::panic("Bad CANIface index.");
    } else {
        can_ = Can[index];
    }
}

// constructor suitable for array
CANIface::CANIface() :
    CANIface(next_interface++)
{}

bool CANIface::computeTimings(uint32_t target_bitrate, Timings& out_timings)
{
    if (target_bitrate < 1) {
        return false;
    }

    /*
     * Hardware configuration
     */
    const uint32_t pclk = STM32_PCLK1;

    static const int MaxBS1 = 16;
    static const int MaxBS2 = 8;

    /*
     * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG
     *      CAN in Automation, 2003
     *
     * According to the source, optimal quanta per bit are:
     *   Bitrate        Optimal Maximum
     *   1000 kbps      8       10
     *   500  kbps      16      17
     *   250  kbps      16      17
     *   125  kbps      16      17
     */
    const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17;

    static const int MaxSamplePointLocation = 900;

    /*
     * Computing (prescaler * BS):
     *   BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2))       -- See the Reference Manual
     *   BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2))                 -- Simplified
     * let:
     *   BS = 1 + BS1 + BS2                                             -- Number of time quanta per bit
     *   PRESCALER_BS = PRESCALER * BS
     * ==>
     *   PRESCALER_BS = PCLK / BITRATE
     */
    const uint32_t prescaler_bs = pclk / target_bitrate;

    /*
     * Searching for such prescaler value so that the number of quanta per bit is highest.
     */
    uint8_t bs1_bs2_sum = uint8_t(max_quanta_per_bit - 1);

    while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) {
        if (bs1_bs2_sum <= 2) {
            return false;          // No solution
        }
        bs1_bs2_sum--;
    }

    const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
    if ((prescaler < 1U) || (prescaler > 1024U)) {
        return false;              // No solution
    }

    /*
     * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum.
     * We need to find the values so that the sample point is as close as possible to the optimal value.
     *
     *   Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2]  (* Where 7/8 is 0.875, the recommended sample point location *)
     *   {{bs2 -> (1 + bs1)/7}}
     *
     * Hence:
     *   bs2 = (1 + bs1) / 7
     *   bs1 = (7 * bs1_bs2_sum - 1) / 8
     *
     * Sample point location can be computed as follows:
     *   Sample point location = (1 + bs1) / (1 + bs1 + bs2)
     *
     * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one:
     *   - With rounding to nearest
     *   - With rounding to zero
     */
    struct BsPair {
        uint8_t bs1;
        uint8_t bs2;
        uint16_t sample_point_permill;

        BsPair() :
            bs1(0),
            bs2(0),
            sample_point_permill(0)
        { }

        BsPair(uint8_t bs1_bs2_sum, uint8_t arg_bs1) :
            bs1(arg_bs1),
            bs2(uint8_t(bs1_bs2_sum - bs1)),
            sample_point_permill(uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
        {}

        bool isValid() const
        {
            return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2);
        }
    };

    // First attempt with rounding to nearest
    BsPair solution(bs1_bs2_sum, uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8));

    if (solution.sample_point_permill > MaxSamplePointLocation) {
        // Second attempt with rounding to zero
        solution = BsPair(bs1_bs2_sum, uint8_t((7 * bs1_bs2_sum - 1) / 8));
    }

    /*
     * Final validation
     * Helpful Python:
     * def sample_point_from_btr(x):
     *     assert 0b0011110010000000111111000000000 & x == 0
     *     ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511
     *     return (1+ts1+1)/(1+ts1+1+ts2+1)
     *
     */
    if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) {
        return false;
    }

    Debug("Timings: quanta/bit: %d, sample point location: %.1f%%",
          int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F);

    out_timings.prescaler = uint16_t(prescaler - 1U);
    out_timings.sjw = 0;                                        // Which means one
    out_timings.bs1 = uint8_t(solution.bs1 - 1);
    out_timings.bs2 = uint8_t(solution.bs2 - 1);
    return true;
}

int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
                       CanIOFlags flags)
{
    if (frame.isErrorFrame() || frame.dlc > 8) {
        return -1;
    }
    PERF_STATS(stats.tx_requests);

    /*
     * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because
     * it is possible that the highest-priority frame between select() and send() could have been
     * replaced with a lower priority one due to TX timeout. But we don't do this check because:
     *
     *  - It is a highly unlikely scenario.
     *
     *  - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new
     *    frame can only have higher priority, which doesn't break the logic.
     *
     *  - If high-priority frames are timing out in the TX queue, there's probably a lot of other
     *    issues to take care of before this one becomes relevant.
     *
     *  - It takes CPU time. Not just CPU time, but critical section time, which is expensive.
     */

    {
        CriticalSectionLocker lock;

        /*
         * Seeking for an empty slot
         */
        uint8_t txmailbox = 0xFF;
        if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) {
            txmailbox = 0;
        } else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) {
            txmailbox = 1;
        } else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) {
            txmailbox = 2;
        } else {
            PERF_STATS(stats.tx_overflow);
#if !defined(HAL_BOOTLOADER_BUILD)
            if (stats.tx_success == 0) {
                /*
                  if we have never successfully transmitted a frame
                  then we may be operating with just MAVCAN or UDP
                  MCAST. Consider the frame sent if the send
                  succeeds. This allows for UDP MCAST and MAVCAN to
                  operate fully when the CAN bus has no cable plugged
                  in
                 */
                return AP_HAL::CANIface::send(frame, tx_deadline, flags);
            }
#endif
            return 0;       // No transmission for you.
        }

        /*
         * Setting up the mailbox
         */
        bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox];
        if (frame.isExtended()) {
            mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskExtID) << 3) | bxcan::TIR_IDE;
        } else {
            mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskStdID) << 21);
        }

        if (frame.isRemoteTransmissionRequest()) {
            mb.TIR |= bxcan::TIR_RTR;
        }

        mb.TDTR = frame.dlc;

        mb.TDHR = frame.data_32[1];
        mb.TDLR = frame.data_32[0];

        mb.TIR |= bxcan::TIR_TXRQ;  // Go.

        /*
         * Registering the pending transmission so we can track its deadline and loopback it as needed
         */
        CanTxItem& txi = pending_tx_[txmailbox];
        txi.deadline       = tx_deadline;
        txi.frame          = frame;
        txi.loopback       = (flags & Loopback) != 0;
        txi.abort_on_error = (flags & AbortOnError) != 0;
        // setup frame initial state
        txi.pushed         = false;
    }

    // also send on MAVCAN, but don't consider it an error if we can't send
    AP_HAL::CANIface::send(frame, tx_deadline, flags);

    return 1;
}

int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
{
    {
        CriticalSectionLocker lock;
        CanRxItem rx_item;
        if (!rx_queue_.pop(rx_item)) {
            return 0;
        }
        out_frame    = rx_item.frame;
        out_timestamp_us = rx_item.timestamp_us;
        out_flags    = rx_item.flags;
    }

    return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
}

#if !defined(HAL_BOOTLOADER_BUILD)
bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
                                uint16_t num_configs)
{
#if !defined(HAL_BUILD_AP_PERIPH)
    // only do filtering for AP_Periph
    can_->FMR &= ~bxcan::FMR_FINIT;
    return true;
#else
    if (mode_ != FilteredMode) {
        return false;
    }
    if (num_configs <= NumFilters && filter_configs != nullptr) {
        CriticalSectionLocker lock;

        can_->FMR |= bxcan::FMR_FINIT;

        // Slave (CAN2) gets half of the filters
        can_->FMR &= ~0x00003F00UL;
        can_->FMR |= static_cast<uint32_t>(NumFilters) << 8;

        can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters
        can_->FM1R = 0; // Identifier Mask mode
        can_->FS1R = 0x7ffffff; // Single 32-bit for all

        const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters;

        if (num_configs == 0) {
            can_->FilterRegister[filter_start_index].FR1 = 0;
            can_->FilterRegister[filter_start_index].FR2 = 0;
            can_->FA1R = 1 << filter_start_index;
        } else {
            for (uint8_t i = 0; i < NumFilters; i++) {
                if (i < num_configs) {
                    uint32_t id   = 0;
                    uint32_t mask = 0;

                    const CanFilterConfig* const cfg = filter_configs + i;

                    if ((cfg->id & AP_HAL::CANFrame::FlagEFF) || !(cfg->mask & AP_HAL::CANFrame::FlagEFF)) {
                        id   = (cfg->id   & AP_HAL::CANFrame::MaskExtID) << 3;
                        mask = (cfg->mask & AP_HAL::CANFrame::MaskExtID) << 3;
                        id |= bxcan::RIR_IDE;
                    } else {
                        id   = (cfg->id   & AP_HAL::CANFrame::MaskStdID) << 21;  // Regular std frames, nothing fancy.
                        mask = (cfg->mask & AP_HAL::CANFrame::MaskStdID) << 21;  // Boring.
                    }

                    if (cfg->id & AP_HAL::CANFrame::FlagRTR) {
                        id |= bxcan::RIR_RTR;
                    }

                    if (cfg->mask & AP_HAL::CANFrame::FlagEFF) {
                        mask |= bxcan::RIR_IDE;
                    }

                    if (cfg->mask & AP_HAL::CANFrame::FlagRTR) {
                        mask |= bxcan::RIR_RTR;
                    }

                    can_->FilterRegister[filter_start_index + i].FR1 = id;
                    can_->FilterRegister[filter_start_index + i].FR2 = mask;

                    can_->FA1R |= (1 << (filter_start_index + i));
                } else {
                    can_->FA1R &= ~(1 << (filter_start_index + i));
                }
            }
        }

        can_->FMR &= ~bxcan::FMR_FINIT;

        return true;
    }

    return false;
#endif // AP_Periph
}
#endif

bool CANIface::waitMsrINakBitStateChange(bool target_state)
{
    const unsigned Timeout = 1000;
    for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) {
        const bool state = (can_->MSR & bxcan::MSR_INAK) != 0;
        if (state == target_state) {
            return true;
        }
        chThdSleep(chTimeMS2I(1));
    }
    return false;
}

void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const uint64_t timestamp_us)
{
    if (mailbox_index > NumTxMailboxes) {
        return;
    }

    had_activity_ = had_activity_ || txok;

    CanTxItem& txi = pending_tx_[mailbox_index];

    if (txi.loopback && txok && !txi.pushed) {
        CanRxItem rx_item;
        rx_item.frame = txi.frame;
        rx_item.timestamp_us = timestamp_us;
        rx_item.flags = AP_HAL::CANIface::Loopback;
        add_to_rx_queue(rx_item);
    }

    if (txok && !txi.pushed) {
        txi.pushed = true;
        PERF_STATS(stats.tx_success);
#if !defined(HAL_BOOTLOADER_BUILD)
        stats.last_transmit_us = timestamp_us;
#endif
    }
}

void CANIface::handleTxInterrupt(const uint64_t utc_usec)
{
    // TXOK == false means that there was a hardware failure
    if (can_->TSR & bxcan::TSR_RQCP0) {
        const bool txok = can_->TSR & bxcan::TSR_TXOK0;
        can_->TSR = bxcan::TSR_RQCP0;
        handleTxMailboxInterrupt(0, txok, utc_usec);
    }
    if (can_->TSR & bxcan::TSR_RQCP1) {
        const bool txok = can_->TSR & bxcan::TSR_TXOK1;
        can_->TSR = bxcan::TSR_RQCP1;
        handleTxMailboxInterrupt(1, txok, utc_usec);
    }
    if (can_->TSR & bxcan::TSR_RQCP2) {
        const bool txok = can_->TSR & bxcan::TSR_TXOK2;
        can_->TSR = bxcan::TSR_RQCP2;
        handleTxMailboxInterrupt(2, txok, utc_usec);
    }

    if (sem_handle != nullptr) {
        PERF_STATS(stats.num_events);
        sem_handle->signal_ISR();
    }

    pollErrorFlagsFromISR();
}

void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
{
    volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R;
    if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) {
        return;
    }

    /*
     * Register overflow as a hardware error
     */
    if ((*rfr_reg & bxcan::RFR_FOVR) != 0) {
        PERF_STATS(stats.rx_errors);
    }

    /*
     * Read the frame contents
     */
    AP_HAL::CANFrame &frame = isr_rx_frame;
    const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index];

    if ((rf.RIR & bxcan::RIR_IDE) == 0) {
        frame.id = AP_HAL::CANFrame::MaskStdID & (rf.RIR >> 21);
    } else {
        frame.id = AP_HAL::CANFrame::MaskExtID & (rf.RIR >> 3);
        frame.id |= AP_HAL::CANFrame::FlagEFF;
    }

    if ((rf.RIR & bxcan::RIR_RTR) != 0) {
        frame.id |= AP_HAL::CANFrame::FlagRTR;
    }

    frame.dlc = rf.RDTR & 15;

    frame.data[0] = uint8_t(0xFF & (rf.RDLR >> 0));
    frame.data[1] = uint8_t(0xFF & (rf.RDLR >> 8));
    frame.data[2] = uint8_t(0xFF & (rf.RDLR >> 16));
    frame.data[3] = uint8_t(0xFF & (rf.RDLR >> 24));
    frame.data[4] = uint8_t(0xFF & (rf.RDHR >> 0));
    frame.data[5] = uint8_t(0xFF & (rf.RDHR >> 8));
    frame.data[6] = uint8_t(0xFF & (rf.RDHR >> 16));
    frame.data[7] = uint8_t(0xFF & (rf.RDHR >> 24));

    *rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL;  // Release FIFO entry we just read

    /*
     * Store with timeout into the FIFO buffer and signal update event
     */
    CanRxItem &rx_item = isr_rx_item;
    rx_item.frame = frame;
    rx_item.timestamp_us = timestamp_us;
    rx_item.flags = 0;
    if (add_to_rx_queue(rx_item)) {
        PERF_STATS(stats.rx_received);
    } else {
        PERF_STATS(stats.rx_overflow);
    }

    had_activity_ = true;

    if (sem_handle != nullptr) {
        PERF_STATS(stats.num_events);
        sem_handle->signal_ISR();
    }

    pollErrorFlagsFromISR();
}

void CANIface::pollErrorFlagsFromISR()
{
    const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
    if (lec != 0) {
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
        stats.esr = can_->ESR; // Record error status
#endif
        can_->ESR = 0;

        // Serving abort requests
        for (int i = 0; i < NumTxMailboxes; i++) {
            CanTxItem& txi = pending_tx_[i];
            if (txi.aborted && txi.abort_on_error) {
                can_->TSR = TSR_ABRQx[i];
                txi.aborted = true;
                PERF_STATS(stats.tx_abort);
            }
        }
    }
}

void CANIface::discardTimedOutTxMailboxes(uint64_t current_time)
{
    CriticalSectionLocker lock;
    for (int i = 0; i < NumTxMailboxes; i++) {
        CanTxItem& txi = pending_tx_[i];
        if (txi.aborted || !txi.setup) {
            continue;
        }
        if (txi.deadline < current_time) {
            can_->TSR = TSR_ABRQx[i];  // Goodnight sweet transmission
            pending_tx_[i].aborted = true;
            PERF_STATS(stats.tx_timedout);
        }
    }
}

void CANIface::clear_rx()
{
    CriticalSectionLocker lock;
    rx_queue_.clear();
}

void CANIface::pollErrorFlags()
{
    CriticalSectionLocker cs_locker;
    pollErrorFlagsFromISR();
}

bool CANIface::canAcceptNewTxFrame(const AP_HAL::CANFrame& frame) const
{
    /*
     * We can accept more frames only if the following conditions are satisfied:
     *  - There is at least one TX mailbox free (obvious enough);
     *  - The priority of the new frame is higher than priority of all TX mailboxes.
     */
    {
        static const uint32_t TME = bxcan::TSR_TME0 | bxcan::TSR_TME1 | bxcan::TSR_TME2;
        const uint32_t tme = can_->TSR & TME;

        if (tme == TME) {   // All TX mailboxes are free (as in freedom).
            return true;
        }

        if (tme == 0) {     // All TX mailboxes are busy transmitting.
            return false;
        }
    }

    /*
     * The second condition requires a critical section.
     */
    CriticalSectionLocker lock;

    for (int mbx = 0; mbx < NumTxMailboxes; mbx++) {
        if (!(pending_tx_[mbx].pushed || pending_tx_[mbx].aborted) && !frame.priorityHigherThan(pending_tx_[mbx].frame)) {
            return false;       // There's a mailbox whose priority is higher or equal the priority of the new frame.
        }
    }

    return true;                // This new frame will be added to a free TX mailbox in the next @ref send().
}

bool CANIface::isRxBufferEmpty() const
{
    CriticalSectionLocker lock;
    return rx_queue_.available() == 0;
}

#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t CANIface::getErrorCount() const
{
    CriticalSectionLocker lock;
    return stats.num_busoff_err +
           stats.rx_errors +
           stats.rx_overflow +
           stats.tx_rejected +
           stats.tx_abort +
           stats.tx_timedout;
}

#endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)

bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle)
{
    sem_handle = handle;
    return true;
}


void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const
{
    write = false;
    read = !isRxBufferEmpty();

    if (pending_tx != nullptr) {
        write = canAcceptNewTxFrame(*pending_tx);
    }
}

bool CANIface::select(bool &read, bool &write,
                      const AP_HAL::CANFrame* pending_tx,
                      uint64_t blocking_deadline)
{
    const bool in_read = read;
    const bool in_write= write;
    uint64_t time = AP_HAL::micros64();

    if (!read && !write) {
        //invalid request
        return false;
    }

    discardTimedOutTxMailboxes(time);              // Check TX timeouts - this may release some TX slots
    pollErrorFlags();

    checkAvailable(read, write, pending_tx);          // Check if we already have some of the requested events
    if ((read && in_read) || (write && in_write)) {
        return true;
    }

#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
    // we don't support blocking select in AP_Periph and bootloader
    while (time < blocking_deadline) {
        if (sem_handle == nullptr) {
            break;
        }
        IGNORE_RETURN(sem_handle->wait(blocking_deadline - time)); // Block until timeout expires or any iface updates
        checkAvailable(read, write, pending_tx);  // Check what we got
        if ((read && in_read) || (write && in_write)) {
            return true;
        }
        time = AP_HAL::micros64();
    }
#endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
    return true;
}

void CANIface::initOnce(bool enable_irq)
{
    /*
     * CAN1, CAN2
     */
    {
        CriticalSectionLocker lock;
        switch (can_interfaces[self_index_]) {
        case 0:
#if defined(RCC_APB1ENR1_CAN1EN)
            RCC->APB1ENR1 |=  RCC_APB1ENR1_CAN1EN;
            RCC->APB1RSTR1 |=  RCC_APB1RSTR1_CAN1RST;
            RCC->APB1RSTR1 &= ~RCC_APB1RSTR1_CAN1RST;
#else
            RCC->APB1ENR  |=  RCC_APB1ENR_CAN1EN;
            RCC->APB1RSTR |=  RCC_APB1RSTR_CAN1RST;
            RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST;
#endif
            break;
#if defined(RCC_APB1ENR1_CAN2EN)
        case 1:
            RCC->APB1ENR1  |=  RCC_APB1ENR1_CAN2EN;
            RCC->APB1RSTR1 |=  RCC_APB1RSTR1_CAN2RST;
            RCC->APB1RSTR1 &= ~RCC_APB1RSTR1_CAN2RST;
            break;
#elif defined(RCC_APB1ENR_CAN2EN)
        case 1:
            RCC->APB1ENR  |=  RCC_APB1ENR_CAN2EN;
            RCC->APB1RSTR |=  RCC_APB1RSTR_CAN2RST;
            RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST;
            break;
#endif
#ifdef RCC_APB1ENR_CAN3EN
        case 2:
            RCC->APB1ENR  |=  RCC_APB1ENR_CAN3EN;
            RCC->APB1RSTR |=  RCC_APB1RSTR_CAN3RST;
            RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN3RST;
            break;
#endif
        }
    }

    /*
     * IRQ
     */
    if (!irq_init_ && enable_irq) {
        CriticalSectionLocker lock;
        switch (can_interfaces[self_index_]) {
        case 0:
#ifdef HAL_CAN_IFACE1_ENABLE
            nvicEnableVector(CAN1_TX_IRQn,  CORTEX_MAX_KERNEL_PRIORITY);
            nvicEnableVector(CAN1_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
            nvicEnableVector(CAN1_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
#endif
            break;
        case 1:
#ifdef HAL_CAN_IFACE2_ENABLE
            nvicEnableVector(CAN2_TX_IRQn,  CORTEX_MAX_KERNEL_PRIORITY);
            nvicEnableVector(CAN2_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
            nvicEnableVector(CAN2_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
#endif
            break;
        case 2:
#ifdef HAL_CAN_IFACE3_ENABLE
            nvicEnableVector(CAN3_TX_IRQn,  CORTEX_MAX_KERNEL_PRIORITY);
            nvicEnableVector(CAN3_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
            nvicEnableVector(CAN3_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
#endif
            break;
        }
        irq_init_ = true;
    }
}

bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
{
    Debug("Bitrate %lu mode %d", static_cast<unsigned long>(bitrate), static_cast<int>(mode));
    if (self_index_ > HAL_NUM_CAN_IFACES) {
        Debug("CAN drv init failed");
        return false;
    }
    if (can_ifaces[self_index_] == nullptr) {
        can_ifaces[self_index_] = this;
#if !defined(HAL_BOOTLOADER_BUILD)
        AP_HAL::get_HAL_mutable().can[self_index_] = this;
#endif
    }

    bitrate_ = bitrate;
    mode_ = mode;

    if (can_ifaces[0] == nullptr) {
        can_ifaces[0] = NEW_NOTHROW CANIface(0);
        Debug("Failed to allocate CAN iface 0");
        if (can_ifaces[0] == nullptr) {
            return false;
        }
    }
    if (self_index_ == 1 && !can_ifaces[0]->is_initialized()) {
        Debug("Iface 0 is not initialized yet but we need it for Iface 1, trying to init it");
        Debug("Enabling CAN iface 0");
        can_ifaces[0]->initOnce(false);
        Debug("Initing iface 0...");
        if (!can_ifaces[0]->init(bitrate, mode)) {
            Debug("Iface 0 init failed");
            return false;
        }

        Debug("Enabling CAN iface");
    }
    initOnce(true);
    /*
     * We need to silence the controller in the first order, otherwise it may interfere with the following operations.
     */
    {
        CriticalSectionLocker lock;

        can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode
        can_->MCR |= bxcan::MCR_INRQ;   // Request init

        can_->IER = 0;                  // Disable interrupts while initialization is in progress
    }

    if (!waitMsrINakBitStateChange(true)) {
        Debug("MSR INAK not set");
        can_->MCR = bxcan::MCR_RESET;
        return false;
    }

    /*
     * Object state - interrupts are disabled, so it's safe to modify it now
     */
    rx_queue_.clear();

    for (uint32_t i=0; i < NumTxMailboxes; i++) {
        pending_tx_[i] = CanTxItem();
    }
    had_activity_ = false;

    /*
     * CAN timings for this bitrate
     */
    Timings timings;
    if (!computeTimings(bitrate, timings)) {
        can_->MCR = bxcan::MCR_RESET;
        return false;
    }
    Debug("Timings: presc=%u sjw=%u bs1=%u bs2=%u",
          unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2));

    /*
     * Hardware initialization (the hardware has already confirmed initialization mode, see above)
     */
    can_->MCR = bxcan::MCR_ABOM | bxcan::MCR_AWUM | bxcan::MCR_INRQ;  // RM page 648

    can_->BTR = ((timings.sjw & 3U)  << 24) |
                ((timings.bs1 & 15U) << 16) |
                ((timings.bs2 & 7U)  << 20) |
                (timings.prescaler & 1023U) |
                ((mode == SilentMode) ? bxcan::BTR_SILM : 0);

    can_->IER = bxcan::IER_TMEIE |   // TX mailbox empty
                bxcan::IER_FMPIE0 |  // RX FIFO 0 is not empty
                bxcan::IER_FMPIE1;   // RX FIFO 1 is not empty

    can_->MCR &= ~bxcan::MCR_INRQ;   // Leave init mode

    if (!waitMsrINakBitStateChange(false)) {
        Debug("MSR INAK not cleared");
        can_->MCR = bxcan::MCR_RESET;
        return false;
    }

    /*
     * Default filter configuration
     */
    if (self_index_ == 0) {
        can_->FMR |= bxcan::FMR_FINIT;

        can_->FMR &= 0xFFFFC0F1;
        can_->FMR |= static_cast<uint32_t>(NumFilters) << 8;  // Slave (CAN2) gets half of the filters

        can_->FFA1R = 0;                           // All assigned to FIFO0 by default
        can_->FM1R = 0;                            // Indentifier Mask mode

#if HAL_NUM_CAN_IFACES > 1
        can_->FS1R = 0x7ffffff;                    // Single 32-bit for all
        can_->FilterRegister[0].FR1 = 0;          // CAN1 accepts everything
        can_->FilterRegister[0].FR2 = 0;
        can_->FilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything
        can_->FilterRegister[NumFilters].FR2 = 0;
        can_->FA1R = 1 | (1 << NumFilters);        // One filter per each iface
#else
        can_->FS1R = 0x1fff;
        can_->FilterRegister[0].FR1 = 0;
        can_->FilterRegister[0].FR2 = 0;
        can_->FA1R = 1;
#endif

        can_->FMR &= ~bxcan::FMR_FINIT;
    }
    initialised_ = true;

    return true;
}

#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
void CANIface::get_stats(ExpandingString &str)
{
    CriticalSectionLocker lock;
    str.printf("tx_requests:    %lu\n"
               "tx_rejected:    %lu\n"
               "tx_success:     %lu\n"
               "tx_timedout:    %lu\n"
               "tx_abort:       %lu\n"
               "rx_received:    %lu\n"
               "rx_overflow:    %lu\n"
               "rx_errors:      %lu\n"
               "num_busoff_err: %lu\n"
               "num_events:     %lu\n"
               "ESR:            %lx\n",
               stats.tx_requests,
               stats.tx_rejected,
               stats.tx_success,
               stats.tx_timedout,
               stats.tx_abort,
               stats.rx_received,
               stats.rx_overflow,
               stats.rx_errors,
               stats.num_busoff_err,
               stats.num_events,
               stats.esr);
}
#endif

/*
 * Interrupt handlers
 */
extern "C"
{
#ifdef HAL_CAN_IFACE1_ENABLE
    // CAN1
    CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler);
    CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleTxInterrupt(0);
        CH_IRQ_EPILOGUE();
    }

    CH_IRQ_HANDLER(CAN1_RX0_IRQ_Handler);
    CH_IRQ_HANDLER(CAN1_RX0_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleRxInterrupt(0, 0);
        CH_IRQ_EPILOGUE();
    }

    CH_IRQ_HANDLER(CAN1_RX1_IRQ_Handler);
    CH_IRQ_HANDLER(CAN1_RX1_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleRxInterrupt(0, 1);
        CH_IRQ_EPILOGUE();
    }
#endif

#ifdef HAL_CAN_IFACE2_ENABLE
    // CAN2
    CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler);
    CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleTxInterrupt(1);
        CH_IRQ_EPILOGUE();
    }

    CH_IRQ_HANDLER(CAN2_RX0_IRQ_Handler);
    CH_IRQ_HANDLER(CAN2_RX0_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleRxInterrupt(1, 0);
        CH_IRQ_EPILOGUE();
    }

    CH_IRQ_HANDLER(CAN2_RX1_IRQ_Handler);
    CH_IRQ_HANDLER(CAN2_RX1_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleRxInterrupt(1, 1);
        CH_IRQ_EPILOGUE();
    }
#endif

#ifdef HAL_CAN_IFACE3_ENABLE
    // CAN3
    CH_IRQ_HANDLER(CAN3_TX_IRQ_Handler);
    CH_IRQ_HANDLER(CAN3_TX_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleTxInterrupt(2);
        CH_IRQ_EPILOGUE();
    }

    CH_IRQ_HANDLER(CAN3_RX0_IRQ_Handler);
    CH_IRQ_HANDLER(CAN3_RX0_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleRxInterrupt(2, 0);
        CH_IRQ_EPILOGUE();
    }

    CH_IRQ_HANDLER(CAN3_RX1_IRQ_Handler);
    CH_IRQ_HANDLER(CAN3_RX1_IRQ_Handler)
    {
        CH_IRQ_PROLOGUE();
        handleRxInterrupt(2, 1);
        CH_IRQ_EPILOGUE();
    }
#endif
    
} // extern "C"

#endif //!defined(STM32H7XX)

#endif //HAL_NUM_CAN_IFACES