473 lines
11 KiB
C++
473 lines
11 KiB
C++
/*
|
|
|
|
Copyright (c) 2015, 2016 Hubert Denkmair
|
|
|
|
This file is part of cangaroo.
|
|
|
|
cangaroo 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 2 of the License, or
|
|
(at your option) any later version.
|
|
|
|
cangaroo 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 cangaroo. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
#include "SocketCanInterface.h"
|
|
|
|
#include <core/Backend.h>
|
|
#include <core/MeasurementInterface.h>
|
|
#include <core/CanMessage.h>
|
|
|
|
#include <stdio.h>
|
|
#include <unistd.h>
|
|
#include <time.h>
|
|
#include <QString>
|
|
#include <QStringList>
|
|
#include <QProcess>
|
|
#include <sys/types.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/time.h>
|
|
|
|
#include <linux/if.h>
|
|
#include <linux/can.h>
|
|
#include <linux/can/raw.h>
|
|
#include <linux/can/netlink.h>
|
|
#include <linux/sockios.h>
|
|
#include <netlink/version.h>
|
|
#include <netlink/route/link.h>
|
|
#include <netlink/route/link/can.h>
|
|
|
|
SocketCanInterface::SocketCanInterface(SocketCanDriver *driver, int index, QString name)
|
|
: CanInterface((CanDriver *)driver),
|
|
_idx(index),
|
|
_isOpen(false),
|
|
_fd(0),
|
|
_name(name),
|
|
_ts_mode(ts_mode_SIOCSHWTSTAMP)
|
|
{
|
|
}
|
|
|
|
SocketCanInterface::~SocketCanInterface() {
|
|
}
|
|
|
|
QString SocketCanInterface::getName() const {
|
|
return _name;
|
|
}
|
|
|
|
void SocketCanInterface::setName(QString name) {
|
|
_name = name;
|
|
}
|
|
|
|
QList<CanTiming> SocketCanInterface::getAvailableBitrates()
|
|
{
|
|
QList<CanTiming> retval;
|
|
QList<unsigned> bitrates({10000, 20000, 50000, 83333, 100000, 125000, 250000, 500000, 800000, 1000000});
|
|
QList<unsigned> samplePoints({500, 625, 750, 875});
|
|
|
|
unsigned i=0;
|
|
foreach (unsigned br, bitrates) {
|
|
foreach (unsigned sp, samplePoints) {
|
|
retval << CanTiming(i++, br, 0, sp);
|
|
}
|
|
}
|
|
|
|
return retval;
|
|
}
|
|
|
|
QString SocketCanInterface::buildIpRouteCmd(const MeasurementInterface &mi)
|
|
{
|
|
QStringList cmd;
|
|
cmd.append("ip");
|
|
cmd.append("link");
|
|
cmd.append("set");
|
|
cmd.append(getName());
|
|
cmd.append("up");
|
|
cmd.append("type");
|
|
cmd.append("can");
|
|
|
|
cmd.append("bitrate");
|
|
cmd.append(QString().number(mi.bitrate()));
|
|
cmd.append("sample-point");
|
|
cmd.append(QString().number((float)mi.samplePoint()/1000.0, 'f', 3));
|
|
|
|
if (mi.isCanFD()) {
|
|
cmd.append("dbitrate");
|
|
cmd.append(QString().number(mi.fdBitrate()));
|
|
cmd.append("dsample-point");
|
|
cmd.append(QString().number((float)mi.fdSamplePoint()/1000.0, 'f', 3));
|
|
cmd.append("fd");
|
|
cmd.append("on");
|
|
}
|
|
|
|
cmd.append("restart-ms");
|
|
if (mi.doAutoRestart()) {
|
|
cmd.append(QString().number(mi.autoRestartMs()));
|
|
} else {
|
|
cmd.append("0");
|
|
}
|
|
|
|
return cmd.join(' ');
|
|
}
|
|
|
|
QStringList SocketCanInterface::buildCanIfConfigArgs(const MeasurementInterface &mi)
|
|
{
|
|
QStringList args;
|
|
args << "-d";
|
|
args << "-i" << getName();
|
|
args << "-b" << QString::number(mi.bitrate());
|
|
args << "-p" << QString::number(mi.samplePoint());
|
|
args << "-u";
|
|
return args;
|
|
}
|
|
|
|
|
|
void SocketCanInterface::applyConfig(const MeasurementInterface &mi)
|
|
{
|
|
if (!mi.doConfigure()) {
|
|
log_info(QString("interface %1 not managed by cangaroo, not touching configuration").arg(getName()));
|
|
return;
|
|
}
|
|
|
|
log_info(QString("calling canifconfig to reconfigure interface %1").arg(getName()));
|
|
QStringList sl = buildCanIfConfigArgs(mi);
|
|
sl.prepend("canifconfig");
|
|
log_info(sl.join(" "));
|
|
|
|
QProcess canIfConfig;
|
|
canIfConfig.start("canifconfig", buildCanIfConfigArgs(mi));
|
|
if (!canIfConfig.waitForFinished()) {
|
|
log_error(QString("timeout waiting for canifconfig"));
|
|
return;
|
|
}
|
|
|
|
if (canIfConfig.exitStatus()!=QProcess::NormalExit) {
|
|
log_error(QString("canifconfig crashed"));
|
|
return;
|
|
}
|
|
|
|
if (canIfConfig.exitCode() != 0) {
|
|
log_error(QString("canifconfig failed: ") + QString(canIfConfig.readAllStandardError()).trimmed());
|
|
return;
|
|
}
|
|
|
|
}
|
|
|
|
#if (LIBNL_CURRENT<=216)
|
|
#warning we need at least libnl3 version 3.2.22 to be able to get link status via netlink
|
|
int rtnl_link_can_state(struct rtnl_link *link, uint32_t *state) {
|
|
(void) link;
|
|
(void) state;
|
|
return -1;
|
|
}
|
|
#endif
|
|
|
|
bool SocketCanInterface::updateStatus()
|
|
{
|
|
bool retval = false;
|
|
|
|
struct nl_sock *sock = nl_socket_alloc();
|
|
struct nl_cache *cache;
|
|
struct rtnl_link *link;
|
|
uint32_t state;
|
|
|
|
_status.can_state = state_unknown;
|
|
|
|
nl_connect(sock, NETLINK_ROUTE);
|
|
if (rtnl_link_alloc_cache(sock, AF_UNSPEC, &cache) >= 0) {
|
|
if (rtnl_link_get_kernel(sock, _idx, 0, &link) == 0) {
|
|
|
|
_status.rx_count = rtnl_link_get_stat(link, RTNL_LINK_RX_PACKETS);
|
|
_status.rx_overruns = rtnl_link_get_stat(link, RTNL_LINK_RX_OVER_ERR);
|
|
_status.tx_count = rtnl_link_get_stat(link, RTNL_LINK_TX_PACKETS);
|
|
_status.tx_dropped = rtnl_link_get_stat(link, RTNL_LINK_TX_DROPPED);
|
|
|
|
if (rtnl_link_is_can(link)) {
|
|
if (rtnl_link_can_state(link, &state)==0) {
|
|
_status.can_state = state;
|
|
}
|
|
_status.rx_errors = rtnl_link_can_berr_rx(link);
|
|
_status.tx_errors = rtnl_link_can_berr_tx(link);
|
|
} else {
|
|
_status.rx_errors = 0;
|
|
_status.tx_errors = 0;
|
|
}
|
|
retval = true;
|
|
}
|
|
}
|
|
|
|
nl_cache_free(cache);
|
|
nl_close(sock);
|
|
nl_socket_free(sock);
|
|
|
|
return retval;
|
|
}
|
|
|
|
bool SocketCanInterface::readConfig()
|
|
{
|
|
bool retval = false;
|
|
|
|
struct nl_sock *sock = nl_socket_alloc();
|
|
struct nl_cache *cache;
|
|
struct rtnl_link *link;
|
|
|
|
nl_connect(sock, NETLINK_ROUTE);
|
|
int result = rtnl_link_alloc_cache(sock, AF_UNSPEC, &cache);
|
|
|
|
if (result>=0) {
|
|
if (rtnl_link_get_kernel(sock, _idx, 0, &link) == 0) {
|
|
retval = readConfigFromLink(link);
|
|
}
|
|
}
|
|
|
|
nl_cache_free(cache);
|
|
nl_close(sock);
|
|
nl_socket_free(sock);
|
|
|
|
return retval;
|
|
}
|
|
|
|
bool SocketCanInterface::readConfigFromLink(rtnl_link *link)
|
|
{
|
|
_config.state = state_unknown;
|
|
_config.supports_canfd = (rtnl_link_get_mtu(link)==72);
|
|
_config.supports_timing = rtnl_link_is_can(link);
|
|
if (_config.supports_timing) {
|
|
rtnl_link_can_freq(link, &_config.base_freq);
|
|
rtnl_link_can_get_ctrlmode(link, &_config.ctrl_mode);
|
|
rtnl_link_can_get_bittiming(link, &_config.bit_timing);
|
|
rtnl_link_can_get_sample_point(link, &_config.sample_point);
|
|
rtnl_link_can_get_restart_ms(link, &_config.restart_ms);
|
|
} else {
|
|
// maybe a vcan interface?
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool SocketCanInterface::supportsTimingConfiguration()
|
|
{
|
|
return _config.supports_timing;
|
|
}
|
|
|
|
bool SocketCanInterface::supportsCanFD()
|
|
{
|
|
return _config.supports_canfd;
|
|
}
|
|
|
|
bool SocketCanInterface::supportsTripleSampling()
|
|
{
|
|
return false;
|
|
}
|
|
|
|
unsigned SocketCanInterface::getBitrate() {
|
|
if (readConfig()) {
|
|
return _config.bit_timing.bitrate;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
uint32_t SocketCanInterface::getCapabilities()
|
|
{
|
|
uint32_t retval =
|
|
CanInterface::capability_config_os |
|
|
CanInterface::capability_listen_only |
|
|
CanInterface::capability_auto_restart;
|
|
|
|
if (supportsCanFD()) {
|
|
retval |= CanInterface::capability_canfd;
|
|
}
|
|
|
|
if (supportsTripleSampling()) {
|
|
retval |= CanInterface::capability_triple_sampling;
|
|
}
|
|
|
|
return retval;
|
|
}
|
|
|
|
bool SocketCanInterface::updateStatistics()
|
|
{
|
|
return updateStatus();
|
|
}
|
|
|
|
uint32_t SocketCanInterface::getState()
|
|
{
|
|
switch (_status.can_state) {
|
|
case CAN_STATE_ERROR_ACTIVE: return state_ok;
|
|
case CAN_STATE_ERROR_WARNING: return state_warning;
|
|
case CAN_STATE_ERROR_PASSIVE: return state_passive;
|
|
case CAN_STATE_BUS_OFF: return state_bus_off;
|
|
case CAN_STATE_STOPPED: return state_stopped;
|
|
default: return state_unknown;
|
|
}
|
|
}
|
|
|
|
int SocketCanInterface::getNumRxFrames()
|
|
{
|
|
return _status.rx_count;
|
|
}
|
|
|
|
int SocketCanInterface::getNumRxErrors()
|
|
{
|
|
return _status.rx_errors;
|
|
}
|
|
|
|
int SocketCanInterface::getNumTxFrames()
|
|
{
|
|
return _status.tx_count;
|
|
}
|
|
|
|
int SocketCanInterface::getNumTxErrors()
|
|
{
|
|
return _status.tx_errors;
|
|
}
|
|
|
|
int SocketCanInterface::getNumRxOverruns()
|
|
{
|
|
return _status.rx_overruns;
|
|
}
|
|
|
|
int SocketCanInterface::getNumTxDropped()
|
|
{
|
|
return _status.tx_dropped;
|
|
}
|
|
|
|
int SocketCanInterface::getIfIndex() {
|
|
return _idx;
|
|
}
|
|
|
|
const char *SocketCanInterface::cname()
|
|
{
|
|
return _name.toStdString().c_str();
|
|
}
|
|
|
|
void SocketCanInterface::open() {
|
|
if((_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
|
perror("Error while opening socket");
|
|
_isOpen = false;
|
|
}
|
|
|
|
struct ifreq ifr;
|
|
struct sockaddr_can addr;
|
|
strcpy(ifr.ifr_name, _name.toStdString().c_str());
|
|
ioctl(_fd, SIOCGIFINDEX, &ifr);
|
|
|
|
addr.can_family = AF_CAN;
|
|
addr.can_ifindex = ifr.ifr_ifindex;
|
|
|
|
if(bind(_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
|
perror("Error in socket bind");
|
|
_isOpen = false;
|
|
}
|
|
|
|
_isOpen = true;
|
|
}
|
|
|
|
bool SocketCanInterface::isOpen()
|
|
{
|
|
return _isOpen;
|
|
}
|
|
|
|
void SocketCanInterface::close() {
|
|
::close(_fd);
|
|
_isOpen = false;
|
|
}
|
|
|
|
void SocketCanInterface::sendMessage(const CanMessage &msg) {
|
|
struct can_frame frame;
|
|
|
|
frame.can_id = msg.getId();
|
|
|
|
if (msg.isExtended()) {
|
|
frame.can_id |= CAN_EFF_FLAG;
|
|
}
|
|
|
|
if (msg.isRTR()) {
|
|
frame.can_id |= CAN_RTR_FLAG;
|
|
}
|
|
|
|
if (msg.isErrorFrame()) {
|
|
frame.can_id |= CAN_ERR_FLAG;
|
|
}
|
|
|
|
uint8_t len = msg.getLength();
|
|
if (len>8) { len = 8; }
|
|
|
|
frame.can_dlc = len;
|
|
for (int i=0; i<len; i++) {
|
|
frame.data[i] = msg.getByte(i);
|
|
}
|
|
|
|
::write(_fd, &frame, sizeof(struct can_frame));
|
|
}
|
|
|
|
bool SocketCanInterface::readMessage(QList<CanMessage> &msglist, unsigned int timeout_ms) {
|
|
|
|
struct can_frame frame;
|
|
struct timespec ts_rcv;
|
|
struct timeval tv_rcv;
|
|
struct timeval timeout;
|
|
//struct ifreq hwtstamp;
|
|
fd_set fdset;
|
|
|
|
timeout.tv_sec = timeout_ms / 1000;
|
|
timeout.tv_usec = 1000 * (timeout_ms % 1000);
|
|
|
|
FD_ZERO(&fdset);
|
|
FD_SET(_fd, &fdset);
|
|
|
|
CanMessage msg;
|
|
|
|
int rv = select(_fd+1, &fdset, NULL, NULL, &timeout);
|
|
if (rv>0) {
|
|
|
|
if (read(_fd, &frame, sizeof(struct can_frame)) < 0) {
|
|
return false;
|
|
}
|
|
|
|
if (_ts_mode == ts_mode_SIOCSHWTSTAMP) {
|
|
// TODO implement me
|
|
_ts_mode = ts_mode_SIOCGSTAMPNS;
|
|
}
|
|
|
|
if (_ts_mode==ts_mode_SIOCGSTAMPNS) {
|
|
if (ioctl(_fd, SIOCGSTAMPNS, &ts_rcv) == 0) {
|
|
msg.setTimestamp(ts_rcv.tv_sec, ts_rcv.tv_nsec/1000);
|
|
} else {
|
|
_ts_mode = ts_mode_SIOCGSTAMP;
|
|
}
|
|
}
|
|
|
|
if (_ts_mode==ts_mode_SIOCGSTAMP) {
|
|
ioctl(_fd, SIOCGSTAMP, &tv_rcv);
|
|
msg.setTimestamp(tv_rcv.tv_sec, tv_rcv.tv_usec);
|
|
}
|
|
|
|
msg.setId(frame.can_id);
|
|
msg.setExtended((frame.can_id & CAN_EFF_FLAG)!=0);
|
|
msg.setRTR((frame.can_id & CAN_RTR_FLAG)!=0);
|
|
msg.setErrorFrame((frame.can_id & CAN_ERR_FLAG)!=0);
|
|
msg.setInterfaceId(getId());
|
|
|
|
uint8_t len = frame.can_dlc;
|
|
if (len>8) { len = 8; }
|
|
|
|
msg.setLength(len);
|
|
for (int i=0; i<len; i++) {
|
|
msg.setByte(i, frame.data[i]);
|
|
}
|
|
|
|
msglist.append(msg);
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|