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)
{
    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;
    /**
     * @brief Создаем стандартный слот для модуля DAP 1 (Device Access Point)
     * Это обязательный модуль для profinet device
     */
    auto dap_slot = make_shared<ProfinetSlot>(PNET_SLOT_DAP_IDENT, PNET_MOD_DAP_IDENT, "DAP 1");
    /**
     * @brief Добавляем слот и подключаем модуль
     * 
     */
    if (!addSlotAndPlugModule(dap_slot))
        return false;

    pnet_data_cfg_t dap_data_cfg = { PNET_DIR_NO_IO, 0, 0 };
    
    /**
     * @brief Создаем стандартный подслот для подмодуля identity 1 который входит в модуль DAP 1
     * Это обязательный подслот для profinet device
     */
    auto dap_subslot_identity_1 = make_shared<ProfinetSubslot>( PNET_SUBSLOT_DAP_IDENT, 
                                                                PNET_SUBMOD_DAP_IDENT, 
                                                                "DAP Identity 1", 
                                                                dap_data_cfg );
    /**
     * @brief Создаем стандартный подслот для подмодуля iface 1 ident который входит в модуль DAP 1
     * Это обязательный подслот для profinet device
     */                                                           
    auto dap_subslot_iface_1_ident = make_shared<ProfinetSubslot>( PNET_SUBSLOT_DAP_INTERFACE_1_IDENT, 
                                                                   PNET_SUBMOD_DAP_INTERFACE_1_IDENT, 
                                                                   "DAP IFACE1 IDENT", 
                                                                   dap_data_cfg );
    /**
     * @brief Создаем стандартный подслот для подмодуля iface 1.port 1 ident который входит в модуль DAP 1
     * Это обязательный подслот для profinet device
     */ 
    auto dap_subslot_iface_1_port1_ident = make_shared<ProfinetSubslot>( PNET_SUBSLOT_DAP_INTERFACE_1_PORT_1_IDENT, 
                                                                         PNET_SUBMOD_DAP_INTERFACE_1_PORT_1_IDENT, 
                                                                         "DAP Port 1", 
                                                                          dap_data_cfg );
    /**
     * @brief Добавляем подслоты в слот и поключаем подмодули в подслоты
     * 
     */
    if (!dap_slot->addSubslotAndPlugSubmodule(m_pnet_data, dap_subslot_identity_1))
        return false;
    
    if (!dap_slot->addSubslotAndPlugSubmodule(m_pnet_data, dap_subslot_iface_1_ident))
        return false;
    
    if (!dap_slot->addSubslotAndPlugSubmodule(m_pnet_data, dap_subslot_iface_1_port1_ident))
        return false;
   
    return true;
}

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;
}

bool Profinet::addSlotAndPlugModule(std::shared_ptr<ProfinetSlot>& slot_ptr)
{
    if (!addSlot(slot_ptr))
        return false;
    
    if (!slot_ptr->plugModule(m_pnet_data))
        return false;
     
    return true;
}

ProfinetSlot * Profinet::getSlotPtrAndCheckModule(uint16_t slot_nbr, uint32_t module_id)
{
    ProfinetSlot * const slot_ptr = getSlotPtr(slot_nbr);

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

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

    return slot_ptr;
}

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

    return m_slots[slot_nbr].get();
}

ProfinetSubslot * Profinet::getSubslotPtr(uint16_t slot_nbr, uint16_t subslot_nbr)
{
    ProfinetSlot * const slot_ptr = getSlotPtr(slot_nbr);

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

    return slot_ptr->getSubslotPtr(subslot_nbr);
}

/**\
 * =========================================================================================
 * 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)
{   
    ProfinetSubslot * const subslot_ptr = getSubslotPtr(slot_nbr, subslot_nbr);

    if (subslot_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;
    }

    ProfinetParameter * param_ptr = subslot_ptr->getParameterPtr(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;
    }

    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;
    }

    *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)
{   
    ProfinetSubslot * const subslot_ptr = getSubslotPtr(slot_nbr, subslot_nbr);

    if (subslot_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;
    }

    ProfinetParameter * param_ptr = subslot_ptr->getParameterPtr(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;
    }

    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;
    }

    /// 
    std::memcpy(param_ptr->data_ptr, p_write_data, param_ptr->length);

    return 0;
}

int Profinet::callbackExpModuleInd ( uint16_t slot, uint32_t module_ident)
{
    ProfinetSlot * const slot_ptr =  getSlotPtrAndCheckModule(slot, module_ident);
    /**
     * @brief Проверка, что слот slot добавлен в конфигурацию и что 
     * идентификатор модуля m_module_id совпадает с module_ident
     * 
     */
    if (slot_ptr == nullptr)
    {
        return -1;
    }

    /// Подключить модуль в слот
    if (!slot_ptr->plugModule(m_pnet_data))
    {
        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)
{
    ProfinetSlot * const slot_ptr =  getSlotPtrAndCheckModule(slot, module_id);
    /**
     * @brief Проверка, что слот slot добавлен в конфигурацию и что 
     * идентификатор модуля m_module_id совпадает с module_ident
     * 
     */
    if (slot_ptr == nullptr)
    {
        return -1;
    }

    /// Проверка, что модуль подключен к слоту
    if (!slot_ptr->isModulePlugged())
    {
        return -1;
    }

    ProfinetSubslot * const subslot_ptr = slot_ptr->getSubslotPtrAndCheckSubmodule(subslot, submodule_id, p_exp_data);
    
    /**
     *  Проверка того, что:
     *  - подслот subslot добавлен в конфигурацию слота,
     *  - идентификатор модуля подслота m_module_id совпадает с submodule_id 
     *  - конфигурация данных слота совпадает с p_exp_data
     */
    if (subslot_ptr == nullptr)
    {
        return -1;
    }
    
    /// Подключить подмодуль submodule_id к подслоту subslot
    if (!subslot_ptr->plugSubmodule(m_pnet_data))
    {
        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;
}