#include "cobs.h"

#include "tcobs.h"

#include "trice.h"

#if TRICE_BUFFER == TRICE_DOUBLE_BUFFER && TRICE_OFF == 0

uint32_t* triceSingleBufferStartWritePosition = (uint32_t*)0;

static void TriceOut(uint32_t* tb, size_t tLen);

static uint32_t triceBuffer[2][TRICE_DEFERRED_BUFFER_SIZE >> 3] = {0};

#define TRICE_WRITE_START_HALF_BUFFER_0 (&triceBuffer[0][TRICE_DATA_OFFSET >> 2])

#define TRICE_WRITE_LIMIT_HALF_BUFFER_0 (&triceBuffer[0][TRICE_DEFERRED_BUFFER_SIZE >> 3])

#define TRICE_WRITE_START_HALF_BUFFER_1 (&triceBuffer[1][TRICE_DATA_OFFSET >> 2])

#define TRICE_WRITE_LIMIT_HALF_BUFFER_1 (&triceBuffer[1][TRICE_DEFERRED_BUFFER_SIZE >> 3])

static uint32_t* const triceWriteStartHalfBuffer0 = TRICE_WRITE_START_HALF_BUFFER_0;

static uint32_t* const triceWriteStartHalfBuffer1 = TRICE_WRITE_START_HALF_BUFFER_1;

#if TRICE_PROTECT == 1

static uint32_t* const triceWriteLimitHalfBuffer0 = TRICE_WRITE_LIMIT_HALF_BUFFER_0;

static uint32_t* const triceWriteLimitHalfBuffer1 = TRICE_WRITE_LIMIT_HALF_BUFFER_1;

static int triceActiveHalfBuffer = 0;

uint32_t* TriceBufferWritePosition = TRICE_WRITE_START_HALF_BUFFER_0; // triceWriteStartHalfBuffer0;

uint32_t* TriceBufferWritePositionStart = TRICE_WRITE_START_HALF_BUFFER_0; // triceWriteStartHalfBuffer0;

#if TRICE_PROTECT == 1

static uint32_t* TriceBufferWritePositionLimit = TRICE_WRITE_LIMIT_HALF_BUFFER_0; // triceWriteLimitHalfBuffer0;

int TriceEnoughSpace(void) {

// space32 is the writable 32-bit value count in the current write buffer.

int const space32 = (int)(TriceBufferWritePositionLimit - TriceBufferWritePosition);

// there need to be at least TRICE_SINGLE_MAX_SIZE bytes space in the current write buffer.

if (space32 >= (TRICE_SINGLE_MAX_SIZE >> 2)) {

return 1;

}

#if TRICE_DIAGNOSTICS == 1

TriceDeferredOverflowCount++;

return 0;

}

#endif // #if TRICE_PROTECT == 1

static uint32_t* triceBufferSwap(void) {

if (triceActiveHalfBuffer == 0) {

triceActiveHalfBuffer = 1;

TriceBufferWritePositionStart = triceWriteStartHalfBuffer1;

TriceBufferWritePosition = TriceBufferWritePositionStart;

#if TRICE_PROTECT == 1

TriceBufferWritePositionLimit = triceWriteLimitHalfBuffer1;

return triceWriteStartHalfBuffer0;

}

triceActiveHalfBuffer = 0;

TriceBufferWritePositionStart = triceWriteStartHalfBuffer0;

TriceBufferWritePosition = TriceBufferWritePositionStart;

#if TRICE_PROTECT == 1

TriceBufferWritePositionLimit = triceWriteLimitHalfBuffer0;

return triceWriteStartHalfBuffer1;

}

#if TRICE_DIAGNOSTICS == 1

unsigned TriceHalfBufferDepthMax = 0;

void TriceTransfer(void) {

if (0 == TriceOutDepth()) { // transmission done for slowest output channel, so a swap is possible.

uint32_t* readBuf;

size_t tLen32;

TRICE_ENTER_CRITICAL_SECTION

tLen32 = TriceBufferWritePosition - TriceBufferWritePositionStart;

if (tLen32) { // Some Trice data are available.

readBuf = triceBufferSwap();

readBuf -= (TRICE_DATA_OFFSET >> 2);

}

TRICE_LEAVE_CRITICAL_SECTION

if (tLen32) {

TriceOut(readBuf, tLen32 << 2);

}

}

}

static int TriceNext(uint8_t** buf, size_t* pSize, const uint8_t** pStart, size_t* pLen) {

uint16_t const * pTID = (uint16_t*)*buf; // lint !e826, get TID address

unsigned const TID = TRICE_TTOHS(*pTID); // type and id

int const triceID = (int)(0x3FFF & TID);

int const triceType = (int)(TID >> 14);

unsigned offset;

size_t size = *pSize;

size_t len;

switch (triceType) {

case TRICE_TYPE_S0: // S0 = no stamp

*pStart = *buf;

offset = 0;

len = 4 + triceDataLen(*pStart + 2); // tyId

break;

case TRICE_TYPE_S2: // S2 = 16-bit stamp

*pStart = *buf; // see Id(n) macro definition

*pStart += 2; // see Id(n) macro definition

offset = 2;

len = 6 + triceDataLen(*pStart + 4); // tyId ts16

break;

case TRICE_TYPE_S4: // S4 = 32-bit stamp

*pStart = *buf;

offset = 0;

len = 8 + triceDataLen(*pStart + 6); // tyId ts32

break;

default: // impossible

// fallthrugh

case TRICE_TYPE_X0:

TriceErrorCount++;

*pStart = 0;

*pLen = 0;

return -__LINE__; // extended Trices not supported (yet)

}

size_t const triceSize = (len + offset + 3) & ~3;

// S16 case example: triceSize len t-0-3 t-o

// 80id 80id 1616 00cc 8 6 3 6

// 80id 80id 1616 01cc dd 12 7 7 10

// 80id 80id 1616 02cc dd dd 12 8 7 10

// 80id 80id 1616 03cc dd dd dd 12 9 7 10

// 80id 80id 1616 04cc dd dd dd dd 12 10 7 10

if (!(triceSize - (offset + 3) <= len && len <= triceSize - offset)) { // corrupt data

TriceErrorCount++;

return -__LINE__;

}

size -= triceSize;

*buf += triceSize;

*pSize = size;

*pLen = len;

return triceID;

}

uint8_t* firstNotModifiedAddress;

int distance;

static void TriceOut(uint32_t* tb, size_t tLen) {

uint8_t* enc = (uint8_t*)tb; // This is the later encoded data starting address.

uint8_t* dat = enc + TRICE_DATA_OFFSET; // This is the start of 32-bit aligned Trices.

uint8_t* nxt = dat; // This is the start of next 32-bit aligned Trices.

size_t encLen = 0;

#if (TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE) && ((TRICE_PROTECT == 1) || (TRICE_DIAGNOSTICS == 1))

uint8_t const * dst; // This value dst must not get > nxt to avoid overwrites.

int triceID = 0; // This assignment is only needed to silence compiler complains about being uninitialized.

#if TRICE_DIAGNOSTICS == 1

int triceDataOffsetDepth;

unsigned const depth = tLen + TRICE_DATA_OFFSET;

TriceHalfBufferDepthMax = depth < TriceHalfBufferDepthMax ? TriceHalfBufferDepthMax : depth;

while (tLen) { // loop begin

#if TRICE_DIAGNOSTICS == 1

firstNotModifiedAddress = enc + encLen;

distance = (int)(nxt - firstNotModifiedAddress);

triceDataOffsetDepth = TRICE_DATA_OFFSET - distance; // distance can get > TRICE_DATA_OFFSET, but that is no problem.

TriceDataOffsetDepthMax = triceDataOffsetDepth < TriceDataOffsetDepthMax ? TriceDataOffsetDepthMax : triceDataOffsetDepth;

#endif // #if TRICE_DIAGNOSTICS == 1

const uint8_t* triceNettoStart;

size_t triceNettoLen; // This is the trice netto length (without padding bytes).

#if (TRICE_DEFERRED_XTEA_ENCRYPT == 1) && (TRICE_DEFERRED_OUT_FRAMING != TRICE_FRAMING_NONE) && (TRICE_DEFERRED_TRANSFER_MODE == TRICE_SINGLE_PACK_MODE)

uint8_t* crypt = nxt - 4; // Only 8-byte groups are encryptable. The Trices are 32-bit aligned and have 0-3 padding zero bytes.

// We need 4 bytes behind them and therefore we move them into the TRICE_DATA_OFFSET space to not allocate a buffer.

// nxt is here the start of the current (unprocessed) Trice buffer.

triceID = TriceNext(&nxt, &tLen, &triceNettoStart, &triceNettoLen);

// Now triceNettoStart, &triceNettoLen are the net data of the

if (triceID <= 0) { // on data error

TriceErrorCount++;

break; // ignore following data

}

#if TRICE_DEFERRED_TRANSFER_MODE == TRICE_SINGLE_PACK_MODE

uint8_t* dst = enc + encLen;

#if (TRICE_DEFERRED_XTEA_ENCRYPT == 1) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_TCOBS)

//! When XTEA enabled, (XTEA can encrypt only multiple of 8 length packages.) we need to copy th data first, because the 4 bytes

//! behind a trice messages could get changed, when the trice length is not a multiple of 8, but only of 4.

memmove(crypt, triceNettoStart, triceNettoLen);

size_t len8 = (triceNettoLen + 7) & ~7; // Only multiple of 8 encryptable, so we adjust len.

memset(crypt + triceNettoLen, 0, len8 - triceNettoLen); // Clear padding space.

XTEAEncrypt((uint32_t*)crypt, len8 >> 2);

size_t len = (size_t)TCOBSEncode(dst, crypt, len8); // encLen is re-used here

encLen += len;

dst[encLen++] = 0; // Add zero as package delimiter.

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 1) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_COBS)

//! When XTEA enabled, (XTEA can encrypt only multiple of 8 length packages.) we need to copy th data first, because the 4 bytes

//! behind a trice messages could get changed, when the trice length is not a multiple of 8, but only of 4.

memmove(crypt, triceNettoStart, triceNettoLen);

size_t len8 = (triceNettoLen + 7) & ~7; // Only multiple of 8 encryptable, so we adjust len.

memset((crypt) + triceNettoLen, 0, len8 - triceNettoLen); // Clear padding space.

XTEAEncrypt((uint32_t*)crypt, len8 >> 2);

size_t len = (size_t)COBSEncode(dst, crypt, len8); // encLen is re-used here

encLen += len;

dst[encLen++] = 0; // Add zero as package delimiter.

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 1) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_NONE)

#if TRICE_CONFIG_WARNINGS == 1

#error configuration: The Trice tool does not support encryted data without COBS or TCOBS framing.

// memmove(dst, triceNettoStart, triceNettoLen );

// size_t len8 = (triceNettoLen + 7) & ~7; // Only multiple of 8 encryptable, so we adjust len.

// memset((enc)+triceNettoLen, 0, len8 - triceNettoLen); // Clear padding space.

// XTEAEncrypt( (uint32_t *)enc, len8>>2 );

// encLen += len8; // encLen is re-used here

// nextPosForEncoding = dst+encLen;

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 0) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_TCOBS)

size_t len = (size_t)TCOBSEncode(dst, triceNettoStart, triceNettoLen);

dst[len++] = 0; // Add zero as package delimiter.

encLen += len;

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 0) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_COBS)

size_t len = (size_t)COBSEncode(dst, triceNettoStart, triceNettoLen);

dst[len++] = 0; // Add zero as package delimiter.

encLen += len;

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 0) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_NONE)

memmove(dst, triceNettoStart, triceNettoLen);

size_t len = triceNettoLen;

encLen += triceNettoLen;

#error configuration: unexpected

//

// TRICE_SINGLE_PACK_MODE

//////////////////////////////////////////////////////////////////////////////

#elif TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE

// pack data

uint8_t* packed = dat + encLen; // After the loop, the packed data start at dat.

memmove(packed, triceNettoStart, triceNettoLen); // This action removes all padding bytes of the Trices, compacting their sequence this way

encLen += triceNettoLen;

#endif // #elif TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE

#if TRICE_DEFERRED_TRANSFER_MODE == TRICE_SINGLE_PACK_MODE

TRICE_ENTER_CRITICAL_SECTION

TriceNonBlockingDeferredWrite8(triceID, dst, len); // Write each message separately to allow ID routing. See https://github.com/rokath/trice/issues/543.

TRICE_LEAVE_CRITICAL_SECTION

#endif // TRICE_DEFERRED_TRANSFER_MODE == TRICE_SINGLE_PACK_MODE

//

// TRICE_SINGLE_PACK_MODE

//////////////////////////////////////////////////////////////////////////////

#if (TRICE_PROTECT == 1) || (TRICE_DIAGNOSTICS == 1)

dst = enc + encLen; // When several Trices in the double buffer, with each encoding the new dst could drift a bit closer towards triceNettoStart.

int const triceDataOffsetSpaceRemained = (int)(nxt - dst); // The beginning of unprocessed data MINUS next dst must not be negative.

#if (TRICE_PROTECT == 1)

if (triceDataOffsetSpaceRemained < 0) {

TriceErrorCount++;

return; // discard broken data to avoid buffer overflow

}

#if TRICE_DIAGNOSTICS == 1

/*int*/ triceDataOffsetDepth = TRICE_DATA_OFFSET - triceDataOffsetSpaceRemained;

TriceDataOffsetDepthMax = triceDataOffsetDepth < TriceDataOffsetDepthMax ? TriceDataOffsetDepthMax : triceDataOffsetDepth;

} // loop end

#if TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE

// At this point the compacted trice messages start TRICE_DATA_OFFSET bytes after tb (now dat) and the encLen is their total net length.

// Behind this up to 7 bytes can be used as scratch pad when XTEA is active. That is ok, because the half buffer should not get totally filled.

// encLen = TriceEncode( TRICE_DEFERRED_XTEA_ENCRYPT, TRICE_DEFERRED_OUT_FRAMING, enc, dat, encLen );

#if (TRICE_DEFERRED_XTEA_ENCRYPT == 1) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_TCOBS) // && (TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE)

// special case: The data are at dat and can be big, are compacted and behind them is space. So we can encrypt them in space

size_t len8 = (encLen + 7) & ~7; // Only multiple of 8 encryptable, so we adjust len.

memset(dat + encLen, 0, len8 - encLen); // clear padding space: ATTENTION! OK only for this compiler switch setting.

XTEAEncrypt((uint32_t*)dat, len8 >> 2);

size_t eLen = (size_t)TCOBSEncode(enc, dat, len8); // encLen is re-used here

enc[eLen++] = 0; // Add zero as package delimiter.

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 1) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_COBS) // && (TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE)

// special case: The data are at dat and can be big, are compacted and behind them is space. So we can encrypt them in space

size_t len8 = (encLen + 7) & ~7; // Only multiple of 8 encryptable, so we adjust len.

memset(dat + encLen, 0, len8 - encLen); // clear padding space: ATTENTION! OK only for this compiler switch setting.

XTEAEncrypt((uint32_t*)dat, len8 >> 2);

size_t eLen = (size_t)COBSEncode(enc, dat, len8); // encLen is re-used here

enc[eLen++] = 0; // Add zero as package delimiter.

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 1) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_NONE) // && (TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE)

size_t eLen = TriceEncode(TRICE_DEFERRED_XTEA_ENCRYPT, TRICE_DEFERRED_OUT_FRAMING, enc, dat, encLen);

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 0) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_TCOBS) // && (TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE)

size_t eLen = TCOBSEncode(enc, dat, encLen);

enc[eLen++] = 0; // Add zero as package delimiter.

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 0) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_COBS) // && (TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE)

size_t eLen = /* (size_t) */ COBSEncode(enc, dat, encLen);

enc[eLen++] = 0; // Add zero as package delimiter.

#elif (TRICE_DEFERRED_XTEA_ENCRYPT == 0) && (TRICE_DEFERRED_OUT_FRAMING == TRICE_FRAMING_NONE) // && (TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE)

enc = dat;

size_t eLen = encLen;

#error configuration:

#if TRICE_DIAGNOSTICS

// before: space = enc[TRICE_DATA_OFFSET], data = dat[encLen]

// after: date = enc[eLen], (dat [encLen])

// Mostly eLen < encLen, but it could be eLen = encLen + 1 + (encLen>>5) in TCOBS worst case.

// dat - enc = TRICE_DATA_OFFSET

// if eLen > encLen, then TriceDataOffsetDepth = eLen - encLen

/*int*/ triceDataOffsetDepth =(int)(eLen - encLen); // usually negative

TriceDataOffsetDepthMax = triceDataOffsetDepth < TriceDataOffsetDepthMax ? TriceDataOffsetDepthMax : triceDataOffsetDepth;

encLen = eLen;

// Reaching here means all trice data in the current half buffer are encoded

// into a single continuous buffer having 0-delimiters between them or not but at the ent is a 0-delimiter.

//

// output

TRICE_ENTER_CRITICAL_SECTION

TriceNonBlockingDeferredWrite8(triceID, enc, encLen); // lint !e771 Info 771: Symbol 'triceID' conceivably not initialized. Comment: tLen is always > 0.

TRICE_LEAVE_CRITICAL_SECTION

#endif // TRICE_DEFERRED_TRANSFER_MODE == TRICE_MULTI_PACK_MODE

//

// TRICE_MULTI_PACK_MODE

//////////////////////////////////////////////////////////////////////////////

}

#endif // #if TRICE_BUFFER == TRICE_DOUBLE_BUFFER && TRICE_OFF == 0