128 lines
3.3 KiB
C++
128 lines
3.3 KiB
C++
/*
|
||
* eth_ecat_telegram.cpp
|
||
*
|
||
* Created on: Jun 5, 2023
|
||
* Author: algin
|
||
*/
|
||
|
||
#include "ethernet_industry/eth_ecat_telegram.hpp"
|
||
|
||
namespace free_rtos {
|
||
|
||
namespace telegram {
|
||
|
||
int32_t EcatTelegram::Process(uint8_t *p_data, uint32_t len) {
|
||
// TODO: Не забывать вычитать из указателя sizeof(TEthFrameHeader) !
|
||
unpack(p_data - sizeof(TEthFrameHeader));
|
||
|
||
/*
|
||
DebugP_log((char*)"Process started\r\n");
|
||
|
||
for(uint8_t *data = p_data; data < p_data + len; data++) {
|
||
DebugP_log((char*)"0x%01x ", *data);
|
||
}
|
||
|
||
DebugP_log((char*)"\r\n");
|
||
*/
|
||
|
||
rx_sem_.post();
|
||
|
||
return 0;
|
||
}
|
||
|
||
uint32_t EcatTelegram::Sender(uint8_t *p_data, size_t scatter_segment) {
|
||
uint8_t *raw = pack(p_data);
|
||
|
||
//DebugP_log((char*)"Sender started\r\n");
|
||
|
||
return raw - p_data;
|
||
}
|
||
|
||
uint8_t* EcatTelegram::pack(uint8_t *raw) {
|
||
TEthFrameHeader *p_eth_hdr = new(raw) TEthFrameHeader{ {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF},
|
||
{0x01, 0x01, 0x01, 0x01, 0x01, 0x01},
|
||
ETH_PROT_ECAT_LE };
|
||
TEcatFrameHeader *p_hdr = new(raw + sizeof(TEthFrameHeader)) TEcatFrameHeader{ .bits{ .length = 0,
|
||
.type = static_cast<uint16_t>(ec_network::PROTOCOL_TYPE) } };
|
||
uint8_t *p_datagram_first = raw + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader);
|
||
uint8_t *p_datagram_last = p_datagram_first;
|
||
|
||
(void)p_eth_hdr;
|
||
(void)p_hdr;
|
||
|
||
auto queue = datagram_queue_; // Копия очереди
|
||
auto next = queue.dequeue();
|
||
|
||
while(next != nullptr) {
|
||
//DebugP_log((char*)"Packet packed\r\n");
|
||
|
||
p_datagram_last = next->pack(p_datagram_last);
|
||
next = queue.dequeue();
|
||
}
|
||
|
||
p_hdr->bits.length = p_datagram_last - p_datagram_first;
|
||
|
||
return p_datagram_last;
|
||
}
|
||
|
||
uint8_t* EcatTelegram::unpack(uint8_t *raw) {
|
||
TEthFrameHeader *p_eth_hdr = reinterpret_cast<TEthFrameHeader*>(raw);
|
||
TEcatFrameHeader *p_hdr = reinterpret_cast<TEcatFrameHeader*>(raw + sizeof(TEthFrameHeader));
|
||
uint8_t *p_datagram_first = raw + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader);
|
||
uint8_t *p_datagram_last = p_datagram_first;
|
||
|
||
if(p_eth_hdr->prot_id != ETH_PROT_ECAT_LE) {
|
||
DebugP_log((char*)"Error: wrong protocol ID\r\n");
|
||
|
||
return raw;
|
||
}
|
||
|
||
if(p_hdr->bits.type != static_cast<uint16_t>(ec_network::PROTOCOL_TYPE)) {
|
||
DebugP_log((char*)"Error: wrong ethercat protocol type\r\n");
|
||
|
||
return raw;
|
||
}
|
||
|
||
auto queue = datagram_queue_; // Копия очереди
|
||
auto next = queue.dequeue();
|
||
|
||
while(next != nullptr) {
|
||
//DebugP_log((char*)"Packet unpacked\r\n");
|
||
|
||
p_datagram_last = next->unpack(p_datagram_last);
|
||
next = queue.dequeue();
|
||
}
|
||
|
||
return p_datagram_last;
|
||
}
|
||
|
||
void EcatTelegram::transfer() {
|
||
bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1);
|
||
|
||
if(stat == false) {
|
||
DebugP_log((char*)"telegram transfer error !\r\n");
|
||
|
||
return;
|
||
}
|
||
|
||
rx_sem_.pend();
|
||
|
||
datagram_queue_.clear();
|
||
}
|
||
|
||
void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
|
||
datagram_queue_ + next;
|
||
|
||
transfer();
|
||
}
|
||
|
||
void EcatTelegram::transfer(queue::Queue<datagram::IEcatDatagram>& next) {
|
||
datagram_queue_ + next;
|
||
|
||
transfer();
|
||
}
|
||
|
||
}
|
||
|
||
}
|