aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_Linux: add uavcan agnostic CANSocket Iface Driver

Browse Source
This commit is contained in:
Siddharth Purohit 2020-05-31 17:58:24 +05:30 committed by Andrew Tridgell
parent 7929efec0d
commit 7de444ec53
6 changed files with 819 additions and 817 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

595
libraries/AP_HAL_Linux/CAN.cpp
View File

@ -1,595 +0,0 @@
/*
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>
#include <AP_BoardConfig/AP_BoardConfig.h>
#if HAL_WITH_UAVCAN
#include "CAN.h"
#include <unistd.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can/raw.h>
extern const AP_HAL::HAL& hal;
using namespace Linux;
uavcan::MonotonicTime getMonotonic()
{
return uavcan::MonotonicTime::fromUSec(AP_HAL::micros64());
}
static can_frame makeSocketCanFrame(const uavcan::CanFrame& uavcan_frame)
{
can_frame sockcan_frame { uavcan_frame.id& uavcan::CanFrame::MaskExtID, uavcan_frame.dlc, { } };
std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data);
if (uavcan_frame.isExtended()) {
sockcan_frame.can_id |= CAN_EFF_FLAG;
}
if (uavcan_frame.isErrorFrame()) {
sockcan_frame.can_id |= CAN_ERR_FLAG;
}
if (uavcan_frame.isRemoteTransmissionRequest()) {
sockcan_frame.can_id |= CAN_RTR_FLAG;
}
return sockcan_frame;
}
static uavcan::CanFrame makeUavcanFrame(const can_frame& sockcan_frame)
{
uavcan::CanFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc);
if (sockcan_frame.can_id & CAN_EFF_FLAG) {
uavcan_frame.id |= uavcan::CanFrame::FlagEFF;
}
if (sockcan_frame.can_id & CAN_ERR_FLAG) {
uavcan_frame.id |= uavcan::CanFrame::FlagERR;
}
if (sockcan_frame.can_id & CAN_RTR_FLAG) {
uavcan_frame.id |= uavcan::CanFrame::FlagRTR;
}
return uavcan_frame;
}
bool CAN::begin(uint32_t bitrate)
{
if (_initialized) {
return _initialized;
}
// TODO: Add possibility change bitrate
_fd = openSocket(HAL_BOARD_CAN_IFACE_NAME);
if (_fd > 0) {
_bitrate = bitrate;
_initialized = true;
} else {
_initialized = false;
}
return _initialized;
}
void CAN::reset()
{
if (_initialized && _bitrate != 0) {
close(_fd);
begin(_bitrate);
}
}
void CAN::end()
{
_initialized = false;
close(_fd);
}
bool CAN::is_initialized()
{
return _initialized;
}
int32_t CAN::tx_pending()
{
if (_initialized) {
return _tx_queue.size();
} else {
return -1;
}
}
int32_t CAN::available()
{
if (_initialized) {
return _rx_queue.size();
} else {
return -1;
}
}
int CAN::openSocket(const std::string& iface_name)
{
errno = 0;
int s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (s < 0) {
return s;
}
std::shared_ptr<void> defer(&s, [](int* fd) { if (*fd >= 0) close(*fd); });
const int ret = s;
// Detect the iface index
auto ifr = ifreq();
if (iface_name.length() >= IFNAMSIZ) {
errno = ENAMETOOLONG;
return -1;
}
std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length());
if (ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) {
return -1;
}
// Bind to the specified CAN iface
{
auto addr = sockaddr_can();
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(s, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
return -1;
}
}
// Configure
{
const int on = 1;
// Timestamping
if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) {
return -1;
}
// Socket loopback
if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) {
return -1;
}
// Non-blocking
if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) {
return -1;
}
}
// Validate the resulting socket
{
int socket_error = 0;
socklen_t errlen = sizeof(socket_error);
getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&socket_error), &errlen);
if (socket_error != 0) {
errno = socket_error;
return -1;
}
}
s = -1;
return ret;
}
int16_t CAN::send(const uavcan::CanFrame& frame, const uavcan::MonotonicTime tx_deadline,
const uavcan::CanIOFlags flags)
{
_tx_queue.emplace(frame, tx_deadline, flags, _tx_frame_counter);
_tx_frame_counter++;
_pollRead(); // Read poll is necessary because it can release the pending TX flag
_pollWrite();
return 1;
}
int16_t CAN::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
{
if (_rx_queue.empty()) {
_pollRead(); // This allows to use the socket not calling poll() explicitly.
if (_rx_queue.empty()) {
return 0;
}
}
{
const RxItem& rx = _rx_queue.front();
out_frame = rx.frame;
out_ts_monotonic = rx.ts_mono;
out_ts_utc = rx.ts_utc;
out_flags = rx.flags;
}
_rx_queue.pop();
return 1;
}
bool CAN::hasReadyTx() const
{
return !_tx_queue.empty() && (_frames_in_socket_tx_queue < _max_frames_in_socket_tx_queue);
}
bool CAN::hasReadyRx() const
{
return !_rx_queue.empty();
}
void CAN::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();
}
}
int16_t CAN::configureFilters(const uavcan::CanFilterConfig* const filter_configs,
const std::uint16_t num_configs)
{
if (filter_configs == nullptr) {
return -1;
}
_hw_filters_container.clear();
_hw_filters_container.resize(num_configs);
for (unsigned i = 0; i < num_configs; i++) {
const uavcan::CanFilterConfig& fc = filter_configs[i];
_hw_filters_container[i].can_id = fc.id & uavcan::CanFrame::MaskExtID;
_hw_filters_container[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID;
if (fc.id & uavcan::CanFrame::FlagEFF) {
_hw_filters_container[i].can_id |= CAN_EFF_FLAG;
}
if (fc.id & uavcan::CanFrame::FlagRTR) {
_hw_filters_container[i].can_id |= CAN_RTR_FLAG;
}
if (fc.mask & uavcan::CanFrame::FlagEFF) {
_hw_filters_container[i].can_mask |= CAN_EFF_FLAG;
}
if (fc.mask & uavcan::CanFrame::FlagRTR) {
_hw_filters_container[i].can_mask |= CAN_RTR_FLAG;
}
}
return 0;
}
/**
* SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited.
* This method returns a constant value.
*/
static constexpr unsigned NumFilters = CAN_FILTER_NUMBER;
uint16_t CAN::getNumFilters() const { return NumFilters; }
uint64_t CAN::getErrorCount() const
{
uint64_t ec = 0;
for (auto& kv : _errors) { ec += kv.second; }
return ec;
}
void CAN::_pollWrite()
{
while (hasReadyTx()) {
const TxItem tx = _tx_queue.top();
if (tx.deadline >= getMonotonic()) {
const int res = _write(tx.frame);
if (res == 1) { // Transmitted successfully
_incrementNumFramesInSocketTxQueue();
if (tx.flags & uavcan::CanIOFlagLoopback) {
_pending_loopback_ids.insert(tx.frame.id);
}
} else if (res == 0) { // Not transmitted, nor is it an error
break; // Leaving the loop, the frame remains enqueued for the next retry
} else { // Transmission error
_registerError(SocketCanError::SocketWriteFailure);
}
} else {
_registerError(SocketCanError::TxTimeout);
}
// Removing the frame from the queue even if transmission failed
_tx_queue.pop();
}
}
void CAN::_pollRead()
{
uint8_t iterations_count = 0;
while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT)
{
iterations_count++;
RxItem rx;
rx.ts_mono = getMonotonic(); // Monotonic timestamp is not required to be precise (unlike UTC)
bool loopback = false;
const int res = _read(rx.frame, rx.ts_utc, loopback);
if (res == 1) {
bool accept = true;
if (loopback) { // We receive loopback for all CAN frames
_confirmSentFrame();
rx.flags |= uavcan::CanIOFlagLoopback;
accept = _wasInPendingLoopbackSet(rx.frame);
}
if (accept) {
_rx_queue.push(rx);
}
} else if (res == 0) {
break;
} else {
_registerError(SocketCanError::SocketReadFailure);
break;
}
}
}
int CAN::_write(const uavcan::CanFrame& frame) const
{
errno = 0;
const can_frame sockcan_frame = makeSocketCanFrame(frame);
const int res = write(_fd, &sockcan_frame, sizeof(sockcan_frame));
if (res <= 0) {
if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error
return 0;
}
return res;
}
if (res != sizeof(sockcan_frame)) {
return -1;
}
return 1;
}
int CAN::_read(uavcan::CanFrame& frame, uavcan::UtcTime& ts_utc, bool& loopback) const
{
auto iov = iovec();
auto sockcan_frame = can_frame();
iov.iov_base = &sockcan_frame;
iov.iov_len = sizeof(sockcan_frame);
union {
uint8_t data[CMSG_SPACE(sizeof(::timeval))];
struct cmsghdr align;
} control;
auto msg = msghdr();
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = control.data;
msg.msg_controllen = sizeof(control.data);
const int res = recvmsg(_fd, &msg, MSG_DONTWAIT);
if (res <= 0) {
return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
}
/*
* Flags
*/
loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
if (!loopback && !_checkHWFilters(sockcan_frame)) {
return 0;
}
frame = makeUavcanFrame(sockcan_frame);
/*
* Timestamp
*/
const cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg);
if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMP) {
auto tv = timeval();
std::memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv)); // Copy to avoid alignment problems
ts_utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * 1000000ULL + tv.tv_usec);
} else {
return -1;
}
return 1;
}
void CAN::_incrementNumFramesInSocketTxQueue()
{
_frames_in_socket_tx_queue++;
}
void CAN::_confirmSentFrame()
{
if (_frames_in_socket_tx_queue > 0) {
_frames_in_socket_tx_queue--;
}
}
bool CAN::_wasInPendingLoopbackSet(const uavcan::CanFrame& frame)
{
if (_pending_loopback_ids.count(frame.id) > 0) {
_pending_loopback_ids.erase(frame.id);
return true;
}
return false;
}
bool CAN::_checkHWFilters(const can_frame& frame) const
{
if (!_hw_filters_container.empty()) {
for (auto& f : _hw_filters_container) {
if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) {
return true;
}
}
return false;
} else {
return true;
}
}
void CANManager::IfaceWrapper::updateDownStatusFromPollResult(const pollfd& pfd)
{
if (!_down&& (pfd.revents & POLLERR)) {
int error = 0;
socklen_t errlen = sizeof(error);
getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&error), &errlen);
_down= error == ENETDOWN || error == ENODEV;
hal.console->printf("Iface %d is dead; error %d", this->getFileDescriptor(), error);
}
}
bool CANManager::begin(uint32_t bitrate, uint8_t can_number)
{
if (init(can_number) >= 0) {
_initialized = true;
}
return _initialized;
}
bool CANManager::is_initialized()
{
return _initialized;
}
void CANManager::initialized(bool val)
{
_initialized = val;
}
CAN* CANManager::getIface(uint8_t iface_index)
{
return (iface_index >= _ifaces.size()) ? nullptr : _ifaces[iface_index].get();
}
int CANManager::init(uint8_t can_number)
{
int res = -1;
char iface_name[16];
sprintf(iface_name, "can%u", can_number);
res = addIface(iface_name);
if (res < 0) {
hal.console->printf("CANManager: init %s failed\n", iface_name);
}
return res;
}
int16_t CANManager::select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline)
{
// Detecting whether we need to block at all
bool need_block = (inout_masks.write == 0); // Write queue is infinite
for (unsigned i = 0; need_block && (i < _ifaces.size()); i++) {
const bool need_read = inout_masks.read & (1 << i);
if (need_read && _ifaces[i]->hasReadyRx()) {
need_block = false;
}
}
if (need_block) {
// Poll FD set setup
pollfd pollfds[uavcan::MaxCanIfaces] = {};
unsigned num_pollfds = 0;
IfaceWrapper* pollfd_index_to_iface[uavcan::MaxCanIfaces] = {};
for (unsigned i = 0; i < _ifaces.size(); i++) {
if (_ifaces[i]->isDown()) {
continue;
}
pollfds[num_pollfds].fd = _ifaces[i]->getFileDescriptor();
pollfds[num_pollfds].events = POLLIN;
if (_ifaces[i]->hasReadyTx() || (inout_masks.write & (1U << i))) {
pollfds[num_pollfds].events |= POLLOUT;
}
pollfd_index_to_iface[num_pollfds] = _ifaces[i].get();
num_pollfds++;
}
if (num_pollfds == 0) {
return 0;
}
// Timeout conversion
const std::int64_t timeout_usec = (blocking_deadline - getMonotonic()).toUSec();
auto ts = timespec();
if (timeout_usec > 0) {
ts.tv_sec = timeout_usec / 1000000LL;
ts.tv_nsec = (timeout_usec % 1000000LL) * 1000;
}
// Blocking here
const int res = ppoll(pollfds, num_pollfds, &ts, nullptr);
if (res < 0) {
return res;
}
// Handling poll output
for (unsigned i = 0; i < num_pollfds; i++) {
pollfd_index_to_iface[i]->updateDownStatusFromPollResult(pollfds[i]);
const bool poll_read = pollfds[i].revents & POLLIN;
const bool poll_write = pollfds[i].revents & POLLOUT;
pollfd_index_to_iface[i]->poll(poll_read, poll_write);
}
}
// Writing the output masks
inout_masks = uavcan::CanSelectMasks();
for (unsigned i = 0; i < _ifaces.size(); i++) {
if (!_ifaces[i]->isDown()) {
inout_masks.write |= std::uint8_t(1U << i); // Always ready to write if not down
}
if (_ifaces[i]->hasReadyRx()) {
inout_masks.read |= std::uint8_t(1U << i); // Readability depends only on RX buf, even if down
}
}
// Return value is irrelevant as long as it's non-negative
return _ifaces.size();
}
int CANManager::addIface(const std::string& iface_name)
{
if (_ifaces.size() >= uavcan::MaxCanIfaces) {
return -1;
}
// Open the socket
const int fd = CAN::openSocket(iface_name);
if (fd < 0) {
return fd;
}
// Construct the iface - upon successful construction the iface will take ownership of the fd.
_ifaces.emplace_back(new IfaceWrapper(fd));
hal.console->printf("New iface '%s' fd %d\n", iface_name.c_str(), fd);
return _ifaces.size() - 1;
}
#endif

221
libraries/AP_HAL_Linux/CAN.h
View File

@ -1,221 +0,0 @@
/*
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/LICENSE
* modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
*/
#pragma once
#include "AP_HAL_Linux.h"
#include <AP_HAL/CAN.h>
#include <linux/can.h>
#include <string>
#include <queue>
#include <memory>
#include <map>
#include <unordered_set>
#include <poll.h>
namespace Linux {
enum class SocketCanError
{
SocketReadFailure,
SocketWriteFailure,
TxTimeout
};
#define CAN_MAX_POLL_ITERATIONS_COUNT 100
#define CAN_MAX_INIT_TRIES_COUNT 100
#define CAN_FILTER_NUMBER 8
class CAN: public AP_HAL::CANHal {
public:
CAN(int socket_fd=0)
: _fd(socket_fd)
, _frames_in_socket_tx_queue(0)
, _max_frames_in_socket_tx_queue(2)
{ }
~CAN() { }
bool begin(uint32_t bitrate) override;
void end() override;
void reset() override;
bool is_initialized() override;
int32_t tx_pending() override;
int32_t available() override;
static int openSocket(const std::string& iface_name);
int getFileDescriptor() const { return _fd; }
int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags) override;
int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override;
bool hasReadyTx() const;
bool hasReadyRx() const;
void poll(bool read, bool write);
int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs) override;
uint16_t getNumFilters() const override;
uint64_t getErrorCount() const override;
private:
struct TxItem
{
uavcan::CanFrame frame;
uavcan::MonotonicTime deadline;
uavcan::CanIOFlags flags = 0;
std::uint64_t order = 0;
TxItem(const uavcan::CanFrame& arg_frame, uavcan::MonotonicTime arg_deadline,
uavcan::CanIOFlags arg_flags, std::uint64_t arg_order)
: frame(arg_frame)
, deadline(arg_deadline)
, flags(arg_flags)
, order(arg_order)
{ }
bool operator<(const TxItem& rhs) const
{
if (frame.priorityLowerThan(rhs.frame)) {
return true;
}
if (frame.priorityHigherThan(rhs.frame)) {
return false;
}
return order > rhs.order;
}
};
struct RxItem
{
uavcan::CanFrame frame;
uavcan::MonotonicTime ts_mono;
uavcan::UtcTime ts_utc;
uavcan::CanIOFlags flags;
RxItem()
: flags(0)
{ }
};
void _pollWrite();
void _pollRead();
int _write(const uavcan::CanFrame& frame) const;
int _read(uavcan::CanFrame& frame, uavcan::UtcTime& ts_utc, bool& loopback) const;
void _incrementNumFramesInSocketTxQueue();
void _confirmSentFrame();
bool _wasInPendingLoopbackSet(const uavcan::CanFrame& frame);
bool _checkHWFilters(const can_frame& frame) const;
void _registerError(SocketCanError e) { _errors[e]++; }
uint32_t _bitrate;
bool _initialized;
int _fd;
const unsigned _max_frames_in_socket_tx_queue;
unsigned _frames_in_socket_tx_queue;
uint64_t _tx_frame_counter;
std::map<SocketCanError, uint64_t> _errors;
std::priority_queue<TxItem> _tx_queue;
std::queue<RxItem> _rx_queue;
std::unordered_multiset<uint32_t> _pending_loopback_ids;
std::vector<can_filter> _hw_filters_container;
};
class CANManager: public AP_HAL::CANManager, public uavcan::ICanDriver {
public:
static CANManager *from(AP_HAL::CANManager *can)
{
return static_cast<CANManager*>(can);
}
CANManager() : AP_HAL::CANManager(this) { _ifaces.reserve(uavcan::MaxCanIfaces); }
~CANManager() { }
//These methods belong to AP_HAL::CANManager
virtual bool begin(uint32_t bitrate, uint8_t can_number) override;
virtual void initialized(bool val) override;
virtual bool is_initialized() override;
//These methods belong to ICanDriver
virtual CAN* getIface(uint8_t iface_index) override;
virtual uint8_t getNumIfaces() const override { return _ifaces.size(); }
virtual int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) override;
int init(uint8_t can_number);
int addIface(const std::string& iface_name);
private:
class IfaceWrapper : public CAN
{
bool _down = false;
public:
IfaceWrapper(int fd) : CAN(fd) { }
void updateDownStatusFromPollResult(const pollfd& pfd);
bool isDown() const { return _down; }
};
bool _initialized;
std::vector<std::unique_ptr<IfaceWrapper>> _ifaces;
};
}

604
libraries/AP_HAL_Linux/CANSocketIface.cpp Normal file
View File

@ -0,0 +1,604 @@
/*
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/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can/raw.h>
#include <cstring>
#include "Scheduler.h"
#include <AP_CANManager/AP_CANManager.h>
extern const AP_HAL::HAL& hal;
using namespace Linux;
#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANLinuxIface", fmt, ##args); } while (0)
CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES];
static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame)
{
can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { } };
std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data);
if (uavcan_frame.isExtended()) {
sockcan_frame.can_id |= CAN_EFF_FLAG;
}
if (uavcan_frame.isErrorFrame()) {
sockcan_frame.can_id |= CAN_ERR_FLAG;
}
if (uavcan_frame.isRemoteTransmissionRequest()) {
sockcan_frame.can_id |= CAN_RTR_FLAG;
}
return sockcan_frame;
}
static AP_HAL::CANFrame makeUavcanFrame(const can_frame& sockcan_frame)
{
AP_HAL::CANFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc);
if (sockcan_frame.can_id & CAN_EFF_FLAG) {
uavcan_frame.id |= AP_HAL::CANFrame::FlagEFF;
}
if (sockcan_frame.can_id & CAN_ERR_FLAG) {
uavcan_frame.id |= AP_HAL::CANFrame::FlagERR;
}
if (sockcan_frame.can_id & CAN_RTR_FLAG) {
uavcan_frame.id |= AP_HAL::CANFrame::FlagRTR;
}
return uavcan_frame;
}
bool CANIface::is_initialized() const
{
return _initialized;
}
int CANIface::_openSocket(const std::string& iface_name)
{
errno = 0;
int s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (s < 0) {
return s;
}
std::shared_ptr<void> defer(&s, [](int* fd) { if (*fd >= 0) close(*fd); });
const int ret = s;
// Detect the iface index
auto ifr = ifreq();
if (iface_name.length() >= IFNAMSIZ) {
errno = ENAMETOOLONG;
return -1;
}
std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length());
if (ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) {
return -1;
}
// Bind to the specified CAN iface
{
auto addr = sockaddr_can();
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(s, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
return -1;
}
}
// Configure
{
const int on = 1;
// Timestamping
if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) {
return -1;
}
// Socket loopback
if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) {
return -1;
}
// Non-blocking
if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) {
return -1;
}
}
// Validate the resulting socket
{
int socket_error = 0;
socklen_t errlen = sizeof(socket_error);
getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&socket_error), &errlen);
if (socket_error != 0) {
errno = socket_error;
return -1;
}
}
s = -1;
return ret;
}
int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline,
const CANIface::CanIOFlags flags)
{
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();
return 1;
}
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
CANIface::CanIOFlags& out_flags)
{
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();
return 1;
}
bool CANIface::_hasReadyTx() const
{
return !_tx_queue.empty() && (_frames_in_socket_tx_queue < _max_frames_in_socket_tx_queue);
}
bool CANIface::_hasReadyRx() const
{
return !_rx_queue.empty();
}
void CANIface::_poll(bool read, bool write)
{
if (read) {
stats.num_poll_rx_events++;
_pollRead(); // Read poll must be executed first because it may decrement _frames_in_socket_tx_queue
}
if (write) {
stats.num_poll_tx_events++;
_pollWrite();
}
}
bool CANIface::configureFilters(const CanFilterConfig* const filter_configs,
const std::uint16_t num_configs)
{
if (filter_configs == nullptr) {
return false;
}
_hw_filters_container.clear();
_hw_filters_container.resize(num_configs);
for (unsigned i = 0; i < num_configs; i++) {
const CanFilterConfig& fc = filter_configs[i];
_hw_filters_container[i].can_id = fc.id & AP_HAL::CANFrame::MaskExtID;
_hw_filters_container[i].can_mask = fc.mask & AP_HAL::CANFrame::MaskExtID;
if (fc.id & AP_HAL::CANFrame::FlagEFF) {
_hw_filters_container[i].can_id |= CAN_EFF_FLAG;
}
if (fc.id & AP_HAL::CANFrame::FlagRTR) {
_hw_filters_container[i].can_id |= CAN_RTR_FLAG;
}
if (fc.mask & AP_HAL::CANFrame::FlagEFF) {
_hw_filters_container[i].can_mask |= CAN_EFF_FLAG;
}
if (fc.mask & AP_HAL::CANFrame::FlagRTR) {
_hw_filters_container[i].can_mask |= CAN_RTR_FLAG;
}
}
return true;
}
/**
* SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited.
* This method returns a constant value.
*/
static constexpr unsigned NumFilters = CAN_FILTER_NUMBER;
uint16_t CANIface::getNumFilters() const { return NumFilters; }
uint32_t CANIface::getErrorCount() const
{
uint32_t ec = 0;
for (auto& kv : _errors) { ec += kv.second; }
return ec;
}
void CANIface::_pollWrite()
{
while (_hasReadyTx()) {
const CanTxItem tx = _tx_queue.top();
uint64_t curr_time = AP_HAL::native_micros64();
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);
const int res = _write(tx.frame);
if (res == 1) { // Transmitted successfully
_incrementNumFramesInSocketTxQueue();
if (tx.loopback) {
_pending_loopback_ids.insert(tx.frame.id);
}
stats.tx_success++;
} else if (res == 0) { // Not transmitted, nor is it an error
stats.tx_full++;
break; // Leaving the loop, the frame remains enqueued for the next retry
} else { // Transmission error
stats.tx_write_fail++;
}
} else {
// hal.console->printf("TDEAD: %lu CURRT: %lu DEL: %lu\n", tx.deadline, curr_time, curr_time-tx.deadline);
stats.tx_timedout++;
}
// Removing the frame from the queue even if transmission failed
(void)_tx_queue.pop();
}
}
bool CANIface::_pollRead()
{
uint8_t iterations_count = 0;
while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT)
{
iterations_count++;
CanRxItem rx;
rx.timestamp_us = AP_HAL::native_micros64(); // Monotonic timestamp is not required to be precise (unlike UTC)
bool loopback = false;
const int res = _read(rx.frame, rx.timestamp_us, loopback);
if (res == 1) {
bool accept = true;
if (loopback) { // We receive loopback for all CAN frames
_confirmSentFrame();
rx.flags |= Loopback;
accept = _wasInPendingLoopbackSet(rx.frame);
stats.tx_confirmed++;
}
if (accept) {
_rx_queue.push(rx);
stats.rx_received++;
return true;
}
} else if (res == 0) {
break;
} else {
stats.rx_errors++;
break;
}
}
return false;
}
int CANIface::_write(const AP_HAL::CANFrame& frame) const
{
errno = 0;
const can_frame sockcan_frame = makeSocketCanFrame(frame);
const int res = write(_fd, &sockcan_frame, sizeof(sockcan_frame));
if (res <= 0) {
if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error
return 0;
}
return res;
}
if (res != sizeof(sockcan_frame)) {
return -1;
}
return 1;
}
int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const
{
auto iov = iovec();
auto sockcan_frame = can_frame();
iov.iov_base = &sockcan_frame;
iov.iov_len = sizeof(sockcan_frame);
union {
uint8_t data[CMSG_SPACE(sizeof(::timeval))];
struct cmsghdr align;
} control;
auto msg = msghdr();
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = control.data;
msg.msg_controllen = sizeof(control.data);
const int res = recvmsg(_fd, &msg, MSG_DONTWAIT);
if (res <= 0) {
return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
}
/*
* Flags
*/
loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
if (!loopback && !_checkHWFilters(sockcan_frame)) {
return 0;
}
frame = makeUavcanFrame(sockcan_frame);
/*
* Timestamp
*/
timestamp_us = AP_HAL::native_micros64();
return 1;
}
// Might block forever, only to be used for testing
void CANIface::flush_tx()
{
do {
_updateDownStatusFromPollResult(_pollfd);
_poll(true, true);
} while(!_tx_queue.empty() && !_down);
}
void CANIface::clear_rx()
{
// Clean Rx Queue
std::queue<CanRxItem> empty;
std::swap( _rx_queue, empty );
}
void CANIface::_incrementNumFramesInSocketTxQueue()
{
_frames_in_socket_tx_queue++;
}
void CANIface::_confirmSentFrame()
{
if (_frames_in_socket_tx_queue > 0) {
_frames_in_socket_tx_queue--;
}
}
bool CANIface::_wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame)
{
if (_pending_loopback_ids.count(frame.id) > 0) {
_pending_loopback_ids.erase(frame.id);
return true;
}
return false;
}
bool CANIface::_checkHWFilters(const can_frame& frame) const
{
if (!_hw_filters_container.empty()) {
for (auto& f : _hw_filters_container) {
if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) {
return true;
}
}
return false;
} else {
return true;
}
}
void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd)
{
if (!_down && (pfd.revents & POLLERR)) {
int error = 0;
socklen_t errlen = sizeof(error);
getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&error), &errlen);
_down= error == ENETDOWN || error == ENODEV;
stats.num_downs++;
Debug("Iface %d is dead; error %d", _fd, error);
}
}
bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
{
char iface_name[16];
sprintf(iface_name, "can%u", _self_index);
if (_initialized) {
return _initialized;
}
// TODO: Add possibility change bitrate
_fd = _openSocket(iface_name);
Debug("Socket opened iface_name: %s fd: %d", iface_name, _fd);
if (_fd > 0) {
_bitrate = bitrate;
_initialized = true;
} else {
_initialized = false;
}
return _initialized;
}
bool CANIface::select(bool &read_select, bool &write_select,
const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline)
{
// Detecting whether we need to block at all
bool need_block = !write_select; // Write queue is infinite
if (read_select && _hasReadyRx()) {
need_block = false;
}
if (need_block) {
if (_down) {
return false;
} else {
_pollfd.fd = _fd;
_pollfd.events |= POLLIN;
stats.num_rx_poll_req++;
if (_hasReadyTx() && write_select) {
_pollfd.events |= POLLOUT;
stats.num_tx_poll_req++;
}
}
if (_evt_handle != nullptr && blocking_deadline > AP_HAL::native_micros64()) {
_evt_handle->wait(blocking_deadline - AP_HAL::native_micros64());
}
}
// Writing the output masks
if (!_down) {
write_select = true; // Always ready to write if not down
} else {
write_select = false;
}
if (_hasReadyRx()) {
read_select = true; // Readability depends only on RX buf, even if down
} else {
read_select = false;
}
// Return value is irrelevant as long as it's non-negative
return true;
}
bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) {
_evt_handle = handle;
evt_can_socket[_self_index]._ifaces[_self_index] = this;
_evt_handle->set_source(&evt_can_socket[_self_index]);
return true;
}
bool CANIface::CANSocketEventSource::wait(uint64_t duration, AP_HAL::EventHandle* evt_handle)
{
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;
}
if (_ifaces[i]->_down) {
continue;
}
pollfds[num_pollfds] = _ifaces[i]->_pollfd;
pollfd_iface_map[num_pollfds] = i;
num_pollfds++;
_ifaces[i]->stats.num_poll_waits++;
}
if (num_pollfds == 0) {
return true;
}
// Timeout conversion
auto ts = timespec();
ts.tv_sec = duration / 1000000LL;
ts.tv_nsec = (duration % 1000000LL) * 1000;
// Blocking here
const int res = ppoll(pollfds, num_pollfds, &ts, nullptr);
if (res < 0) {
return false;
}
// Handling poll output
for (unsigned i = 0; i < num_pollfds; i++) {
if (_ifaces[pollfd_iface_map[i]] == nullptr) {
continue;
}
_ifaces[pollfd_iface_map[i]]->_updateDownStatusFromPollResult(pollfds[i]);
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;
}
uint32_t CANIface::get_stats(char* data, uint32_t max_size)
{
if (data == nullptr) {
return 0;
}
uint32_t ret = snprintf(data, max_size,
"tx_requests: %u\n"
"tx_write_fail: %u\n"
"tx_full: %u\n"
"tx_confirmed: %u\n"
"tx_success: %u\n"
"tx_timedout: %u\n"
"rx_received: %u\n"
"rx_errors: %u\n"
"num_downs: %u\n"
"num_rx_poll_req: %u\n"
"num_tx_poll_req: %u\n"
"num_poll_waits: %u\n"
"num_poll_tx_events: %u\n"
"num_poll_rx_events: %u\n",
stats.tx_requests,
stats.tx_write_fail,
stats.tx_full,
stats.tx_confirmed,
stats.tx_success,
stats.tx_timedout,
stats.rx_received,
stats.rx_errors,
stats.num_downs,
stats.num_rx_poll_req,
stats.num_tx_poll_req,
stats.num_poll_waits,
stats.num_poll_tx_events,
stats.num_poll_rx_events);
return ret;
}
#endif

197
libraries/AP_HAL_Linux/CANSocketIface.h Normal file
View File

@ -0,0 +1,197 @@
/*
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/LICENSE
* modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
*/
#pragma once
#include "AP_HAL_Linux.h"
#if HAL_NUM_CAN_IFACES
#include <AP_HAL/CANIface.h>
#include <linux/can.h>
#include <string>
#include <queue>
#include <memory>
#include <map>
#include <unordered_set>
#include <poll.h>
namespace Linux {
enum class SocketCanError
{
SocketReadFailure,
SocketWriteFailure,
TxTimeout
};
#define CAN_MAX_POLL_ITERATIONS_COUNT 100
#define CAN_MAX_INIT_TRIES_COUNT 100
#define CAN_FILTER_NUMBER 8
class CANIface: public AP_HAL::CANIface {
public:
CANIface(int index)
: _self_index(index)
, _frames_in_socket_tx_queue(0)
, _max_frames_in_socket_tx_queue(2)
{ }
~CANIface() { }
// Initialise CAN Peripheral
bool init(const uint32_t bitrate, const OperatingMode mode) override;
// Put frame into Tx FIFO returns negative on error, 0 on buffer full,
// 1 on successfully pushing a frame into FIFO
int16_t send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
CanIOFlags flags) override;
// Receive frame from Rx Buffer, returns negative on error, 0 on nothing available,
// 1 on successfully poping a frame
int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
CanIOFlags& out_flags) override;
// Set Filters to ignore frames not to be handled by us
bool configureFilters(const CanFilterConfig* filter_configs,
uint16_t num_configs) override;
// Always return false, there's no busoff condition in Linux CAN
bool is_busoff() const override
{
return false;
}
void flush_tx() override;
void clear_rx() override;
// Get number of Filter configurations
uint16_t getNumFilters() const override;
// Get total number of Errors discovered
uint32_t getErrorCount() const override;
// returns true if init was successfully called
bool is_initialized() const override;
/******************************************
* Select Method *
* ****************************************/
// wait until selected event is available, false when timed out waiting else true
bool select(bool &read, bool &write,
const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override;
// setup event handle for waiting on events
bool set_event_handle(AP_HAL::EventHandle* handle) override;
// fetch stats text and return the size of the same,
// results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt
uint32_t get_stats(char* data, uint32_t max_size) override;
class CANSocketEventSource : public AP_HAL::EventSource {
friend class CANIface;
CANIface *_ifaces[HAL_NUM_CAN_IFACES];
public:
// we just poll fd, no signaling is done
void signal(uint32_t evt_mask) override { return; }
bool wait(uint64_t duration, AP_HAL::EventHandle* evt_handle) override;
};
private:
void _pollWrite();
bool _pollRead();
int _write(const AP_HAL::CANFrame& frame) const;
int _read(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const;
void _incrementNumFramesInSocketTxQueue();
void _confirmSentFrame();
bool _wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame);
bool _checkHWFilters(const can_frame& frame) const;
bool _hasReadyTx() const;
bool _hasReadyRx() const;
void _poll(bool read, bool write);
int _openSocket(const std::string& iface_name);
void _updateDownStatusFromPollResult(const pollfd& pfd);
uint32_t _bitrate;
bool _down;
bool _initialized;
int _fd;
const uint8_t _self_index;
const unsigned _max_frames_in_socket_tx_queue;
unsigned _frames_in_socket_tx_queue;
uint32_t _tx_frame_counter;
AP_HAL::EventHandle *_evt_handle;
static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES];
pollfd _pollfd;
std::map<SocketCanError, uint64_t> _errors;
std::priority_queue<CanTxItem> _tx_queue;
std::queue<CanRxItem> _rx_queue;
std::unordered_multiset<uint32_t> _pending_loopback_ids;
std::vector<can_filter> _hw_filters_container;
struct {
uint32_t tx_requests;
uint32_t tx_full;
uint32_t tx_confirmed;
uint32_t tx_write_fail;
uint32_t tx_success;
uint32_t tx_timedout;
uint32_t rx_received;
uint32_t rx_errors;
uint32_t num_downs;
uint32_t num_rx_poll_req;
uint32_t num_tx_poll_req;
uint32_t num_poll_waits;
uint32_t num_poll_tx_events;
uint32_t num_poll_rx_events;
} stats;
};
}
#endif //#if HAL_NUM_CAN_IFACES

12
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -44,6 +44,7 @@
#include "UARTDriver.h"
#include "Util.h"
#include "Util_RPI.h"
#include "CANSocketIface.h"
using namespace Linux;
@ -218,6 +219,10 @@ static Empty::OpticalFlow opticalFlow;
static Empty::DSP dspDriver;
static Empty::Flash flashDriver;
#if HAL_NUM_CAN_IFACES
static CANIface* canDrivers[HAL_NUM_CAN_IFACES];
#endif
HAL_Linux::HAL_Linux() :
AP_HAL::HAL(
&uartADriver,
@ -241,7 +246,12 @@ HAL_Linux::HAL_Linux() :
&opticalFlow,
&flashDriver,
&dspDriver,
nullptr)
#if HAL_NUM_CAN_IFACES
(AP_HAL::CANIface**)canDrivers
#else
nullptr
#endif
)
{}
void _usage(void)

7
libraries/AP_HAL_Linux/HAL_Linux_Class.h
View File

@ -14,3 +14,10 @@ public:
protected:
bool _should_exit = false;
};
#if HAL_NUM_CAN_IFACES
namespace Linux {
class CANIface;
}
typedef Linux::CANIface HAL_CANIface;
#endif