104 lines
2.8 KiB
C++
104 lines
2.8 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;
|
||
datagram::IEcatDatagram *next = datagram_queue_;
|
||
|
||
(void)p_eth_hdr;
|
||
(void)p_hdr;
|
||
|
||
while(next != nullptr) {
|
||
p_datagram_last = next->pack(p_datagram_last);
|
||
next = next->get_next();
|
||
}
|
||
|
||
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;
|
||
datagram::IEcatDatagram *next = datagram_queue_;
|
||
|
||
(void)p_eth_hdr;
|
||
(void)p_hdr;
|
||
|
||
while(next != nullptr) {
|
||
p_datagram_last = next->unpack(p_datagram_last);
|
||
next = next->get_next();
|
||
}
|
||
|
||
return p_datagram_last;
|
||
}
|
||
|
||
void EcatTelegram::transfer(datagram::IEcatDatagram& first) {
|
||
datagram_queue_ = &first;
|
||
|
||
//uint8_t *raw = pack(buffer_out_.data);
|
||
//bool stat = tx_flow_.send(port_id_, buffer_out_.data, raw - buffer_out_.data);
|
||
|
||
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_ = nullptr;
|
||
}
|
||
|
||
}
|
||
|
||
}
|