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_udp_types.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(uint32_t src_ip, 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);

        DebugP_log((char *)"Warning ! UDP client buffer overflow !\r\n");
        return;
    }

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

    src_ip_ = src_ip;

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

int32_t free_rtos::EthUpdClient::get_data(uint8_t * p_data, uint32_t len)
{
    std::vector<uint8_t>& buff = buff_[out_toggle_];
    uint32_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 true;
    }

    out_toggle_ = expected;

    return buff_[expected].empty();
}

size_t free_rtos::EthUpdClient::get_data_size()
{
    if (buff_[out_toggle_].empty() == true) {
        if(capture_buffer() == true) {
            return 0;
        }
    }

    return buff_[out_toggle_].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_].empty() == true) {
        while (capture_buffer() == true) {
            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_].empty() == true) {
        if(capture_buffer() == true) {
            return 0;
        }
    }

    return get_data(p_data, len);
}

void free_rtos::EthUpdClient::clear()
{
    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_);
}