fsfw/src/fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.cpp

#include "fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.h"

#include "fsfw/serviceinterface/ServiceInterface.h"

#include <cstring>

TcPacketStoredPus::TcPacketStoredPus(uint16_t apid,  uint8_t service,
        uint8_t subservice, uint8_t sequenceCount, const uint8_t* data,
        size_t size, uint8_t ack) :
        TcPacketPus(nullptr) {
    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
    if (not this->checkAndSetStore()) {
        return;
    }
    uint8_t* pData = nullptr;
    ReturnValue_t returnValue = this->store->getFreeElement(&this->storeAddress,
            (TC_PACKET_MIN_SIZE + size), &pData);
    if (returnValue != this->store->RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "TcPacketStoredBase: Could not get free element from store!"
                << std::endl;
#endif
        return;
    }
    this->setData(pData, TC_PACKET_MIN_SIZE + size);
#if FSFW_USE_PUS_C_TELECOMMANDS == 1
    pus::PusVersion pusVersion = pus::PusVersion::PUS_C_VERSION;
#else
    pus::PusVersion pusVersion = pus::PusVersion::PUS_A_VERSION;
#endif
    initializeTcPacket(apid, sequenceCount, ack, service, subservice, pusVersion);
    std::memcpy(&tcData->appData, data, size);
    this->setPacketDataLength(
            size + sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
    this->setErrorControl();
}

TcPacketStoredPus::TcPacketStoredPus(): TcPacketStoredBase(), TcPacketPus(nullptr) {
}

TcPacketStoredPus::TcPacketStoredPus(store_address_t setAddress): TcPacketPus(nullptr) {
    TcPacketStoredBase::setStoreAddress(setAddress, this);
}

TcPacketStoredPus::TcPacketStoredPus(const uint8_t* data, size_t size): TcPacketPus(data) {
    if (this->getFullSize() != size) {
        return;
    }
    if (this->checkAndSetStore()) {
        ReturnValue_t status = store->addData(&storeAddress, data, size);
        if (status != HasReturnvaluesIF::RETURN_OK) {
            this->setData(nullptr, size);
        }
        const uint8_t* storePtr = nullptr;
        // Repoint base data pointer to the data in the store.
        store->getData(storeAddress, &storePtr, &size);
        this->setData(const_cast<uint8_t*>(storePtr), size);
    }
}

ReturnValue_t TcPacketStoredPus::deletePacket() {
    ReturnValue_t result = this->store->deleteData(this->storeAddress);
    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
    // To circumvent size checks
    this->setData(nullptr, -1);
    return result;
}

TcPacketPusBase* TcPacketStoredPus::getPacketBase() {
    return this;
}


bool TcPacketStoredPus::isSizeCorrect() {
    const uint8_t* temp_data = nullptr;
    size_t temp_size;
    ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data,
            &temp_size);
    if (status == StorageManagerIF::RETURN_OK) {
        if (this->getFullSize() == temp_size) {
            return true;
        }
    }
    return false;
}