Cppcheck report - ethercatcpp-core: /builds/ethercatcpp/ethercatcpp-core/src/ethercat_master/soem_master_pimpl.cpp

/*      File: soem_master_pimpl.cpp
 *       This file is part of the program ethercatcpp-core
 *       Program description : EtherCAT driver libraries for UNIX
 *       Copyright (C) 2017-2024 -  Robin Passama (LIRMM / CNRS) Arnaud Meline
 * (LIRMM / CNRS) Benjamin Navarro (LIRMM / CNRS). All Right reserved.
 *
 *       This software is free software: you can redistribute it and/or modify
 *       it under the terms of the CeCILL-C license as published by
 *       the CEA CNRS INRIA, either version 1
 *       of the License, or (at your option) any later version.
 *       This software 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
 *       CeCILL-C License for more details.
 *
 *       You should have received a copy of the CeCILL-C License
 *       along with this software. If not, it can be found on the official
 * website of the CeCILL licenses family (http://www.cecill.info/index.en.html).
 */
#include "soem_master_pimpl.h"

#include <pid/log/ethercatcpp-core_ethercatcpp-core.h>

namespace ethercatcpp {

Master::Impl::Impl()
    : expected_wkc_(0),
      max_run_steps_(0),
      max_init_steps_(0),
      max_end_steps_(0),
      max_timewait_steps_(1000),                 // Waiting 1 ms = 1000us
      dc_sync_compensate_shift_time_(50000000) { // 50 ms
    context_.port = new ecx_portt;
    context_.port->redport = new ecx_redportt;
    // number of maximum slave on the network
    context_.maxslave = EC_MAXSLAVE;
    // Main slave data array.
    // Each slave found on the network gets its own record.
    // ec_slave[0] is reserved for the master. Structure gets filled
    // in by the configuration function ec_config().
    context_.slavelist = new ec_slavet[EC_MAXSLAVE];
    // number of slaves found in configuration
    context_.slavecount = new int;
    *context_.slavecount = 0;
    // number of maximum slave group Structure
    context_.maxgroup = EC_MAXGROUP;
    // slave group structure
    context_.grouplist = new ec_groupt[EC_MAXGROUP];

    // cache for EEPROM read functions
    context_.esibuf = new uint8[EC_MAXEEPBUF];
    // bitmap for filled cache buffer bytes
    context_.esimap = new uint32[EC_MAXEEPBITMAP];
    // current slave for eeprom cache
    context_.esislave = static_cast<uint16>(0);
    // error list
    context_.elist = new ec_eringt;
    // processdata stack buffer info
    context_.idxstack = new ec_idxstackT;
    // Global variable TRUE if error available in error stack
    context_.ecaterror = new boolean;
    *context_.ecaterror = false;
    // position of DC datagram in process data packet
    context_.DCtO = static_cast<uint16_t>(0);
    // length of DC datagram
    context_.DCl = static_cast<uint16_t>(0);
    // reference to last DC time from slaves
    context_.DCtime = new int64;
    // SyncManager Communication Type struct to store data of one slave
    context_.SMcommtype = new ec_SMcommtypet[EC_MAX_MAPT];
    // PDO assign struct to store data of one slave
    context_.PDOassign = new ec_PDOassignt[EC_MAX_MAPT];
    // PDO description struct to store data of one slave
    context_.PDOdesc = new ec_PDOdesct[EC_MAX_MAPT];
    // buffer for EEPROM SM data
    context_.eepSM = new ec_eepromSMt;
    // buffer for EEPROM FMMU data
    context_.eepFMMU = new ec_eepromFMMUt;
    // registered FoE hook
    context_.FOEhook = nullptr;

    // WARNING !!
    // *input and *output in "slavelist" (from ec_slavet struct) haven't memory
    // space reserved. *input and *output in "grouplist" (from ec_groupt struct)
    // haven't memory space reserved. void *data[EC_MAXBUF] in "idxstack" (from
    // idxstackT struct) haven't memory space reserved FOEhook maybe problem in
    // used (function pointer non define).
}

Master::Impl::~Impl() {
    delete context_.port->redport;
    delete context_.port;
    delete[] context_.slavelist;
    delete context_.slavecount;
    delete[] context_.grouplist;
    delete[] context_.esibuf;
    delete[] context_.esimap;
    delete context_.elist;
    delete context_.idxstack;
    delete context_.ecaterror;
    delete context_.DCtime;
    delete[] context_.SMcommtype;
    delete[] context_.PDOassign;
    delete[] context_.PDOdesc;
    delete context_.eepSM;
    delete context_.eepFMMU;
}

// -----------------------------------------------------------------------------

void Master::Impl::set_primary_interface(const std::string& interface_primary) {
    ifname_primary_ = interface_primary;
}

void Master::Impl::set_redundant_interface(
    const std::string& interface_redundant) {
    ifname_redundant_ = interface_redundant;
}

int Master::Impl::init_interface() {
    if (ecx_init(&context_, ifname_primary_.c_str())) {
        pid_log << pid::info << "Interface " << ifname_primary_
                << " configured." << pid::flush;
        return 1;
    } else {
        pid_log << pid::critical << "No socket connection on "
                << ifname_primary_ << pid::flush;
        pid_log << pid::info << "Using correct interface and excecute as root ?"
                << pid::flush;
        throw std::runtime_error(
            "ethercatcpp: failed to create the communication socket");
    }
    return 0;
}

int Master::Impl::init_redundant_interface() {
    pid_log << pid::info
            << "Redundant mode :Primary interface : " << ifname_primary_
            << " Redundant interface : " << ifname_redundant_ << pid::endl;

    if (ecx_init_redundant(&context_, context_.port->redport,
                           ifname_primary_.c_str(),
                           const_cast<char*>(ifname_redundant_.c_str()))) {
        pid_log << "Interface " << ifname_primary_ << " and "
                << ifname_redundant_ << " configured." << pid::flush;
        return 1;
    } else {
        pid_log << pid::critical << "No socket connection on "
                << ifname_primary_ << " and " << ifname_redundant_
                << pid::flush;
        pid_log << pid::info << "Using correct interface and excecute as root ?"
                << pid::flush;
        throw std::runtime_error(
            "ethercatcpp: failed to create the communication socket");
    }
}

bool Master::Impl::is_redundant() const {
    return not ifname_redundant_.empty();
}
// -----------------------------------------------------------------------------

void Master::Impl::set_dc_compensate_shift_time(int32_t shift_time) {
    dc_sync_compensate_shift_time_ = shift_time * 1000000; // convert ms to ns.
}

void Master::Impl::configure_slaves_distributed_clock() {
    ecx_configdc(&context_); // Init DC counter
    // Active DC Sync0 to all slave
    for (auto& slave : slave_vector_ptr_) {
        if (slave->sync0_used()) {
            ecx_dcsync0(&context_, slave->ec_bus_position(),
                        slave->sync0_used(), slave->sync0_cycle_time(),
                        slave->sync_cycle_shift() +
                            dc_sync_compensate_shift_time_);
            // dc_sync_compensate_shift_time_ compensate difference of speed
            // between computers
        }
        if (slave->sync0_1_used()) {
            ecx_dcsync01(
                &context_, slave->ec_bus_position(), slave->sync0_1_used(),
                slave->sync0_cycle_time(), slave->sync1_cycle_time(),
                slave->sync_cycle_shift() + dc_sync_compensate_shift_time_);
            // dc_sync_compensate_shift_time_ compensate difference of speed
            // between computers
        }
    }
}

//------------------------------------------------------------------------------
// Initialze network

bool Master::Impl::configure_slaves() {
    // all slaves launch their configuration sequence.
    for (auto& slave : slave_vector_ptr_) {
        slave->launch_init_configuration();
    }
    configure_io_map();                    // Init global IOmap
    configure_slaves_distributed_clock();  // Configure slaves DC Sync 0-1
    if (not switch_slaves_to_op_state()) { // All slaves to OP state
        return false;
    }
    update_slaves_config_from_network(); // Update slave IO pointers in Slave
                                         // and UnitDevice
    init_slaves();                       // Call init steps of slaves
    return true;
}

bool Master::Impl::init_network() {
    // Init network interface
    if (is_redundant()) { // redundant mode
        if (not init_redundant_interface()) {
            pid_log << pid::critical
                    << "Problem in network interface initialisation (redundant "
                       "mode)"
                    << pid::flush;
            return false;
        }
    } else { // Non redundant mode
        if (not init_interface()) {
            pid_log << pid::critical
                    << "Problem in network interface initialisation"
                    << pid::flush;
            return false;
        }
    }
    // Init ethercat network
    if (ecx_config_init(&context_, false)) {
        // Force slave to PRE-OP state (sometime slave don't change state with
        // the first request)
        return switch_slaves_to_preop_state();
    } else {
        pid_log << pid::critical << "No slave found on bus" << pid::flush;
        // stop SOEM, close socket
        return false;
    }
}

bool Master::Impl::add_slave(uint16_t slave_id, SlaveInfo* slave_ptr) {
    // check if slave eep_Man and eep_Id is the same between real bus (master)
    // and configured bus.
    auto& slave = context_.slavelist[slave_id];<--- Variable 'slave' can be declared with const
    if (not((slave.eep_man == slave_ptr->eep_manufacturer()) &&
            (slave.eep_id == slave_ptr->eep_device()))) {
        pid_log
            << pid::critical << "Slave configured ( " << slave_ptr->name()
            << ", number " << slave_id
            << " ) is not the slave at this bus position !! Manufacturer id "
               "waited: "
            << slave_ptr->eep_manufacturer() << ", detected: " << slave.eep_man
            << ", device ID waited: " << slave_ptr->eep_device()
            << ", detected:" << slave.eep_id << pid::flush;
        return false;
    }

    // Read serial number from SII EEPROM (not provided by soem)
    ecx_readeeprom1(&context_, slave_id,
                    ECT_SII_REV + 2); // Serial number is 2 bytes after revision
    auto serial_number =
        etohl(ecx_readeeprom2(&context_, slave_id, EC_TIMEOUTEEP));

    // If the slave has a defined serial number, check that it matches
    if (slave_ptr->serial_number() and
        serial_number != slave_ptr->serial_number()) {
        pid_log << pid::critical << "Slave configured ( " << slave_ptr->name()
                << ", number " << slave_id
                << " ) doesn't have the expected serial number (expected: "
                << slave_ptr->serial_number() << ", received: " << serial_number
                << ")" << pid::flush;
        return false;
    }
    // Otherwise set the slave serial number from the one read on the bus
    else {
        slave_ptr->set_serial_number(serial_number);
    }

    // Update Master vector of slaves with info from bus config
    update_init_masters_from_slave(slave_id, slave_ptr);
    // Update SlaveInfo data from master config
    update_init_slave_from_master(slave_id, slave_ptr);
    return true;
}

void Master::Impl::update_init_masters_from_slave(uint16_t slave_id,
                                                  SlaveInfo* slave_ptr) {

    // We forced SlaveInfo user description.
    // If Master have datas from a slave and user set datas, the users datas is
    // set (forced users datas) SlaveInfo -> Master Obits, Obytes Ibits, Ibytes
    // SM[] ,SMtype hasdc uncomment to force end-user config configindex

    if (slave_ptr->output_size_bits()) {
        context_.slavelist[slave_id].Obits = slave_ptr->output_size_bits();
    }
    if (slave_ptr->output_size()) {
        context_.slavelist[slave_id].Obytes = slave_ptr->output_size();
    }
    if (slave_ptr->input_size_bits()) {
        context_.slavelist[slave_id].Ibits = slave_ptr->input_size_bits();
    }
    if (slave_ptr->input_size()) {
        context_.slavelist[slave_id].Ibytes = slave_ptr->input_size();
    }

    for (int nSM = 0; nSM < slave_ptr->max_SMs(); nSM++) {
        if (slave_ptr->SM_start_address(nSM)) {
            context_.slavelist[slave_id].SM[nSM].StartAddr =
                slave_ptr->SM_start_address(nSM);
        }
        if (slave_ptr->SM_length(nSM)) {
            context_.slavelist[slave_id].SM[nSM].SMlength =
                slave_ptr->SM_length(nSM);
        }
        if (slave_ptr->SM_flags(nSM)) {
            context_.slavelist[slave_id].SM[nSM].SMflags =
                slave_ptr->SM_flags(nSM);
        }
        context_.slavelist[slave_id].SMtype[nSM] = slave_ptr->SM_type(nSM);
    }

    // Uncomment to force value fixed by end-user
    // if (slave_ptr->get_Hasdc()) {
    //  context_.slavelist[slave_id].hasdc =
    //  slave_ptr->get_Hasdc();
    //}

    if (slave_ptr->configured_index()) {
        context_.slavelist[slave_id].configindex =
            slave_ptr->configured_index();
    }
}

void Master::Impl::update_init_slave_from_master(uint16_t slave_id,
                                                 SlaveInfo* slave_ptr) {
    // configadr, aliasadr, hasdc, pdelay, ec_bus_position
    slave_ptr->configure_address(context_.slavelist[slave_id].configadr);
    slave_ptr->set_alias_addr(context_.slavelist[slave_id].aliasadr);
    // Forced master DC config (config detected by master on slave)
    slave_ptr->set_dc(context_.slavelist[slave_id].hasdc);
    slave_ptr->set_delay(context_.slavelist[slave_id].pdelay);
    slave_ptr->set_ec_bus_position(slave_id);

    slave_ptr->set_master_context(
        &context_); // Send to the slave the master context address
}

// Uodate slave IO pointers

void Master::Impl::update_slaves_config_from_network() {
    // for each slave update
    // Output pointer and output start bit
    // Input pointer and input start bit
    // FMMU / group
    // DC sync active ?
    // state => non implement
    // max_step value for init, run and end
    // Update pointer to buffers datas in UnitDevice

    for (uint16_t slv = 1; slv <= *context_.slavecount; ++slv) {

        slave_vector_ptr_.at(slv - 1)->set_output_address(
            context_.slavelist[slv].outputs);
        slave_vector_ptr_.at(slv - 1)->set_output_start_bit(
            context_.slavelist[slv].Ostartbit);

        slave_vector_ptr_.at(slv - 1)->set_input_address(
            context_.slavelist[slv].inputs);
        slave_vector_ptr_.at(slv - 1)->set_input_start_bit(
            context_.slavelist[slv].Istartbit);

        for (int nFMMU = 0; nFMMU < slave_vector_ptr_.at(slv - 1)->max_FMMUs();
             ++nFMMU) {
            // Logical addressing
            slave_vector_ptr_.at(slv - 1)->set_FMMU_logical_start(
                nFMMU, context_.slavelist[slv].FMMU[nFMMU].LogStart);
            slave_vector_ptr_.at(slv - 1)->set_FMMU_logical_length(
                nFMMU, context_.slavelist[slv].FMMU[nFMMU].LogLength);
            slave_vector_ptr_.at(slv - 1)->set_FMMU_logical_start_bit(
                nFMMU, context_.slavelist[slv].FMMU[nFMMU].LogStartbit);
            slave_vector_ptr_.at(slv - 1)->set_FMMU_logical_end_bit(
                nFMMU, context_.slavelist[slv].FMMU[nFMMU].LogEndbit);
            // Physical addressing
            slave_vector_ptr_.at(slv - 1)->set_FMMU_physical_start(
                nFMMU, context_.slavelist[slv].FMMU[nFMMU].PhysStart);
            slave_vector_ptr_.at(slv - 1)->set_FMMU_physical_start_bit(
                nFMMU, context_.slavelist[slv].FMMU[nFMMU].PhysStartBit);
            // FMMU config and type
            slave_vector_ptr_.at(slv - 1)->set_FMMU_type(
                nFMMU, context_.slavelist[slv].FMMU[nFMMU].FMMUtype);
            slave_vector_ptr_.at(slv - 1)->set_FMMU_active(
                nFMMU, context_.slavelist[slv].FMMU[nFMMU].FMMUactive);
        }
        // Fisrt FMMU unused
        slave_vector_ptr_.at(slv - 1)->set_FMMU_unused(
            context_.slavelist[slv].FMMUunused);
        // Group of slave
        slave_vector_ptr_.at(slv - 1)->set_group_id(
            context_.slavelist[slv].group);

        // DC sync active ?
        slave_vector_ptr_.at(slv - 1)->activate_dc(
            context_.slavelist[slv].DCactive);

        // search max_step value in all slaves
        if (max_run_steps_ < slave_vector_ptr_.at(slv - 1)->run_steps()) {
            max_run_steps_ = slave_vector_ptr_.at(slv - 1)->run_steps();
        }
        if (max_init_steps_ < slave_vector_ptr_.at(slv - 1)->init_steps()) {
            max_init_steps_ = slave_vector_ptr_.at(slv - 1)->init_steps();
        }
        if (max_end_steps_ < slave_vector_ptr_.at(slv - 1)->end_steps()) {
            max_end_steps_ = slave_vector_ptr_.at(slv - 1)->end_steps();
        }
        // Search max value of timewait in all slaves
        if (max_timewait_steps_ <
            slave_vector_ptr_.at(slv - 1)->step_wait_time()) {
            max_timewait_steps_ =
                slave_vector_ptr_.at(slv - 1)->step_wait_time();
        }

        // Update pointer to buffers datas in UnitDevice
        slave_vector_ptr_.at(slv - 1)->update_device_buffers();
    }
}

//------------------------------------------------------------------------------
// Initialize and create IOmap

void Master::Impl::configure_io_map() {
    // Create memory space for I/O map
    unsigned int io_size = 0; // in bits
    for (int slv = 1; slv <= *context_.slavecount; slv++) {
        io_size += context_.slavelist[slv].Obits;
        io_size += context_.slavelist[slv].Ibits;
    }

    iomap_.resize(io_size / 8 + 1); // io_size is in bits so (IS_size/8 +
                                    // 1) to obain a full ended bytes

    ecx_config_map_group(&context_, iomap_.data(),
                         0); // mapped slave in IOmap

    expected_wkc_ = (context_.grouplist[0].outputsWKC * 2) +
                    context_.grouplist[0].inputsWKC;
}

//------------------------------------------------------------------------------

void Master::Impl::init_slaves() {

    for (uint8_t step = 0; step < max_init_steps_; ++step) {
        for (auto& slave : slave_vector_ptr_) {
            slave->pre_init_step(step);
        }

        // Emmission
        // adding sleep to simulate a cyclic loop and let time to slave to
        // receive and send datas
        std::this_thread::sleep_for(
            std::chrono::microseconds(max_timewait_steps_));
        ecx_send_processdata(&context_);
        ecx_receive_processdata(&context_, EC_TIMEOUTRET);

        for (auto& slave : slave_vector_ptr_) {
            slave->post_init_step(step);
        }
    }
}

//------------------------------------------------------------------------------
void Master::Impl::end_slaves() {

    for (uint8_t step = 0; step < max_end_steps_; ++step) {
        for (auto& slave : slave_vector_ptr_) {
            slave->pre_end_step(step);
        }
        // Emmission
        // adding sleep to simulate a cyclic loop and let time to slave to
        // receive and send datas
        std::this_thread::sleep_for(
            std::chrono::microseconds(max_timewait_steps_));
        ecx_send_processdata(&context_);

        ecx_receive_processdata(&context_, EC_TIMEOUTRET);

        for (auto& slave : slave_vector_ptr_) {
            slave->post_end_step(step);
        }
    }
}

bool Master::Impl::switch_slaves_to_init_state() {
    // Force slave to init state
    int counter = 100;
    ecx_readstate(&context_);
    // wait for all slaves to reach INIT state
    do {
        // Reconfigure slave who are not in init state
        for (uint16_t slave_id = 1; slave_id <= *context_.slavecount;
             ++slave_id) {
            if ((context_.slavelist[slave_id].state & 0x0f) > EC_STATE_INIT) {
                context_.slavelist[slave_id].state = EC_STATE_INIT;
                ecx_writestate(&context_, slave_id);
            }
        }
        std::this_thread::sleep_for(std::chrono::microseconds(EC_TIMEOUTSTATE));
        ecx_readstate(&context_);
        ecx_statecheck(&context_, 0, EC_STATE_INIT, EC_TIMEOUTSTATE);
    } while (counter-- && (context_.slavelist[0].state != EC_STATE_INIT));
    if (counter <= 0) {
        pid_log << pid::error << "At least one slave can't change to INIT state"
                << pid::flush;
        return false;
    }
    return true;
}

bool Master::Impl::switch_slaves_to_preop_state() {
    // Force slave to Pre-Op state
    int counter = 100;
    ecx_readstate(&context_);
    do {
        // change state of slaves who are not in Pre-Op state
        for (uint16_t slave_id = 1; slave_id <= *context_.slavecount;
             ++slave_id) {
            auto& slave = context_.slavelist[slave_id];
            // check if slave state has error and send acknowledgement
            if (slave.state == (EC_STATE_ERROR | EC_STATE_PRE_OP)) {
                if (slave.ALstatuscode) {
                    pid_log << pid::warning << "force PRE-OP - WARNING : slave "
                            << slave_id - 1 << " is in PRE_OP + ERROR "
                            << "(0x" << std::hex << slave.ALstatuscode
                            << std::dec << ": "
                            << ec_ALstatuscode2string(slave.ALstatuscode) << ")"
                            << ", send acknowledgement" << pid::flush;
                }
                slave.state = (EC_STATE_ACK | EC_STATE_PRE_OP);
                ecx_writestate(&context_, slave_id);
            }
            // ask change state to PRE-OP when slave isn't in PRE-OP state
            if (slave.state != EC_STATE_PRE_OP and
                slave.state !=
                    EC_STATE_BOOT) // avoid state switch if in boot mode.
            {
                slave.state = EC_STATE_PRE_OP;
                ecx_writestate(&context_, slave_id);
            }
        }
        std::this_thread::sleep_for(std::chrono::microseconds(EC_TIMEOUTSTATE));
        ecx_readstate(&context_);
        ecx_statecheck(&context_, 0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE);
    } while (counter-- and (context_.slavelist[0].state != EC_STATE_PRE_OP));
    if (counter <= 0) {
        std::string ids_not_preop = "";
        for (uint16_t slave_id = 1; slave_id <= *context_.slavecount;
             ++slave_id) {
            auto& slave = context_.slavelist[slave_id];
            if ((slave.state) !=
                EC_STATE_PRE_OP) // TODO check boot mode = 0x03 !
            {
                ids_not_preop += std::to_string(slave_id - 1) + " ";
            }
        }
        pid_log << pid::critical
                << "At least one slave can't change to PRE-OP state. Slaves "
                   "involved: "
                << ids_not_preop << pid::flush;
        return false;
    }
    return true;
}

bool Master::Impl::switch_slaves_to_op_state() {

    int timeout_mon = 500; // Value from SOEM default config
    int counter = 300;

    // update slave state
    ecx_readstate(&context_);
    // wait for all slaves to reach OP state
    do {
        // send one valid process data to make outputs in slaves happy
        // to avoid syncmanager timeout (alstatus arror 0x001b) !!
        ecx_send_processdata(&context_);
        ecx_receive_processdata(&context_, EC_TIMEOUTRET);
        // Reconfigure slave who are not in OP state
        for (uint16_t slv = 1; slv <= *context_.slavecount; slv++) {
            if ((context_.slavelist[slv].state & 0x0f) ==
                EC_STATE_SAFE_OP) // adding 0x0f mask to chech only state
                                  // without take in account error byte.
            {
                // Check if slave is in Error and send acknowledgement
                if ((context_.slavelist[slv].state & EC_STATE_ERROR) ==
                    EC_STATE_ERROR) {
                    pid_log << pid::warning << "slave " << slv
                            << " is in SAFE_OP + ERROR (0x" << std::hex
                            << context_.slavelist[slv].ALstatuscode << std::dec
                            << ": "
                            << ec_ALstatuscode2string(
                                   context_.slavelist[slv].ALstatuscode)
                            << "), send acknowledgement." << pid::flush;

                    context_.slavelist[slv].state =
                        (EC_STATE_SAFE_OP | EC_STATE_ACK);
                    ecx_writestate(&context_, slv);
                }

                context_.slavelist[slv].state = EC_STATE_OPERATIONAL;
                ecx_writestate(&context_, slv);
            } else if ((context_.slavelist[slv].state & 0x0f) ==
                       EC_STATE_PRE_OP) {
                // Check if slave is in Error and send acknowledgement
                if ((context_.slavelist[slv].state & EC_STATE_ERROR) ==
                    EC_STATE_ERROR) {
                    pid_log << pid::warning << "slave " << slv
                            << " is in SAFE_OP + ERROR (0x" << std::hex
                            << context_.slavelist[slv].ALstatuscode << std::dec
                            << ": "
                            << ec_ALstatuscode2string(
                                   context_.slavelist[slv].ALstatuscode)
                            << "), send acknowledgement." << pid::flush;
                    context_.slavelist[slv].state =
                        (EC_STATE_PRE_OP | EC_STATE_ACK);
                    ecx_writestate(&context_, slv);
                }

                context_.slavelist[slv].state = EC_STATE_SAFE_OP;
                ecx_writestate(&context_, slv);
            } else if ((context_.slavelist[slv].state & 0x0f) <
                       EC_STATE_PRE_OP) // Check if state is init adding
                                        // 0x0f to check only operationnal
                                        // state and not error byte
            {
                if (ecx_reconfig_slave(&context_, slv, timeout_mon)) {
                    context_.slavelist[slv].islost = false;
                }
            }
        }
        // Add sleep to let time to slave to change state
        std::this_thread::sleep_for(std::chrono::microseconds(EC_TIMEOUTSTATE));
        // Read_state update state and ALstatus of all slaves and master
        ecx_readstate(&context_);
        ecx_statecheck(&context_, 0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
    } while (counter-- and
             ((context_.slavelist[0].state) != EC_STATE_OPERATIONAL)); //
    if (counter <= 0) {
        pid_log << pid::critical
                << "One or more slave can't change to OP state." << pid::flush;
        return false;
    }
    return true;
}

//------------------------------------------------------------------------------

void Master::Impl::init() {
    // initialize network and detect all slave on hardware bus.
    if (not init_network()) {
        ecx_close(&context_);
        throw std::runtime_error(
            "ethercatcpp: failed to initialize the network");
    }

    pid_log << pid::info << "Number of slave declared in program: "
            << static_cast<int>(slave_vector_ptr_.size())
            << " and number of slave detected on bus: " << *context_.slavecount
            << pid::flush;

    // Assert to check if user declared correct number of slave.
    if (*context_.slavecount != static_cast<int>(slave_vector_ptr_.size())) {
        pid_log << pid::critical << "Slaves on hardware ethercat bus ("
                << *context_.slavecount << ") != slaves declared ("
                << static_cast<int>(slave_vector_ptr_.size()) << ")"
                << pid::flush;
        ecx_close(&context_);
        throw std::runtime_error("ethercatcpp: failed to initialize due to an "
                                 "incorrect description of the bus");
    }

    // For all slave on master, we add configuration from SlaveInfo bus.
    for (uint16_t slv = 1; slv <= *context_.slavecount; ++slv) {
        // Add slave with "slv-1" because this first slave on master is
        // reserved for Master himself so slave vector from master start at 1
        // and slave vector from bus start at 0.
        if (not add_slave(slv,
                          slave_vector_ptr_.at(static_cast<size_t>(slv - 1)))) {
            reset_network();
            throw std::runtime_error("ethercatcpp: incorrect slave found");
        }
    }

    // 1) configure the slaves
    // 2) all slaves go to OP mode
    // 3) initialize all slaves
    // => ready to go !
    if (not configure_slaves()) {
        reset_network();
        throw std::runtime_error(
            "ethercatcpp: problem during slaves configuration");
    }
}

void Master::Impl::reset_network() {
    if (switch_slaves_to_preop_state()) {
        for (auto& slave : slave_vector_ptr_) {
            slave->launch_end_configuration();
        }
    }
    switch_slaves_to_init_state();
    // stop SOEM, close socket
    ecx_close(&context_);

    if (is_redundant()) {
        pid_log << pid::info << "Ethercat master : network interfaces "
                << ifname_primary_ << " and " << ifname_redundant_
                << " and socket closed" << pid::flush;
    } else {
        pid_log << pid::info << "Ethercat master : network interface "
                << ifname_primary_ << " and socket closed" << pid::flush;
    }
}

void Master::Impl::end() {
    end_slaves();
    reset_network();
}

void Master::Impl::add(Device& device) {
    auto devices = device.device_vector();
    for (auto dev : devices) {
        SlaveInfo* device_slave_address = dev->slave_address();
        if (device_slave_address != nullptr) {
            slave_vector_ptr_.push_back(device_slave_address);
        }
    }
}

bool Master::Impl::next_cycle() {
    bool ret = true;
    for (auto& slave : slave_vector_ptr_) {
        // Check and define step have to be launch
        if (slave->current_step() >= slave->run_steps()) {
            slave->set_current_step(0);
        }
        slave->pre_run_step(slave->current_step());
        // increment step when post_run_step finished
    }

    // Emmission
    ecx_send_processdata(&context_);

    if (ecx_receive_processdata(&context_, EC_TIMEOUTRET) < expected_wkc_) {
        ret = false;
    }

    for (auto& slave : slave_vector_ptr_) {
        slave->post_run_step(slave->current_step());
        // Post run step finish => increment current step for next step ...
        slave->increment_current_step();
    }
    return ret;
}

//------------------------------------------------------------------------------

void Master::Impl::manage_ethercat_error() {

    ec_errort Ec;

    if (ecx_poperror(&context_, &Ec)) {
        pid_log << pid::error
                << "Time: " << Ec.Time.sec + (Ec.Time.usec / 1000000.0);
        switch (Ec.Etype) {
        case EC_ERR_TYPE_SDO_ERROR: {
            pid_log << " SDO slave: " << Ec.Slave << " index: " << std::hex
                    << Ec.Index << "." << Ec.SubIdx << std::dec << " error: "
                    << ec_sdoerror2string(static_cast<uint32_t>(Ec.AbortCode))
                    << pid::flush;
            break;
        }
        case EC_ERR_TYPE_EMERGENCY: {
            pid_log << " EMERGENCY slave: " << Ec.Slave
                    << " error: " << std::hex << Ec.ErrorCode << pid::flush;
            break;
        }
        case EC_ERR_TYPE_PACKET_ERROR: {
            pid_log << " PACKET slave: " << Ec.Slave << " index: " << std::hex
                    << Ec.Index << "." << Ec.SubIdx << std::dec
                    << " error: " << Ec.ErrorCode << pid::flush;
            break;
        }
        case EC_ERR_TYPE_SDOINFO_ERROR: {
            pid_log << " SDO slave: " << Ec.Slave << " index: " << std::hex
                    << Ec.Index << "." << Ec.SubIdx << std::dec << " error: "
                    << ec_sdoerror2string(static_cast<uint32_t>(Ec.AbortCode))
                    << pid::flush;
            break;
        }
        case EC_ERR_TYPE_SOE_ERROR: {
            pid_log << " SoE slave: " << Ec.Slave << " IDN: " << std::hex
                    << Ec.Index << std::dec << " error: "
                    << ec_soeerror2string(static_cast<uint16_t>(Ec.AbortCode))
                    << pid::flush;
            break;
        }
        case EC_ERR_TYPE_MBX_ERROR: {
            pid_log << " MBX slave: " << Ec.Slave << " error: " << std::hex
                    << Ec.ErrorCode << std::dec << " "
                    << ec_mbxerror2string(Ec.ErrorCode) << pid::flush;
            break;
        }
        default: {
            pid_log << " error: " << std::hex << Ec.AbortCode << std::dec
                    << pid::flush;
            break;
        }
        }
    }
}

void Master::Impl::print_slave_info() {
    ecx_readstate(&context_);
    for (uint16_t slv = 1; slv <= *context_.slavecount; slv++) {
        const auto& slave = context_.slavelist[slv];
        pid_log << pid::info << pid::endl;
        pid_log << "Slave:" << slv << pid::endl;
        pid_log << " Name:" << slave.name << pid::endl;
        pid_log << " Output size:" << slave.Obits << "bits = " << slave.Obytes
                << "bytes" << pid::endl;
        pid_log << " Input size:" << slave.Ibits << "bits = " << slave.Ibytes
                << "bytes" << pid::endl;
        pid_log << " State:" << slave.state << pid::endl;
        pid_log << " Delay:" << slave.pdelay << "[ns]" << pid::endl;
        pid_log << " Has DC:" << slave.hasdc << pid::endl;

        if (context_.slavelist[slv].hasdc) {
            pid_log << " DCParentport:" << slave.parentport << pid::endl;
        }
        pid_log << " DCParentport:" << slave.parentport << pid::endl;
        pid_log << " Configured slave EtherCat address: 0x" << std::hex
                << slave.configadr << std::dec << pid::endl;

        pid_log << " Manufacturer ID: Ox" << std::hex << slave.eep_man
                << " Slave ID: Ox" << slave.eep_id << " Rev: 0x"
                << slave.eep_rev << std::dec << pid::endl;

        // Print all configured SyncManager
        for (int nSM = 0; nSM < EC_MAXSM; nSM++) {
            if (context_.slavelist[slv].SM[nSM].StartAddr > 0) {
                const auto& sm = slave.SM[nSM];
                pid_log << " SM" << nSM << " StartAdd: Ox" << std::hex
                        << sm.StartAddr << " Length: 0x" << sm.SMlength
                        << " Flags: 0x" << sm.SMflags << std::dec
                        << " Type: " << slave.SMtype[nSM] << pid::endl;
            }
        }
        // Print all configured FMMU
        for (int nFMMU = 0; nFMMU < context_.slavelist[slv].FMMUunused;
             nFMMU++) {
            const auto& fmmu = slave.FMMU[nFMMU];
            pid_log << " FMMU" << nFMMU << pid::endl;
            pid_log << " Logical start: 0x" << std::hex << fmmu.LogStart
                    << std::dec << " Log Length: " << fmmu.LogLength
                    << " Log start bit: " << fmmu.LogStartbit
                    << " Log end bit: " << fmmu.LogEndbit << pid::endl;
            pid_log << " Physical start: 0x" << std::hex << fmmu.PhysStart
                    << std::dec << " Phy start bit: " << fmmu.PhysStartBit
                    << pid::endl;
            pid_log << " Type: 0x" << std::hex << fmmu.FMMUtype << " Active: Ox"
                    << fmmu.FMMUactive << std::dec << pid::endl;
        }
        pid_log << " MBX length wr: " << slave.mbx_l << " rd: " << slave.mbx_rl
                << " MBX protocols: Ox" << std::hex << slave.mbx_proto
                << pid::endl;

        /* SII general section */
        pid_log << " CoE details: 0x" << std::hex << slave.CoEdetails
                << " FoE details: 0x" << slave.FoEdetails << " EoE details: 0x"
                << slave.EoEdetails << " SoE details: 0x" << slave.SoEdetails
                << std::dec << pid::endl;
        pid_log << " Ebus current: " << slave.Ebuscurrent << "[mA]"
                << pid::endl;
        pid_log << " only LRD/LWR: " << slave.blockLRW << pid::endl;
    }
    pid_log << pid::flush;
}

} // namespace ethercatcpp