From 5f9c82f4d6bbc9a6783de14f1b3ffb9d06372967 Mon Sep 17 00:00:00 2001 From: Arek Marud Date: Mon, 30 Jul 2018 09:53:07 +0200 Subject: [PATCH] Serial port forwarder frames support --- .../src/linux/serialfw/SensorNetwork.cpp | 390 ++++++++++++++++++ .../src/linux/serialfw/SensorNetwork.h | 125 ++++++ 2 files changed, 515 insertions(+) create mode 100644 MQTTSNGateway/src/linux/serialfw/SensorNetwork.cpp create mode 100644 MQTTSNGateway/src/linux/serialfw/SensorNetwork.h diff --git a/MQTTSNGateway/src/linux/serialfw/SensorNetwork.cpp b/MQTTSNGateway/src/linux/serialfw/SensorNetwork.cpp new file mode 100644 index 0000000..7c35133 --- /dev/null +++ b/MQTTSNGateway/src/linux/serialfw/SensorNetwork.cpp @@ -0,0 +1,390 @@ +/************************************************************************************** + * Copyright (c) 2016, Tomoaki Yamaguchi + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Tomoaki Yamaguchi - initial API and implementation + **************************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "SensorNetwork.h" +#include "MQTTSNGWProcess.h" + +#include + +using namespace std; +using namespace MQTTSNGW; + +/*=========================================== + Class SensorNetAddress + ============================================*/ +SensorNetAddress::SensorNetAddress() +{ + _broadcast = false; + _address.push_back(0); +} + +SensorNetAddress::~SensorNetAddress() +{ + +} + +void SensorNetAddress::setAddress(const std::vector &value) { + _address = value; +} + +bool SensorNetAddress::broadcast() const { + return _broadcast; +} + +void SensorNetAddress::setAddress(uint8_t* address, uint8_t size) +{ + _address.clear(); + std::copy_n(address, size, std::back_inserter(_address)); +} + +int SensorNetAddress::setAddress(string* address) +{ + _address.clear(); + std::copy(address->begin(), address->end(), std::back_inserter(_address)); + return 0; +} + +uint32_t SensorNetAddress::length() const { + return _address.size(); +} + +void SensorNetAddress::setBroadcastAddress(void) +{ + _broadcast = true; +} + +bool SensorNetAddress::isMatch(SensorNetAddress* addr) +{ + return addr->_address == _address; +} + +SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress& addr) +{ + _address = addr._address; + return *this; +} + +char* SensorNetAddress::sprint(char* buf) +{ + char* pbuf = buf; + for ( int i = 0; i < 8; i++ ) + { + sprintf(pbuf, "%02X", _address[i]); + pbuf += 2; + } + return buf; +} + +uint8_t *SensorNetAddress::ptr() { + return &*_address.begin(); +} + +/*=========================================== + Class SensorNetwork + ============================================*/ +SensorNetwork::SensorNetwork() +{ + +} + +SensorNetwork::~SensorNetwork() +{ + +} + +int SensorNetwork::unicast(const uint8_t* payload, uint16_t payloadLength, SensorNetAddress* sendToAddr) +{ + return Sfw::unicast(payload, payloadLength, sendToAddr); +} + +int SensorNetwork::broadcast(const uint8_t* payload, uint16_t payloadLength) +{ + return Sfw::broadcast(payload, payloadLength); +} + +int SensorNetwork::read(uint8_t* buf, uint16_t bufLen) +{ + return Sfw::recv(buf, bufLen, &_clientAddr); +} + +int SensorNetwork::initialize(void) +{ + char param[MQTTSNGW_PARAM_MAX]; + uint32_t baudrate = 9600; + + _description += param; + + if (theProcess->getParam("Baudrate", param) == 0) + { + baudrate = (uint32_t)atoi(param); + } + _description += ", Baudrate "; + sprintf(param ,"%d", baudrate); + _description += param; + + theProcess->getParam("SerialDevice", param); + _description += ", SerialDevice "; + _description += param; + + return Sfw::open(param, baudrate); +} + +const char* SensorNetwork::getDescription(void) +{ + return _description.c_str(); +} + +SensorNetAddress* SensorNetwork::getSenderAddress(void) +{ + return &_clientAddr; +} + +/*=========================================== + Class Sfw + ============================================*/ +Sfw::Sfw(){ + _serialPort = new SerialPort(); +} + +Sfw::~Sfw(){ + if ( _serialPort ) + { + delete _serialPort; + } +} + +int Sfw::open(char* device, int baudrate) +{ + int rate = B9600; + + switch (baudrate) + { + case 9600: + rate = B9600; + break; + case 19200: + rate = B19200; + break; + case 38400: + rate = B38400; + break; + case 57600: + rate = B57600; + break; + case 115200: + rate = B115200; + break; + default: + return -1; + } + + return _serialPort->open(device, rate, false, 1, O_RDWR | O_NOCTTY); +} + +int Sfw::broadcast(const uint8_t* payload, uint16_t payloadLen){ + SensorNetAddress addr; + addr.setBroadcastAddress(); + return send(payload, (uint8_t) payloadLen, &addr); +} + +int Sfw:: unicast(const uint8_t* payload, uint16_t payloadLen, SensorNetAddress* addr){ + return send(payload, (uint8_t) payloadLen, addr); +} + +int Sfw::recv(uint8_t* buf, uint16_t bufLen, SensorNetAddress* clientAddr) +{ + uint8_t value = 0; + + // Check length of the header. It should be more then 4 + if (_serialPort->recv(&value) != 1 || value < 4) + return 0; + + // Node id length is equal to header length - 3 bytes + int nodeIdLength = value - 3; + + // Check MsgType + if (_serialPort->recv(&value) != 1 || value != 0xFE) + return 0; + + // Ctrl fiels + if (_serialPort->recv(&value) != 1 ) + return 0; + + bool broadcast = (value && 1) != 0; + + std::vector nodeId; + + for (uint i = 0; i < nodeIdLength; ++i) { + + if (_serialPort->recv(&value) != 1) + return 0; + + nodeId.push_back(value); + } + + clientAddr->setAddress(nodeId); + uint8_t *iterator = buf; + + uint32_t mqtt_frame_len = 0; + + if (_serialPort->recv(iterator++) != 1) + return 0; + + if (*buf == 1) { + // Three octet length + + if (_serialPort->recv(iterator++) != 1) + return 0; + if (_serialPort->recv(iterator++) != 1) + return 0; + + mqtt_frame_len = buf[2] << 8; + mqtt_frame_len |= buf[3]; + + } else { + // Single octet length + mqtt_frame_len = buf[0]; + } + + mqtt_frame_len -= (iterator - buf); + + for (uint32_t i = 0; i < mqtt_frame_len; ++i) { + if (_serialPort->recv(iterator++) != 1) + return 0; + } + + int frame_size = iterator - buf; + + return frame_size; +} + +int Sfw::send(const uint8_t* payload, uint8_t pLen, SensorNetAddress* addr){ + + D_NWSTACK("\r\n===> Send: "); + + _serialPort->send( addr->length() + 3); + _serialPort->send(0xFE); + + if (addr->broadcast()) { + _serialPort->send(1); + } else { + _serialPort->send(0); + } + + uint8_t *ptr = addr->ptr(); + for (int i = 0; i < addr->length(); ++i) { + _serialPort->send(*ptr++); + } + + for (int i = 0; i < pLen; ++i) { + _serialPort->send(*payload++); + } + + return pLen; +} + +/*========================================= + Class SerialPort + =========================================*/ +SerialPort::SerialPort() +{ + _tio.c_iflag = IGNBRK | IGNPAR; + _tio.c_cflag = CS8 | CLOCAL | CRTSCTS | CREAD; + _tio.c_cc[VINTR] = 0; + _tio.c_cc[VTIME] = 10; // 1 sec. + _tio.c_cc[VMIN] = 1; + _fd = 0; +} + +SerialPort::~SerialPort() +{ + if (_fd) + { + ::close(_fd); + } +} + +int SerialPort::open(char* devName, unsigned int baudrate, bool parity, + unsigned int stopbit, unsigned int flg) +{ + _fd = ::open(devName, flg); + if (_fd < 0) + { + return _fd; + } + + if (parity) + { + _tio.c_cflag = _tio.c_cflag | PARENB; + } + if (stopbit == 2) + { + _tio.c_cflag = _tio.c_cflag | CSTOPB; + } + + if (cfsetspeed(&_tio, baudrate) < 0) + { + return errno; + } + return tcsetattr(_fd, TCSANOW, &_tio); +} + +bool SerialPort::send(unsigned char b) +{ + if (write(_fd, &b, 1) < 0) + { + return false; + } + else + { + D_NWSTACK( " %02x", b); + return true; + } +} + +bool SerialPort::recv(unsigned char* buf) +{ + struct timeval timeout; + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(_fd, &rfds); + timeout.tv_sec = 0; + timeout.tv_usec = 500000; // 500ms + if ( select(_fd + 1, &rfds, 0, 0, &timeout) > 0 ) + { + if (read(_fd, buf, 1) > 0) + { + D_NWSTACK( " %02x",buf[0] ); + return true; + } + } + return false; +} + +void SerialPort::flush(void) +{ + tcsetattr(_fd, TCSAFLUSH, &_tio); +} + diff --git a/MQTTSNGateway/src/linux/serialfw/SensorNetwork.h b/MQTTSNGateway/src/linux/serialfw/SensorNetwork.h new file mode 100644 index 0000000..33feadd --- /dev/null +++ b/MQTTSNGateway/src/linux/serialfw/SensorNetwork.h @@ -0,0 +1,125 @@ + +/************************************************************************************** + * Copyright (c) 2016, Tomoaki Yamaguchi + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Tomoaki Yamaguchi - initial API and implementation + **************************************************************************************/ +#ifndef SENSORNETWORKX_H_ +#define SENSORNETWORKX_H_ + +#include "MQTTSNGWDefines.h" +#include "MQTTSNGWProcess.h" +#include +#include +#include + +using namespace std; + +namespace MQTTSNGW +{ +//#define DEBUG_NWSTACK + +#ifdef DEBUG_NWSTACK + #define D_NWSTACK(...) printf(__VA_ARGS__) +#else + #define D_NWSTACK(...) +#endif + +/*=========================================== + Class SerialPort + ============================================*/ +class SerialPort{ +public: + SerialPort(); + ~SerialPort(); + int open(char* devName, unsigned int baudrate, bool parity, unsigned int stopbit, unsigned int flg); + bool send(unsigned char b); + bool recv(unsigned char* b); + void flush(); + +private: + int _fd; // file descriptor + struct termios _tio; +}; + +/*=========================================== + Class SensorNetAddreess + ============================================*/ +class SensorNetAddress +{ + friend class Sfw; +public: + SensorNetAddress(); + ~SensorNetAddress(); + void setAddress(uint8_t* address, uint8_t size); + void setAddress(const std::vector &); + int setAddress(string* data); + void setBroadcastAddress(void); + bool isMatch(SensorNetAddress* addr); + SensorNetAddress& operator =(SensorNetAddress& addr); + char* sprint(char*); + bool broadcast() const; + uint8_t *ptr(); + uint32_t length() const; +private: + + bool _broadcast; + std::vector _address; +}; + +/*======================================== + Class Sfw + =======================================*/ +class Sfw +{ +public: + Sfw(); + ~Sfw(); + + int open(char* device, int boudrate); + void close(void); + int unicast(const uint8_t* buf, uint16_t length, SensorNetAddress* sendToAddr); + int broadcast(const uint8_t* buf, uint16_t length); + int recv(uint8_t* buf, uint16_t len, SensorNetAddress* addr); + +private: + int send(const uint8_t* payload, uint8_t pLen, SensorNetAddress* addr); + + Semaphore _sem; + SerialPort* _serialPort; +}; + +/*=========================================== + Class SensorNetwork + ============================================*/ +class SensorNetwork: public Sfw +{ +public: + SensorNetwork(); + ~SensorNetwork(); + + int unicast(const uint8_t* payload, uint16_t payloadLength, SensorNetAddress* sendto); + int broadcast(const uint8_t* payload, uint16_t payloadLength); + int read(uint8_t* buf, uint16_t bufLen); + int initialize(void); + const char* getDescription(void); + SensorNetAddress* getSenderAddress(void); + +private: + SensorNetAddress _clientAddr; // Sender's address. not gateway's one. + string _description; +}; + +} + +#endif /* SENSORNETWORKX_H_ */