First implementation

CanDriver/driver/SocketCanDriver/SocketCanDriver.cpp

@@ -0,0 +1,99 @@
+/*
+
+  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 "SocketCanDriver.h"
+#include "SocketCanInterface.h"
+#include <core/Backend.h>
+#include <driver/GenericCanSetupPage.h>
+
+#include <sys/socket.h>
+#include <linux/if.h>
+#include <linux/if_arp.h>
+#include <linux/can/netlink.h>
+#include <netlink/route/link.h>
+#include <netlink/route/link/can.h>
+#include <errno.h>
+#include <cstring>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+
+SocketCanDriver::SocketCanDriver(Backend &backend)
+  : CanDriver(backend),
+    setupPage(new GenericCanSetupPage())
+{
+    QObject::connect(&backend, SIGNAL(onSetupDialogCreated(SetupDialog&)), setupPage, SLOT(onSetupDialogCreated(SetupDialog&)));
+}
+
+SocketCanDriver::~SocketCanDriver() {
+}
+
+bool SocketCanDriver::update() {
+
+    deleteAllInterfaces();
+
+    struct nl_sock *sock = nl_socket_alloc();
+    struct nl_cache *cache;
+
+    nl_connect(sock, NETLINK_ROUTE);
+    int result = rtnl_link_alloc_cache(sock, AF_UNSPEC, &cache);
+
+    if (result < 0) {
+        log_error(QString("Could not access netlink device list: %1").arg(result));
+    } else {
+
+        for (struct nl_object *obj = nl_cache_get_first(cache); obj!=0; obj=nl_cache_get_next(obj)) {
+            struct rtnl_link *link = (struct rtnl_link *)obj;
+
+            if (rtnl_link_get_arptype(link)==ARPHRD_CAN) {
+                SocketCanInterface *intf = createOrUpdateInterface(rtnl_link_get_ifindex(link), QString(rtnl_link_get_name(link)));
+                intf->readConfigFromLink(link);
+            }
+        }
+    }
+
+    nl_cache_free(cache);
+    nl_close(sock);
+    nl_socket_free(sock);
+
+    return true;
+}
+
+QString SocketCanDriver::getName() {
+	return "SocketCAN";
+}
+
+SocketCanInterface *SocketCanDriver::createOrUpdateInterface(int index, QString name) {
+
+    foreach (CanInterface *intf, getInterfaces()) {
+        SocketCanInterface *scif = dynamic_cast<SocketCanInterface*>(intf);
+		if (scif->getIfIndex() == index) {
+			scif->setName(name);
+            return scif;
+		}
+	}
+
+
+    SocketCanInterface *scif = new SocketCanInterface(this, index, name);
+    addInterface(scif);
+    return scif;
+}

CanDriver/driver/SocketCanDriver/SocketCanDriver.h

@@ -0,0 +1,43 @@
+/*
+
+  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/>.
+
+*/
+
+#pragma once
+
+#include <QString>
+#include <core/Backend.h>
+#include <driver/CanDriver.h>
+
+class SocketCanInterface;
+class SetupDialogInterfacePage;
+class GenericCanSetupPage;
+
+class SocketCanDriver: public CanDriver {
+public:
+    SocketCanDriver(Backend &backend);
+    virtual ~SocketCanDriver();
+
+    virtual QString getName();
+    virtual bool update();
+
+private:
+    SocketCanInterface *createOrUpdateInterface(int index, QString name);
+    GenericCanSetupPage *setupPage;
+};

CanDriver/driver/SocketCanDriver/SocketCanDriver.pri

@@ -0,0 +1,12 @@
+CONFIG += c++11
+
+SOURCES += \
+    $$PWD/SocketCanInterface.cpp \
+    $$PWD/SocketCanDriver.cpp
+
+HEADERS  += \
+    $$PWD/SocketCanInterface.h \
+    $$PWD/SocketCanDriver.h
+
+FORMS +=
+

CanDriver/driver/SocketCanDriver/SocketCanInterface.cpp

@@ -0,0 +1,472 @@
+/*
+
+  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;
+    }
+}

CanDriver/driver/SocketCanDriver/SocketCanInterface.h

@@ -0,0 +1,115 @@
+/*
+
+  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/>.
+
+*/
+
+#pragma once
+
+#include "../CanInterface.h"
+#include <linux/can/netlink.h>
+
+class SocketCanDriver;
+
+typedef struct {
+    bool supports_canfd;
+    bool supports_timing;
+    uint32_t state;
+    uint32_t base_freq;
+    uint32_t sample_point;
+    uint32_t ctrl_mode;
+    uint32_t restart_ms;
+    struct can_bittiming bit_timing;
+} can_config_t;
+
+typedef struct {
+    uint32_t can_state;
+
+    uint64_t rx_count;
+    int rx_errors;
+    uint64_t rx_overruns;
+
+    uint64_t tx_count;
+    int tx_errors;
+    uint64_t tx_dropped;
+} can_status_t;
+
+class SocketCanInterface: public CanInterface {
+public:
+    SocketCanInterface(SocketCanDriver *driver, int index, QString name);
+	virtual ~SocketCanInterface();
+
+    virtual QString getName() const;
+    void setName(QString name);
+
+    virtual QList<CanTiming> getAvailableBitrates();
+
+    virtual void applyConfig(const MeasurementInterface &mi);
+    virtual bool readConfig();
+    virtual bool readConfigFromLink(struct rtnl_link *link);
+
+    bool supportsTimingConfiguration();
+    bool supportsCanFD();
+    bool supportsTripleSampling();
+
+    virtual unsigned getBitrate();
+    virtual uint32_t getCapabilities();
+
+
+	virtual void open();
+    virtual bool isOpen();
+	virtual void close();
+
+    virtual void sendMessage(const CanMessage &msg);
+    virtual bool readMessage(QList<CanMessage> &msglist, unsigned int timeout_ms);
+
+    virtual bool updateStatistics();
+    virtual uint32_t getState();
+    virtual int getNumRxFrames();
+    virtual int getNumRxErrors();
+    virtual int getNumRxOverruns();
+
+    virtual int getNumTxFrames();
+    virtual int getNumTxErrors();
+    virtual int getNumTxDropped();
+
+
+    int getIfIndex();
+
+private:
+    typedef enum {
+        ts_mode_SIOCSHWTSTAMP,
+        ts_mode_SIOCGSTAMPNS,
+        ts_mode_SIOCGSTAMP
+    } ts_mode_t;
+
+    int _idx;
+    bool _isOpen;
+	int _fd;
+    QString _name;
+
+    can_config_t _config;
+    can_status_t _status;
+    ts_mode_t _ts_mode;
+
+    const char *cname();
+    bool updateStatus();
+
+    QString buildIpRouteCmd(const MeasurementInterface &mi);
+    QStringList buildCanIfConfigArgs(const MeasurementInterface &mi);
+};