2020-05-31 09:32:19 -03:00
|
|
|
/*
|
|
|
|
This program 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 program 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/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Many thanks to members of the UAVCAN project:
|
|
|
|
* Pavel Kirienko <pavel.kirienko@gmail.com>
|
|
|
|
* Ilia Sheremet <illia.sheremet@gmail.com>
|
|
|
|
*
|
|
|
|
* license info can be found in the uavcan submodule located:
|
|
|
|
* modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_HAL/system.h>
|
|
|
|
|
|
|
|
#if HAL_NUM_CAN_IFACES
|
|
|
|
|
|
|
|
#include "CANSocketIface.h"
|
|
|
|
|
|
|
|
#include <unistd.h>
|
|
|
|
#include <fcntl.h>
|
|
|
|
|
|
|
|
#include <sys/ioctl.h>
|
|
|
|
#include <net/if.h>
|
|
|
|
#include <cstring>
|
|
|
|
#include "Scheduler.h"
|
|
|
|
#include <AP_CANManager/AP_CANManager.h>
|
2020-12-30 02:23:27 -04:00
|
|
|
#include <AP_Common/ExpandingString.h>
|
2023-08-25 21:52:28 -03:00
|
|
|
#include "CAN_Multicast.h"
|
|
|
|
#include "CAN_SocketCAN.h"
|
2020-12-30 02:23:27 -04:00
|
|
|
|
2020-05-31 09:32:19 -03:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
using namespace HALSITL;
|
|
|
|
|
2021-06-20 07:29:15 -03:00
|
|
|
#if HAL_CANMANAGER_ENABLED
|
2023-08-25 21:52:28 -03:00
|
|
|
#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANSITLIface", fmt, ##args); } while (0)
|
2020-09-12 16:04:38 -03:00
|
|
|
#else
|
|
|
|
#define Debug(fmt, args...)
|
|
|
|
#endif
|
2020-05-31 09:32:19 -03:00
|
|
|
|
|
|
|
CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES];
|
|
|
|
|
2023-08-17 17:18:36 -03:00
|
|
|
uint8_t CANIface::_num_interfaces;
|
2021-03-14 22:05:07 -03:00
|
|
|
|
2020-05-31 09:32:19 -03:00
|
|
|
bool CANIface::is_initialized() const
|
|
|
|
{
|
2023-08-25 21:52:28 -03:00
|
|
|
return transport != nullptr;
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline,
|
|
|
|
const CANIface::CanIOFlags flags)
|
|
|
|
{
|
2022-02-06 17:22:54 -04:00
|
|
|
WITH_SEMAPHORE(sem);
|
2020-05-31 09:32:19 -03:00
|
|
|
CanTxItem tx_item {};
|
|
|
|
tx_item.frame = frame;
|
|
|
|
if (flags & Loopback) {
|
|
|
|
tx_item.loopback = true;
|
|
|
|
}
|
|
|
|
if (flags & AbortOnError) {
|
|
|
|
tx_item.abort_on_error = true;
|
|
|
|
}
|
|
|
|
tx_item.setup = true;
|
|
|
|
tx_item.index = _tx_frame_counter;
|
|
|
|
tx_item.deadline = tx_deadline;
|
|
|
|
_tx_queue.emplace(tx_item);
|
|
|
|
_tx_frame_counter++;
|
|
|
|
stats.tx_requests++;
|
|
|
|
_pollRead(); // Read poll is necessary because it can release the pending TX flag
|
|
|
|
_pollWrite();
|
2022-02-06 17:22:54 -04:00
|
|
|
|
|
|
|
return AP_HAL::CANIface::send(frame, tx_deadline, flags);
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
|
|
|
|
CANIface::CanIOFlags& out_flags)
|
|
|
|
{
|
2022-02-06 17:22:54 -04:00
|
|
|
WITH_SEMAPHORE(sem);
|
2020-05-31 09:32:19 -03:00
|
|
|
if (_rx_queue.empty()) {
|
|
|
|
_pollRead(); // This allows to use the socket not calling poll() explicitly.
|
|
|
|
if (_rx_queue.empty()) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
{
|
|
|
|
const CanRxItem& rx = _rx_queue.front();
|
|
|
|
out_frame = rx.frame;
|
|
|
|
out_timestamp_us = rx.timestamp_us;
|
|
|
|
out_flags = rx.flags;
|
|
|
|
}
|
|
|
|
(void)_rx_queue.pop();
|
2022-02-06 17:22:54 -04:00
|
|
|
return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
2022-02-06 17:22:54 -04:00
|
|
|
bool CANIface::_hasReadyTx()
|
2020-05-31 09:32:19 -03:00
|
|
|
{
|
2022-02-06 17:22:54 -04:00
|
|
|
WITH_SEMAPHORE(sem);
|
|
|
|
return !_tx_queue.empty();
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
2022-02-06 17:22:54 -04:00
|
|
|
bool CANIface::_hasReadyRx()
|
2020-05-31 09:32:19 -03:00
|
|
|
{
|
2022-02-06 17:22:54 -04:00
|
|
|
WITH_SEMAPHORE(sem);
|
2020-05-31 09:32:19 -03:00
|
|
|
return !_rx_queue.empty();
|
|
|
|
}
|
|
|
|
|
|
|
|
void CANIface::_poll(bool read, bool write)
|
|
|
|
{
|
|
|
|
if (read) {
|
|
|
|
_pollRead(); // Read poll must be executed first because it may decrement _frames_in_socket_tx_queue
|
|
|
|
}
|
|
|
|
if (write) {
|
|
|
|
_pollWrite();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t CANIface::getErrorCount() const
|
|
|
|
{
|
2023-08-25 21:52:28 -03:00
|
|
|
return 0;
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void CANIface::_pollWrite()
|
|
|
|
{
|
2023-08-25 21:52:28 -03:00
|
|
|
if (transport == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
2020-05-31 09:32:19 -03:00
|
|
|
while (_hasReadyTx()) {
|
2022-02-06 17:22:54 -04:00
|
|
|
WITH_SEMAPHORE(sem);
|
2020-05-31 09:32:19 -03:00
|
|
|
const CanTxItem tx = _tx_queue.top();
|
2023-08-25 21:52:28 -03:00
|
|
|
const uint64_t curr_time = AP_HAL::micros64();
|
2020-05-31 09:32:19 -03:00
|
|
|
if (tx.deadline >= curr_time) {
|
|
|
|
// hal.console->printf("%x TDEAD: %lu CURRT: %lu DEL: %lu\n",tx.frame.id, tx.deadline, curr_time, tx.deadline-curr_time);
|
2023-08-25 21:52:28 -03:00
|
|
|
bool ok = transport->send(tx.frame);
|
|
|
|
if (ok) {
|
2020-05-31 09:32:19 -03:00
|
|
|
stats.tx_success++;
|
2023-08-25 21:52:28 -03:00
|
|
|
} else {
|
|
|
|
break;
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
} else {
|
|
|
|
stats.tx_timedout++;
|
|
|
|
}
|
|
|
|
|
2023-08-25 21:52:28 -03:00
|
|
|
// Removing the frame from the queue
|
2020-05-31 09:32:19 -03:00
|
|
|
(void)_tx_queue.pop();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
bool CANIface::_pollRead()
|
|
|
|
{
|
2023-08-25 21:52:28 -03:00
|
|
|
if (transport == nullptr) {
|
|
|
|
return false;
|
2022-03-26 16:41:40 -03:00
|
|
|
}
|
2023-08-25 21:52:28 -03:00
|
|
|
CanRxItem rx {};
|
|
|
|
bool ok = transport->receive(rx.frame);
|
|
|
|
if (!ok) {
|
|
|
|
return false;
|
2022-03-26 16:41:40 -03:00
|
|
|
}
|
2023-08-25 21:52:28 -03:00
|
|
|
rx.timestamp_us = AP_HAL::micros64();
|
|
|
|
WITH_SEMAPHORE(sem);
|
|
|
|
add_to_rx_queue(rx);
|
|
|
|
stats.rx_received++;
|
|
|
|
return true;
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Might block forever, only to be used for testing
|
|
|
|
void CANIface::flush_tx()
|
|
|
|
{
|
2022-02-06 17:22:54 -04:00
|
|
|
WITH_SEMAPHORE(sem);
|
2020-05-31 09:32:19 -03:00
|
|
|
do {
|
|
|
|
_poll(true, true);
|
2023-08-25 21:52:28 -03:00
|
|
|
} while(!_tx_queue.empty());
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void CANIface::clear_rx()
|
|
|
|
{
|
2022-02-06 17:22:54 -04:00
|
|
|
WITH_SEMAPHORE(sem);
|
2020-05-31 09:32:19 -03:00
|
|
|
// Clean Rx Queue
|
|
|
|
std::queue<CanRxItem> empty;
|
|
|
|
std::swap( _rx_queue, empty );
|
|
|
|
}
|
|
|
|
|
|
|
|
void CANIface::_confirmSentFrame()
|
|
|
|
{
|
|
|
|
if (_frames_in_socket_tx_queue > 0) {
|
|
|
|
_frames_in_socket_tx_queue--;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-08-25 21:52:28 -03:00
|
|
|
bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode)
|
2020-05-31 09:32:19 -03:00
|
|
|
{
|
2023-08-25 21:52:28 -03:00
|
|
|
return init(bitrate, mode);
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
2023-08-25 21:52:28 -03:00
|
|
|
bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
|
2020-05-31 09:32:19 -03:00
|
|
|
{
|
2023-08-25 21:52:28 -03:00
|
|
|
const auto *_sitl = AP::sitl();
|
|
|
|
if (_sitl == nullptr) {
|
2020-05-31 09:32:19 -03:00
|
|
|
return false;
|
|
|
|
}
|
2023-08-25 21:52:28 -03:00
|
|
|
if (_self_index >= HAL_NUM_CAN_IFACES) {
|
2022-03-26 16:41:40 -03:00
|
|
|
return false;
|
|
|
|
}
|
2023-08-25 21:52:28 -03:00
|
|
|
const SITL::SIM::CANTransport can_type = _sitl->can_transport[_self_index];
|
|
|
|
switch (can_type) {
|
|
|
|
case SITL::SIM::CANTransport::MulticastUDP:
|
|
|
|
transport = new CAN_Multicast();
|
|
|
|
break;
|
|
|
|
case SITL::SIM::CANTransport::SocketCAN:
|
|
|
|
#if HAL_CAN_WITH_SOCKETCAN
|
|
|
|
transport = new CAN_SocketCAN();
|
|
|
|
#endif
|
|
|
|
break;
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
2023-08-25 21:52:28 -03:00
|
|
|
if (transport == nullptr) {
|
|
|
|
return false;
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
2023-08-25 21:52:28 -03:00
|
|
|
if (!transport->init(_self_index)) {
|
|
|
|
delete transport;
|
|
|
|
transport = nullptr;
|
|
|
|
return false;
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
2023-08-25 21:52:28 -03:00
|
|
|
return true;
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
bool CANIface::select(bool &read_select, bool &write_select,
|
2023-08-25 21:52:28 -03:00
|
|
|
const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline)
|
2020-05-31 09:32:19 -03:00
|
|
|
{
|
2023-08-25 21:52:28 -03:00
|
|
|
if (transport == nullptr) {
|
|
|
|
return false;
|
|
|
|
}
|
2020-05-31 09:32:19 -03:00
|
|
|
// Detecting whether we need to block at all
|
|
|
|
bool need_block = !write_select; // Write queue is infinite
|
2023-08-25 21:52:28 -03:00
|
|
|
|
2020-09-12 15:58:10 -03:00
|
|
|
// call poll here to flush some tx
|
|
|
|
_poll(true, true);
|
2020-05-31 09:32:19 -03:00
|
|
|
|
|
|
|
if (read_select && _hasReadyRx()) {
|
|
|
|
need_block = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (need_block) {
|
2023-08-25 21:52:28 -03:00
|
|
|
_pollfd.fd = transport->get_read_fd();
|
|
|
|
_pollfd.events |= POLLIN;
|
|
|
|
}
|
|
|
|
if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) {
|
|
|
|
_evt_handle->wait(blocking_deadline - AP_HAL::micros64());
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Writing the output masks
|
2023-08-25 21:52:28 -03:00
|
|
|
write_select = true;
|
|
|
|
read_select = _hasReadyRx();
|
2020-05-31 09:32:19 -03:00
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2023-08-25 21:52:28 -03:00
|
|
|
bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
|
|
|
|
{
|
2020-05-31 09:32:19 -03:00
|
|
|
_evt_handle = handle;
|
|
|
|
evt_can_socket[_self_index]._ifaces[_self_index] = this;
|
|
|
|
_evt_handle->set_source(&evt_can_socket[_self_index]);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2023-04-17 01:34:22 -03:00
|
|
|
bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle)
|
2020-05-31 09:32:19 -03:00
|
|
|
{
|
|
|
|
if (evt_handle == nullptr) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
pollfd pollfds[HAL_NUM_CAN_IFACES] {};
|
|
|
|
uint8_t pollfd_iface_map[HAL_NUM_CAN_IFACES] {};
|
|
|
|
unsigned long int num_pollfds = 0;
|
|
|
|
|
|
|
|
// Poll FD set setup
|
|
|
|
for (unsigned i = 0; i < HAL_NUM_CAN_IFACES; i++) {
|
|
|
|
if (_ifaces[i] == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
pollfds[num_pollfds] = _ifaces[i]->_pollfd;
|
|
|
|
pollfd_iface_map[num_pollfds] = i;
|
|
|
|
num_pollfds++;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (num_pollfds == 0) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2023-08-17 18:55:20 -03:00
|
|
|
const uint32_t start_us = AP_HAL::micros();
|
|
|
|
do {
|
2023-08-17 17:18:36 -03:00
|
|
|
uint16_t wait_us = MIN(100, duration_us);
|
2020-05-31 09:32:19 -03:00
|
|
|
|
2023-08-17 18:55:20 -03:00
|
|
|
// check FD for input
|
2023-08-25 21:52:28 -03:00
|
|
|
const int res = poll(pollfds, num_pollfds, wait_us/1000U);
|
2020-05-31 09:32:19 -03:00
|
|
|
|
2023-08-17 17:18:36 -03:00
|
|
|
if (res < 0) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (res > 0) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure simulator runs
|
|
|
|
hal.scheduler->delay_microseconds(wait_us);
|
2023-08-17 18:55:20 -03:00
|
|
|
} while (AP_HAL::micros() - start_us < duration_us);
|
2020-05-31 09:32:19 -03:00
|
|
|
|
2023-08-17 17:18:36 -03:00
|
|
|
|
2020-05-31 09:32:19 -03:00
|
|
|
// Handling poll output
|
|
|
|
for (unsigned i = 0; i < num_pollfds; i++) {
|
|
|
|
if (_ifaces[pollfd_iface_map[i]] == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
const bool poll_read = pollfds[i].revents & POLLIN;
|
|
|
|
const bool poll_write = pollfds[i].revents & POLLOUT;
|
|
|
|
_ifaces[pollfd_iface_map[i]]->_poll(poll_read, poll_write);
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2020-12-30 02:23:27 -04:00
|
|
|
void CANIface::get_stats(ExpandingString &str)
|
2020-05-31 09:32:19 -03:00
|
|
|
{
|
2020-12-30 02:23:27 -04:00
|
|
|
str.printf("tx_requests: %u\n"
|
2022-12-06 22:35:59 -04:00
|
|
|
"tx_rejected: %u\n"
|
2020-12-30 02:23:27 -04:00
|
|
|
"tx_success: %u\n"
|
|
|
|
"tx_timedout: %u\n"
|
|
|
|
"rx_received: %u\n"
|
2023-08-25 21:52:28 -03:00
|
|
|
"rx_errors: %u\n",
|
2020-12-30 02:23:27 -04:00
|
|
|
stats.tx_requests,
|
2022-12-06 22:35:59 -04:00
|
|
|
stats.tx_rejected,
|
2020-12-30 02:23:27 -04:00
|
|
|
stats.tx_success,
|
|
|
|
stats.tx_timedout,
|
|
|
|
stats.rx_received,
|
2023-08-25 21:52:28 -03:00
|
|
|
stats.rx_errors);
|
2020-05-31 09:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|