438 lines
14 KiB
C++
438 lines
14 KiB
C++
/*
|
|
* 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, // 0x1100 write, output, rx buffer offset
|
|
ECT_PDOINPUTOFFSET = 0x1400 // 0x1400, 0x1140 read, input, tx buffer offset
|
|
};
|
|
|
|
enum {
|
|
ECT_RXPDOMAPINDEX = 0x1C12, // write, output, rx pdo map index
|
|
ECT_TXPDOMAPINDEX = 0x1C13 // read, input, tx pdo map index
|
|
};
|
|
|
|
namespace ecat_buffer {
|
|
|
|
/*
|
|
block_index_map[0x1C..][n]
|
|
subindex[0x00] = block_count (1 byte из 2 bytes)
|
|
subindex[0x01] = block_0_index (2 bytes)
|
|
...
|
|
subindex[n] = block_n-1_index (2 bytes)
|
|
|
|
object_descriptor_map[block_n_index][m]
|
|
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)
|
|
|
|
data_size_map[0x1C..]
|
|
pdo_data_size
|
|
*/
|
|
|
|
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; // Размеры в байтах !
|
|
|
|
const uint16_t pdo_output_offset{ECT_PDOOUTPUTOFFSET};
|
|
const uint16_t pdo_input_offset{ECT_PDOINPUTOFFSET};
|
|
|
|
const uint16_t rx_pdo_map_index{ECT_RXPDOMAPINDEX};
|
|
const uint16_t tx_pdo_map_index{ECT_TXPDOMAPINDEX};
|
|
};
|
|
|
|
struct FMMUGlobalProperties {
|
|
address::Logical logical_start_address{0x00000000};
|
|
address::Logical logical_end_address{logical_start_address};
|
|
|
|
uint32_t logical_full_length_write{0x00000000};
|
|
uint32_t logical_full_length_read{0x00000000};
|
|
};
|
|
|
|
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_;
|
|
}
|
|
|
|
template<typename TypeT>
|
|
void read_info_from_eeprom(eeprom::EEPROM& eeprom) {
|
|
auto slave_address = slave_.get_slave_address<TypeT>();
|
|
uint32_t word{0x00000000};
|
|
|
|
for(uint16_t addr = 0x0000; addr < 128; addr += 2) {
|
|
eeprom.read<TypeT>(slave_address, addr, word);
|
|
|
|
DebugP_log((char*)"0x%04x \r\n", word);
|
|
}
|
|
|
|
DebugP_log((char*)"\r\n");
|
|
}
|
|
|
|
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((char*)"buffer_properties_write_ = 0x%04x\r\n", buffer_properties_write_);
|
|
DebugP_log((char*)"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) {
|
|
using TDatagram = datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, BufferProperties, uint32_t>;
|
|
|
|
auto slave_address = slave_.get_slave_address<TypeT>();
|
|
|
|
return TDatagram{ {{slave_address, sync_manager.offset}}, buffer, sync_manager.default_setting };
|
|
}
|
|
|
|
template<typename TypeT>
|
|
void init_sync_manager(telegram::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_);
|
|
|
|
auto queue = datagram_write + datagram_read;
|
|
|
|
do {
|
|
telegram.transfer(queue);
|
|
} 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((char*)"datagram_write.get_wkc() = %d\r\n", datagram_write.get_wkc());
|
|
DebugP_log((char*)"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) {
|
|
using TDatagram = datagram::EcatDatagram<command::EcatCommand<TypeT, command::WR>, FMMUSettings>;
|
|
|
|
auto slave_address = slave_.get_slave_address<TypeT>();
|
|
address::Offset offset = fmmu_regs_[static_cast<size_t>(fmmu_x)];
|
|
|
|
return TDatagram{ {{slave_address, offset}}, settings};
|
|
}
|
|
|
|
template<typename TypeT>
|
|
void init_fmmu_write(telegram::EcatTelegram& telegram, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
|
|
fmmu_write_ = fmmu;
|
|
|
|
FMMUSettings settings {
|
|
.log_start_address = fmmu_global_properties.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 = make_fmmu_datagram<TypeT>(fmmu, settings);
|
|
|
|
fmmu_properties_write_.address = fmmu_global_properties.logical_end_address;
|
|
fmmu_properties_write_.length = buffer_properties_write_.length;
|
|
|
|
fmmu_global_properties.logical_end_address += buffer_properties_write_.length;
|
|
fmmu_global_properties.logical_full_length_write += buffer_properties_write_.length;
|
|
|
|
do {
|
|
telegram.transfer(datagram);
|
|
} while(datagram.get_wkc() < 0x0001);
|
|
}
|
|
|
|
template<typename TypeT>
|
|
void init_fmmu_read(telegram::EcatTelegram& telegram, fmmu fmmu, FMMUGlobalProperties& fmmu_global_properties) {
|
|
fmmu_read_ = fmmu;
|
|
|
|
FMMUSettings settings {
|
|
.log_start_address = fmmu_global_properties.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 = make_fmmu_datagram<TypeT>(fmmu, settings);
|
|
|
|
fmmu_properties_read_.address = fmmu_global_properties.logical_end_address;
|
|
fmmu_properties_read_.length = buffer_properties_read_.length;
|
|
|
|
fmmu_global_properties.logical_end_address += buffer_properties_read_.length;
|
|
fmmu_global_properties.logical_full_length_read += buffer_properties_read_.length;
|
|
|
|
do {
|
|
telegram.transfer(datagram);
|
|
} while(datagram.get_wkc() < 0x0001);
|
|
}
|
|
|
|
template<typename TypeT>
|
|
void init_fmmu(telegram::EcatTelegram& telegram, fmmu fmmu_write, fmmu fmmu_read, FMMUGlobalProperties& fmmu_global_properties) {
|
|
fmmu_write_ = fmmu_write;
|
|
fmmu_read_ = fmmu_read;
|
|
|
|
FMMUSettings settings_write {
|
|
.log_start_address = fmmu_global_properties.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 = fmmu_global_properties.logical_end_address;
|
|
fmmu_properties_write_.length = buffer_properties_write_.length;
|
|
|
|
fmmu_global_properties.logical_end_address += buffer_properties_write_.length;
|
|
fmmu_global_properties.logical_full_length_write += buffer_properties_write_.length;
|
|
|
|
FMMUSettings settings_read {
|
|
.log_start_address = fmmu_global_properties.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 = fmmu_global_properties.logical_end_address;
|
|
fmmu_properties_read_.length = buffer_properties_read_.length;
|
|
|
|
fmmu_global_properties.logical_end_address += buffer_properties_read_.length;
|
|
fmmu_global_properties.logical_full_length_read += 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
|
|
}};
|
|
|
|
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} { }
|
|
|
|
void init(uint16_t rx_eeprom_addr, uint16_t tx_eeprom_addr);
|
|
|
|
EthEcat& get_ecat() {
|
|
return ecat_;
|
|
}
|
|
|
|
std::vector<EcatBufferSlave>& get_buffer_slaves() {
|
|
return buffer_slaves_;
|
|
}
|
|
|
|
FMMUGlobalProperties& get_fmmu_global_properties() {
|
|
return fmmu_global_properties_;
|
|
}
|
|
|
|
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_;
|
|
|
|
FMMUGlobalProperties fmmu_global_properties_;
|
|
|
|
std::vector<EcatBufferSlave> buffer_slaves_;
|
|
};
|
|
|
|
} // namespace ecat_buffer
|
|
|
|
} // namespace free_rtos
|
|
|
|
#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_BUFFER_HPP_ */
|