1494 lines
46 KiB
C
1494 lines
46 KiB
C
/*
|
|
* FIFO circular buffer
|
|
*
|
|
* @file CO_fifo.c
|
|
* @ingroup CO_CANopen_309_fifo
|
|
* @author Janez Paternoster
|
|
* @copyright 2020 Janez Paternoster
|
|
*
|
|
* This file is part of <https://github.com/CANopenNode/CANopenNode>, a CANopen Stack.
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License"); you may not use this
|
|
* file except in compliance with the License. You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software distributed under the License is
|
|
* distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and limitations under the License.
|
|
*/
|
|
|
|
#include <string.h>
|
|
|
|
#include "301/CO_fifo.h"
|
|
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ENABLE) != 0
|
|
|
|
#include <ctype.h>
|
|
#include <stdlib.h>
|
|
#include "crc16-ccitt.h"
|
|
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ASCII_COMMANDS) != 0
|
|
#include <stdio.h>
|
|
#include <inttypes.h>
|
|
|
|
/* Non-graphical character for command delimiter */
|
|
#define DELIM_COMMAND ((uint8_t)'\n')
|
|
/* Graphical character for comment delimiter */
|
|
#define DELIM_COMMENT ((uint8_t)'#')
|
|
/* Graphical character for double quotes */
|
|
#define DELIM_DQUOTE ((uint8_t)'"')
|
|
#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS */
|
|
|
|
/* verify configuration */
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_CRC16_CCITT) != 0
|
|
#if ((CO_CONFIG_CRC16)&CO_CONFIG_CRC16_ENABLE) == 0
|
|
#error CO_CONFIG_CRC16_ENABLE must be enabled.
|
|
#endif
|
|
#endif
|
|
|
|
void
|
|
CO_fifo_init(CO_fifo_t* fifo, uint8_t* buf, size_t bufSize) {
|
|
|
|
if ((fifo == NULL) || (buf == NULL) || (bufSize < 2U)) {
|
|
return;
|
|
}
|
|
|
|
fifo->readPtr = 0;
|
|
fifo->writePtr = 0;
|
|
fifo->buf = buf;
|
|
fifo->bufSize = bufSize;
|
|
|
|
return;
|
|
}
|
|
|
|
/* Circular FIFO buffer example for fifo->bufSize = 7 (usable size = 6): ******
|
|
* *
|
|
* 0 * * * * *
|
|
* 1 rp==wp readPtr writePtr * *
|
|
* 2 * * * * *
|
|
* 3 * * * writePtr *
|
|
* 4 * writePtr readPtr readPtr *
|
|
* 5 * * * * *
|
|
* 6 * * * * *
|
|
* *
|
|
* empty 3 bytes 4 bytes buffer *
|
|
* buffer in buff in buff full *
|
|
******************************************************************************/
|
|
size_t
|
|
CO_fifo_write(CO_fifo_t* fifo, const uint8_t* buf, size_t count, uint16_t* crc) {
|
|
size_t i;
|
|
uint8_t* bufDest;
|
|
|
|
if ((fifo == NULL) || (fifo->buf == NULL) || (buf == NULL)) {
|
|
return 0;
|
|
}
|
|
|
|
bufDest = &fifo->buf[fifo->writePtr];
|
|
for (i = count; i > 0U; i--) {
|
|
size_t writePtrNext = fifo->writePtr + 1U;
|
|
|
|
/* is circular buffer full */
|
|
if ((writePtrNext == fifo->readPtr) || ((writePtrNext == fifo->bufSize) && (fifo->readPtr == 0U))) {
|
|
break;
|
|
}
|
|
|
|
*bufDest = *buf;
|
|
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_CRC16_CCITT) != 0
|
|
if (crc != NULL) {
|
|
crc16_ccitt_single(crc, *buf);
|
|
}
|
|
#endif
|
|
|
|
/* increment variables */
|
|
if (writePtrNext == fifo->bufSize) {
|
|
fifo->writePtr = 0;
|
|
bufDest = &fifo->buf[0];
|
|
} else {
|
|
fifo->writePtr++;
|
|
bufDest++;
|
|
}
|
|
buf++;
|
|
}
|
|
|
|
return count - i;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_read(CO_fifo_t* fifo, uint8_t* buf, size_t count, bool_t* eof) {
|
|
size_t i;
|
|
const uint8_t* bufSrc;
|
|
bool_t alive_cycle = true;
|
|
|
|
if (eof != NULL) {
|
|
*eof = false;
|
|
}
|
|
if ((fifo == NULL) || (buf == NULL) || (fifo->readPtr == fifo->writePtr)) {
|
|
return 0;
|
|
}
|
|
|
|
bufSrc = &fifo->buf[fifo->readPtr];
|
|
for (i = count; (i > 0U) && alive_cycle;) {
|
|
const uint8_t c = *bufSrc;
|
|
|
|
/* is circular buffer empty */
|
|
if (fifo->readPtr == fifo->writePtr) {
|
|
alive_cycle = false;
|
|
} else {
|
|
*buf = c;
|
|
buf++;
|
|
|
|
/* increment variables */
|
|
if (++fifo->readPtr == fifo->bufSize) {
|
|
fifo->readPtr = 0;
|
|
bufSrc = &fifo->buf[0];
|
|
} else {
|
|
bufSrc++;
|
|
}
|
|
i--;
|
|
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ASCII_COMMANDS) != 0
|
|
/* is delimiter? */
|
|
if ((eof != NULL) && (c == DELIM_COMMAND)) {
|
|
*eof = true;
|
|
alive_cycle = false;
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
|
|
return count - i;
|
|
}
|
|
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ALT_READ) != 0
|
|
size_t
|
|
CO_fifo_altBegin(CO_fifo_t* fifo, size_t offset) {
|
|
size_t i;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
fifo->altReadPtr = fifo->readPtr;
|
|
for (i = offset; i > 0U; i--) {
|
|
/* is circular buffer empty */
|
|
if (fifo->altReadPtr == fifo->writePtr) {
|
|
break;
|
|
}
|
|
|
|
/* increment variable */
|
|
if (++fifo->altReadPtr == fifo->bufSize) {
|
|
fifo->altReadPtr = 0;
|
|
}
|
|
}
|
|
|
|
return offset - i;
|
|
}
|
|
|
|
void
|
|
CO_fifo_altFinish(CO_fifo_t* fifo, uint16_t* crc) {
|
|
|
|
if (fifo == NULL) {
|
|
return;
|
|
}
|
|
|
|
if (crc == NULL) {
|
|
fifo->readPtr = fifo->altReadPtr;
|
|
} else {
|
|
const uint8_t* bufSrc = &fifo->buf[fifo->readPtr];
|
|
while (fifo->readPtr != fifo->altReadPtr) {
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_CRC16_CCITT) != 0
|
|
crc16_ccitt_single(crc, *bufSrc);
|
|
#endif
|
|
/* increment variable */
|
|
if (++fifo->readPtr == fifo->bufSize) {
|
|
fifo->readPtr = 0;
|
|
bufSrc = &fifo->buf[0];
|
|
} else {
|
|
bufSrc++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_altRead(CO_fifo_t* fifo, uint8_t* buf, size_t count) {
|
|
size_t i;
|
|
const uint8_t* bufSrc;
|
|
|
|
bufSrc = &fifo->buf[fifo->altReadPtr];
|
|
for (i = count; i > 0U; i--) {
|
|
const uint8_t c = *bufSrc;
|
|
|
|
/* is there no more data */
|
|
if (fifo->altReadPtr == fifo->writePtr) {
|
|
break;
|
|
}
|
|
|
|
*buf = c;
|
|
buf++;
|
|
|
|
/* increment variables */
|
|
if (++fifo->altReadPtr == fifo->bufSize) {
|
|
fifo->altReadPtr = 0;
|
|
bufSrc = &fifo->buf[0];
|
|
} else {
|
|
bufSrc++;
|
|
}
|
|
}
|
|
|
|
return count - i;
|
|
}
|
|
#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ */
|
|
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ASCII_COMMANDS) != 0
|
|
bool_t
|
|
CO_fifo_CommSearch(CO_fifo_t* fifo, bool_t clear) {
|
|
bool_t newCommand = false;
|
|
size_t count;
|
|
uint8_t* commandEnd;
|
|
|
|
if ((fifo == NULL) || (fifo->readPtr == fifo->writePtr)) {
|
|
return false;
|
|
}
|
|
|
|
/* search delimiter until writePtr or until end of buffer */
|
|
if (fifo->readPtr < fifo->writePtr) {
|
|
count = fifo->writePtr - fifo->readPtr;
|
|
} else {
|
|
count = fifo->bufSize - fifo->readPtr;
|
|
}
|
|
commandEnd = (uint8_t*)memchr((const void*)&fifo->buf[fifo->readPtr], (int32_t)DELIM_COMMAND, count);
|
|
if (commandEnd != NULL) {
|
|
newCommand = true;
|
|
} else if (fifo->readPtr > fifo->writePtr) {
|
|
/* not found, search in the beginning of the circular buffer */
|
|
commandEnd = (uint8_t*)memchr((const void*)&fifo->buf[0], (int32_t)DELIM_COMMAND, fifo->writePtr);
|
|
if ((commandEnd != NULL) || (fifo->readPtr == (fifo->writePtr + 1U))) {
|
|
/* command delimiter found or buffer full */
|
|
newCommand = true;
|
|
}
|
|
} else if ((fifo->readPtr == 0U) && (fifo->writePtr == (fifo->bufSize - 1U))) {
|
|
/* buffer full */
|
|
newCommand = true;
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
|
|
/* Clear buffer if set so */
|
|
if (clear) {
|
|
if (commandEnd != NULL) {
|
|
fifo->readPtr = ((size_t)commandEnd - (size_t)fifo->buf) + 1U;
|
|
if (fifo->readPtr == fifo->bufSize) {
|
|
fifo->readPtr = 0;
|
|
}
|
|
} else {
|
|
fifo->readPtr = fifo->writePtr;
|
|
}
|
|
}
|
|
|
|
return newCommand;
|
|
}
|
|
|
|
bool_t
|
|
CO_fifo_trimSpaces(CO_fifo_t* fifo, bool_t* insideComment) {
|
|
bool_t delimCommandFound = false;
|
|
bool_t alive_cycle = true;
|
|
|
|
if ((fifo != NULL) && (insideComment != NULL)) {
|
|
while ((fifo->readPtr != fifo->writePtr) && alive_cycle) {
|
|
uint8_t c = fifo->buf[fifo->readPtr];
|
|
|
|
if (c == DELIM_COMMENT) {
|
|
*insideComment = true;
|
|
} else if ((isgraph((int)c) != 0) && !(*insideComment)) {
|
|
alive_cycle = false;
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
|
|
if (alive_cycle) {
|
|
if (++fifo->readPtr == fifo->bufSize) {
|
|
fifo->readPtr = 0;
|
|
}
|
|
if (c == DELIM_COMMAND) {
|
|
delimCommandFound = true;
|
|
*insideComment = false;
|
|
alive_cycle = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return delimCommandFound;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readToken(CO_fifo_t* fifo, char* buf, size_t count, uint8_t* closed, bool_t* err) {
|
|
bool_t delimCommandFound = false;
|
|
bool_t delimCommentFound = false;
|
|
size_t tokenSize = 0;
|
|
|
|
if ((fifo != NULL) && (buf != NULL) && (count > 1U) && ((err == NULL) || (*err == false))
|
|
&& (fifo->readPtr != fifo->writePtr)) {
|
|
bool_t finished = false;
|
|
uint8_t step = 0;
|
|
size_t ptr = fifo->readPtr; /* current pointer (integer, 0 based) */
|
|
uint8_t* c = &fifo->buf[ptr]; /* current character */
|
|
do {
|
|
switch (step) {
|
|
case 0: /* skip leading empty characters, stop on delimiter */
|
|
if (isgraph((int)*c) != 0) {
|
|
if (*c == DELIM_COMMENT) {
|
|
delimCommentFound = true;
|
|
} else {
|
|
buf[tokenSize] = (char)*c;
|
|
tokenSize++;
|
|
step++;
|
|
}
|
|
} else if (*c == DELIM_COMMAND) {
|
|
delimCommandFound = true;
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
break;
|
|
case 1: /* search for end of the token */
|
|
if (isgraph((int)*c) != 0) {
|
|
if (*c == DELIM_COMMENT) {
|
|
delimCommentFound = true;
|
|
} else if (tokenSize < count) {
|
|
buf[tokenSize] = (char)*c;
|
|
tokenSize++;
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
} else {
|
|
if (*c == DELIM_COMMAND) {
|
|
delimCommandFound = true;
|
|
}
|
|
step++;
|
|
}
|
|
break;
|
|
case 2: /* skip trailing empty characters */
|
|
if (isgraph((int)*c) != 0) {
|
|
if (*c == DELIM_COMMENT) {
|
|
delimCommentFound = true;
|
|
} else {
|
|
fifo->readPtr = ptr;
|
|
finished = true;
|
|
}
|
|
} else if (*c == DELIM_COMMAND) {
|
|
delimCommandFound = true;
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
break;
|
|
default:
|
|
/* MISRA C 2004 15.3 */
|
|
break;
|
|
}
|
|
if (delimCommentFound == true) {
|
|
/* Comment delimiter found, clear all till end of the line. */
|
|
fifo->readPtr = ptr;
|
|
delimCommandFound = CO_fifo_CommSearch(fifo, true);
|
|
finished = true;
|
|
} else if (delimCommandFound) {
|
|
/* command delimiter found, set readPtr behind it. */
|
|
if (++ptr == fifo->bufSize) {
|
|
ptr = 0;
|
|
}
|
|
fifo->readPtr = ptr;
|
|
finished = true;
|
|
} else if (!finished) {
|
|
/* find next character in the circular buffer */
|
|
if (++ptr == fifo->bufSize) {
|
|
ptr = 0;
|
|
c = &fifo->buf[ptr];
|
|
} else {
|
|
c++;
|
|
}
|
|
/* end, if buffer is now empty */
|
|
if (ptr == fifo->writePtr) {
|
|
if (step == 2U) {
|
|
fifo->readPtr = ptr;
|
|
} else {
|
|
tokenSize = 0;
|
|
}
|
|
finished = true;
|
|
}
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
} while (!finished);
|
|
}
|
|
|
|
/* set 'err' return value */
|
|
if ((err != NULL) && (*err == false)) {
|
|
if ((tokenSize == count)
|
|
|| ((closed != NULL)
|
|
&& (((*closed == 1U) && (!delimCommandFound || (tokenSize == 0U)))
|
|
|| ((*closed == 0U) && (delimCommandFound || (tokenSize == 0U)))))) {
|
|
*err = true;
|
|
}
|
|
}
|
|
/* set 'closed' return value */
|
|
if (closed != NULL) {
|
|
*closed = delimCommandFound ? 1U : 0U;
|
|
}
|
|
|
|
/* token was larger then size of the buffer, all was cleaned, return empty */
|
|
if (tokenSize == count) {
|
|
tokenSize = 0;
|
|
}
|
|
/* write string terminator character */
|
|
if ((buf != NULL) && (count > tokenSize)) {
|
|
buf[tokenSize] = '\0';
|
|
}
|
|
|
|
return tokenSize;
|
|
}
|
|
#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS */
|
|
|
|
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ASCII_DATATYPES) != 0
|
|
/* Tables for mime-base64 encoding, as specified in RFC 2045, (without CR-LF, but one long string).
|
|
* Base64 is used for encoding binary data into easy transferable printable characters. In general,
|
|
* each three bytes of binary data are translated into four characters, where characters are
|
|
* selected from 64 characters long table. See https://en.wikipedia.org/wiki/Base64 */
|
|
static const char base64EncTable[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
|
|
|
|
static const uint8_t base64DecTable[] = {
|
|
255, 255, 255, 255, 255, 255, 255, 255, 255, 103, 101, 255, 255, 102, 255, 255, 255, 255, 255, 255, 255, 255,
|
|
255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 103, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 62,
|
|
255, 255, 255, 63, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 255, 255, 255, 100, 255, 255, 255, 0,
|
|
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22,
|
|
23, 24, 25, 255, 255, 255, 255, 255, 255, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38,
|
|
39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 255, 255, 255, 255, 255};
|
|
|
|
size_t
|
|
CO_fifo_readU82a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
uint8_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 6U)) {
|
|
(void)CO_fifo_read(fifo, &n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%" PRIu8, n);
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readU162a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
uint16_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 8U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%" PRIu16, CO_SWAP_16(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readU322a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
uint32_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 12U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%" PRIu32, CO_SWAP_32(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readU642a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
uint64_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 20U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%" PRIu64, CO_SWAP_64(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readX82a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
uint8_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 6U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "0x%02" PRIX8, (uint32_t)n);
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readX162a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
uint16_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 8U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "0x%04" PRIX16, (uint32_t)CO_SWAP_16(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readX322a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
uint32_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 12U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "0x%08" PRIX32, CO_SWAP_32(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readX642a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
uint64_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 20U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "0x%016" PRIX64, CO_SWAP_64(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readI82a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
int8_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 6U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%" PRId8, n);
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readI162a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
int16_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 8U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%" PRId16, CO_SWAP_16(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readI322a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
int32_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 13U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%" PRId32, CO_SWAP_32(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readI642a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
int64_t n = 0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 23U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%" PRId64, CO_SWAP_64(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readR322a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
float32_t n = (float32_t)0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 20U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%g", (float32_t)CO_SWAP_32(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readR642a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
float64_t n = (float64_t)0;
|
|
|
|
if (fifo == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
if ((CO_fifo_getOccupied(fifo) == sizeof(n)) && (count >= 30U)) {
|
|
(void)CO_fifo_read(fifo, (uint8_t*)&n, sizeof(n), NULL);
|
|
return (size_t)sprintf(buf, "%g", (float64_t)CO_SWAP_64(n));
|
|
} else {
|
|
return CO_fifo_readHex2a(fifo, buf, count, end);
|
|
}
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readHex2a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
(void)end; /* unused */
|
|
|
|
size_t len = 0;
|
|
|
|
if ((fifo != NULL) && (count > 3U)) {
|
|
/* Very first write is without leading space */
|
|
if (!fifo->started) {
|
|
uint8_t c;
|
|
if (CO_fifo_getc(fifo, &c)) {
|
|
len = (size_t)sprintf(&buf[0], "%02" PRIX8, (uint32_t)c);
|
|
fifo->started = true;
|
|
}
|
|
}
|
|
|
|
while ((len + 3U) < count) {
|
|
uint8_t c;
|
|
if (!CO_fifo_getc(fifo, &c)) {
|
|
break;
|
|
}
|
|
len += (size_t)sprintf(&buf[len], " %02" PRIX8, (uint32_t)c);
|
|
}
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readVs2a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
size_t len = 0;
|
|
|
|
if ((fifo != NULL) && (count > 3U)) {
|
|
/* Start with '"' */
|
|
if (!fifo->started) {
|
|
buf[len] = '"';
|
|
len++;
|
|
fifo->started = true;
|
|
}
|
|
|
|
while ((len + 2U) < count) {
|
|
uint8_t c;
|
|
if (!CO_fifo_getc(fifo, &c)) {
|
|
if (end) {
|
|
buf[len] = '"';
|
|
len++;
|
|
}
|
|
break;
|
|
} else if ((c != 0U) && (c != (uint8_t)'\r')) {
|
|
/* skip null and CR inside string */
|
|
buf[len] = (char)c;
|
|
len++;
|
|
if (c == DELIM_DQUOTE) {
|
|
buf[len] = '"';
|
|
len++;
|
|
}
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
}
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_readB642a(CO_fifo_t* fifo, char* buf, size_t count, bool_t end) {
|
|
/* mime-base64 encoding, see description above base64EncTable */
|
|
|
|
size_t len = 0;
|
|
|
|
if ((fifo != NULL) && (count >= 4U)) {
|
|
uint8_t step;
|
|
uint16_t word;
|
|
|
|
if (!fifo->started) {
|
|
fifo->started = true;
|
|
step = 0;
|
|
word = 0;
|
|
} else {
|
|
/* get memorized variables from previous function calls */
|
|
step = (uint8_t)(fifo->aux >> 16);
|
|
word = (uint16_t)fifo->aux;
|
|
}
|
|
|
|
while ((len + 3U) <= count) {
|
|
uint8_t c;
|
|
|
|
if (!CO_fifo_getc(fifo, &c)) {
|
|
/* buffer is empty, is also SDO communication finished? */
|
|
if (end) {
|
|
/* add padding if necessary */
|
|
switch (step) {
|
|
case 1:
|
|
buf[len] = base64EncTable[(word >> 4) & 0x3FU];
|
|
len++;
|
|
buf[len] = '=';
|
|
len++;
|
|
buf[len] = '=';
|
|
len++;
|
|
break;
|
|
case 2:
|
|
buf[len] = base64EncTable[(word >> 6) & 0x3FU];
|
|
len++;
|
|
buf[len] = '=';
|
|
len++;
|
|
break;
|
|
default:
|
|
/* MISRA C 2004 15.3 */
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
word |= c;
|
|
|
|
switch (step++) {
|
|
case 0:
|
|
buf[len] = base64EncTable[(word >> 2) & 0x3FU];
|
|
len++;
|
|
break;
|
|
case 1:
|
|
buf[len] = base64EncTable[(word >> 4) & 0x3FU];
|
|
len++;
|
|
break;
|
|
default:
|
|
buf[len] = base64EncTable[(word >> 6) & 0x3FU];
|
|
len++;
|
|
buf[len] = base64EncTable[word & 0x3FU];
|
|
len++;
|
|
step = 0;
|
|
break;
|
|
}
|
|
word <<= 8;
|
|
}
|
|
|
|
/* memorize variables for next iteration */
|
|
fifo->aux = ((uint32_t)step << 16) | word;
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2U8(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[15];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
uint32_t u32 = strtoul(buf, &sRet, 0);
|
|
if ((sRet != strchr(buf, (int32_t)('\0'))) || (u32 > (uint32_t)UINT8_MAX)) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
uint8_t num = (uint8_t)u32;
|
|
nWr = CO_fifo_write(dest, &num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2U16(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[15];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
uint32_t u32 = strtoul(buf, &sRet, 0);
|
|
if ((sRet != strchr(buf, (int32_t)('\0'))) || (u32 > (uint32_t)UINT16_MAX)) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
uint16_t num = CO_SWAP_16((uint16_t)u32);
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2U32(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[15];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
uint32_t u32 = strtoul(buf, &sRet, 0);
|
|
if (sRet != strchr(buf, (int32_t)('\0'))) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
uint32_t num = CO_SWAP_32(u32);
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2U64(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[25];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
uint64_t u64 = strtoull(buf, &sRet, 0);
|
|
if (sRet != strchr(buf, (int32_t)('\0'))) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
uint64_t num = CO_SWAP_64(u64);
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = (uint8_t)st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2I8(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[15];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
int32_t i32 = strtol(buf, &sRet, 0);
|
|
if ((sRet != strchr(buf, (int32_t)('\0'))) || (i32 < INT8_MIN) || (i32 > INT8_MAX)) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
int8_t num = (int8_t)i32;
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2I16(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[15];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
int32_t i32 = strtol(buf, &sRet, 0);
|
|
if ((sRet != strchr(buf, (int32_t)('\0'))) || (i32 < INT16_MIN) || (i32 > INT16_MAX)) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
int16_t num = CO_SWAP_16((int16_t)i32);
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2I32(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[15];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
int32_t i32 = strtol(buf, &sRet, 0);
|
|
if (sRet != strchr(buf, (int32_t)('\0'))) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
int32_t num = CO_SWAP_32(i32);
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2I64(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[25];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
int64_t i64 = strtoll(buf, &sRet, 0);
|
|
if (sRet != strchr(buf, (int32_t)('\0'))) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
int64_t num = CO_SWAP_64(i64);
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = (uint8_t)st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2R32(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[30];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
float32_t f32 = strtof(buf, &sRet);
|
|
if (sRet != strchr(buf, (int32_t)('\0'))) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
float32_t num = CO_SWAP_32(f32);
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2R64(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
char buf[40];
|
|
uint8_t closed = 0xFFU;
|
|
bool_t err = false;
|
|
size_t nWr = 0;
|
|
size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
|
|
uint8_t st = closed;
|
|
if ((nRd == 0U) || err) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
char* sRet;
|
|
float64_t f64 = strtof(buf, &sRet);
|
|
if (sRet != strchr(buf, (int32_t)('\0'))) {
|
|
st |= CO_fifo_st_errVal;
|
|
} else {
|
|
float64_t num = CO_SWAP_64(f64);
|
|
nWr = CO_fifo_write(dest, (uint8_t*)&num, sizeof(num), NULL);
|
|
if (nWr != sizeof(num)) {
|
|
st |= CO_fifo_st_errBuf;
|
|
}
|
|
}
|
|
}
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
return nWr;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2Hex(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
size_t destSpace, destSpaceStart;
|
|
bool_t finished = false;
|
|
uint8_t step;
|
|
uint8_t firstChar;
|
|
uint8_t st = 0;
|
|
|
|
if ((dest == NULL) || (src == NULL)) {
|
|
return 0;
|
|
}
|
|
|
|
/* get free space of the dest fifo */
|
|
destSpaceStart = CO_fifo_getSpace(dest);
|
|
destSpace = destSpaceStart;
|
|
|
|
/* is this the first write into dest? */
|
|
if (!dest->started) {
|
|
bool_t insideComment = false;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
|
|
/* command delimiter found without string, this is an error */
|
|
st |= CO_fifo_st_errTok;
|
|
}
|
|
dest->started = true;
|
|
step = 0;
|
|
firstChar = 0;
|
|
} else {
|
|
/* get memorized variables from previous function calls */
|
|
step = (uint8_t)(dest->aux >> 8);
|
|
firstChar = (uint8_t)(dest->aux & 0xFFU);
|
|
}
|
|
|
|
/* repeat until destination space available and no error and not finished
|
|
* and source characters available */
|
|
while ((destSpace > 0U) && ((st & CO_fifo_st_errMask) == 0U) && !finished) {
|
|
uint8_t c;
|
|
if (!CO_fifo_getc(src, &c)) {
|
|
break;
|
|
}
|
|
|
|
if (step == 6U) {
|
|
/* command is inside comment, waiting for command delimiter */
|
|
bool_t insideComment = true;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || (c == DELIM_COMMAND)) {
|
|
st |= CO_fifo_st_closed;
|
|
finished = true;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if ((int32_t)(isxdigit((int32_t)c)) != 0) {
|
|
/* first or second hex digit */
|
|
if (step == 0U) {
|
|
firstChar = c;
|
|
step = 1;
|
|
} else {
|
|
/* write the byte */
|
|
uint8_t s[3];
|
|
int32_t num;
|
|
s[0] = firstChar;
|
|
s[1] = c;
|
|
s[2] = 0;
|
|
num = strtol((char*)&s[0], NULL, 16);
|
|
(void)CO_fifo_putc(dest, (uint8_t)num);
|
|
destSpace--;
|
|
step = 0;
|
|
}
|
|
} else if ((int32_t)(isgraph((int32_t)c)) != 0) {
|
|
/* printable character, not hex digit */
|
|
if (c == DELIM_COMMENT) { /* comment start */
|
|
step = 6;
|
|
} else { /* syntax error */
|
|
st |= CO_fifo_st_errTok;
|
|
}
|
|
} else {
|
|
/* this is space or delimiter */
|
|
if (step == 1U) {
|
|
/* write the byte */
|
|
uint8_t s[2];
|
|
int32_t num;
|
|
s[0] = firstChar;
|
|
s[1] = 0;
|
|
num = strtol((char*)&s[0], NULL, 16);
|
|
(void)CO_fifo_putc(dest, (uint8_t)num);
|
|
destSpace--;
|
|
step = 0;
|
|
}
|
|
bool_t insideComment = false;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || (c == DELIM_COMMAND)) {
|
|
/* newline found, finish */
|
|
st |= CO_fifo_st_closed;
|
|
finished = true;
|
|
} else if (insideComment) {
|
|
step = 6;
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
}
|
|
} /* while ... */
|
|
|
|
if (!finished) {
|
|
st |= CO_fifo_st_partial;
|
|
/* memorize variables for next iteration */
|
|
dest->aux = ((uint32_t)step << 8) | firstChar;
|
|
}
|
|
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
|
|
return destSpaceStart - destSpace;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2Vs(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
size_t destSpace, destSpaceStart;
|
|
bool_t finished = false;
|
|
uint8_t step;
|
|
uint8_t st = 0;
|
|
|
|
if ((dest == NULL) || (src == NULL)) {
|
|
return 0;
|
|
}
|
|
|
|
/* get free space of the dest fifo */
|
|
destSpaceStart = CO_fifo_getSpace(dest);
|
|
destSpace = destSpaceStart;
|
|
|
|
/* is this the first write into dest? */
|
|
if (!dest->started) {
|
|
bool_t insideComment = false;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
|
|
/* command delimiter found without string, this is an error */
|
|
st |= CO_fifo_st_errTok;
|
|
}
|
|
dest->started = true;
|
|
step = 0;
|
|
} else {
|
|
/* get memorized variables from previous function calls */
|
|
step = (uint8_t)dest->aux;
|
|
}
|
|
|
|
/* repeat until destination space available and no error and not finished and source characters available */
|
|
while ((destSpace > 0U) && ((st & CO_fifo_st_errMask) == 0U) && !finished) {
|
|
uint8_t c;
|
|
if (!CO_fifo_getc(src, &c)) {
|
|
break;
|
|
}
|
|
|
|
switch (step) {
|
|
case 0: /* beginning of the string, first write into dest */
|
|
if (c == DELIM_DQUOTE) {
|
|
/* Indicated beginning of the string, skip this character. */
|
|
step = 1;
|
|
} else {
|
|
/* this must be a single word string without '"' */
|
|
/* copy the character */
|
|
(void)CO_fifo_putc(dest, c);
|
|
destSpace--;
|
|
step = 2;
|
|
}
|
|
break;
|
|
|
|
case 1: /* inside string, quoted string */
|
|
case 2: /* inside string, single word, no quotes */
|
|
if (c == DELIM_DQUOTE) {
|
|
/* double quote found, this may be end of the string or escaped
|
|
* double quote (with two double quotes) */
|
|
step += 2U;
|
|
} else if ((isgraph((int)c) == 0) && (step == 2U)) {
|
|
/* end of single word string */
|
|
bool_t insideComment = false;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || (c == DELIM_COMMAND)) {
|
|
st |= CO_fifo_st_closed;
|
|
finished = true;
|
|
} else {
|
|
step = insideComment ? 6U : 5U;
|
|
}
|
|
} else if (c == DELIM_COMMAND) {
|
|
/* no closing quote, error */
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
/* copy the character */
|
|
(void)CO_fifo_putc(dest, c);
|
|
destSpace--;
|
|
}
|
|
break;
|
|
|
|
case 3: /* previous was double quote, parsing quoted string */
|
|
case 4: /* previous was double quote, parsing no quoted word */
|
|
if (c == DELIM_DQUOTE) {
|
|
/* escaped double quote, copy the character and continue */
|
|
(void)CO_fifo_putc(dest, c);
|
|
destSpace--;
|
|
step -= 2U;
|
|
} else {
|
|
/* previous character was closing double quote */
|
|
if (step == 4U) {
|
|
/* no opening double quote, syntax error */
|
|
st |= CO_fifo_st_errTok;
|
|
} else {
|
|
if (isgraph((int)c) == 0) {
|
|
/* end of quoted string */
|
|
bool_t insideComment = false;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || (c == DELIM_COMMAND)) {
|
|
st |= CO_fifo_st_closed;
|
|
finished = true;
|
|
} else {
|
|
step = insideComment ? 6U : 5U;
|
|
}
|
|
} else {
|
|
/* space must follow closing double quote, error */
|
|
st |= CO_fifo_st_errTok;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
|
|
case 5: { /* String token is finished, waiting for command delimiter */
|
|
bool_t insideComment = false;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || (c == DELIM_COMMAND)) {
|
|
st |= CO_fifo_st_closed;
|
|
finished = true;
|
|
} else if (insideComment) {
|
|
step = 6;
|
|
} else if (isgraph((int)c) != 0) {
|
|
if (c == DELIM_COMMENT) { /* comment start */
|
|
step = 6;
|
|
} else { /* syntax error */
|
|
st |= CO_fifo_st_errTok;
|
|
}
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
break;
|
|
}
|
|
case 6: { /* String token is finished, waiting for command delimiter */
|
|
bool_t insideComment = true;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || (c == DELIM_COMMAND)) {
|
|
st |= CO_fifo_st_closed;
|
|
finished = true;
|
|
}
|
|
break;
|
|
}
|
|
default: /* internal error */ st |= CO_fifo_st_errInt; break;
|
|
}
|
|
}
|
|
|
|
if (!finished) {
|
|
st |= CO_fifo_st_partial;
|
|
/* memorize variables for next iteration */
|
|
dest->aux = step;
|
|
}
|
|
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
|
|
return destSpaceStart - destSpace;
|
|
}
|
|
|
|
size_t
|
|
CO_fifo_cpyTok2B64(CO_fifo_t* dest, CO_fifo_t* src, uint8_t* status) {
|
|
/* mime-base64 decoding, see description above base64EncTable */
|
|
|
|
size_t destSpace, destSpaceStart;
|
|
bool_t finished = false;
|
|
uint8_t step;
|
|
uint32_t dword;
|
|
uint8_t st = 0;
|
|
|
|
if ((dest == NULL) || (src == NULL)) {
|
|
return 0;
|
|
}
|
|
|
|
/* get free space of the dest fifo */
|
|
destSpaceStart = CO_fifo_getSpace(dest);
|
|
destSpace = destSpaceStart;
|
|
|
|
/* is this the first write into dest? */
|
|
if (!dest->started) {
|
|
bool_t insideComment = false;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
|
|
/* command delimiter found without string, this is an error */
|
|
st |= CO_fifo_st_errTok;
|
|
}
|
|
dest->started = true;
|
|
step = 0;
|
|
dword = 0;
|
|
} else {
|
|
/* get memorized variables from previous function calls */
|
|
step = (uint8_t)(dest->aux >> 24);
|
|
dword = dest->aux & 0xFFFFFFU;
|
|
}
|
|
|
|
/* repeat until destination space available and no error and not finished and source characters available */
|
|
while ((destSpace >= 3U) && ((st & CO_fifo_st_errMask) == 0U) && !finished) {
|
|
uint8_t c;
|
|
if (!CO_fifo_getc(src, &c)) {
|
|
break;
|
|
}
|
|
|
|
if (step >= 5U) {
|
|
/* String token is finished, waiting for command delimiter */
|
|
bool_t insideComment = step > 5U;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || (c == DELIM_COMMAND)) {
|
|
st |= CO_fifo_st_closed;
|
|
finished = true;
|
|
} else if (insideComment) {
|
|
step = 6;
|
|
} else if ((isgraph((int)c) != 0) && (c != (uint8_t)'=')) {
|
|
if (c == DELIM_COMMENT) { /* comment start */
|
|
step = 6;
|
|
} else { /* syntax error */
|
|
st |= CO_fifo_st_errTok;
|
|
}
|
|
} else { /* MISRA C 2004 14.10 */
|
|
}
|
|
continue;
|
|
}
|
|
|
|
uint8_t code = base64DecTable[c & 0x7FU];
|
|
|
|
if (((c & 0x80U) != 0U) || ((code & 0x80U) != 0U)) {
|
|
st |= CO_fifo_st_errTok;
|
|
} else if (code >= 64U /* '=' (pad) or DELIM_COMMAND or space */) {
|
|
/* base64 string finished, write remaining bytes */
|
|
switch (step) {
|
|
case 2:
|
|
(void)CO_fifo_putc(dest, (uint8_t)(dword >> 4));
|
|
destSpace--;
|
|
break;
|
|
case 3:
|
|
(void)CO_fifo_putc(dest, (uint8_t)(dword >> 10));
|
|
(void)CO_fifo_putc(dest, (uint8_t)(dword >> 2));
|
|
destSpace -= 2U;
|
|
break;
|
|
default:
|
|
/* MISRA C 2004 15.3 */
|
|
break;
|
|
}
|
|
|
|
bool_t insideComment = false;
|
|
if (CO_fifo_trimSpaces(src, &insideComment) || (c == DELIM_COMMAND)) {
|
|
st |= CO_fifo_st_closed;
|
|
finished = true;
|
|
} else {
|
|
step = insideComment ? 6U : 5U;
|
|
}
|
|
} else {
|
|
dword = (dword << 6) | code;
|
|
if (step++ == 3U) {
|
|
(void)CO_fifo_putc(dest, (uint8_t)((dword >> 16) & 0xFFU));
|
|
(void)CO_fifo_putc(dest, (uint8_t)((dword >> 8) & 0xFFU));
|
|
(void)CO_fifo_putc(dest, (uint8_t)(dword & 0xFFU));
|
|
destSpace -= 3U;
|
|
dword = 0;
|
|
step = 0;
|
|
}
|
|
}
|
|
} /* while ... */
|
|
|
|
if (!finished) {
|
|
st |= CO_fifo_st_partial;
|
|
/* memorize variables for next iteration */
|
|
dest->aux = ((uint32_t)step << 24) | (dword & 0xFFFFFFU);
|
|
}
|
|
|
|
if (status != NULL) {
|
|
*status = st;
|
|
}
|
|
|
|
return destSpaceStart - destSpace;
|
|
}
|
|
|
|
#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES */
|
|
|
|
#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE */
|