RtosBuffer.cpp

//------------------------------------------------------------------------------
//This file is part of embKernel. 
//See license.txt for the full license governing this code.
//------------------------------------------------------------------------------
#include "RtosBuffer.hpp"
#include "RtosHeap.hpp"

RtosBuffer::RtosBuffer(int size) :
        RtosSyncObject(size, 0) {
    mBuffer = (uint8_t*) RtosHeap::alloc(size);
    if (!mBuffer) {
        mMaxCount = size;
    }
    mIdxIn = 0;
    mIdxOut = 0;
}

RtosBuffer::~RtosBuffer() {
    if (mBuffer) {
        RtosHeap::free(mBuffer);
    }
}

int RtosBuffer::write(const void* buffer, int len, Rtos::TICK tick) {
    uint8_t* src = (uint8_t*) buffer;
    int left = len;
    while (true) {
        int written = writeChunk(src, left, tick);
        if (written <= 0) {
            break;
        }
        src += written;
        left -= written;
        if (left <= 0) {
            break;
        }
        Rtos::sleep(1);
    }
    return (len - left);
}

int RtosBuffer::writeChunk(const void* buffer, int len, Rtos::TICK tick) {
    int count = internalGive(tick, len);
    if (count) {
        writeBuffer(buffer, count);
        internalWakeUpWaitingToTake();
    }
    exitCritical();
    return count;
}

int RtosBuffer::writeFromInt(const void* buffer, int len) {
    int count = mMaxCount - mCurrentCount;
    if (count < len) {
        len = count;
    }
    if (len > 0) {
        mCurrentCount += len;
        writeBuffer(buffer, len);
        internalWakeUpWaitingToTake();
    }
    return len;
}

int RtosBuffer::read(void* buffer, int len, Rtos::TICK tick) {
    uint8_t* dst = (uint8_t*) buffer;
    int left = len;
    while (true) {
        int read = readChunk(dst, left, tick);
        if (read <= 0) {
            break;
        }
        dst += read;
        left -= read;
        if (left <= 0) {
            break;
        }
        Rtos::sleep(1);
    }
    return (len - left);
}

int RtosBuffer::readChunk(void* buffer, int len, Rtos::TICK tick) {
    int count = internalTake(tick, len);
    if (count) {
        readBuffer(buffer, count);
        internalWakeUpWaitingToGive();
    }
    exitCritical();
    return count;
}

int RtosBuffer::readFromInt(void* buffer, int len) {
    if (len > mCurrentCount) {
        len = mCurrentCount;
    }
    if (len >= 0) {
        mCurrentCount -= len;
        readBuffer(buffer, len);
        internalWakeUpWaitingToGive();
    }
    return len;
}

void RtosBuffer::writeBuffer(const void* buffer, int len) {
    const uint8_t* src = (const uint8_t*) buffer;
    if (mIdxIn + len > mMaxCount) {
        int left = mMaxCount - mIdxIn;
        memcpy(&mBuffer[mIdxIn], src, left);
        src += left;
        left = len - left;
        memcpy(&mBuffer[0], src, left);
        mIdxIn = left;
    }
    else {
        memcpy(&mBuffer[mIdxIn], src, len);
        mIdxIn += len;
    }
}

void RtosBuffer::readBuffer(void* buffer, int len) {
    uint8_t* dst = (uint8_t*) buffer;
    if (mIdxOut + len > mMaxCount) {
        int left = mMaxCount - mIdxOut;
        memcpy(dst, &mBuffer[mIdxOut], left);
        dst += left;
        left = len - left;
        memcpy(dst, &mBuffer[0], left);
        mIdxOut = left;
    }
    else {
        memcpy(dst, &mBuffer[mIdxOut], len);
        mIdxOut += len;
    }
}