open_source / modbus

/*****************************************************************************
 * Copyright (c) 2022 Priva b.v.
 *****************************************************************************/

#include "ConnectionConfig.h"
#include "modbus.h"
#include "ModbusAdapter.h"
#include "privautils/PrivaLogger.h"

#include <cstring>  /// Added for memset functionality



ModbusAdapter::ModbusAdapter()
    : m_modbus( nullptr )
{
}

ModbusAdapter::~ModbusAdapter()
{

}

bool ModbusAdapter::ModbusConnect( const ConnectionConfig &config )
{
    if( m_connected )
    {
        this->ModbusDisconnect();   // Will already set m_connected
    }

    m_connType = config.getType();

    switch( m_connType )
    {
        case ConnectionType::CT_SERIAL:
            m_connected = this->ModbusConnectRTU( config.getPort(), config.getBaudRate(),
                                                  config.getParity(), config.getDataBits(),
                                                  config.getStopBits(), config.getTimeOut());
            break;

        case ConnectionType::CT_TCP:
            m_connected = this->ModbusConnectTCP( config.getIpAddress(), config.getTcpPort(), 10 );
            break;

        default:
            // throw a sensible message or return an errorcode.
            break;
    }
    return m_connected;
}

bool ModbusAdapter::ModbusDisconnect()
{
    if( m_modbus != nullptr )
    {
        if( m_connected )
        {
            modbus_close( m_modbus );
            modbus_free( m_modbus );
        }
        // Clean up after ourselves.
        m_modbus = nullptr;
    }

    m_connected = false;
    return true;
}

modbusData ModbusAdapter::ModbusReadData( int slaveId, int functionCode, int startAddress, int noOfItems )
{
    if( m_modbus == nullptr )
    {
        // No context
        return modbusData();
    }

    int resultValue = -1;
    bool is16Bit = false;

    modbus_set_slave( m_modbus, slaveId );

    // Request data from modbus.
    switch( functionCode )
    {
        case MODBUS_FC_READ_COILS:
            resultValue = modbus_read_bits( m_modbus, startAddress, noOfItems, m_dest );
            break;
        case MODBUS_FC_READ_DISCRETE_INPUTS:
            resultValue = modbus_read_input_bits( m_modbus, startAddress, noOfItems, m_dest );
            break;
        case MODBUS_FC_READ_HOLDING_REGISTERS:
            resultValue = modbus_read_registers( m_modbus, startAddress, noOfItems, m_dest16 );
            break;
        case MODBUS_FC_READ_INPUT_REGISTERS:
            resultValue = modbus_read_input_registers( m_modbus, startAddress, noOfItems, m_dest16 );
            break;

        default:
            break;

    }

    // Read the databuffers
    if( resultValue != noOfItems )
    {
        return modbusData();
    }

    modbusData resultData;
    for( int index = 0; index < noOfItems; ++index )
    {
        resultData.push_back( (is16Bit ? static_cast<uint16_t>(m_dest16[index]) : static_cast<uint16_t>(m_dest[index])) );
    }

    modbus_flush( m_modbus );   // flush data
    this->ClearBuffers();

    return resultData;
}

modbusData ModbusAdapter::ModbusReadHoldReg( int slaveId, int startAddress, int noOfItems )
{
    if( m_modbus == nullptr )
    {
        return modbusData();
    }

    int resultValue = -1;       /// Return value from read functions

    // -- Start Critical Section --- ?? //

    modbus_set_slave( m_modbus, slaveId );
    /// Request data from modbus
    resultValue = modbus_read_registers( m_modbus, startAddress, noOfItems, m_dest16 );

    /// Read the databuffers
    if( resultValue != noOfItems )
    {
        return modbusData();
    }

    modbusData resultData;
    for( int index = 0; index < noOfItems; ++index )
    {
        resultData.push_back( static_cast<uint16_t>(m_dest16[index]) );
    }

    modbus_flush( m_modbus );

    // -- End Critical Section --- ?? //

    return resultData;
}

void ModbusAdapter::ModBusWriteData( int slaveId, int functionCode, int startAddress, int noOfItems, std::vector<int>values )
{
    if( m_modbus == nullptr )
    {
        // No modbus context. Sensible log-line and exit.
        return;
    }

    // ------------------------------------------------
    // Create 8 bits databuffer
    auto * data8 = new uint8_t[noOfItems];
    for( int index = 0; index < noOfItems; ++index )
    {
        data8[index] = values[index];
    }

    // Create same buffer for 16 bits data
    auto * data16 = new uint8_t[noOfItems];
    for( int index = 0; index < noOfItems; ++index )
    {
        data16[index] = values[index];
    }
    // ------------------------------------------------

    int resultValue = -1;

    modbus_set_slave( m_modbus, slaveId );

    // Request data from modbus
    switch( functionCode )
    {
        case MODBUS_FC_WRITE_SINGLE_COIL:
            resultValue = modbus_write_bit( m_modbus, startAddress, values[0] );
            noOfItems = 1;
            break;
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
            resultValue = modbus_write_register( m_modbus, startAddress, values[0] );
            noOfItems = 1;
            break;
        case MODBUS_FC_WRITE_MULTIPLE_COILS:

            resultValue = modbus_write_bits( m_modbus, startAddress, noOfItems, data8 );
            break;
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            resultValue = modbus_write_bits( m_modbus, startAddress, noOfItems, data16 );
            break;
    }

    modbus_flush(m_modbus); // Flush data.

    if( resultValue != noOfItems )
    {
        // Create a log line that writing the register failed. Maybe increment ErrorCounter(s)
    }
}

bool ModbusAdapter::isConnected() const
{
    return m_connected;
}

std::string ModbusAdapter::ErrorString( int errnum ) const
{
    switch(errnum)
    {
        case EINVAL:
            return "Protocol context is NULL";
            break;
        case ETIMEDOUT:
            return "Timeout";
            break;
        case ECONNRESET:
            return "Connection reset";
            break;
        case ECONNREFUSED:
            return "Connection refused";
            break;
        case EPIPE:
            return "Socket error";
            break;
        default:
            return modbus_strerror(errno);
    }
}

/* ============= PRIVATE METHODS ============= */
bool ModbusAdapter::ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int timeOut )
{
    {   // Keep logging scope as short as possible
        std::string logLine( "Connecting : " + serialport + " ( "
                            + std::to_string( baud ) + " : "
                            + std::to_string( dataBits ) + ","
                            + std::string( parity ) + " "
                            + std::to_string( stopBits ) + " ) with timeout : "
                            + std::to_string( timeout * 0.1 ) + " seconds." );
        PRIVALOG_INFO(this->logZone, logLine );
    }
    m_modbus = modbus_new_rtu( serialport.c_str(), baud, parity, dataBits, stopBits );

#ifdef LIB_MODBUS_DEBUG_OUTPUT
    // Do sensible logging through PRIVA_LOG
    m_modbus_set_debug( m_modbus, 1 );
#endif
    if( m_modbus == nullptr )
    {
        // We can stop here. Nothing to be done as we don't have a valid context
        // Log to PRIVA_LOG
        m_connected = false;
        return m_connected;
    }
    else if( m_modbus && modbus_connect( m_modbus ) == -1 )
    {
        // We could not connect to the selected serial port.
        // We can stop here. Nothing to be done as we don't have a valid connection
        modbus_free( m_modbus );
        m_connected = false;
        return m_connected;
    }

    m_connected = true;

    // Set recovery mode
    modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL | MODBUS_ERROR_RECOVERY_LINK );

    // Set the response timeout
    modbus_set_response_timeout( m_modbus, timeOut, 0 );

    return m_connected;
}

bool ModbusAdapter::ModbusConnectTCP( const std::string &ip, int port, int timeOut )
{
    if( ip.empty() )
    {
        // Nothing to be done. Set an Logline to PRIVA_LOG and exit.
        return false;
    }

    m_modbus = modbus_new_tcp( ip.c_str(), port );

#ifdef LIB_MODBUS_DEBUG_OUTPUT
    // Do sensible logging through PRIVA_LOG
    m_modbus_set_debug( m_modbus, 1 );
#endif

    if( m_modbus == nullptr )
    {
        // We can stop here. Nothing to be done as we don't have a valid context
        // Log to PRIVA_LOG
        return false;
    }
    else if( m_modbus && modbus_connect( m_modbus ) == -1 )
    {
        // We could not connect to the selected serial port.
        // We can stop here. Nothing to be done as we don't have a valid connection
        modbus_free( m_modbus );
        m_connected = false;
        return m_connected;
    }

    m_connected = true;

    // Set recovery mode
    modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL | MODBUS_ERROR_RECOVERY_LINK );

    // Set the response timeout
    modbus_set_response_timeout( m_modbus, timeOut, 0 );

    return m_connected;
}