fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-08 04:08:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

socketcan HW filters moved to userspace

Browse Source
This commit is contained in:
Ilia
2015-12-14 00:20:35 +00:00
parent 426f349e36
commit a2ed997cb4
1 changed files with 48 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
+48 -11
View File
@@ -148,6 +148,9 @@ class SocketCanIface : public uavcan::ICanIface
std::queue<RxItem> rx_queue_; // TODO: Use pool allocator
std::unordered_multiset<std::uint32_t> pending_loopback_ids_; // TODO: Use pool allocator
std::vector<uavcan::CanFilterConfig> hw_filters_container_;
bool filters_configured_ = false;
void registerError(SocketCanError e) { errors_[e]++; }
void incrementNumFramesInSocketTxQueue()
@@ -301,7 +304,17 @@ class SocketCanIface : public uavcan::ICanIface
if (accept)
{
rx.ts_utc += clock_.getPrivateAdjustment();
rx_queue_.push(rx);
if (filters_configured_)
{
if (checkHWFilters(rx.frame))
{
rx_queue_.push(rx);
}
}
else
{
rx_queue_.push(rx);
}
}
}
else if (res == 0)
@@ -315,6 +328,22 @@ class SocketCanIface : public uavcan::ICanIface
}
}
/**
* Returns true if a frame accepted by HW filters
*/
bool checkHWFilters(const uavcan::CanFrame frame)
{
uint16_t container_size = hw_filters_container_.size();
for (uint16_t i = 0; i < container_size; i++)
{
if (((frame.id & hw_filters_container_[i].mask) ^ hw_filters_container_[i].id) == 0)
{
return 1;
}
}
return 0;
}
public:
/**
* Takes ownership of socket's file descriptor.
@@ -404,38 +433,46 @@ public:
assert(0);
return -1;
}
std::vector< ::can_filter> filts(num_configs);
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];
filts[i].can_id = fc.id & uavcan::CanFrame::MaskExtID;
filts[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID;
hw_filters_container_[i].id = fc.id & uavcan::CanFrame::MaskExtID;
hw_filters_container_[i].mask = fc.mask & uavcan::CanFrame::MaskExtID;
if (fc.id & uavcan::CanFrame::FlagEFF)
{
filts[i].can_id |= CAN_EFF_FLAG;
hw_filters_container_[i].id |= CAN_EFF_FLAG;
}
if (fc.id & uavcan::CanFrame::FlagRTR)
{
filts[i].can_id |= CAN_RTR_FLAG;
hw_filters_container_[i].id |= CAN_RTR_FLAG;
}
if (fc.mask & uavcan::CanFrame::FlagEFF)
{
filts[i].can_mask |= CAN_EFF_FLAG;
hw_filters_container_[i].mask |= CAN_EFF_FLAG;
}
if (fc.mask & uavcan::CanFrame::FlagRTR)
{
filts[i].can_mask |= CAN_RTR_FLAG;
hw_filters_container_[i].mask |= CAN_RTR_FLAG;
}
}
int ret = setsockopt(fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filts.data(), sizeof(::can_filter) * num_configs);
return (ret < 0) ? -1 : 0;
if (! hw_filters_container_.empty())
{
filters_configured_ = true;
return 0;
}
return -1;
}
/**
* SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited.
* This method returns a constant value.
*/
std::uint16_t getNumFilters() const override { return 255; }
std::uint16_t getNumFilters() const override { return 8; }
/**
* Returns total number of errors of each kind detected since the object was created.