sitara_depot/components/free_rtos/ethernet_industry/eth_ecat_buffer.hpp

/*
 * eth_ecat_buffer.hpp
 *
 *  Created on: May 3, 2023
 *      Author: algin
 */

#ifndef FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_BUFFER_HPP_
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_BUFFER_HPP_

#include <vector>
#include <map>
#include <kernel/dpl/ClockP.h>

#include "ethernet_industry/eth_ecat.hpp"

namespace free_rtos {

enum {
    ECT_PDOOUTPUTOFFSET    = 0x1100,
    ECT_PDOINPUTOFFSET     = 0x1400
};

enum {
    ECT_RXPDOMAPINDEX    = 0x1C12,
    ECT_TXPDOMAPINDEX    = 0x1C13
};

namespace ecat_buffer {

/*
index[0x1C..]
    subindex[0x00] = block_count (1 byte)
    subindex[0x01] = block_0_index (2 bytes)
    ...
    subindex[n] = block_n-1_index (2 bytes)

block_index
    block_subindex[0x00] = object_count (1 byte / 4 bytes)
    block_subindex[0x01] = object_0_descriptor (4 bytes)
    ...
    block_subindex[m] = object_m_descriptor (4 bytes)

object_descriptor
    size (1 byte)
    object_subindex (1 byte)
    object_index (2 bytes)
*/

struct PDODescriptor {
    uint8_t size;
    uint8_t subindex;
    uint16_t index;
} __attribute__ ((packed));

struct PDOMap {
    std::map<uint16_t, std::map<uint8_t, uint16_t>> block_index_map;
    std::map<uint16_t, std::map<uint8_t, PDODescriptor>> object_descriptor_map;
    std::map<uint16_t, uint16_t> data_size_map; // Размеры в байтах !

    uint16_t pdo_output_offset{ECT_PDOOUTPUTOFFSET};
    uint16_t pdo_input_offset{ECT_PDOINPUTOFFSET};
};

struct FMMUSettings {
    uint32_t log_start_address;
    uint16_t log_data_len;
    uint8_t log_start_bit;
    uint8_t log_end_bit;
    uint16_t phys_start_address;
    uint8_t phys_start_bit;
    uint8_t direction;
    uint8_t activate;
} __attribute__ ((packed));

struct FMMUProperties {
    address::Logical address;
    uint16_t length;
};

struct BufferSettings {
    uint8_t control;
    uint8_t status;
    uint8_t activate;
    uint8_t pdi_control;
};

struct BufferProperties {
    address::Offset offset;
    uint16_t length;
} __attribute__ ((packed));

struct SyncManager {
    address::Offset offset;
    uint32_t default_setting;
};

class EcatBufferSlave {
public:
    EcatBufferSlave(EcatSlave& slave)
    : slave_(slave) { }

    EcatSlave& get_slave()
    {
        return slave_;
    }

    std::array<address::Offset, 4>& get_buffer_regs() {
        return buffer_regs_;
    }

    void set_buffer_offset(uint16_t rx_offset, uint16_t tx_offset) {
        buffer_properties_write_.offset = static_cast<address::Offset>(rx_offset);
        buffer_properties_read_.offset = static_cast<address::Offset>(tx_offset);

        buffer_regs_[MailboxesRegs::WRITE] = buffer_properties_write_.offset;
        buffer_regs_[MailboxesRegs::READ] = buffer_properties_read_.offset;
    }

    // Размер в байтах !
    void set_buffer_length(uint16_t rx_length, uint16_t tx_length) {
        buffer_properties_write_.length = rx_length;
        buffer_properties_read_.length = tx_length;
    }

    BufferProperties& get_buffer_properties_write() {
        return buffer_properties_write_;
    }

    BufferProperties& get_buffer_properties_read() {
        return buffer_properties_read_;
    }

    FMMUProperties& get_fmmu_properties_write() {
        return fmmu_properties_write_;
    }

    FMMUProperties& get_fmmu_properties_read() {
        return fmmu_properties_read_;
    }

    uint32_t get_logical_full_length_write() {
        return logical_full_length_write_;
    }

    uint32_t get_logical_full_length_read() {
        return logical_full_length_read_;
    }

    template<typename TypeT>
    void read_buffer_info_from_eeprom(eeprom::EEPROM& eeprom, uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr) {
        auto slave_address = slave_.get_slave_address<TypeT>();

        eeprom.read<TypeT>(slave_address, rx_eeprom_addr, buffer_properties_write_);
        eeprom.read<TypeT>(slave_address, tx_eeprom_addr, buffer_properties_read_);

        buffer_regs_[MailboxesRegs::WRITE] = buffer_properties_write_.offset;
        buffer_regs_[MailboxesRegs::READ] = buffer_properties_read_.offset;

        DebugP_log("buffer_properties_write_ = 0x%04x\r\n", buffer_properties_write_);
        DebugP_log("buffer_properties_read_ = 0x%04x\r\n", buffer_properties_read_);
    }

    template<typename TypeT>
    datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, BufferProperties, uint32_t>
    make_sync_manager_datagram(SyncManager& sync_manager, BufferProperties& buffer) {
        auto slave_address = slave_.get_slave_address<TypeT>();

        return datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, BufferProperties, uint32_t>{    {{slave_address, sync_manager.offset}},
                                                                                                                buffer,
                                                                                                                sync_manager.default_setting };
    }

    template<typename TypeT>
    void init_sync_manager(datagram::EcatTelegram& telegram, sync_manager sm_write, sync_manager sm_read) {
        SyncManager sync_manager_write = sync_managers_[static_cast<size_t>(sm_write)];
        auto datagram_write = make_sync_manager_datagram<TypeT>(sync_manager_write, buffer_properties_write_);

        SyncManager sync_manager_read = sync_managers_[static_cast<size_t>(sm_read)];
        auto datagram_read = make_sync_manager_datagram<TypeT>(sync_manager_read, buffer_properties_read_);

        datagram_write + datagram_read;

        do {
            telegram.transfer(datagram_write);
        } while(datagram_write.get_all_wkc() < 0x0002);
/*
        do {
            telegram.transfer(datagram_write);
        } while(datagram_write.get_wkc() < 0x0001);

        do {
            telegram.transfer(datagram_read);
        } while(datagram_read.get_wkc() < 0x0001);
*/
        buffer_regs_[MailboxesRegs::EMPTY] = sync_manager_write.offset + 0x05;
        buffer_regs_[MailboxesRegs::AVAILABLE] = sync_manager_read.offset + 0x05;

        DebugP_log("datagram_write.get_wkc() = %d\r\n", datagram_write.get_wkc());
        DebugP_log("datagram_read.get_wkc() = %d\r\n", datagram_read.get_wkc());
    }

    template<typename TypeT>
    datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, FMMUSettings>
    make_fmmu_datagram(fmmu fmmu_x, FMMUSettings& settings) {
        auto slave_address = slave_.get_slave_address<TypeT>();

        return datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, FMMUSettings>{ {{slave_address, fmmu_regs_[static_cast<size_t>(fmmu_x)]}}, settings};
    }

    template<typename TypeT>
    void init_fmmu(datagram::EcatTelegram& telegram, fmmu fmmu_write, fmmu fmmu_read) {
        static address::Logical logical_end_address{logical_start_address_};

        fmmu_write_ = fmmu_write;
        fmmu_read_ = fmmu_read;

        FMMUSettings settings_write {
            .log_start_address = logical_end_address,
            .log_data_len = buffer_properties_write_.length,
            .log_start_bit = 0,
            .log_end_bit = 7,
            .phys_start_address = buffer_properties_write_.offset,
            .phys_start_bit = 0,
            .direction = static_cast<uint8_t>(DataDirection::WRITE),
            .activate = 0x01
        };

        auto datagram_write = make_fmmu_datagram<TypeT>(fmmu_write, settings_write);

        fmmu_properties_write_.address = logical_end_address;
        fmmu_properties_write_.length = buffer_properties_write_.length;
        logical_full_length_write_ += buffer_properties_write_.length;
        logical_end_address += buffer_properties_write_.length;

        FMMUSettings settings_read {
            .log_start_address = logical_end_address,
            .log_data_len = buffer_properties_read_.length,
            .log_start_bit = 0,
            .log_end_bit = 7,
            .phys_start_address = buffer_properties_read_.offset,
            .phys_start_bit = 0,
            .direction = static_cast<uint8_t>(DataDirection::READ),
            .activate = 0x01
        };

        auto datagram_read = make_fmmu_datagram<TypeT>(fmmu_read, settings_read);

        fmmu_properties_read_.address = logical_end_address;
        fmmu_properties_read_.length = buffer_properties_read_.length;
        logical_full_length_read_ += buffer_properties_read_.length;
        logical_end_address += buffer_properties_read_.length;

        datagram_write + datagram_read;

        do {
            telegram.transfer(datagram_write);
        } while(datagram_write.get_all_wkc() < 0x0002);
/*
        do {
            telegram.transfer(datagram_write);
        } while(datagram_write.get_wkc() < 0x0001);

        do {
            telegram.transfer(datagram_read);
        } while(datagram_read.get_wkc() < 0x0001);
*/
        DebugP_log("datagram_read.get_wkc() = %d\r\n", datagram_read.get_wkc());
        DebugP_log("datagram_write.get_wkc() = %d\r\n", datagram_write.get_wkc());
    }

private:
    static constexpr std::array<SyncManager, 4> sync_managers_ = {{
        {ECT_REG_SM0, EC_DEFAULTMBXSM0},
        {ECT_REG_SM1, EC_DEFAULTMBXSM1},
        {ECT_REG_SM2, EC_DEFAULTMBXSM2},
        {ECT_REG_SM3, EC_DEFAULTMBXSM3}
    }};

    static constexpr std::array<address::Offset, 4> fmmu_regs_ = {{
        ECT_REG_FMMU0,
        ECT_REG_FMMU1,
        ECT_REG_FMMU2,
        ECT_REG_FMMU3
    }};

    static constexpr uint32_t logical_start_address_{0x00000000};

    static uint32_t logical_full_length_write_;
    static uint32_t logical_full_length_read_;

    std::array<address::Offset, 4> buffer_regs_ = {
        static_cast<address::Offset>(0x0000),
        static_cast<address::Offset>(0x0000),
        static_cast<address::Offset>(0x0000),
        static_cast<address::Offset>(0x0000),
    };

    BufferProperties buffer_properties_write_;
    BufferProperties buffer_properties_read_;

    fmmu fmmu_write_;
    fmmu fmmu_read_;

    FMMUProperties fmmu_properties_write_;
    FMMUProperties fmmu_properties_read_;

    EcatSlave& slave_;
};


class EthEcatBuffer {
public:
    EthEcatBuffer(EthEcat& ecat): ecat_{ecat} { }

    EthEcat& get_ecat()
    {
        return ecat_;
    }

    std::vector<EcatBufferSlave>& get_buffer_slaves()
    {
        return buffer_slaves_;
    }

    void init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr);

    void set_buffer_offset(std::vector<PDOMap>& pdo_map) {
        uint32_t i = 0;

        for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
            buffer_slave.set_buffer_offset(pdo_map[i].pdo_output_offset, pdo_map[i].pdo_input_offset);
            i++;
        }
    }

    void set_buffer_length(std::vector<PDOMap>& pdo_map) {
        uint32_t i = 0;

        for(EcatBufferSlave& buffer_slave : buffer_slaves_) {
            buffer_slave.set_buffer_length(pdo_map[i].data_size_map[ECT_RXPDOMAPINDEX], pdo_map[i].data_size_map[ECT_TXPDOMAPINDEX]);
            i++;
        }
    }

    void init_sync_manager(sync_manager sm_write, sync_manager sm_read);
    void init_fmmu(fmmu fmmu_write, fmmu fmmu_read);

private:
    EthEcat& ecat_;

    std::vector<EcatBufferSlave> buffer_slaves_;
};

} // namespace ecat_buffer

} // namespace free_rtos

#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_BUFFER_HPP_ */