sitara_depot/components/free_rtos/ethernet_ip/eth_ip.cpp

/*
 * eth_ip.cpp
 *
 *  Created on: 13 мар. 2023 г.
 *      Author: sychev
 */
#include "free_rtos/ethernet_ip/eth_ip.hpp"
#include "free_rtos/ethernet_ip/eth_ip_types.h"
#include "free_rtos/ethernet_ip/eth_checksum.h"

#include "free_rtos/ethernet/eth_frame.h"
#include "free_rtos/base/swap.h"


#include <cstring>

#define IP_TIME_TO_LIVE 64
#define IP_VER 4
#define IP_HEADER_WORD_SIZE 5

static void ip_swap_ip ( uint8_t * const dest_ip, uint8_t * const source_ip )
{
    uint8_t i = 0;

    for (i = 0; i < ETH_IP_ADDR_LEN; i++)
    {
        uint8_t tmp = dest_ip[i];
        dest_ip[i] = source_ip[i];
        source_ip[i] = tmp;
    }
}

free_rtos::EthIp::EthIp(EthStackIface& eth_stack, EthArpIface& arp) :
        eth_stack_{eth_stack},
        arp_{arp}
{

}

bool free_rtos::EthIp::send(TEthMacPorts port_id, uint32_t dest_ip, uint8_t prot_id, TEthPkt& pkt)
{
    uint64_t mac_dst;


    if (!arp_.getMacAddr(dest_ip, mac_dst)) {

        return false;
    }

    uint16_t cksum = 0;
    uint16_t len = 0;

    TIpHeader * ip_header = (TIpHeader *)(pkt.data + sizeof(TEthFrameHeader));

    memcpy(ip_header->ip_src, &self_ip_, ETH_IP_ADDR_LEN);

    memcpy(ip_header->ip_dst, &dest_ip, ETH_IP_ADDR_LEN);

    ip_header->ip_hl_v = (IP_VER << 4) | IP_HEADER_WORD_SIZE;

    ip_header->ip_tos = 0;

    ip_header->ip_id = BASE_SWAP16(ip_indet_);

    ++ip_indet_;

    ip_header->ip_off = 0;

    ip_header->ip_ttl = IP_TIME_TO_LIVE;

    ip_header->ip_p = prot_id;

    len = (ip_header->ip_hl_v & 0x0F) * 4;

    pkt.length += len;

    ip_header->ip_len = BASE_SWAP16(pkt.length);

    ip_header->ip_sum = 0;

    cksum = eth_calcChksum(0,(uint8_t *)ip_header, len);

    ip_header->ip_sum = BASE_SWAP16(cksum);


    return eth_stack_.send_pkt(port_id, mac_dst, ETH_PROT_IP_BE, pkt);
}

bool free_rtos::EthIp::send(IpHandlerArgs& handlerArgs)
{
    handlerArgs.stack_handler = this;

    return eth_stack_.send_pkt(handlerArgs);
}

inline uint16_t free_rtos::EthIp::fillIpHeader(IpHandlerArgs& ipHandlerArgs)
{
    static_assert(sizeof(TIpHeader) == (IP_HEADER_WORD_SIZE * 4), "Ip header size doesn't match version !");

    TIpHeader * ip_hdr = (TIpHeader *)(ipHandlerArgs.buffer + sizeof(TEthFrameHeader));
    uint16_t ip_len = sizeof(TIpHeader) + ipHandlerArgs.ip_header_len + ipHandlerArgs.ip_data_len;
    TIpHeader ip_header;
    uint16_t cksum = 0;

    memcpy(ip_header.ip_src, &self_ip_, ETH_IP_ADDR_LEN);
    memcpy(ip_header.ip_dst, &ipHandlerArgs.dest_ip, ETH_IP_ADDR_LEN);

    ip_header.ip_hl_v = (IP_VER << 4) | IP_HEADER_WORD_SIZE;
    ip_header.ip_tos = 0;
    ip_header.ip_len = BASE_SWAP16(ip_len);
    ip_header.ip_id = BASE_SWAP16(ip_indet_);
    ip_header.ip_off = 0;
    ip_header.ip_ttl = IP_TIME_TO_LIVE;
    ip_header.ip_p = ipHandlerArgs.ip_prot_id;
    ip_header.ip_sum = 0;

    //cksum = eth_calcChksum(0, (uint8_t *)&ipHandlerArgs.ip_header, sizeof(TIpHeader));
    cksum = eth_calcChksum3(ip_hdr, 0,
                            &ip_header, sizeof(TIpHeader),
                            nullptr, 0);

    ip_hdr->ip_sum = BASE_SWAP16(cksum);

    ++ip_indet_;

    return sizeof(TIpHeader);
}

inline uint16_t free_rtos::EthIp::fillEthFrameHeader(IpHandlerArgs& ipHandlerArgs)
{
    TEthFrameHeader * p_eth_hdr = (TEthFrameHeader *)ipHandlerArgs.buffer;

    p_eth_hdr->prot_id = ETH_PROT_IP_BE;

    memcpy(p_eth_hdr->mac_src, &self_mac_, ETH_FRAME_MAC_ADDR_LEN_BYTES);
    memcpy(p_eth_hdr->mac_dest, &ipHandlerArgs.dest_mac, ETH_FRAME_MAC_ADDR_LEN_BYTES);

    return sizeof(TEthFrameHeader);
}

uint16_t free_rtos::EthIp::Sender(HandlerArgs& handlerArgs, size_t scatter_segment)
{
    IpHandlerArgs& ipHandlerArgs = static_cast<IpHandlerArgs&>(handlerArgs); // downcasting back to derived type

    fillEthFrameHeader(ipHandlerArgs);
    fillIpHeader(ipHandlerArgs);

    ipHandlerArgs.ip_handler->Sender(ipHandlerArgs, scatter_segment);

    return sizeof(TEthFrameHeader) + sizeof(TIpHeader) + ipHandlerArgs.ip_header_len + ipHandlerArgs.ip_data_len;
}

int32_t free_rtos::EthIp::Process(uint8_t * p_data, uint32_t len)
{
    TIpHeader * ip_hdr = (TIpHeader *)p_data;
    uint32_t ip_addr;

    memcpy(&ip_addr, ip_hdr->ip_dst, ETH_IP_ADDR_LEN);

    if (ip_addr != self_ip_) {
        return 0;
    }

    int32_t reply = handlers_.Process(ip_hdr->ip_p, p_data + sizeof(TIpHeader), len - sizeof(TIpHeader));

    if (reply > 0) {
        uint16_t cksum;
        uint16_t len;
        /* Swap the IP destination  address and the IP source address */
        ip_swap_ip(ip_hdr->ip_dst, ip_hdr->ip_src);
        reply += sizeof(TIpHeader);
        ip_hdr->ip_len = BASE_SWAP16(reply);
        ip_hdr->ip_id  = BASE_SWAP16(ip_indet_);
        //-------------------------------------------------------
        ip_hdr->ip_sum = 0;
        len = (ip_hdr->ip_hl_v & 0x0F) * 4;
        cksum = eth_calcChksum(0,(uint8_t *)ip_hdr, len);
        ip_hdr->ip_sum = BASE_SWAP16(cksum);
        //-------------------------------------------------------
        ++ip_indet_;
    }

    return reply;
}

bool free_rtos::EthIp::Register(uint8_t prot_id, Handler * p_handler)
{
    return handlers_.Register(prot_id, p_handler);
}