ProfinetConnector/src/profinet/profinet.cpp

#include "profinet.hpp"

#include "profinet_cb_state_ind.hpp"
#include "profinet_cb_connect_ind.hpp" 
#include "profinet_cb_release_ind.hpp" 
#include "profinet_cb_dcontrol_ind.hpp" 
#include "profinet_cb_ccontrol_ind.hpp" 
#include "profinet_cb_read_ind.hpp" 
#include "profinet_cb_write_ind.hpp" 
#include "profinet_cb_exp_module_ind.hpp" 
#include "profinet_cb_exp_submodule_ind.hpp" 
#include "profinet_cb_new_data_status_ind.hpp" 
#include "profinet_cb_alarm_ind.hpp" 
#include "profinet_cb_alarm_cnf.hpp" 
#include "profinet_cb_alarm_ack_cnf.hpp" 
#include "profinet_cb_reset_ind.hpp" 
#include "profinet_cb_signal_led_ind.hpp" 

#include "utils.h"
#include <cstring>

#include <cstdio>
#include <cstring>
#include <iostream>


using namespace std;


#define GET_HIGH_BYTE(id) ((id >> 8) & 0xFF)
#define GET_LOW_BYTE(id)  (id & 0xFF)

#define APP_MAIN_SLEEPTIME_US          5000 * 1000
#define APP_SNMP_THREAD_PRIORITY       1
#define APP_SNMP_THREAD_STACKSIZE      256 * 1024 /* bytes */
#define APP_ETH_THREAD_PRIORITY        10
#define APP_ETH_THREAD_STACKSIZE       4096 /* bytes */
#define APP_BG_WORKER_THREAD_PRIORITY  5
#define APP_BG_WORKER_THREAD_STACKSIZE 4096 /* bytes */

static void pnetConfigure(pnet_cfg_t& cfg, ProfinetSettings& Settings, ProfinetDeviceSettings& DevSettings)
{   
    cfg.tick_us = Settings.ticks_us;
    cfg.im_0_data.im_vendor_id_hi = GET_HIGH_BYTE(DevSettings.im_0.vendor_id);
    cfg.im_0_data.im_vendor_id_lo = GET_LOW_BYTE (DevSettings.im_0.vendor_id);
    cfg.im_0_data.im_hardware_revision = DevSettings.im_0.hw_revision;
    cfg.im_0_data.im_sw_revision_prefix = DevSettings.im_0.sw_revision.prefix;
    cfg.im_0_data.im_sw_revision_functional_enhancement = DevSettings.im_0.sw_revision.functional_enhancement;
    cfg.im_0_data.im_sw_revision_bug_fix = DevSettings.im_0.sw_revision.bug_fix;
    cfg.im_0_data.im_sw_revision_internal_change = DevSettings.im_0.sw_revision.internal_change;
    cfg.im_0_data.im_revision_counter = DevSettings.im_0.revision_counter;
    cfg.im_0_data.im_profile_id = DevSettings.im_0.profile_id;
    cfg.im_0_data.im_profile_specific_type = DevSettings.im_0.profile_specific_type;
    cfg.im_0_data.im_version_major = 1;
    cfg.im_0_data.im_version_minor = 1;
    cfg.im_0_data.im_supported = DevSettings.im_0.supported;

    std::snprintf (
      cfg.im_0_data.im_order_id,
      sizeof (cfg.im_0_data.im_order_id),
      "%s",
      DevSettings.im_0.order_id.c_str());
    std::snprintf (
        cfg.im_0_data.im_serial_number,
        sizeof (cfg.im_0_data.im_serial_number),
        "%s",
        DevSettings.im_0.serial_number.c_str());
    std::snprintf (
        cfg.im_1_data.im_tag_function,
        sizeof (cfg.im_1_data.im_tag_function),
        "%s",
        DevSettings.im_1.tag_function.c_str());
    std::snprintf (
        cfg.im_1_data.im_tag_location,
        sizeof (cfg.im_1_data.im_tag_location),
        "%s",
        DevSettings.im_1.tag_location.c_str());
    std::snprintf (
        cfg.im_2_data.im_date,
        sizeof (cfg.im_2_data.im_date),
        "%s",
        DevSettings.im_2.date.c_str());
    std::snprintf (
        cfg.im_3_data.im_descriptor,
        sizeof (cfg.im_3_data.im_descriptor),
        "%s",
        DevSettings.im_3.descriptor.c_str());
    std::snprintf (
        cfg.im_4_data.im_signature,
        sizeof (cfg.im_4_data.im_signature),
        "%s",
        DevSettings.im_4.signature.c_str());

    cfg.state_cb = profinet_cb_state_ind;
    cfg.connect_cb = profinet_cb_connect_ind;
    cfg.release_cb = profinet_cb_release_ind;
    cfg.dcontrol_cb = profinet_cb_dcontrol_ind;
    cfg.ccontrol_cb = profinet_cb_ccontrol_ind;
    cfg.read_cb = profinet_cb_read_ind;
    cfg.write_cb = profinet_cb_write_ind;
    cfg.exp_module_cb = profinet_cb_exp_module_ind;
    cfg.exp_submodule_cb = profinet_cb_exp_submodule_ind;
    cfg.new_data_status_cb = profinet_cb_new_data_status_ind;
    cfg.alarm_ind_cb = profinet_cb_alarm_ind;
    cfg.alarm_cnf_cb = profinet_cb_alarm_cnf;
    cfg.alarm_ack_cnf_cb = profinet_cb_alarm_ack_cnf;
    cfg.reset_cb = profinet_cb_reset_ind;
    cfg.signal_led_cb = profinet_cb_signal_led_ind;
    
    cfg.device_id.vendor_id_hi = GET_HIGH_BYTE (DevSettings.im_0.vendor_id);
    cfg.device_id.vendor_id_lo = GET_LOW_BYTE (DevSettings.im_0.vendor_id);
    cfg.device_id.device_id_hi = GET_HIGH_BYTE (DevSettings.device_id);
    cfg.device_id.device_id_lo = GET_LOW_BYTE (DevSettings.device_id);
    cfg.oem_device_id.vendor_id_hi = GET_HIGH_BYTE (DevSettings.oem_vendor_id);
    cfg.oem_device_id.vendor_id_lo = GET_LOW_BYTE (DevSettings.oem_vendor_id);
    cfg.oem_device_id.device_id_hi = GET_HIGH_BYTE (DevSettings.oem_device_id);
    cfg.oem_device_id.device_id_lo = GET_LOW_BYTE (DevSettings.oem_device_id);


    snprintf (
      cfg.product_name,
      sizeof (cfg.product_name),
      "%s",
      DevSettings.product_name.c_str());

    cfg.send_hello = true;

    /* Timing */
    cfg.min_device_interval = DevSettings.min_device_interval;

    /* Should be set by application as part of network configuration. */
    cfg.num_physical_ports = 1;

    snprintf (
        cfg.station_name,
        sizeof (cfg.station_name),
        "%s",
        DevSettings.station_name.c_str());

    /* Diagnosis mechanism */
    /* We prefer using "Extended channel diagnosis" instead of
        * "Qualified channel diagnosis" format on the wire,
        * as this is better supported by Wireshark.
        */
    cfg.use_qualified_diagnosis = false;
}

Profinet::Profinet(void) : m_pnet_data({nullptr, 0})
{
    
}

bool Profinet::Config(ProfinetSettings& Settings, ProfinetDeviceSettings& DevSettings)
{
    bool ret = true;
    pnet_cfg_t pnet_cfg = {0};
    utils_netif_namelist_t netif_name_list;
    uint16_t number_of_ports = 1;
    pnet_if_cfg_t netif_cfg = {0};

    std::memset(&pnet_cfg, 0, sizeof(pnet_cfg));

    pnetConfigure(pnet_cfg, Settings, DevSettings);
    ///TODO Определить что передавать в cb_cfg
    pnet_cfg.cb_arg = static_cast<void*>(this);
    
    utils_pnet_cfg_init_netifs(Settings.EthIface.c_str(),
        &netif_name_list,
        &number_of_ports,
        &netif_cfg,
        0x10 /* Copper 100 Mbit/s Full duplex */
    );

    pnet_cfg.if_cfg = netif_cfg;
    pnet_cfg.num_physical_ports = number_of_ports;

    pnet_cfg.pnal_cfg.snmp_thread.prio = APP_SNMP_THREAD_PRIORITY;
    pnet_cfg.pnal_cfg.snmp_thread.stack_size = APP_SNMP_THREAD_STACKSIZE;
    pnet_cfg.pnal_cfg.eth_recv_thread.prio = APP_ETH_THREAD_PRIORITY;
    pnet_cfg.pnal_cfg.eth_recv_thread.stack_size = APP_ETH_THREAD_STACKSIZE;
    pnet_cfg.pnal_cfg.bg_worker_thread.prio = APP_BG_WORKER_THREAD_PRIORITY;
    pnet_cfg.pnal_cfg.bg_worker_thread.stack_size = APP_BG_WORKER_THREAD_STACKSIZE;
    
    /// Инициализация библиотеки pnet
    m_pnet_data.pnet_ptr = pnet_init (&pnet_cfg);

    if (m_pnet_data.pnet_ptr == nullptr)
        return false;
    
    ///
    pnet_data_cfg_t dap_data_cfg = { PNET_DIR_NO_IO, 0, 0 };
    /// Создаем обязательный модуль DAP
    auto dap_module = make_shared<ProfinetModule>(PNET_MOD_DAP_IDENT, "DAP 1");
    /// Создаем обязательный подмодуль DAP Identity 1
    auto dap_submodule_indent_1 = make_shared<ProfinetSubmodule>(PNET_SUBMOD_DAP_IDENT, "DAP Identity 1", dap_data_cfg);
    /// Создаем обязательный подмодуль DAP IFACE1 IDENT
    auto dap_submodule_iface_1_indent_1 = make_shared<ProfinetSubmodule>(PNET_SUBMOD_DAP_INTERFACE_1_IDENT, "DAP IFACE1 IDENT", dap_data_cfg);
    /// Создаем обязательный подмодуль DAP Port 1
    auto dap_subslot_iface_1_port1_ident = make_shared<ProfinetSubmodule>(PNET_SUBMOD_DAP_INTERFACE_1_PORT_1_IDENT, "DAP Port 1", dap_data_cfg);
    /// Добавляем подмодули к модулю
    ret&= dap_module->addSubmodule(dap_submodule_indent_1);
    ret&= dap_module->addSubmodule(dap_submodule_iface_1_indent_1);
    ret&= dap_module->addSubmodule(dap_subslot_iface_1_port1_ident);

    /// Создаем слот для модуля DAP и доключаем к нему модуль dap_module
    ret&= addSlotAndPlugModule(PNET_SLOT_DAP_IDENT, dap_module);
    /// Добавляем подслоты к слоту и подключаем к подслоту подмодуль
    /// 1. Подмодуль "DAP Identity 1"
    ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_IDENT, dap_submodule_indent_1);
    /// 2. Подмодуль "DAP IFACE1 IDENT"
    ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_INTERFACE_1_IDENT, dap_submodule_iface_1_indent_1);
    /// 3. Подмодуль "DAP Port 1"
    ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_INTERFACE_1_PORT_1_IDENT, dap_subslot_iface_1_port1_ident);
   
    return ret;
}

bool Profinet::addSlot(std::shared_ptr<ProfinetSlot>& slot_ptr)
{
    if (slot_ptr == nullptr)
        return false;

    m_slots[slot_ptr->m_slot_nbr] = slot_ptr;

    return true;
}

std::shared_ptr<ProfinetSlot> Profinet::addSlot(uint16_t slot_nbr)
{
    std::shared_ptr<ProfinetSlot> slot_ptr(nullptr);

    if (m_slots.count(slot_nbr))
    {
        slot_ptr = m_slots[slot_nbr];
    }
    else
    {
        slot_ptr = make_shared<ProfinetSlot>(slot_nbr);
    }

    if (slot_ptr != nullptr)
    {
        m_slots[slot_nbr] = slot_ptr;
    }

    return slot_ptr;
}

bool Profinet::addModule(std::shared_ptr<ProfinetModule>& module_ptr)
{
    if (module_ptr == nullptr)
        return false;
    
    m_modules[module_ptr->m_id] = module_ptr;

    return true;
}

std::shared_ptr<ProfinetModule> Profinet::getModule(uint32_t module_id)
{
    if (!m_modules.count(module_id))
    {
        return nullptr;
    }

    return m_modules[module_id];
}


bool Profinet::addSlotAndPlugModule(uint16_t slot_nbr, std::shared_ptr<ProfinetModule>& module_ptr)
{
    auto slot = addSlot(slot_nbr);

    if (slot == nullptr)
    {
        return false;
    }

    return slot->plugModule(m_pnet_data, module_ptr);
}

bool Profinet::addSlotAndPlugModule(uint16_t slot_nbr, uint32_t module_id)
{
    /// 1. Проверяем, что module_id поддерживается
    auto module_ptr = getModule(module_id);

    if (module_ptr == nullptr)
    {
        return false;
    }
    ///2. Добавляет слот 
    auto slot = addSlot(slot_nbr);

    if (slot == nullptr)
    {
        return false;
    }
    /// 3. Подключаем модуль к слоту
    return slot->plugModule(m_pnet_data, module_ptr);
}


bool Profinet::addSubslotAndPlugSubmodule(uint16_t slot_nbr, uint16_t subslot_nbr, std::shared_ptr<ProfinetSubmodule>& submodule_ptr)
{
    auto slot_ptr = getSlotPtr(slot_nbr);

    if (slot_ptr == nullptr)
    {
        return false;
    }

    auto subslot_ptr = slot_ptr->addSubslot(subslot_nbr);

    if (subslot_ptr == nullptr)
    {
        return false;
    }

    return subslot_ptr->plugSubmodule(m_pnet_data, submodule_ptr);
}

bool Profinet::addSubslotAndPlugSubmodule(uint16_t slot_nbr, 
                                uint16_t subslot_nbr, 
                                uint32_t module_id,
                                uint32_t submodule_id,
                                const pnet_data_cfg_t * p_exp_data)
{
    ///1. Проверяем наличие слота
    auto slot_ptr = getSlotPtr(slot_nbr);

    if (slot_ptr == nullptr)
    {
        return false;
    }

    ///2. Проверяем, что к слоту подключен модуль с идентификатором module_id
    auto module_ptr = slot_ptr->getModulePtr();
    if (module_ptr == nullptr)
    {
        return false;
    }

    if (module_ptr->m_id != module_id)
    {
        return false;
    }

    ///3. Проверяем, что в модуль входит подмодуль с идентфикатором submodule_id
    auto submodule_ptr = module_ptr->getSubmodulePtr(submodule_id);
    if (submodule_ptr == nullptr)
    {
        return false;
    }

    ///4. Проверяем, что конфигурация данных совпадает
    if ((submodule_ptr->m_data_cfg.data_dir != p_exp_data->data_dir) ||
        (submodule_ptr->m_data_cfg.insize != p_exp_data->insize) ||
        (submodule_ptr->m_data_cfg.outsize != p_exp_data->outsize))
    {
        return false;
    }

    return addSubslotAndPlugSubmodule(slot_nbr, subslot_nbr, submodule_ptr);
}


std::shared_ptr<ProfinetSlot> Profinet::getSlotPtr(uint16_t slot_nbr)
{
    /// 1. Проверить, что слот slot добавлен в конфигурацию
    if (!m_slots.count(slot_nbr))
    {
       return nullptr;
    }

    return m_slots[slot_nbr];
}

std::shared_ptr<ProfinetSubslot> Profinet::getSubslotPtr(uint16_t slot_nbr, uint16_t subslot_nbr)
{
    auto slot_ptr = getSlotPtr(slot_nbr);

    if (slot_ptr == nullptr)
    {
        return nullptr;
    }

    return slot_ptr->getSubslotPtr(subslot_nbr);
}

std::shared_ptr<ProfinetParameter> Profinet::getSubmoduleParameter(uint16_t slot_nbr, uint16_t subslot_nbr, uint32_t param_index)
{
    ///1. Проверяем что существует подслот \a subslot_nbr входящий к \a slot_nbr
    auto subslot_ptr = getSubslotPtr(slot_nbr, subslot_nbr);

    if (subslot_ptr == nullptr)
    {
        return nullptr;
    }

    ///2. Проверяем что к подслоту \a subslot_nbr подключен подмодуль
    auto submodule_ptr = subslot_ptr->getSubmodulePtr();

    if (submodule_ptr == nullptr)
    {
        return nullptr; 
    }
    ///3. Проверяем, что у подмодуля есть параметр с индексом idx
    auto param_ptr = submodule_ptr->getParameterPtr(param_index);

    if (param_ptr == nullptr)
    {
        return nullptr;
    }

    return param_ptr;
}

/**\
 * =========================================================================================
 * Callbacks
 * =========================================================================================
 */

int Profinet::callbackStateInd ( uint32_t arep, pnet_event_values_t event)
{
    return 0;
}

int Profinet::callbackConnectInd ( uint32_t arep, pnet_result_t * p_result)
{
    /**
     * @brief 
     * Если будет возвращено 0, то соединение будет установлено. 
     * Если будет возвращено значение отличное от 0, то не будет, в этом случае нужно указать p_result.
     */

    /// Сообщить приложению об установлении связи с контроллером.

    return 0;
}

int Profinet::callbackReleaseInd ( uint32_t arep, pnet_result_t * p_result)
{
    /**
     * @brief 
     * Cоединение будет разорвано при любом возвращаемом значении
     */

    /// Сообщить приложению об разрыве связи с контроллером.

    return 0;
}

int Profinet::callbackDcontrolInd ( uint32_t arep, 
                                    pnet_control_command_t control_command,
                                    pnet_result_t * p_result)
{
    /**
     * @brief 
     * Можно оставить пустым
     */
    return 0;
}

int Profinet::callbackCcontrolInd ( uint32_t arep, pnet_result_t * p_result)
{
    /**
     * @brief 
     * Можно оставить пустым
     */
    return 0;
}

int Profinet::callbackReadInd ( uint32_t arep,
                                uint16_t slot_nbr,
                                uint16_t subslot_nbr,
                                uint16_t idx,
                                uint16_t sequence_number,
                                uint8_t ** pp_read_data,
                                uint16_t * p_read_length,
                                pnet_result_t * p_result)
{   
    ///1. Проверяем, что у подмодуля есть параметр с индексом idx
    auto param_ptr = getSubmoduleParameter(slot_nbr, subslot_nbr, idx);

    if (param_ptr == nullptr)
    {
        p_result->pnio_status.error_code = PNET_ERROR_CODE_READ;
        p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
        p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_READ_ERROR;
        p_result->pnio_status.error_code_2 = 0; /* User specific */
        return -1;
    }

    ///2. Проверяем корректность длины данных (меньше быть не должно, если больше - это норм)
    if (*p_read_length < param_ptr->length)
    {
        p_result->pnio_status.error_code = PNET_ERROR_CODE_READ;
        p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
        p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_READ_ERROR;
        p_result->pnio_status.error_code_2 = 0; /* User specific */
        return -1;
    }

    ///3. Передаем данные параметра
    *pp_read_data = param_ptr->data_ptr;
    *p_read_length = param_ptr->length;

    return 0;
}

int Profinet::callbackWriteInd ( uint32_t arep,
        uint16_t slot_nbr,
        uint16_t subslot_nbr,
        uint16_t idx,
        uint16_t sequence_number,
        uint16_t write_length,
        const uint8_t * p_write_data,
        pnet_result_t * p_result)
{
    ///1. Проверяем, что у подмодуля есть параметр с индексом idx
    auto param_ptr = getSubmoduleParameter(slot_nbr, subslot_nbr, idx);

    if (param_ptr == nullptr)
    {   
        p_result->pnio_status.error_code = PNET_ERROR_CODE_WRITE;
        p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
        p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_WRITE_ERROR;
        p_result->pnio_status.error_code_2 = 0; /* User specific */
        return -1;
    }
    ///2. Проверяем корректность длины данных
    if (write_length != param_ptr->length)
    {
        p_result->pnio_status.error_code = PNET_ERROR_CODE_WRITE;
        p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
        p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_WRITE_ERROR;
        p_result->pnio_status.error_code_2 = 0; /* User specific */
        return -1;
    }

    ///3. Копируем данные
    std::memcpy(param_ptr->data_ptr, p_write_data, param_ptr->length);

    return 0;
}

int Profinet::callbackExpModuleInd ( uint16_t slot, uint32_t module_ident)
{
    bool ret = addSlotAndPlugModule(slot, module_ident);
    
    if (ret == false)
    {
        return -1;
    }
    
    return 0;
}

int Profinet::callbackExpSubmoduleInd ( uint16_t slot,
                              uint16_t subslot,
                              uint32_t module_id,
                              uint32_t submodule_id,
                              const pnet_data_cfg_t * p_exp_data)
{
    bool ret = addSubslotAndPlugSubmodule(slot, subslot, module_id, submodule_id, p_exp_data);
    
    if (ret == false)
    {
        return -1;
    }
    
    return 0;
}

int Profinet::callbackNewDataStatusInd ( uint32_t arep,
                               uint32_t crep,
                               uint8_t changes,
                               uint8_t data_status)
{
    /// Пока ничего не делаем
    return 0;
}

int Profinet::callbackAlarmInd ( uint32_t arep,
                       const pnet_alarm_argument_t * p_alarm_arg,
                       uint16_t data_len,
                       uint16_t data_usi,
                       const uint8_t * p_data)
{
   /**
    * @brief Должен быть ответ pnet_alarm_send_ack
    * Пока заглушка
    * 
    */
    pnet_pnio_status_t pnio_status = {0, 0, 0, 0};

    pnet_alarm_send_ack (m_pnet_data.pnet_ptr, arep, p_alarm_arg, &pnio_status);

    return 0;
}

int Profinet::callbackAlarmCnf ( uint32_t arep, const pnet_pnio_status_t * p_pnio_status)
{
    /// Пока ничего не делаем
    return 0;
}

int Profinet::callbackAlarmAckCnf ( uint32_t arep, int res)
{
    /// Пока ничего не делаем
    return 0;
}

int Profinet::callbackResetInd ( bool should_reset_application, uint16_t reset_mode)
{
    /// Пока ничего не делаем
    return 0;
}

int Profinet::callbackSignalLedInd (bool led_state)
{
    /// Пока ничего не делаем
    return 0;
}