First implementation

CanDriver/driver/SLCANDriver/SLCANInterface.cpp

@@ -0,0 +1,742 @@
+/*
+
+  Copyright (c) 2022 Ethan Zonca
+
+  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 "SLCANInterface.h"
+
+#include <core/Backend.h>
+#include <core/MeasurementInterface.h>
+#include <core/CanMessage.h>
+
+#include <stdio.h>
+#include <unistd.h>
+#include <time.h>
+#include <fcntl.h>
+#include <QString>
+#include <QStringList>
+#include <QProcess>
+#include <QtSerialPort/QSerialPort>
+#include <QtSerialPort/QSerialPortInfo>
+#include <QThread>
+
+SLCANInterface::SLCANInterface(SLCANDriver *driver, int index, QString name, bool fd_support)
+  : CanInterface((CanDriver *)driver),
+	_idx(index),
+    _isOpen(false),
+    _serport(NULL),
+    _msg_queue(),
+    _name(name),
+    _rx_linbuf_ctr(0),
+    _rxbuf_head(0),
+    _rxbuf_tail(0),
+    _ts_mode(ts_mode_SIOCSHWTSTAMP)
+{
+    // Set defaults
+    _settings.setBitrate(500000);
+    _settings.setSamplePoint(875);
+
+    _config.supports_canfd = fd_support;
+}
+
+SLCANInterface::~SLCANInterface() {
+}
+
+QString SLCANInterface::getDetailsStr() const {
+    if(_config.supports_canfd)
+    {
+        return "CANable with CANFD support";
+    }
+    else
+    {
+        return "CANable with standard CAN support";
+    }
+}
+
+QString SLCANInterface::getName() const {
+	return _name;
+}
+
+void SLCANInterface::setName(QString name) {
+    _name = name;
+}
+
+QList<CanTiming> SLCANInterface::getAvailableBitrates()
+{
+    QList<CanTiming> retval;
+    QList<unsigned> bitrates({10000, 20000, 50000, 83333, 100000, 125000, 250000, 500000, 800000, 1000000});
+    QList<unsigned> bitrates_fd({0, 2000000, 5000000});
+
+    QList<unsigned> samplePoints({875});
+
+    unsigned i=0;
+    foreach (unsigned br, bitrates) {
+        foreach(unsigned br_fd, bitrates_fd) {
+            foreach (unsigned sp, samplePoints) {
+                retval << CanTiming(i++, br, br_fd, sp);
+            }
+        }
+    }
+
+    return retval;
+}
+
+
+void SLCANInterface::applyConfig(const MeasurementInterface &mi)
+{
+    // Save settings for port configuration
+    _settings = mi;
+}
+
+bool SLCANInterface::updateStatus()
+{
+    return false;
+}
+
+bool SLCANInterface::readConfig()
+{
+    return false;
+}
+
+bool SLCANInterface::readConfigFromLink(rtnl_link *link)
+{
+    return false;
+}
+
+bool SLCANInterface::supportsTimingConfiguration()
+{
+    return _config.supports_timing;
+}
+
+bool SLCANInterface::supportsCanFD()
+{
+    return _config.supports_canfd;
+}
+
+bool SLCANInterface::supportsTripleSampling()
+{
+    return false;
+}
+
+unsigned SLCANInterface::getBitrate()
+{
+    return _settings.bitrate();
+}
+
+uint32_t SLCANInterface::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 SLCANInterface::updateStatistics()
+{
+    return updateStatus();
+}
+
+uint32_t SLCANInterface::getState()
+{
+    if(_isOpen)
+        return state_ok;
+    else
+        return state_bus_off;
+}
+
+int SLCANInterface::getNumRxFrames()
+{
+    return _status.rx_count;
+}
+
+int SLCANInterface::getNumRxErrors()
+{
+    return _status.rx_errors;
+}
+
+int SLCANInterface::getNumTxFrames()
+{
+    return _status.tx_count;
+}
+
+int SLCANInterface::getNumTxErrors()
+{
+    return _status.tx_errors;
+}
+
+int SLCANInterface::getNumRxOverruns()
+{
+    return _status.rx_overruns;
+}
+
+int SLCANInterface::getNumTxDropped()
+{
+    return _status.tx_dropped;
+}
+
+int SLCANInterface::getIfIndex() {
+    return _idx;
+}
+
+void SLCANInterface::open()
+{
+    if(_serport != NULL)
+    {
+        delete _serport;
+    }
+
+    _serport = new QSerialPort();
+
+    _serport_mutex.lock();
+    _serport->setPortName(_name);
+    _serport->setBaudRate(1000000);
+    _serport->setDataBits(QSerialPort::Data8);
+    _serport->setParity(QSerialPort::NoParity);
+    _serport->setStopBits(QSerialPort::OneStop);
+    _serport->setFlowControl(QSerialPort::NoFlowControl);
+    _serport->setReadBufferSize(2048);
+    if (_serport->open(QIODevice::ReadWrite)) {
+        //perror("Serport connected!");
+    } else {
+        perror("Serport connect failed!");
+        _serport_mutex.unlock();
+        _isOpen = false;
+        return;
+    }
+    _serport->flush();
+    _serport->clear();
+
+    // Set the classic CAN bitrate
+    switch(_settings.bitrate())
+    {
+        case 1000000:
+            _serport->write("S8\r", 3);
+            _serport->flush();
+            break;
+        case 750000:
+            _serport->write("S7\r", 3);
+            _serport->flush();
+            break;
+        case 500000:
+            _serport->write("S6\r", 3);
+            _serport->flush();
+            break;
+        case 250000:
+            _serport->write("S5\r", 3);
+            _serport->flush();
+            break;
+        case 125000:
+            _serport->write("S4\r", 3);
+            _serport->flush();
+            break;
+        case 100000:
+            _serport->write("S3\r", 3);
+            _serport->flush();
+            break;
+        case 83333:
+            _serport->write("S9\r", 3);
+            _serport->flush();
+            break;
+        case 50000:
+            _serport->write("S2\r", 3);
+            _serport->flush();
+            break;
+        case 20000:
+            _serport->write("S1\r", 3);
+            _serport->flush();
+            break;
+        case 10000:
+            _serport->write("S0\r", 3);
+            _serport->flush();
+            break;
+        default:
+            // Default to 10k
+            _serport->write("S0\r", 3);
+            _serport->flush();
+            break;
+    }
+
+    _serport->waitForBytesWritten(300);
+
+
+
+    // Set configured BRS rate
+    if(_config.supports_canfd)
+    {
+        switch(_settings.fdBitrate())
+        {
+            case 2000000:
+                _serport->write("Y2\r", 3);
+                _serport->flush();
+                break;
+            case 5000000:
+                _serport->write("Y5\r", 3);
+                _serport->flush();
+                break;
+        }
+    }
+
+    _serport->waitForBytesWritten(300);
+
+
+    // Open the port
+    _serport->write("O\r\n", 3);
+    _serport->flush();
+
+    _isOpen = true;
+
+    // Release port mutex
+    _serport_mutex.unlock();
+}
+
+void SLCANInterface::close()
+{
+    _serport_mutex.lock();
+
+    if (_serport->isOpen())
+    {
+        // Close CAN port
+        _serport->write("C\r", 2);        
+        _serport->flush();
+        _serport->waitForBytesWritten(300);
+        _serport->clear();
+        _serport->close();
+    }
+
+    _isOpen = false;
+    _serport_mutex.unlock();
+}
+
+bool SLCANInterface::isOpen()
+{
+    return _isOpen;
+}
+
+void SLCANInterface::sendMessage(const CanMessage &msg) {
+
+    // SLCAN_MTU plus null terminator
+    char buf[SLCAN_MTU+1] = {0};
+
+    uint8_t msg_idx = 0;
+
+    // Message is FD
+    // Add character for frame type
+    if(msg.isFD())
+    {
+        if(msg.isBRS())
+        {
+            buf[msg_idx] = 'b';
+
+        }
+        else
+        {
+            buf[msg_idx] = 'd';
+        }
+    }
+    // Message is not FD
+    // Add character for frame type
+    else
+    {
+        if (msg.isRTR()) {
+            buf[msg_idx] = 'r';
+        }
+        else
+        {
+            buf[msg_idx] = 't';
+        }
+    }
+
+    // Assume standard identifier
+    uint8_t id_len = SLCAN_STD_ID_LEN;
+    uint32_t tmp = msg.getId();
+
+    // Check if extended
+    if (msg.isExtended())
+    {
+        // Convert first char to upper case for extended frame
+        buf[msg_idx] -= 32;
+        id_len = SLCAN_EXT_ID_LEN;
+    }
+    msg_idx++;
+
+    // Add identifier to buffer
+    for(uint8_t j = id_len; j > 0; j--)
+    {
+        // Add nibble to buffer
+        buf[j] = (tmp & 0xF);
+        tmp = tmp >> 4;
+        msg_idx++;
+    }
+
+    // Sanity check length
+    int8_t bytes = msg.getLength();
+
+
+    if(bytes < 0)
+        return;
+    if(bytes > 64)
+        return;
+
+    // If canfd
+    if(bytes > 8)
+    {
+        switch(bytes)
+        {
+        case 12:
+            bytes = 0x9;
+            break;
+        case 16:
+            bytes = 0xA;
+            break;
+        case 20:
+            bytes = 0xB;
+            break;
+        case 24:
+            bytes = 0xC;
+            break;
+        case 32:
+            bytes = 0xD;
+            break;
+        case 48:
+            bytes = 0xE;
+            break;
+        case 64:
+            bytes = 0xF;
+            break;
+        }
+    }
+
+    // Add DLC to buffer
+    buf[msg_idx++] = bytes;
+
+    // Add data bytes
+    for (uint8_t j = 0; j < msg.getLength(); j++)
+    {
+        buf[msg_idx++] = (msg.getByte(j) >> 4);
+        buf[msg_idx++] = (msg.getByte(j) & 0x0F);
+    }
+
+    // Convert to ASCII (2nd character to end)
+    for (uint8_t j = 1; j < msg_idx; j++)
+    {
+        if (buf[j] < 0xA) {
+            buf[j] += 0x30;
+        } else {
+            buf[j] += 0x37;
+        }
+    }
+
+    // Add CR for slcan EOL
+    buf[msg_idx++] = '\r';
+
+    // Ensure null termination
+    buf[msg_idx] = '\0';
+
+    _msg_queue.append(QString(buf));
+
+}
+
+bool SLCANInterface::readMessage(QList<CanMessage> &msglist, unsigned int timeout_ms)
+{
+    // Don't saturate the thread. Read the buffer every 1ms.
+    QThread().msleep(1);
+
+    // Transmit all items that are queued
+    while(!_msg_queue.empty())
+    {
+        // Consume first item
+        QString tmp = _msg_queue.front();
+        _msg_queue.pop_front();
+
+        _serport_mutex.lock();
+        // Write string to serial device
+        _serport->write(tmp.toStdString().c_str(), tmp.length());
+        _serport->flush();
+        _serport->waitForBytesWritten(300);
+        _serport_mutex.unlock();
+    }
+
+    // RX doesn't work on windows unless we call this for some reason
+    _serport->waitForReadyRead(1);
+
+    if(_serport->bytesAvailable())
+    {
+        // This is called when readyRead() is emitted
+        QByteArray datas = _serport->readAll();
+        _rxbuf_mutex.lock();
+        for(int i=0; i<datas.count(); i++)
+        {
+            // If incrementing the head will hit the tail, we've filled the buffer. Reset and discard all data.
+            if(((_rxbuf_head + 1) % RXCIRBUF_LEN) == _rxbuf_tail)
+            {
+                _rxbuf_head = 0;
+                _rxbuf_tail = 0;
+            }
+            else
+            {
+                // Put inbound data at the head locatoin
+                _rxbuf[_rxbuf_head] = datas.at(i);
+                _rxbuf_head = (_rxbuf_head + 1) % RXCIRBUF_LEN; // Wrap at MTU
+            }
+        }
+        _rxbuf_mutex.unlock();
+    }
+
+
+
+    //////////////////////////
+
+    bool ret = false;
+    _rxbuf_mutex.lock();
+    while(_rxbuf_tail != _rxbuf_head)
+    {
+        // Save data if room
+        if(_rx_linbuf_ctr < SLCAN_MTU)
+        {
+            _rx_linbuf[_rx_linbuf_ctr++] = _rxbuf[_rxbuf_tail];
+
+            // If we have a newline, then we just finished parsing a CAN message.
+            if(_rxbuf[_rxbuf_tail] == '\r')
+            {
+                CanMessage msg;
+                ret = parseMessage(msg);
+                msglist.append(msg);
+                _rx_linbuf_ctr = 0;
+            }
+        }
+        // Discard data if not
+        else
+        {
+            perror("Linbuf full");
+            _rx_linbuf_ctr = 0;
+        }
+
+        _rxbuf_tail = (_rxbuf_tail + 1) % RXCIRBUF_LEN;
+    }
+    _rxbuf_mutex.unlock();
+
+    return ret;
+}
+
+bool SLCANInterface::parseMessage(CanMessage &msg)
+{
+    // Set timestamp to current time
+    struct timeval tv;
+    gettimeofday(&tv,NULL);
+    msg.setTimestamp(tv);
+
+    // Defaults
+    msg.setErrorFrame(0);
+    msg.setInterfaceId(getId());
+    msg.setId(0);
+
+    bool msg_is_fd = false;
+
+    // Convert from ASCII (2nd character to end)
+    for (int i = 1; i < _rx_linbuf_ctr; i++)
+    {
+        // Lowercase letters
+        if(_rx_linbuf[i] >= 'a')
+            _rx_linbuf[i] = _rx_linbuf[i] - 'a' + 10;
+        // Uppercase letters
+        else if(_rx_linbuf[i] >= 'A')
+            _rx_linbuf[i] = _rx_linbuf[i] - 'A' + 10;
+        // Numbers
+        else
+            _rx_linbuf[i] = _rx_linbuf[i] - '0';
+    }
+
+
+    // Handle each incoming command
+    switch(_rx_linbuf[0])
+    {
+
+        // Transmit data frame command
+        case 'T':
+            msg.setExtended(1);
+            break;
+        case 't':
+            msg.setExtended(0);
+            break;
+
+        // Transmit remote frame command
+        case 'r':
+            msg.setExtended(0);
+            msg.setRTR(1);
+            break;
+        case 'R':
+            msg.setExtended(1);
+            msg.setRTR(1);
+            break;
+
+        // CANFD transmit - no BRS
+        case 'd':
+            msg.setExtended(0);
+            msg_is_fd = true;
+            break;
+        case 'D':
+            msg.setExtended(1);
+            msg_is_fd = true;
+            break;
+
+        // CANFD transmit - with BRS
+        case 'b':
+            msg.setExtended(0);
+            msg_is_fd = true;
+            break;
+        case 'B':
+            msg.setExtended(1);
+            msg_is_fd = true;
+            break;
+
+
+
+        // Invalid command
+        default:
+            return false;
+    }
+
+    // Start parsing at second byte (skip command byte)
+    uint8_t parse_loc = 1;
+
+    // Default to standard id len
+    uint8_t id_len = SLCAN_STD_ID_LEN;
+
+    // Update length if message is extended ID
+    if(msg.isExtended())
+        id_len = SLCAN_EXT_ID_LEN;
+
+    uint32_t id_tmp = 0;
+
+    // Iterate through ID bytes
+    while(parse_loc <= id_len)
+    {
+        id_tmp *= 16;
+        id_tmp += _rx_linbuf[parse_loc++];
+    }
+
+
+    msg.setId(id_tmp);
+
+    // Attempt to parse DLC and check sanity
+    uint8_t dlc_code_raw = _rx_linbuf[parse_loc++];
+
+    // If dlc is too long for an FD frame
+    if(msg_is_fd && dlc_code_raw > 0xF)
+    {
+        return false;
+    }
+    if(!msg_is_fd && dlc_code_raw > 0x8)
+    {
+        return false;
+    }
+
+    if(dlc_code_raw > 0x8)
+    {
+        switch(dlc_code_raw)
+        {
+        case 0x9:
+            dlc_code_raw = 12;
+            break;
+        case 0xA:
+            dlc_code_raw = 16;
+            break;
+        case 0xB:
+            dlc_code_raw = 20;
+            break;
+        case 0xC:
+            dlc_code_raw = 24;
+            break;
+        case 0xD:
+            dlc_code_raw = 32;
+            break;
+        case 0xE:
+            dlc_code_raw = 48;
+            break;
+        case 0xF:
+            dlc_code_raw = 64;
+            break;
+        default:
+            dlc_code_raw = 0;
+            perror("Invalid length");
+            break;
+        }
+    }
+
+    msg.setLength(dlc_code_raw);
+
+    // Calculate number of bytes we expect in the message
+    int8_t bytes_in_msg = dlc_code_raw;
+
+    if(bytes_in_msg < 0) {
+        perror("Invalid length < 0");
+        return false;
+    }
+    if(bytes_in_msg > 64) {
+        perror("Invalid length > 64");
+        return false;
+    }
+
+    // Parse data
+    // TODO: Guard against walking off the end of the string!
+    for (uint8_t i = 0; i < bytes_in_msg; i++)
+    {
+        msg.setByte(i,  (_rx_linbuf[parse_loc] << 4) + _rx_linbuf[parse_loc+1]);
+        parse_loc += 2;
+    }
+
+    // Reset buffer
+    _rx_linbuf_ctr = 0;
+    _rx_linbuf[0] = '\0';
+    return true;
+
+
+/*
+
+    // FIXME
+    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);
+    }*/
+
+
+}