From df056a7948bc15ca045713b76c245acf6cf0b18a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 13 Oct 2015 15:29:15 +0300 Subject: [PATCH] LPC11C24 acceptance filters --- libuavcan_drivers/lpc11c24/driver/src/can.cpp | 160 ++++++++++++------ .../test_olimex_lpc_p11c24/src/main.cpp | 52 +++++- 2 files changed, 158 insertions(+), 54 deletions(-) diff --git a/libuavcan_drivers/lpc11c24/driver/src/can.cpp b/libuavcan_drivers/lpc11c24/driver/src/can.cpp index f77b4ee89e..405588eb76 100644 --- a/libuavcan_drivers/lpc11c24/driver/src/can.cpp +++ b/libuavcan_drivers/lpc11c24/driver/src/can.cpp @@ -271,59 +271,57 @@ uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)()) int CanDriver::init(uavcan::uint32_t bitrate) { - CriticalSectionLocker locker; + { + auto bit_timings = computeBitTimings(bitrate); + if (!bit_timings.isValid()) + { + return -1; + } - error_cnt = 0; - tx_pending = false; - last_irq_utc_timestamp = 0; - had_activity = false; + CriticalSectionLocker locker; + + error_cnt = 0; + tx_abort_on_error = false; + tx_pending = false; + last_irq_utc_timestamp = 0; + had_activity = false; + + /* + * C_CAN init + */ + Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN); + + LPC_CCAN_API->init_can(reinterpret_cast(&bit_timings), true); + + static CCAN_CALLBACKS_T ccan_callbacks = + { + canRxCallback, + canTxCallback, + canErrorCallback, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr + }; + LPC_CCAN_API->config_calb(&ccan_callbacks); + + /* + * Interrupts + */ + c_can::CAN.CNTL |= c_can::CNTL_SIE; // This is necessary for transmission aborts on error + + NVIC_EnableIRQ(CAN_IRQn); + } /* - * C_CAN init + * Applying default filter configuration (accept all) */ - Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN); - - auto bit_timings = computeBitTimings(bitrate); - if (!bit_timings.isValid()) + if (configureFilters(nullptr, 0) < 0) { return -1; } - LPC_CCAN_API->init_can(reinterpret_cast(&bit_timings), true); - - static CCAN_CALLBACKS_T ccan_callbacks = - { - canRxCallback, - canTxCallback, - canErrorCallback, - nullptr, - nullptr, - nullptr, - nullptr, - nullptr - }; - LPC_CCAN_API->config_calb(&ccan_callbacks); - - /* - * Default RX msgobj config: - * 31 - all STD - * 32 - all EXT - * RTR ignored - */ - CCAN_MSG_OBJ_T msg_obj = CCAN_MSG_OBJ_T(); - msg_obj.msgobj = 31; - LPC_CCAN_API->config_rxmsgobj(&msg_obj); - msg_obj.mode_id = CAN_MSGOBJ_EXT; - msg_obj.msgobj = 32; - LPC_CCAN_API->config_rxmsgobj(&msg_obj); - - /* - * Interrupts - */ - c_can::CAN.CNTL |= c_can::CNTL_SIE; // This is necessary for transmission aborts on error - - NVIC_EnableIRQ(CAN_IRQn); - return 0; } @@ -469,9 +467,73 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, uavcan::int16_t CanDriver::configureFilters(const uavcan::CanFilterConfig* filter_configs, uavcan::uint16_t num_configs) { - (void)filter_configs; - (void)num_configs; - return -1; + CriticalSectionLocker locker; + + /* + * If C_CAN is active (INIT=0) and the CAN bus has intensive traffic, RX object configuration may fail. + * The solution is to disable the controller while configuration is in progress. + * The documentation, as always, doesn't bother to mention this detail. Shame on you, NXP. + */ + struct RAIIDisabler + { + RAIIDisabler() + { + c_can::CAN.CNTL |= c_can::CNTL_INIT; + } + ~RAIIDisabler() + { + c_can::CAN.CNTL &= ~c_can::CNTL_INIT; + } + } can_disabler; // Must be instantiated AFTER the critical section locker + + if (num_configs == 0) + { + auto msg_obj = CCAN_MSG_OBJ_T(); + msg_obj.msgobj = NumberOfTxMessageObjects + 1; + LPC_CCAN_API->config_rxmsgobj(&msg_obj); // all STD frames + + msg_obj.mode_id = CAN_MSGOBJ_EXT; + msg_obj.msgobj = NumberOfTxMessageObjects + 2; + LPC_CCAN_API->config_rxmsgobj(&msg_obj); // all EXT frames + } + else if (num_configs <= NumberOfRxMessageObjects) + { + // Making sure the configs use only EXT frames; otherwise we can't accept them + for (unsigned i = 0; i < num_configs; i++) + { + auto& f = filter_configs[i]; + if ((f.id & f.mask & uavcan::CanFrame::FlagEFF) == 0) + { + return -1; + } + } + + // Installing the configuration + for (unsigned i = 0; i < NumberOfRxMessageObjects; i++) + { + auto msg_obj = CCAN_MSG_OBJ_T(); + msg_obj.msgobj = std::uint8_t(i + 1U + NumberOfTxMessageObjects); // Message objects are numbered from 1 + + if (i < num_configs) + { + msg_obj.mode_id = (filter_configs[i].id & uavcan::CanFrame::MaskExtID) | CAN_MSGOBJ_EXT; // Only EXT + msg_obj.mask = filter_configs[i].mask & uavcan::CanFrame::MaskExtID; + } + else + { + msg_obj.mode_id = CAN_MSGOBJ_RTR; // Using this configuration to disable the object + msg_obj.mask = uavcan::CanFrame::MaskStdID; + } + + LPC_CCAN_API->config_rxmsgobj(&msg_obj); + } + } + else + { + return -1; + } + + return 0; } uavcan::uint64_t CanDriver::getErrorCount() const @@ -507,7 +569,7 @@ void canRxCallback(std::uint8_t msg_obj_num) { using namespace uavcan_lpc11c24; - CCAN_MSG_OBJ_T msg_obj = CCAN_MSG_OBJ_T(); + auto msg_obj = CCAN_MSG_OBJ_T(); msg_obj.msgobj = msg_obj_num; LPC_CCAN_API->can_receive(&msg_obj); diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp index dbc0c1b694..a935b600ca 100644 --- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp @@ -65,7 +65,7 @@ uavcan::Logger& getLogger() } #if __GNUC__ -__attribute__((noinline)) +__attribute__((noinline, optimize(2))) // Higher optimization breaks the code. #endif void init() { @@ -97,6 +97,7 @@ void init() { board::die(); } + board::syslog("CAN init ok\r\n"); board::resetWatchdog(); @@ -126,6 +127,38 @@ void init() */ getNode().setNodeID(72); // TODO + /* + * Example filter configuration. + * Can be removed safely. + */ + { + constexpr unsigned NumFilters = 3; + uavcan::CanFilterConfig filters[NumFilters]; + + // Acepting all service transfers addressed to us + filters[0].id = (unsigned(getNode().getNodeID().get()) << 8) | (1U << 7) | uavcan::CanFrame::FlagEFF; + filters[0].mask = 0x7F80 | uavcan::CanFrame::FlagEFF; + + // Accepting time sync messages + filters[1].id = (4U << 8) | uavcan::CanFrame::FlagEFF; + filters[1].mask = 0xFFFF80 | uavcan::CanFrame::FlagEFF; + + // Accepting zero CAN ID (just for the sake of testing) + filters[2].id = 0 | uavcan::CanFrame::FlagEFF; + filters[2].mask = uavcan::CanFrame::MaskExtID | uavcan::CanFrame::FlagEFF; + + const auto before = uavcan_lpc11c24::clock::getMonotonic(); + if (uavcan_lpc11c24::CanDriver::instance().configureFilters(filters, NumFilters) < 0) + { + board::syslog("Filter init failed\r\n"); + board::die(); + } + const auto duration = uavcan_lpc11c24::clock::getMonotonic() - before; + board::syslog("CAN filter configuration took "); + board::syslog(intToString(duration.toUSec()).c_str()); + board::syslog(" usec\r\n"); + } + /* * Starting the node */ @@ -167,19 +200,28 @@ int main() if ((ts - prev_log_at).toMSec() >= 1000) { prev_log_at = ts; - // CAN bus off state monitoring + + /* + * CAN bus off state monitoring + */ if (uavcan_lpc11c24::CanDriver::instance().isInBusOffState()) { board::syslog("CAN BUS OFF\r\n"); } - // CAN error counter, for debugging purposes + + /* + * CAN error counter, for debugging purposes + */ board::syslog("CAN errors: "); board::syslog(intToString(static_cast(uavcan_lpc11c24::CanDriver::instance().getErrorCount())).c_str()); board::syslog(" "); board::syslog(intToString(uavcan_lpc11c24::CanDriver::instance().getRxQueueOverflowCount()).c_str()); board::syslog("\r\n"); - // We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf() - // hence we need to construct the message manually: + + /* + * We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf(), + * so we need to construct the message manually: + */ uavcan::protocol::debug::LogMessage logmsg; logmsg.level.value = uavcan::protocol::debug::LogLevel::INFO; logmsg.source = "app";