paho.mqtt-sn.embedded-c/MQTTSNGateway/src/linux/xbee/SensorNetwork.cpp

/**************************************************************************************
 * 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 <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <string.h>
#include <stdlib.h>
#include "SensorNetwork.h"
#include "MQTTSNGWProcess.h"

using namespace std;
using namespace MQTTSNGW;

/*===========================================
 Class  SensorNetAddreess
 ============================================*/
SensorNetAddress::SensorNetAddress()
{
	memset(_address64, 0, 8);
	memset(_address16, 0, 2);
}

SensorNetAddress::~SensorNetAddress()
{

}

void SensorNetAddress::setAddress(uint8_t* address64, uint8_t* address16)
{
	memcpy(_address64, address64, 8);
	memcpy(_address16, address16, 2);
}


int SensorNetAddress::setAddress(string* address64)
{
	memcpy(_address64, address64->c_str(), 8);
	memset(_address16, 0, sizeof(_address16));
	return 0;
}

void SensorNetAddress::setBroadcastAddress(void)
{
	memset(_address64, 0, 6);
	_address64[6] = 0xff;
	_address64[7] = 0xff;
	_address16[0] = 0xff;
	_address16[1] = 0xfe;
}

bool SensorNetAddress::isMatch(SensorNetAddress* addr)
{

	return (memcmp(this->_address64, addr->_address64, 8 ) == 0 &&  memcmp(this->_address16, addr->_address16, 2) == 0);
}

SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress& addr)
{
	memcpy(_address64, addr._address64, 8);
	memcpy(_address16, addr._address16, 2);
	return *this;
}

char* SensorNetAddress::sprint(char* buf)
{
	char* pbuf = buf;
	for ( int i = 0; i < 8; i++ )
	{
		sprintf(pbuf, "%02X", _address64[i]);
		pbuf += 2;
	}
	return buf;
}

/*===========================================
 Class  SensorNetwork
 ============================================*/
SensorNetwork::SensorNetwork()
{

}

SensorNetwork::~SensorNetwork()
{

}

int SensorNetwork::unicast(const uint8_t* payload, uint16_t payloadLength, SensorNetAddress* sendToAddr)
{
	return XBee::unicast(payload, payloadLength, sendToAddr);
}

int SensorNetwork::broadcast(const uint8_t* payload, uint16_t payloadLength)
{
	return XBee::broadcast(payload, payloadLength);
}

int SensorNetwork::read(uint8_t* buf, uint16_t bufLen)
{
	return XBee::recv(buf, bufLen, &_clientAddr);
}

int SensorNetwork::initialize(void)
{
	char param[MQTTSNGW_PARAM_MAX];
	uint32_t baudrate = 9600;
	uint8_t apimode = 2;

	if (theProcess->getParam("ApiMode", param) == 0)
	{
		apimode = (uint8_t)atoi(param);
	}
	setApiMode(apimode);
	_description = "API mode ";
	sprintf(param, "%d", apimode);
	_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 XBee::open(param, baudrate);
}

const char* SensorNetwork::getDescription(void)
{
	return _description.c_str();
}

SensorNetAddress* SensorNetwork::getSenderAddress(void)
{
	return &_clientAddr;
}

/*===========================================
              Class  XBee
 ============================================*/
XBee::XBee(){
    _serialPort = new SerialPort();
    _respCd = 0;
    _respId = 0;
    _dataLen = 0;
    _frameId = 0;
    _apiMode = 2;
}

XBee::~XBee(){
	if ( _serialPort )
	{
		delete _serialPort;
	}
}

int XBee::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 XBee::broadcast(const uint8_t* payload, uint16_t payloadLen){
	SensorNetAddress addr;
	addr.setBroadcastAddress();
	return send(payload, (uint8_t) payloadLen, &addr);
}

int XBee:: unicast(const uint8_t* payload, uint16_t payloadLen, SensorNetAddress* addr){
	return send(payload, (uint8_t) payloadLen, addr);
}

int XBee::recv(uint8_t* buf, uint16_t bufLen, SensorNetAddress* clientAddr)
{
	uint8_t data[128];
	int len;

	while ( true )
	{

		if ( (len = readApiFrame(data)) > 0 )
		{

			if ( data[0] == API_RESPONSE )
			{
				memcpy(clientAddr->_address64, data + 1, 8);
				memcpy(clientAddr->_address16, data + 9, 2);
				len -= 12;
				memcpy( buf, data + 12, len);
				return len;
			}
			else if ( data[0] == API_XMITSTATUS )
			{
				_respCd = data[5];
				_respId = data[1];
				_sem.post();
			}
		}
	}
}

int XBee::readApiFrame(uint8_t* recvData){
	uint8_t buf;
	uint8_t pos = 0;
	uint8_t checksum = 0;
	uint8_t len = 0;

	 while ( _serialPort->recv(&buf) )
	 {
		 if ( buf == START_BYTE)
		 {
			pos = 1;
			 D_NWSTACK("\r\n===> Recv:    ");
			 break;
		 }
	 }

	if ( pos == 0 )
	{
		goto errexit;
	}

    if ( recv(&buf) < 0 ) // MSB length
    {
    	goto errexit;
    }

    if ( recv(&buf) < 0 ) // LSB length
	{
    	goto errexit;
	}
	else
	{
		len = buf;
	}

    pos = 0;
    while ( len-- )
    {
    	if ( recv(&buf) < 0 )
    	{
    		goto errexit;
    	}
    	recvData[pos++] = buf;
    	checksum += buf;
    }

    recv(&buf);        // checksum
    if ( (0xff - checksum ) == buf ){
    	D_NWSTACK("    checksum ok\r\n");
    	return pos;
    }
    else
    {
    	D_NWSTACK("    checksum error  %02x\r\n", 0xff - checksum);
    	goto errexit;
    }
errexit:
	_serialPort->flush();
	return -1;
}

int XBee::send(const uint8_t* payload, uint8_t pLen, SensorNetAddress* addr){
	D_NWSTACK("\r\n===> Send:    ");
    uint8_t checksum = 0;
    _respCd = -1;

    _serialPort->send(START_BYTE);
    send(0x00);              // Message Length
    send(14 + pLen);         // Message Length

    _serialPort->send(API_XMITREQUEST); // Transmit Request API
    checksum += API_XMITREQUEST;

    if (_frameId++ == 0x00 ) // Frame ID
	{
    	_frameId = 1;
	}
    send(_frameId);
    checksum += _frameId;

	for ( int i = 0; i < 8; i++)    // Address64
	{
		send(addr->_address64[i]);
		checksum += addr->_address64[i];
	}
	for ( int i = 0; i < 2; i++)    // Address16
	{
		send(addr->_address16[i]);
		checksum += addr->_address16[i];
	}

    send(0x00);   // Broadcast Radius
    checksum += 0x00;

    send(0x00);   // Option: Use the extended transmission timeout 0x40
    checksum += 0x00;

    D_NWSTACK("\r\n     Payload: ");

    for ( uint8_t i = 0; i < pLen; i++ ){
        send(payload[i]);     // Payload
        checksum += payload[i];
    }

    checksum = 0xff - checksum;
    D_NWSTACK("   checksum  ");
    send(checksum);
    D_NWSTACK("\r\n");

    /* wait Txim Status 0x8B */
    _sem.timedwait(XMIT_STATUS_TIME_OVER);

    if ( _respCd || _frameId != _respId )
    {
    	 D_NWSTACK(" frameId = %02x  Not Acknowleged\r\n", _frameId);
    	return -1;
    }
    return (int)pLen;
}

void XBee::send(uint8_t c)
{
  if(_apiMode == 2 && (c == START_BYTE || c == ESCAPE || c == XON || c == XOFF)){
	  _serialPort->send(ESCAPE);
	  _serialPort->send(c ^ 0x20);
  }else{
	  _serialPort->send(c);
  }
}

int XBee::recv(uint8_t* buf)
{
	if (_serialPort->recv(buf) )
	{
		if ( *buf == ESCAPE && _apiMode == 2 )
		{
			_serialPort->recv(buf);
			*buf = 0x20 ^ *buf;
		}
		return 0;
	}
	return -1;
}

void XBee::setApiMode(uint8_t mode)
{
	_apiMode = mode;
}

/*=========================================
 Class SerialPort
 =========================================*/
SerialPort::SerialPort()
{
	_tio.c_iflag = IGNBRK | IGNPAR;
	_tio.c_cflag = CS8 | CLOCAL | CRTSCTS;
	_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)
{
	if (read(_fd, buf, 1) == 0)
	{
		return false;
	}
	else
	{
		D_NWSTACK( " %02x",buf[0] );
		return true;
	}
}

void SerialPort::flush(void)
{
	tcsetattr(_fd, TCSAFLUSH, &_tio);
}