sitara_depot/components/free_rtos/ethernet_ip/eth_udp_client.cpp

/*
 * eth_udp_client.cpp
 *
 *  Created on: 15 <20><><EFBFBD>. 2023 <20>.
 *      Author: sychev
 */
#include "free_rtos/ethernet_ip/eth_udp_client.hpp"
#include "free_rtos/base/swap.h"
#include "free_rtos/ethernet_ip/eth_ip_types.h"
#include "free_rtos/ethernet_ip/eth_checksum.h"
#include "free_rtos/ethernet_ip/eth_ip_prots_id.h"

#include <algorithm>
#include <cstring>


free_rtos::EthUpdClient::EthUpdClient(EthIpIface& ip_iface, TEthMacPorts port_id,
                                      uint16_t port_dst_be, uint16_t port_src_be,
                                      bool use_chksum) :
    port_dst_be_{port_dst_be},
    port_src_be_{port_src_be},
    use_checksum_{use_chksum},
    ip_iface_{ip_iface},
    port_id_{port_id}
{ }

void free_rtos::EthUpdClient::put_data(uint8_t * p_data, uint32_t len)
{
    if ((p_data == nullptr) || (len == 0)) {
        return;
    }

    UDPToggle toggle = in_toggle_.exchange(UDPToggle::UDP_BUSY);

    std::vector<uint8_t>& buff = buff_[toggle];
    uint32_t new_size = buff.size() + len;

    if(new_size > max_buf_size) {
        in_toggle_.exchange(toggle);

        EnetAppUtils_print("Warning ! UDP client buffer overflow !\r\n");
        return;
    }

    buff.reserve(new_size);
    buff.insert(buff.end(), p_data, p_data + len);

    in_toggle_.exchange(toggle);
    rx_sem_.post();
}

size_t free_rtos::EthUpdClient::get_data(uint8_t * p_data, uint32_t len)
{
    std::vector<uint8_t>& buff = buff_[out_toggle_];
    size_t size = buff.size();

    if (size > len) {
        size = len;
    }

    auto item_begin = buff.begin();
    auto item_end = item_begin + size;

    std::copy(item_begin, item_end, p_data);
    buff.erase(item_begin, item_end);

    return size;
}

bool free_rtos::EthUpdClient::capture_buffer()
{
    UDPToggle desired = out_toggle_;
    UDPToggle expected = static_cast<UDPToggle>((desired + 1)%UDPToggle::UDP_BUF_NUM);

    if(in_toggle_.compare_exchange_weak(expected, desired) == false) {
        return false;
    }

    out_toggle_ = expected;

    return true;
}

size_t free_rtos::EthUpdClient::get_data_size()
{
    size_t size = buff_[out_toggle_].size();

    if (size == 0) {
        if(capture_buffer() != false) {
            size = buff_[out_toggle_].size();
        }
    }

    return size;
}

int32_t free_rtos::EthUpdClient::read(uint8_t * p_data, uint32_t len)
{
    if ((p_data == nullptr) || (len == 0)) {
        return 0;
    }

    if(buff_[out_toggle_].size() == 0) {
        while (capture_buffer() == false) {
            rx_sem_.pend();
        }
    }

    return get_data(p_data, len);
}

int32_t free_rtos::EthUpdClient::read_async(uint8_t * p_data, uint32_t len)
{
    if ((p_data == nullptr) || (len == 0)) {
        return 0;
    }

    if (buff_[out_toggle_].size() == 0) {
        if(capture_buffer() == false) {
            return 0;
        }
    }

    return get_data(p_data, len);
}

void free_rtos::EthUpdClient::clear()
{
    buff_[out_toggle_].clear();

    while (capture_buffer() == false) {
        rx_sem_.pend();
    }

    buff_[out_toggle_].clear();
}

/*
bool free_rtos::EthUpdClient::write(uint32_t ip_dst_be, uint8_t * p_data, uint32_t len)
{
    TUdpHeader * hdr = (TUdpHeader *)(eth_pkt_.data + sizeof(TIpHeader) + sizeof(TEthFrameHeader));
    uint8_t * p_udp_data = (uint8_t *)(eth_pkt_.data + sizeof(TIpHeader) + sizeof(TEthFrameHeader) + sizeof(TUdpHeader));

    if (ip_dst_be == 0) {
        ip_dst_be = src_ip_;
    }

    eth_pkt_.length = len + sizeof(TUdpHeader);

    hdr->Src_port = port_src_be_;
    hdr->Dst_port = port_dst_be_;
    hdr->Length = BASE_SWAP16(eth_pkt_.length);
    hdr->Chk_sum = 0;

    memcpy(p_udp_data, p_data, len);

    if (use_checksum_) {
        uint32_t self_ip = ip_iface_.getSelfIpAddr();
        uint32_t chk_sum32 = 0;

        uint8_t * const dst_ip = (uint8_t *)&ip_dst_be;
        uint8_t * const src_ip = (uint8_t *)&self_ip;

        for (int j = 0; j < ETH_IP_ADDR_LEN; j += 2)
        {
            chk_sum32 += ((uint16_t)dst_ip[j] << 8) + dst_ip[j+1];
            chk_sum32 += ((uint16_t)src_ip[j] << 8) + src_ip[j+1];
        }

        chk_sum32 += IP_PROT_UDP + eth_pkt_.length;

        uint16_t chk_sum16 = eth_calcChksum(chk_sum32, (uint8_t *)hdr, eth_pkt_.length);

        hdr->Chk_sum = BASE_SWAP16(chk_sum16);
    }

    return ip_iface_.send(port_id_, ip_dst_be, IP_PROT_UDP, eth_pkt_);
}
*/

uint16_t free_rtos::EthUpdClient::fill_udp_header(UDPHandlerArgs& udpHandlerArgs) {
    uint16_t udp_len = udpHandlerArgs.ip_header_len + udpHandlerArgs.ip_data_len;

    udpHandlerArgs.udp_header.Src_port = port_src_be_;
    udpHandlerArgs.udp_header.Dst_port = port_dst_be_;
    udpHandlerArgs.udp_header.Length = BASE_SWAP16(udp_len);
    udpHandlerArgs.udp_header.Chk_sum = 0;

    if (use_checksum_) {
        uint32_t self_ip = ip_iface_.getSelfIpAddr();
        uint32_t chk_sum32 = 0;
        uint8_t * const dst_ip = (uint8_t *)&udpHandlerArgs.dest_ip;
        uint8_t * const src_ip = (uint8_t *)&self_ip;

        for (int j = 0; j < ETH_IP_ADDR_LEN; j += 2)
        {
            chk_sum32 += ((uint16_t)dst_ip[j] << 8) + dst_ip[j+1];
            chk_sum32 += ((uint16_t)src_ip[j] << 8) + src_ip[j+1];
        }

        chk_sum32 += IP_PROT_UDP + udp_len;

        uint16_t chk_sum16 = eth_calcChksum2(chk_sum32, (uint8_t *)&udpHandlerArgs.udp_header, sizeof(TUdpHeader), (uint8_t *)udpHandlerArgs.p_data, udpHandlerArgs.ip_data_len);

        udpHandlerArgs.udp_header.Chk_sum = BASE_SWAP16(chk_sum16);
    }

    return sizeof(TUdpHeader);
}

bool free_rtos::EthUpdClient::write(uint32_t ip_dst_be, uint8_t * p_data, uint32_t len)
{
    UDPHandlerArgs udpHandlerArgs;

    if (ip_dst_be == 0) {
        ip_dst_be = src_ip_;
    }

    //udpHandlerArgs.buffer = nullptr;
    udpHandlerArgs.port_id = port_id_;
    //udpHandlerArgs.stack_handler = nullptr;
    udpHandlerArgs.ip_header_len = sizeof(TUdpHeader);
    udpHandlerArgs.ip_data_len = len;
    udpHandlerArgs.dest_ip = ip_dst_be;
    udpHandlerArgs.ip_prot_id = IP_PROT_UDP;
    udpHandlerArgs.ip_handler = this;
    udpHandlerArgs.p_data = p_data;

    fill_udp_header(udpHandlerArgs);

    return ip_iface_.send(udpHandlerArgs);
}

uint16_t free_rtos::EthUpdClient::Sender(HandlerArgs& handlerArgs, size_t scatter_segment)
{
    UDPHandlerArgs& udpHandlerArgs = static_cast<UDPHandlerArgs&>(handlerArgs);  // downcasting back to derived type
    TUdpHeader * hdr = (TUdpHeader *)(udpHandlerArgs.buffer + sizeof(TEthFrameHeader) + sizeof(TIpHeader));
    uint8_t * p_udp_data = (uint8_t *)(udpHandlerArgs.buffer + sizeof(TEthFrameHeader) + sizeof(TIpHeader) + sizeof(TUdpHeader));

    *hdr = udpHandlerArgs.udp_header;
    memcpy(p_udp_data, udpHandlerArgs.p_data, udpHandlerArgs.ip_data_len);

    return udpHandlerArgs.ip_header_len + udpHandlerArgs.ip_data_len;
}

int32_t free_rtos::EthUpdClient::Process(uint8_t * p_data, uint32_t len)
{
    put_data(p_data, len);

    return 0;
}