115 lines
3.8 KiB
C++
115 lines
3.8 KiB
C++
/*
|
||
* eth_stack.cpp
|
||
*
|
||
* Created on: 14 <20><><EFBFBD>. 2023 <20>.
|
||
* Author: sychev
|
||
*/
|
||
#include "free_rtos/ethernet_ip/eth_stack.hpp"
|
||
#include "free_rtos/ethernet_ip/eth_prots_id.h"
|
||
#include "free_rtos/ethernet/eth_frame.h"
|
||
|
||
#include "free_rtos/ethernet_ip/eth_ip_prots_id.h"
|
||
|
||
#include <cstring>
|
||
|
||
free_rtos::EthStack::EthStack(EthTxFlowIface& tx_flow) :
|
||
tx_flow_{tx_flow},
|
||
arp_{*this},
|
||
ip_{*this, arp_},
|
||
udp_{ip_}
|
||
{
|
||
|
||
}
|
||
|
||
bool free_rtos::EthStack::init(Settings& sett) {
|
||
|
||
eth_.self_mac_ = sett.mac_addr_be;
|
||
eth_.mac_port_ = sett.mac_port;
|
||
eth_.self_ip_ = sett.ip_addr_be;
|
||
|
||
rx_pkt_handler_.Register(ETH_PROT_ARP_BE, &arp_);
|
||
rx_pkt_handler_.Register(ETH_PROT_IP_BE, &ip_);
|
||
|
||
udp_.setPortId(eth_.mac_port_);
|
||
|
||
ip_.setSelfIpAddr(eth_.self_ip_);
|
||
|
||
ip_.Register(IP_PROT_ICMP, &icmp_);
|
||
ip_.Register(IP_PROT_UDP, &udp_);
|
||
|
||
return true;
|
||
}
|
||
|
||
bool free_rtos::EthStack::Register(uint32_t prot_id, Handler * p_handler) {
|
||
return rx_pkt_handler_.Register(prot_id, p_handler);
|
||
}
|
||
|
||
void free_rtos::EthStack::set_mac_address(uint64_t mac_be) {
|
||
eth_.self_mac_ = mac_be;
|
||
arp_.init(eth_.mac_port_, eth_.self_mac_, eth_.self_ip_);
|
||
}
|
||
|
||
bool free_rtos::EthStack::send_pkt(TEthMacPorts port_id, uint64_t mac_dst, uint16_t prot_id, TEthPkt& pkt)
|
||
{
|
||
TEthFrameHeader * p_eth_hdr = (TEthFrameHeader *)pkt.data;
|
||
p_eth_hdr->prot_id = prot_id;
|
||
memcpy(p_eth_hdr->mac_src, ð_.self_mac_, ETH_FRAME_MAC_ADDR_LEN_BYTES);
|
||
memcpy(p_eth_hdr->mac_dest, &mac_dst, ETH_FRAME_MAC_ADDR_LEN_BYTES);
|
||
|
||
pkt.length+= sizeof(TEthFrameHeader);
|
||
|
||
return tx_flow_.send(eth_.mac_port_, pkt.data, pkt.length);
|
||
}
|
||
|
||
bool free_rtos::EthStack::send_pkt(TEthMacPorts port_id, uint16_t prot_id, uint32_t numScatterSegments)
|
||
{
|
||
Handler* handler = rx_pkt_handler_.GetHandler(prot_id);
|
||
|
||
if(handler == nullptr) {
|
||
DebugP_log((char*)"Error: can't find requested protocol !");
|
||
|
||
return false;
|
||
}
|
||
|
||
return tx_flow_.send(port_id, handler, numScatterSegments);
|
||
}
|
||
|
||
void free_rtos::EthStack::rx_handler(uint8_t * p_data, uint32_t len)
|
||
{
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> eth-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> = 60 <20><><EFBFBD><EFBFBD>
|
||
if ( len < ETH_FRAME_MIN_LEN) {
|
||
return;
|
||
}
|
||
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
uint64_t dest_addr = *((uint64_t*)p_data);
|
||
dest_addr &= ETH_FRAME_MAC_ADDR_MASK; /// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 6 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>
|
||
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
if ((ETH_FRAME_MAC_ADDR_BROADCAST != dest_addr) &&
|
||
(eth_.self_mac_ != dest_addr)) {
|
||
return;
|
||
}
|
||
|
||
TEthFrameHeader * p_eth_hdr = (TEthFrameHeader *)p_data;
|
||
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
int32_t reply_len = rx_pkt_handler_.Process(p_eth_hdr->prot_id,
|
||
p_data + sizeof(TEthFrameHeader),
|
||
len - sizeof(TEthFrameHeader));
|
||
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||
if (reply_len > 0) {
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> mac-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
memcpy(p_eth_hdr->mac_dest, p_eth_hdr->mac_src, ETH_FRAME_MAC_ADDR_LEN_BYTES);
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> mac-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
memcpy(p_eth_hdr->mac_src, ð_.self_mac_, ETH_FRAME_MAC_ADDR_LEN_BYTES);
|
||
|
||
reply_len+=sizeof(TEthFrameHeader);
|
||
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
tx_flow_.send(eth_.mac_port_, p_data, reply_len);
|
||
}
|
||
}
|
||
|