sitara_depot/components/free_rtos/ethernet_industry/eth_ecat_telegram.cpp

/*
 * eth_ecat_telegram.cpp
 *
 *  Created on: Jun 5, 2023
 *      Author: algin
 */

#include "free_rtos/ethernet_industry/eth_ecat_telegram.hpp"

namespace free_rtos {

namespace telegram {

const char EcatTelegramStatus::SuccessString[]              = "Success !";
const char EcatTelegramStatus::BusyString[]                 = "Busy. Transfer in progress...";
const char EcatTelegramStatus::WarningTransferErrorString[] = "Warning ! Transfer error";
const char EcatTelegramStatus::WarningTimeoutErrorString[]  = "Warning ! Connection timeout error";
const char EcatTelegramStatus::WarningWkcErrorString[]      = "Warning ! WKC error";
const char EcatTelegramStatus::FatalErrorString[]           = "Fatal error ! Transfer attempts exceeded";

const EcatDescription EcatTelegramStatus::descriptions[] = {
                                         {EcatTelegramStatus::SuccessString, sizeof(EcatTelegramStatus::SuccessString)},
                                         {EcatTelegramStatus::BusyString, sizeof(EcatTelegramStatus::BusyString)},
                                         {EcatTelegramStatus::WarningTransferErrorString, sizeof(EcatTelegramStatus::WarningTransferErrorString)},
                                         {EcatTelegramStatus::WarningTimeoutErrorString, sizeof(EcatTelegramStatus::WarningTimeoutErrorString)},
                                         {EcatTelegramStatus::WarningWkcErrorString, sizeof(EcatTelegramStatus::WarningWkcErrorString)},
                                         {EcatTelegramStatus::FatalErrorString, sizeof(EcatTelegramStatus::FatalErrorString)}
};

int32_t EcatTelegram::Process(uint8_t *p_data, uint32_t len) {
    // TODO: Don't forget to subtract sizeof(TEthFrameHeader) !
    uint8_t *p_datagram_last = unpack(p_data - sizeof(TEthFrameHeader));

    if(p_datagram_last == nullptr) {
        //DebugP_log((char*)"Warning ! Repeated packet skipped\r\n");
        return 0;
    }

/*
    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;
}

uint16_t EcatTelegram::Sender(HandlerArgs& handlerArgs, size_t scatter_segment) {
    uint8_t *raw = pack(handlerArgs.buffer);

    //DebugP_log((char*)"Sender started\r\n");

    return raw - handlerArgs.buffer;
}

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;
    auto first = datagram_queue_.get_first();

    (void)p_eth_hdr;
    (void)p_hdr;

    if(first != nullptr) {
        p_datagram_last = first->pack_all(p_datagram_first, idx_);
    }

    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;
    auto first = datagram_queue_.get_first();

    if(first != nullptr) {
        p_datagram_last = first->unpack_all(p_datagram_first, idx_);
    }

    return p_datagram_last;
}

bool EcatTelegram::transfer() {
    MAKE_AUTO_PROFILER(transfer, 32);

    auto first = datagram_queue_.get_first();
    TxFlowHandlerArgs txFlowHandlerArgs;
    uint32_t transfer_attempts = max_transfer_attempts_ + 1;
    uint32_t tick_counter_start;
    uint32_t tick_counter_diff;
    bool status = (first == nullptr);
    int32_t sts;

    if(status == true) {
        status_.result = EcatTelegramResult::SUCCESS;

        return status;
    }

    status_.result = EcatTelegramResult::BUSY;

    first->reset_all_wkc();

    txFlowHandlerArgs.port_id = port_id_;
    txFlowHandlerArgs.stack_handler = this;
    txFlowHandlerArgs.buffer = nullptr;

    while(1) {
        tick_counter_start = ecat_timer_.Wait();

        transfer_attempts--;
        status = (transfer_attempts != 0);

        if(status == false) {
            DebugP_log((char*)"%s\r\n", status_.get_description().string);

            status_.attempts_exceeded_errors++;
            status_.result = EcatTelegramResult::FATAL_ERROR;

            DebugP_log((char*)"%s: %d\r\n", status_.get_description().string, max_transfer_attempts_);
            DebugP_log((char*)"%s: %d\r\n", status_.get_description(EcatTelegramResult::WARNING_TRANSFER_ERROR).string, status_.transfer_errors);
            DebugP_log((char*)"%s: %d\r\n", status_.get_description(EcatTelegramResult::WARNING_WKC_ERROR).string, status_.expected_wkc_errors);
            DebugP_log((char*)"Tick counter diff: %d/%d\r\n", tick_counter_diff, counter_timeout_ticks_);
            DebugP_log((char*)"Last profiler item %lld cycles\r\n", GET_LAST_PROFILER_ITEM(transfer));
            FORCE_DUMP_PROFILER(transfer);

            break;
        }

        status = tx_flow_.send(txFlowHandlerArgs, 1);

        if(status == false) {
            status_.transfer_errors++;
            status_.result = EcatTelegramResult::WARNING_TRANSFER_ERROR;

            //DebugP_log((char*)"%s: %d\r\n", status_.get_description().string, status_.transfer_errors);

            continue;
        }

        {
            MAKE_PROFILER_AUTO_ITEM(transfer, 32, 0);

            do {
                sts = rx_sem_.pend(semaphore_timeout_ticks_);

                tick_counter_diff = diff(tick_counter_start, ecat_timer_.GetTickCounter());

                if((sts != SystemP_SUCCESS) && ( tick_counter_diff < (counter_timeout_ticks_ - semaphore_timeout_tolerance_) )) {
                    //DebugP_log((char*)"rx_sem_ fake timeout detected !\r\n", );

                    continue;
                }

                break;
            } while(1);
        }

        if(sts != SystemP_SUCCESS) {
            status_.transfer_errors++;
            status_.result = EcatTelegramResult::WARNING_TIMEOUT_ERROR;

            //DebugP_log((char*)"%s: %d\r\n", status_.get_description().string, status_.transfer_errors);
            //DebugP_log((char*)"Last profiler item: %lld\r\n", GET_LAST_PROFILER_ITEM(Test_static));
            //FORCE_DUMP_PROFILER(Test_static);

            continue;
        }

        status = first->all_as_expected();

        if(status == false) {
            status_.expected_wkc_errors++;
            status_.result = EcatTelegramResult::WARNING_WKC_ERROR;

            //DebugP_log((char*)"%s: %d\r\n", status_.get_description().string, status_.expected_wkc_errors);

            continue;
        }

        status_.result = EcatTelegramResult::SUCCESS;

        break;
    }

    datagram_queue_.clear();
    ++idx_;

    return status;
}

bool EcatTelegram::transfer(datagram::IEcatDatagram& next) {
    datagram_queue_ + next;

    return transfer();
}

bool EcatTelegram::transfer(queue::Queue<datagram::IEcatDatagram>& next) {
    datagram_queue_ + next;

    return transfer();
}

}

}