148 lines
4.7 KiB
C++
148 lines
4.7 KiB
C++
#include "devices/physical/CanDevice.h"
|
|
|
|
#include <cstring>
|
|
|
|
#include <QCanBusDevice>
|
|
#include <QCanBusFrame>
|
|
#include <QDateTime>
|
|
|
|
#include "core/memory/MemoryPool.h"
|
|
#include "core/models/MessageRoutingUtils.h"
|
|
#include "core/models/RawBusMessage.h"
|
|
#include "message_bus/ingress/IIngressPort.h"
|
|
#include "utils/logs/logging.h"
|
|
|
|
namespace softbus::devices::physical
|
|
{
|
|
|
|
static QString socketCanInterfaceName(const QString& endpoint)
|
|
{
|
|
QString ep = endpoint.trimmed();
|
|
if (ep.startsWith(QStringLiteral("can:"), Qt::CaseInsensitive)) {
|
|
return ep.mid(4);
|
|
}
|
|
return ep;
|
|
}
|
|
|
|
CanDevice::CanDevice(std::unique_ptr<QCanBusDevice> bus, const Config& config, const softbus::devices::DeviceInfo& info)
|
|
: m_bus(std::move(bus))
|
|
, m_config(config)
|
|
, m_info(info)
|
|
{
|
|
}
|
|
|
|
CanDevice::~CanDevice()
|
|
{
|
|
stop();
|
|
}
|
|
|
|
bool CanDevice::start()
|
|
{
|
|
if (!m_bus) {
|
|
LOG_ERROR() << "CanDevice: null bus";
|
|
return false;
|
|
}
|
|
QObject::connect(m_bus.get(), &QCanBusDevice::framesReceived, m_bus.get(), [this]() { onFramesAvailable(); });
|
|
QObject::connect(m_bus.get(), &QCanBusDevice::errorOccurred, m_bus.get(), [this](QCanBusDevice::CanBusError) {
|
|
onError();
|
|
});
|
|
|
|
if (m_config.bitrate > 0) {
|
|
m_bus->setConfigurationParameter(QCanBusDevice::BitRateKey, m_config.bitrate);
|
|
}
|
|
LOG_INFO() << "CanDevice: connectDevice" << m_config.endpoint << "iface" << socketCanInterfaceName(m_config.endpoint);
|
|
return m_bus->connectDevice();
|
|
}
|
|
|
|
void CanDevice::stop()
|
|
{
|
|
if (!m_bus) {
|
|
return;
|
|
}
|
|
QObject::disconnect(m_bus.get(), nullptr, m_bus.get(), nullptr);
|
|
if (m_bus->state() == QCanBusDevice::ConnectedState) {
|
|
m_bus->disconnectDevice();
|
|
}
|
|
m_bus.reset();
|
|
}
|
|
|
|
softbus::devices::DeviceInfo CanDevice::deviceInfo() const
|
|
{
|
|
return m_info;
|
|
}
|
|
|
|
QString CanDevice::endpoint() const
|
|
{
|
|
return m_config.endpoint;
|
|
}
|
|
|
|
softbus::devices::DeviceKind CanDevice::kind() const
|
|
{
|
|
return softbus::devices::DeviceKind::Can;
|
|
}
|
|
|
|
void CanDevice::bindIngress(softbus::message_bus::ingress::IIngressPort* ingress,
|
|
std::shared_ptr<softbus::core::memory::MemoryPool> pool)
|
|
{
|
|
m_ingress = ingress;
|
|
m_pool = std::move(pool);
|
|
}
|
|
|
|
void CanDevice::onFramesAvailable()
|
|
{
|
|
if (!m_ingress || !m_pool || !m_bus) {
|
|
return;
|
|
}
|
|
while (m_bus->framesAvailable()) {
|
|
const QCanBusFrame frame = m_bus->readFrame();
|
|
if (frame.frameType() != QCanBusFrame::DataFrame && frame.frameType() != QCanBusFrame::RemoteRequestFrame) {
|
|
continue;
|
|
}
|
|
const QByteArray busPayload = frame.payload();
|
|
auto block = m_pool->allocate(static_cast<std::size_t>(busPayload.size()));
|
|
if (!block || !block->data || block->capacity < static_cast<std::size_t>(busPayload.size())) {
|
|
m_ingress->reportError(m_config.endpoint, QStringLiteral("memory pool exhausted"));
|
|
return;
|
|
}
|
|
if (!busPayload.isEmpty()) {
|
|
std::memcpy(block->data, busPayload.constData(), static_cast<std::size_t>(busPayload.size()));
|
|
}
|
|
block->size = static_cast<std::size_t>(busPayload.size());
|
|
|
|
softbus::core::models::RawBusMessage ctx;
|
|
ctx.direction = softbus::core::models::BusDirection::Upstream;
|
|
ctx.endpointHash = softbus::core::models::makeEndpointHash(m_config.endpoint);
|
|
ctx.protocol = softbus::core::models::protocolTypeFromHint(m_info.protocol);
|
|
if (ctx.protocol == softbus::core::models::ProtocolType::UNKNOWN) {
|
|
ctx.protocol = softbus::core::models::ProtocolType::CAN_RAW;
|
|
}
|
|
ctx.header.can.cobId = static_cast<std::uint32_t>(frame.frameId() & 0x1FFFFFFFU);
|
|
ctx.header.can.isExtended = frame.hasExtendedFrameFormat();
|
|
ctx.header.can.isRtr = (frame.frameType() == QCanBusFrame::RemoteRequestFrame);
|
|
ctx.logicalAddress = ctx.header.can.cobId & 0xFFU;
|
|
ctx.routingKey = softbus::core::models::makeRoutingKey(ctx.endpointHash, m_info.id);
|
|
ctx.timestampMs = QDateTime::currentMSecsSinceEpoch();
|
|
ctx.deviceId = m_info.id;
|
|
ctx.payload.block = std::move(block);
|
|
ctx.payload.length = static_cast<std::size_t>(busPayload.size());
|
|
ctx.traceId = QStringLiteral("can:%1:%2:%3")
|
|
.arg(m_config.endpoint)
|
|
.arg(ctx.header.can.cobId)
|
|
.arg(ctx.timestampMs);
|
|
|
|
if (!m_ingress->push(std::move(ctx))) {
|
|
m_ingress->reportError(m_config.endpoint, QStringLiteral("ingress queue full"));
|
|
}
|
|
}
|
|
}
|
|
|
|
void CanDevice::onError()
|
|
{
|
|
if (!m_ingress || !m_bus) {
|
|
return;
|
|
}
|
|
m_ingress->reportError(m_config.endpoint, m_bus->errorString());
|
|
}
|
|
|
|
} // namespace softbus::devices::physical
|