743 lines
16 KiB
C++
743 lines
16 KiB
C++
/*
|
|
|
|
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);
|
|
}*/
|
|
|
|
|
|
}
|