eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

failed approach

Browse Source
This commit is contained in:
Jakob Meier
2020-11-19 18:24:03 +01:00
parent 349897d878
commit 6c23b00c22
324 changed files with 57839 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
gomspace/libutil/src/base16.c Normal file
View File

@ -0,0 +1,61 @@
/*
* Copyright (C) 2010 Michael Brown <mbrown@fensystems.co.uk>.
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License as
* published by the Free Software Foundation; either version 2 of the
* License, or any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include <gs/util/base16.h>
#include <gs/util/log.h>
#include <stdio.h>
#include <stdlib.h>
void base16_encode(const uint8_t * raw, size_t len, char *encoded)
{
const uint8_t *raw_bytes = raw;
char *encoded_bytes = encoded;
size_t remaining = len;
for (; remaining--; encoded_bytes += 2)
snprintf(encoded_bytes, 3, "%02X", *(raw_bytes++));
}
int base16_decode(const char *encoded, uint8_t *raw)
{
uint8_t *raw_bytes = raw;
if (encoded) {
const char *encoded_bytes = encoded;
char buf[3];
char *endp;
while (encoded_bytes[0]) {
if (!encoded_bytes[1]) {
log_error("Base16-encoded string \"%s\" has invalid length\n",
encoded);
return GS_ERROR_ARG;
}
memcpy(buf, encoded_bytes, 2);
buf[2] = '\0';
*(raw_bytes++) = (uint8_t) strtoul(buf, &endp, 16);
if (*endp != '\0') {
log_error("Base16-encoded string \"%s\" has invalid byte \"%s\"\n",
encoded, buf);
return GS_ERROR_ARG;
}
encoded_bytes += 2;
}
}
return (int)(raw_bytes - raw);
}

73
gomspace/libutil/src/bindings/python/pyutil.c Normal file
View File

@ -0,0 +1,73 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <Python.h>
#include <gs/util/clock.h>
#include <gs/util/error.h>
#if PY_MAJOR_VERSION == 3
#define IS_PY3
#endif
/**
* Helpers
*/
static PyObject* pyutil_get_clock_time(PyObject *self, PyObject *args) {
gs_timestamp_t ts;
gs_clock_get_time(&ts);
return Py_BuildValue("II", ts.tv_sec, ts.tv_nsec);
}
static PyObject* pyutil_error_string(PyObject *self, PyObject *args)
{
int error;
if (!PyArg_ParseTuple(args, "i", &error))
{
Py_RETURN_NONE;
}
return Py_BuildValue("s", gs_error_string(error));
}
static PyMethodDef methods[] = {
/* helpers */
{"get_clock_time", pyutil_get_clock_time, METH_NOARGS, ""},
{"error_string", pyutil_error_string, METH_VARARGS, ""},
/* sentinel */
{NULL, NULL, 0, NULL}
};
#ifdef IS_PY3
static struct PyModuleDef moduledef = {
PyModuleDef_HEAD_INIT,
"libgsutil_py3",
NULL, /* module documentation, may be NULL */
-1, /* size of per-interpreter state of the module,
or -1 if the module keeps state in global variables. */
methods,
NULL,
NULL,
NULL,
NULL
};
#endif
#ifdef IS_PY3
PyMODINIT_FUNC PyInit_libgsutil_py3(void) {
#else
PyMODINIT_FUNC initlibgsutil_py2(void) {
#endif
#ifdef IS_PY3
PyObject* m = PyModule_Create(&moduledef);
#else
Py_InitModule("libgsutil_py2", methods);
#endif
#ifdef IS_PY3
return m;
#endif
}

128
gomspace/libutil/src/bytebuffer.c Normal file
View File

@ -0,0 +1,128 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/bytebuffer.h>
#include <gs/util/check.h>
#include <stdio.h>
#define GS_BYTEBUFFER_F_FAILED 0x01
#define GS_BYTEBUFFER_F_OVERRUN 0x02
gs_error_t gs_bytebuffer_init(gs_bytebuffer_t * bb, void * buffer, size_t buffer_size)
{
GS_CHECK_HANDLE(bb != NULL);
memset(bb, 0, sizeof(*bb));
if (buffer) {
if (buffer_size < 2) {
// must always have room for NUL termination.
return GS_ERROR_ARG;
}
bb->buffer = buffer;
bb->size = buffer_size;
} else {
// dry run - don't insert anything in buffer, but increment used
}
return GS_OK;
}
void gs_bytebuffer_vprintf(gs_bytebuffer_t * bb, const char * format, va_list ap)
{
int res;
if (bb->buffer == NULL) {
// dry run
char buf[3];
res = vsnprintf(buf, 0, format, ap);
if (res >= 0) {
bb->used += res;
}
} else {
const size_t free_bytes = gs_bytebuffer_get_free(bb);
res = vsnprintf((char*)&bb->buffer[bb->used], free_bytes, format, ap);
if (res > 0) {
if ((size_t)res >= free_bytes) {
// over run
bb->flags |= GS_BYTEBUFFER_F_OVERRUN;
bb->used = bb->size;
bb->buffer[bb->size - 1] = 0;
} else {
bb->used += res;
}
}
}
if (res < 0) {
bb->flags |= GS_BYTEBUFFER_F_FAILED;
}
}
void gs_bytebuffer_printf(gs_bytebuffer_t * bb, const char * format, ...)
{
va_list ap;
va_start(ap, format);
gs_bytebuffer_vprintf(bb, format, ap);
va_end(ap);
}
void gs_bytebuffer_append(gs_bytebuffer_t * bb, const void * data, size_t length)
{
if (bb->buffer == NULL) {
// dry run
bb->used += length;
} else {
const size_t free_bytes = gs_bytebuffer_get_free(bb);
if (free_bytes >= length) {
memcpy(&bb->buffer[bb->used], data, length);
bb->used += length;
} else {
memcpy(&bb->buffer[bb->used], data, free_bytes);
bb->flags |= GS_BYTEBUFFER_F_OVERRUN;
bb->used += free_bytes;
}
}
}
void gs_bytebuffer_append_string(gs_bytebuffer_t * bb, const char * string)
{
if (gs_string_empty(string) == false) {
gs_bytebuffer_append(bb, string, strlen(string));
}
}
void gs_bytebuffer_append_string_max(gs_bytebuffer_t * bb, const char * string, size_t max_length)
{
if (gs_string_empty(string) == false) {
gs_bytebuffer_append(bb, string, strnlen(string, max_length));
}
}
char * gs_bytebuffer_get_as_string(gs_bytebuffer_t * bb, gs_error_t * error)
{
if (bb && bb->buffer) {
// handle NUL termination
if (bb->used < bb->size) {
bb->buffer[bb->used] = 0;
} else {
// overrun - truncation buffer
bb->flags |= GS_BYTEBUFFER_F_OVERRUN;
bb->buffer[bb->used - 1] = 0;
}
if (error) {
*error = gs_bytebuffer_get_state(bb);
}
return (char*) bb->buffer;
}
return NULL;
}
gs_error_t gs_bytebuffer_get_state(gs_bytebuffer_t * bb)
{
if (bb) {
if (bb->flags & GS_BYTEBUFFER_F_FAILED) {
return GS_ERROR_DATA;
}
if (bb->flags & GS_BYTEBUFFER_F_OVERRUN) {
return GS_ERROR_OVERFLOW;
}
return GS_OK;
}
return GS_ERROR_HANDLE;
}

323
gomspace/libutil/src/byteorder.c Normal file
View File

@ -0,0 +1,323 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/byteorder.h>
#include <conf_util.h>
/* Convert 16-bit number from host byte order to network byte order */
extern inline uint16_t __attribute__ ((__const__)) util_hton16(uint16_t h16) {
#if UTIL_BIG_ENDIAN
return h16;
#elif UTIL_LITTLE_ENDIAN
return (uint16_t)(((h16 & 0xff00) >> 8) |
((h16 & 0x00ff) << 8));
#endif
}
/* Convert 16-bit number from network byte order to host byte order */
extern inline uint16_t __attribute__ ((__const__)) util_ntoh16(uint16_t n16) {
return util_hton16(n16);
}
/* Convert 32-bit number from host byte order to network byte order */
extern inline uint32_t __attribute__ ((__const__)) util_hton32(uint32_t h32) {
#if UTIL_BIG_ENDIAN
return h32;
#elif UTIL_LITTLE_ENDIAN
return (((h32 & 0xff000000) >> 24) |
((h32 & 0x000000ff) << 24) |
((h32 & 0x0000ff00) << 8) |
((h32 & 0x00ff0000) >> 8));
#endif
}
/* Convert 32-bit number from network byte order to host byte order */
extern inline uint32_t __attribute__ ((__const__)) util_ntoh32(uint32_t n32) {
return util_hton32(n32);
}
/* Convert 64-bit number from host byte order to network byte order */
extern inline uint64_t __attribute__ ((__const__)) util_hton64(uint64_t h64) {
#if UTIL_BIG_ENDIAN
return h64;
#elif UTIL_LITTLE_ENDIAN
return (((h64 & 0xff00000000000000LL) >> 56) |
((h64 & 0x00000000000000ffLL) << 56) |
((h64 & 0x00ff000000000000LL) >> 40) |
((h64 & 0x000000000000ff00LL) << 40) |
((h64 & 0x0000ff0000000000LL) >> 24) |
((h64 & 0x0000000000ff0000LL) << 24) |
((h64 & 0x000000ff00000000LL) >> 8) |
((h64 & 0x00000000ff000000LL) << 8));
#endif
}
/* Convert 64-bit number from host byte order to network byte order */
extern inline uint64_t __attribute__ ((__const__)) util_ntoh64(uint64_t n64) {
return util_hton64(n64);
}
/* Convert float from host byte order to network byte order */
extern inline float __attribute__ ((__const__)) util_htonflt(float f) {
#if UTIL_BIG_ENDIAN
return f;
#elif UTIL_LITTLE_ENDIAN
union v {
float f;
uint32_t i;
};
union v val;
val.f = f;
val.i = util_hton32(val.i);
return val.f;
#endif
}
/* Convert float from host byte order to network byte order */
extern inline float __attribute__ ((__const__)) util_ntohflt(float f) {
return util_htonflt(f);
}
/* Convert double from host byte order to network byte order */
extern inline double __attribute__ ((__const__)) util_htondbl(double d) {
#if UTIL_BIG_ENDIAN
return d;
#elif UTIL_LITTLE_ENDIAN
union v {
double d;
uint64_t i;
};
union v val;
val.d = d;
val.i = util_hton64(val.i);
return val.d;
#endif
}
/* Convert float from host byte order to network byte order */
extern inline double __attribute__ ((__const__)) util_ntohdbl(double d) {
return util_htondbl(d);
}
/* Convert 16-bit number from host byte order to big endian byte order */
extern inline uint16_t __attribute__ ((__const__)) util_htobe16(uint16_t h16) {
return util_hton16(h16);
}
/* Convert 16-bit number from host byte order to little endian byte order */
extern inline uint16_t __attribute__ ((__const__)) util_htole16(uint16_t h16) {
#if UTIL_LITTLE_ENDIAN
return h16;
#elif UTIL_BIG_ENDIAN
return (uint16_t)(((h16 & 0xff00) >> 8) |
((h16 & 0x00ff) << 8));
#endif
}
/* Convert 16-bit number from big endian byte order to little endian byte order */
extern inline uint16_t __attribute__ ((__const__)) util_betoh16(uint16_t be16) {
return util_ntoh16(be16);
}
/* Convert 16-bit number from little endian byte order to host byte order */
extern inline uint16_t __attribute__ ((__const__)) util_letoh16(uint16_t le16) {
return util_htole16(le16);
}
/* Convert 32-bit number from host byte order to big endian byte order */
extern inline uint32_t __attribute__ ((__const__)) util_htobe32(uint32_t h32) {
return util_hton32(h32);
}
/* Convert 32-bit number from little endian byte order to host byte order */
extern inline uint32_t __attribute__ ((__const__)) util_htole32(uint32_t h32) {
#if UTIL_LITTLE_ENDIAN
return h32;
#elif UTIL_BIG_ENDIAN
return (((h32 & 0xff000000) >> 24) |
((h32 & 0x000000ff) << 24) |
((h32 & 0x0000ff00) << 8) |
((h32 & 0x00ff0000) >> 8));
#endif
}
/* Convert 32-bit number from big endian byte order to host byte order */
extern inline uint32_t __attribute__ ((__const__)) util_betoh32(uint32_t be32) {
return util_ntoh32(be32);
}
/* Convert 32-bit number from little endian byte order to host byte order */
extern inline uint32_t __attribute__ ((__const__)) util_letoh32(uint32_t le32) {
return util_htole32(le32);
}
/* Convert 64-bit number from host byte order to big endian byte order */
extern inline uint64_t __attribute__ ((__const__)) util_htobe64(uint64_t h64) {
return util_hton64(h64);
}
/* Convert 64-bit number from host byte order to little endian byte order */
extern inline uint64_t __attribute__ ((__const__)) util_htole64(uint64_t h64) {
#if UTIL_LITTLE_ENDIAN
return h64;
#elif UTIL_BIG_ENDIAN
return (((h64 & 0xff00000000000000LL) >> 56) |
((h64 & 0x00000000000000ffLL) << 56) |
((h64 & 0x00ff000000000000LL) >> 40) |
((h64 & 0x000000000000ff00LL) << 40) |
((h64 & 0x0000ff0000000000LL) >> 24) |
((h64 & 0x0000000000ff0000LL) << 24) |
((h64 & 0x000000ff00000000LL) >> 8) |
((h64 & 0x00000000ff000000LL) << 8));
#endif
}
/* Convert 64-bit number from big endian byte order to host byte order */
extern inline uint64_t __attribute__ ((__const__)) util_betoh64(uint64_t be64) {
return util_ntoh64(be64);
}
/* Convert 64-bit number from little endian byte order to host byte order */
extern inline uint64_t __attribute__ ((__const__)) util_letoh64(uint64_t le64) {
return util_htole64(le64);
}
/* Convert 16-bit number from host byte order to network byte order */
extern inline uint16_t __attribute__ ((__const__)) util_htons(uint16_t h16) {
return util_hton16(h16);
}
/* Convert 16-bit number from network byte order to host byte order */
extern inline uint16_t __attribute__ ((__const__)) util_ntohs(uint16_t n16) {
return util_ntoh16(n16);
}
/* Convert 32-bit number from host byte order to network byte order */
extern inline uint32_t __attribute__ ((__const__)) util_htonl(uint32_t h32) {
return util_hton32(h32);
}
/* Convert 32-bit number from network byte order to host byte order */
extern inline uint32_t __attribute__ ((__const__)) util_ntohl(uint32_t n32) {
return util_ntoh32(n32);
}
#define BYTEORDER_ARRAY(convert, from, to, count) { \
for (unsigned int i = 0; i < count; ++i, ++from, ++to) { \
*to = convert(*from); \
} \
}
void util_hton16_array(const uint16_t * from, uint16_t * to, size_t count)
{
BYTEORDER_ARRAY(util_hton16, from, to, count);
}
void util_hton32_array(const uint32_t * from, uint32_t * to, size_t count)
{
BYTEORDER_ARRAY(util_hton32, from, to, count);
}
void util_hton64_array(const uint64_t * from, uint64_t * to, size_t count)
{
BYTEORDER_ARRAY(util_hton64, from, to, count);
}
void util_ntoh16_array(const uint16_t * from, uint16_t * to, size_t count)
{
BYTEORDER_ARRAY(util_ntoh16, from, to, count);
}
void util_ntoh32_array(const uint32_t * from, uint32_t * to, size_t count)
{
BYTEORDER_ARRAY(util_ntoh32, from, to, count);
}
void util_ntoh64_array(const uint64_t * from, uint64_t * to, size_t count)
{
BYTEORDER_ARRAY(util_ntoh64, from, to, count);
}
void util_htonflt_array(const float * from, float * to, size_t count)
{
BYTEORDER_ARRAY(util_htonflt, from, to, count);
}
void util_ntohflt_array(const float * from, float * to, size_t count)
{
BYTEORDER_ARRAY(util_ntohflt, from, to, count);
}
void util_htondbl_array(const double * from, double * to, size_t count)
{
BYTEORDER_ARRAY(util_htondbl, from, to, count);
}
void util_ntohdbl_array(const double * from, double * to, size_t count)
{
BYTEORDER_ARRAY(util_ntohdbl, from, to, count);
}
uint16_t gs_bswap_16(uint16_t value)
{
return (uint16_t)(((value & 0xff00) >> 8) |
((value & 0x00ff) << 8));
}
void gs_bswap_16_array(const uint16_t * from, uint16_t * to, size_t count)
{
BYTEORDER_ARRAY(gs_bswap_16, from, to, count);
}
uint32_t gs_bswap_32(uint32_t value)
{
return (((value & 0xff000000) >> 24) |
((value & 0x000000ff) << 24) |
((value & 0x0000ff00) << 8) |
((value & 0x00ff0000) >> 8));
}
void gs_bswap_32_array(const uint32_t * from, uint32_t * to, size_t count)
{
BYTEORDER_ARRAY(gs_bswap_32, from, to, count);
}
uint64_t gs_bswap_64(uint64_t value)
{
return (((value & 0xff00000000000000LL) >> 56) |
((value & 0x00000000000000ffLL) << 56) |
((value & 0x00ff000000000000LL) >> 40) |
((value & 0x000000000000ff00LL) << 40) |
((value & 0x0000ff0000000000LL) >> 24) |
((value & 0x0000000000ff0000LL) << 24) |
((value & 0x000000ff00000000LL) >> 8) |
((value & 0x00000000ff000000LL) << 8));
}
void gs_bswap_64_array(const uint64_t * from, uint64_t * to, size_t count)
{
BYTEORDER_ARRAY(gs_bswap_64, from, to, count);
}
float gs_bswap_float(float value)
{
union v {
float f;
uint32_t i;
} val;
val.f = value;
val.i = gs_bswap_32(val.i);
return val.f;
}
void gs_bswap_float_array(const float * from, float * to, size_t count)
{
BYTEORDER_ARRAY(gs_bswap_float, from, to, count);
}

113
gomspace/libutil/src/clock.c Normal file
View File

@ -0,0 +1,113 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/clock.h>
#include <stdio.h>
#include <inttypes.h>
#include <string.h>
#if !__AVR__
#include <time.h>
#endif
gs_error_t gs_clock_to_iso8601_string2(uint32_t utc_sec, char * buf, size_t buf_size)
{
if ((buf == NULL) || (buf_size == 0)) {
return GS_ERROR_ARG;
}
#if __AVR__
int res = snprintf(buf, buf_size, "%"PRIu32"Z", utc_sec);
if ((res < 0) || ((size_t)res >= buf_size)) {
buf[buf_size - 1] = 0;
return GS_ERROR_RANGE;
}
#else
const time_t time_seconds = (time_t) utc_sec;
struct tm tm_buf;
struct tm * tm = gmtime_r(&time_seconds, &tm_buf);
if (tm == NULL) {
int res = snprintf(buf, buf_size, "%ldZ", time_seconds);
if ((res < 0) || ((size_t)res >= buf_size)) {
buf[buf_size - 1] = 0;
}
return GS_ERROR_DATA;
}
// ISO8601 timestamp: 2017-03-30T06:20:45Z
int res = snprintf(buf, buf_size, "%04d-%02d-%02dT%02d:%02d:%02dZ",
tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday,
tm->tm_hour, tm->tm_min, tm->tm_sec);
if ((res < 0) || ((size_t)res >= buf_size)) {
buf[buf_size - 1] = 0;
return GS_ERROR_RANGE;
}
#endif
return GS_OK;
}
gs_error_t gs_clock_to_iso8601_string(const gs_timestamp_t * utc_time, char * buf, size_t buf_size)
{
if (utc_time == NULL) {
return GS_ERROR_ARG;
}
return gs_clock_to_iso8601_string2(utc_time->tv_sec, buf, buf_size);
}
gs_error_t gs_clock_from_string(const char * str, gs_timestamp_t * ts)
{
if (!str || !str[0] || !ts) {
return GS_ERROR_ARG;
}
// check for <seconds>.<nano-seconds>
{
uint32_t sec;
uint32_t nsec;
int res = sscanf(str, "%" SCNu32 ".%" SCNu32, &sec, &nsec);
if (res == 2) {
ts->tv_sec = sec;
ts->tv_nsec = nsec;
return GS_OK;
}
}
#if !__AVR__
// check for ISO8601
{
struct tm tm;
memset(&tm, 0, sizeof(tm)); // no daylight saving
//int res = sscanf(str, "%" SCNd32 "-%" SCNd32 "-%" SCNd32 "T%" SCNd32 ":%" SCNd32 ":%" SCNd32 "Z",
int res = sscanf(str, "%d-%d-%dT%d:%d:%dZ",
&tm.tm_year, &tm.tm_mon, &tm.tm_mday,
&tm.tm_hour, &tm.tm_min, &tm.tm_sec);
if ((res == 6) &&
(tm.tm_year >= 1970) && (tm.tm_year <= 2038) &&
(tm.tm_mon >= 1) && (tm.tm_mon <= 12) &&
(tm.tm_mday >= 1) && (tm.tm_mday <= 31) &&
(tm.tm_hour >= 0) && (tm.tm_hour <= 23) &&
(tm.tm_min >= 0) && (tm.tm_min <= 59) &&
(tm.tm_sec >= 0) && (tm.tm_sec <= 60))
{
tm.tm_year -= 1900;
tm.tm_mon -= 1;
#if __linux__
// not posix compliant
time_t sec = timegm(&tm);
#else
// embedded platforms do not have timezones/daylight-saving - so standard mktime works
time_t sec = mktime(&tm);
#endif
if (sec >= 0) {
ts->tv_sec = (uint32_t) sec;
ts->tv_nsec = 0;
return GS_OK;
}
}
}
#endif
return GS_ERROR_DATA;
}

79
gomspace/libutil/src/crc32.c Normal file
View File

@ -0,0 +1,79 @@
/*
* efone - Distributed internet phone system.
*
* (c) 1999,2000 Krzysztof Dabrowski
* (c) 1999,2000 ElysiuM deeZine
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version
* 2 of the License, or (at your option) any later version.
*
* Based on implementation by Finn Yannick Jacobs
*/
#include <gs/util/crc32.h>
#include <gs/util/pgm.h>
static const uint32_t crc_tab[256] GS_PGM_OBJECT = {
0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA, 0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988, 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE, 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC, 0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172, 0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940, 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116, 0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924, 0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A, 0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818, 0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E, 0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C, 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2, 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0, 0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086, 0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4, 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A, 0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8, 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE, 0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC, 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252, 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60, 0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236, 0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04, 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A, 0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38, 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E, 0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C, 0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2, 0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0, 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6, 0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94, 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D
};
#define CALC_CRC32_STEP(crc,byte) (((crc >> 8) & 0x00FFFFFF) ^ GS_PGM_UINT32_BY_PTR(&crc_tab[(crc ^ byte) & (uint32_t) 0xFF]))
uint32_t gs_crc32_init(void)
{
return 0xFFFFFFFF;
}
uint32_t gs_crc32_update(uint32_t crc, const void * block, size_t length)
{
if (block && length) {
const uint8_t * u8 = block;
for (unsigned int i = 0; i < length; i++) {
crc = CALC_CRC32_STEP(crc, *u8++);
}
}
return crc;
}
uint32_t gs_crc32_finalize(uint32_t crc)
{
return (crc ^ 0xFFFFFFFF);
}
uint32_t gs_crc32(const void * block, size_t length)
{
return gs_crc32_finalize(gs_crc32_update(gs_crc32_init(), block, length));
}

68
gomspace/libutil/src/crc8.c Normal file
View File

@ -0,0 +1,68 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/crc8.h>
#include <gs/util/pgm.h>
static const uint8_t crc_tab[256] GS_PGM_OBJECT = {
0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
};
#define CALC_CRC8_STEP(crc,byte) ((crc >> 8) ^ GS_PGM_UINT8_BY_PTR(&crc_tab[(crc ^ byte) & (uint8_t) 0xFF]))
uint8_t gs_crc8_init(void)
{
return 0xFF;
}
uint8_t gs_crc8_update(uint8_t crc, const void * block, size_t length)
{
if (block && length) {
const uint8_t * u8 = block;
for (unsigned int i = 0; i < length; i++) {
crc = CALC_CRC8_STEP(crc, *u8++);
}
}
return crc;
}
uint8_t gs_crc8_finalize(uint8_t crc)
{
return (crc ^ 0x00);
}
uint8_t gs_crc8(const void * block, size_t length)
{
return gs_crc8_finalize(gs_crc8_update(gs_crc8_init(), block, length));
}

6
gomspace/libutil/src/drivers/can/can.c Normal file
View File

@ -0,0 +1,6 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/drivers/can/can.h>
// deifne common log group.
GS_LOG_GROUP(gs_can_log, "can", GS_LOG_CAT_DRIVER, GS_LOG_DEFAULT_MASK);

6
gomspace/libutil/src/drivers/i2c/i2c.c Normal file
View File

@ -0,0 +1,6 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/drivers/i2c/common.h>
// define common log group.
GS_LOG_GROUP(gs_i2c_log, "i2c", GS_LOG_CAT_DRIVER, GS_LOG_DEFAULT_MASK);

6
gomspace/libutil/src/drivers/spi/spi.c Normal file
View File

@ -0,0 +1,6 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/drivers/spi/common.h>
// define common log group.
GS_LOG_GROUP(gs_spi_log, "spi", GS_LOG_CAT_DRIVER, GS_LOG_DEFAULT_MASK);

37
gomspace/libutil/src/drivers/sys/memory.c Normal file
View File

@ -0,0 +1,37 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/drivers/sys/memory.h>
#include <gs/util/check.h>
static const char * long_to_string(char * buf, size_t buf_size, long lvalue)
{
if (lvalue >= 0) {
snprintf(buf, buf_size, "%ld", lvalue);
return buf;
}
return "Unknown";
}
gs_error_t gs_mem_get_ram_stat(gs_mem_ram_type_t type, gs_mem_ram_stat_t * ram_stat)
{
if (type == GS_MEM_RAM_TYPE_INTERNAL) {
return gs_mem_get_int_ram_stat(ram_stat);
} else if (type == GS_MEM_RAM_TYPE_EXTERNAL) {
return gs_mem_get_ext_ram_stat(ram_stat);
}
/* Unsupported memory type */
return GS_ERROR_NOT_SUPPORTED;
}
gs_error_t gs_mem_print_ram_stat(gs_mem_ram_stat_t * ram_stat, FILE * out)
{
GS_CHECK_ARG(ram_stat != NULL);
char buf[20];
fprintf(out, "Total: %s\r\n", long_to_string(buf, sizeof(buf), ram_stat->total));
fprintf(out, "Max available: %s\r\n", long_to_string(buf, sizeof(buf), ram_stat->max_available));
fprintf(out, "Min available: %s\r\n", long_to_string(buf, sizeof(buf), ram_stat->min_available));
fprintf(out, "Available: %s\r\n", long_to_string(buf, sizeof(buf), ram_stat->available));
return GS_OK;
}

106
gomspace/libutil/src/error.c Normal file
View File

@ -0,0 +1,106 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#define GS_UTIL_DEPRECATED_ERROR_CODES 1
#include <gs/util/error.h>
#include <gs/util/string.h>
#include <stdlib.h>
#include <stdio.h>
#ifdef __AVR__
const char * gs_error_string(int error)
{
/**
avr: const strings are not automatically stored in program space (see gs/util/pgm.h), and if stored
in program space, they require special formatting in logs (i.e. "%S").
So we settle for simple error string, with the error nnumber - no need to change log/(s)printf etc.
@note: solution is not 100% thread/task safe.
*/
static char buffer[15]; // large enough to always keep zero termination, due to no thread/task lock
snprintf(buffer, sizeof(buffer), "%d", error);
return buffer;
}
#else
const char * gs_error_string(int error)
{
switch (error) {
case GS_OK: return "GS_OK(0)";
case GS_ERROR_PERM: return GS_DEF2STRING(GS_ERROR_PERM) "(-1)";
case GS_ERROR_INTR: return GS_DEF2STRING(GS_ERROR_INTR) "(-4)";
case GS_ERROR_IO: return GS_DEF2STRING(GS_ERROR_IO) "(-5)";
case GS_ERROR_AGAIN: return GS_DEF2STRING(GS_ERROR_AGAIN) "(-11)";
case GS_ERROR_ALLOC: return GS_DEF2STRING(GS_ERROR_ALLOC) "(-12)";
case GS_ERROR_ACCESS: return GS_DEF2STRING(GS_ERROR_ACCESS) "(-13)";
case GS_ERROR_BUSY: return GS_DEF2STRING(GS_ERROR_BUSY) "(-16)";
case GS_ERROR_EXIST: return GS_DEF2STRING(GS_ERROR_EXIST) "(-17)";
case GS_ERROR_ARG: return GS_DEF2STRING(GS_ERROR_ARG) "(-22)";
case GS_ERROR_NOT_IMPLEMENTED: return GS_DEF2STRING(GS_ERROR_NOT_IMPLEMENTED) "(-38)";
case GS_ERROR_OVERFLOW: return GS_DEF2STRING(GS_ERROR_OVERFLOW) "(-75)";
case GS_ERROR_NOT_SUPPORTED: return GS_DEF2STRING(GS_ERROR_NOT_SUPPORTED) "(-95)";
case GS_ERROR_IN_USE: return GS_DEF2STRING(GS_ERROR_IN_USE) "(-98)";
case GS_ERROR_CONNECTION_RESET: return GS_DEF2STRING(GS_ERROR_CONNECTION_RESET) "(-104)";
case GS_ERROR_NO_BUFFERS: return GS_DEF2STRING(GS_ERROR_NO_BUFFERS) "(-105)";
case GS_ERROR_TIMEOUT: return GS_DEF2STRING(GS_ERROR_TIMEOUT) "(-110)";
case GS_ERROR_ALREADY_IN_PROGRESS: return GS_DEF2STRING(GS_ERROR_ALREADY_IN_PROGRESS) "(-114)";
case GS_ERROR_HANDLE: return GS_DEF2STRING(GS_ERROR_HANDLE) "(-2000)";
case GS_ERROR_NOT_FOUND: return GS_DEF2STRING(GS_ERROR_NOT_FOUND) "(-2001)";
case GS_ERROR_FULL: return GS_DEF2STRING(GS_ERROR_FULL) "(-2002)";
case GS_ERROR_RANGE: return GS_DEF2STRING(GS_ERROR_RANGE) "(-2003)";
case GS_ERROR_DATA: return GS_DEF2STRING(GS_ERROR_DATA) "(-2004)";
case GS_ERROR_UNKNOWN: return GS_DEF2STRING(GS_ERROR_UNKNOWN) "(-2005)";
case GS_ERROR_NO_DATA: return GS_DEF2STRING(GS_ERROR_NO_DATA) "(-2006)";
case GS_ERROR_STALE: return GS_DEF2STRING(GS_ERROR_STALE) "(-2007)";
case GS_ERROR_TYPE: return GS_DEF2STRING(GS_ERROR_TYPE) "(-2008)";
case GS_ERROR_AMBIGUOUS: return GS_DEF2STRING(GS_ERROR_AMBIGUOUS) "(-2009)";
case GS_ERROR_STATE: return GS_DEF2STRING(GS_ERROR_STATE) "(-2010)";
}
// as fallback we use standard POSIX error string
const int posix_error = abs(error);
return strerror(posix_error);
}
#endif
gs_error_t gs_error(int error)
{
return (abs(error) * -1);
}
#ifndef __AVR__
const char * error_string(int code)
{
switch (code) {
case E_NO_ERR:
return "No error";
case E_NO_DEVICE:
return "No device";
case E_MALLOC_FAIL:
return "Malloc fail";
case E_THREAD_FAIL:
return "Thread failure";
case E_NO_QUEUE:
return "No such queue";
case E_INVALID_BUF_SIZE:
return "Invalid buffer size";
case E_INVALID_PARAM:
return "Invalid paramater";
case E_NO_SS:
return "No such subsystem";
case E_GARBLED_BUFFER:
return "Rubbish in buffer";
case E_FLASH_ERROR:
return "FLASH error";
case E_BOOT_SER:
return "Thread boot fail: serial driver";
case E_BOOT_DEBUG:
return "Thread boot fail: debug console";
case E_BOOT_FLASH:
return "Thread boot fail: flash driver";
case E_NO_BUFFER:
return "No buffer";
default:
return "Unknown error";
}
}
#endif

77
gomspace/libutil/src/fletcher.c Normal file
View File

@ -0,0 +1,77 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/fletcher.h>
#include <gs/util/string.h>
#include <gs/util/pgm.h>
uint16_t gs_fletcher16_memcpy(const void * data_in, size_t count, void * (*memcpyfcn)(void *, const void *, size_t))
{
if (memcpyfcn == NULL) {
memcpyfcn = &memcpy;
}
uint16_t sum1 = 0;
uint16_t sum2 = 0;
if (data_in && count) {
const uint8_t * data = data_in;
for (unsigned int idx = 0; idx < count; ++idx) {
uint8_t byte;
(*memcpyfcn)(&byte, &data[idx], 1);
sum1 = (uint16_t)((sum1 + byte) % 255);
sum2 = (uint16_t)((sum2 + sum1) % 255);
}
}
return (uint16_t)((sum2 << 8) | sum1);
}
uint16_t gs_fletcher16_P(const void * data_in, size_t count)
{
uint16_t sum1 = 0;
uint16_t sum2 = 0;
if (data_in && count) {
const uint8_t * data = data_in;
for (unsigned int idx = 0; idx < count; ++idx) {
sum1 = (uint16_t)((sum1 + GS_PGM_UINT8_BY_PTR(data++)) % 255);
sum2 = (uint16_t)((sum2 + sum1) % 255);
}
}
return (uint16_t)((sum2 << 8) | sum1);
}
uint16_t gs_fletcher16(const void * data_in, size_t size)
{
uint16_t sum1 = 0;
uint16_t sum2 = 0;
if (data_in && size) {
const uint8_t * data = data_in;
for (unsigned int idx = 0; idx < size; ++idx) {
sum1 = (uint16_t)((sum1 + (*data++)) % 255);
sum2 = (uint16_t)((sum2 + sum1) % 255);
}
}
return (uint16_t)((sum2 << 8) | sum1);
}
void gs_fletcher16_init(gs_fletcher16_t * f16)
{
f16->sum1 = f16->sum2 = 0;
}
void gs_fletcher16_update(gs_fletcher16_t * f16, const void * data_in, size_t size)
{
if (f16 && data_in && size) {
const uint8_t * data = data_in;
for (unsigned int idx = 0; idx < size; ++idx) {
f16->sum1 = (uint16_t)((f16->sum1 + (*data++)) % 255);
f16->sum2 = (uint16_t)((f16->sum2 + f16->sum1) % 255);
}
}
}
uint16_t gs_fletcher16_finalize(gs_fletcher16_t * f16)
{
return (uint16_t)((f16->sum2 << 8) | f16->sum1);
}

111
gomspace/libutil/src/function_scheduler.c Normal file
View File

@ -0,0 +1,111 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/function_scheduler.h>
#include <gs/util/check.h>
#include <gs/util/time.h>
#include <gs/util/minmax.h>
#include <gs/util/check.h>
#include <stdlib.h>
#include <limits.h>
typedef struct {
// function to call
gs_function_scheduler_function_t function;
// function's user data
void * user_data;
// timeout in mS
uint32_t timeout_ms;
// last execution time in mS
uint32_t last_exec_ms;
} gs_function_scheduler_entry_t;
struct gs_function_scheduler {
// Max timeout in mS
uint32_t max_timeout_ms;
// allocated entries
unsigned int max_entries;
// entries
gs_function_scheduler_entry_t * entries;
};
GS_CHECK_STATIC_ASSERT(sizeof(int) >= 2, int_must_be_at_least_16bit);
gs_error_t gs_function_scheduler_create(uint32_t max_timeout_ms, unsigned int max_entries, gs_function_scheduler_t ** return_scheduler)
{
GS_CHECK_ARG(max_timeout_ms <= INT_MAX);
GS_CHECK_ARG(max_entries > 0);
GS_CHECK_ARG(return_scheduler != NULL);
gs_function_scheduler_entry_t * entries = calloc(max_entries, sizeof(*entries));
if (entries == NULL) {
return GS_ERROR_ALLOC;
}
gs_function_scheduler_t * scheduler = calloc(1, sizeof(*scheduler));
if (scheduler == NULL) {
free (entries);
return GS_ERROR_ALLOC;
}
scheduler->max_timeout_ms = max_timeout_ms;
scheduler->entries = entries;
scheduler->max_entries = max_entries;
*return_scheduler = scheduler;
return GS_OK;
}
gs_error_t gs_function_scheduler_destroy(gs_function_scheduler_t * scheduler)
{
GS_CHECK_HANDLE(scheduler);
free(scheduler->entries);
free(scheduler);
return GS_OK;
}
gs_error_t gs_function_scheduler_register_ms(gs_function_scheduler_t * scheduler,
uint32_t first_timeout_ms, gs_function_scheduler_function_t func, void * user_data)
{
GS_CHECK_HANDLE(scheduler != NULL);
GS_CHECK_ARG(func != NULL);
gs_function_scheduler_entry_t * entry = scheduler->entries;
for (unsigned int i = 0; i < scheduler->max_entries; ++i, ++entry) {
if (entry->function == NULL) {
entry->function = func;
entry->user_data = user_data;
entry->timeout_ms = first_timeout_ms;
entry->last_exec_ms = gs_time_rel_ms();
return GS_OK;
}
}
return GS_ERROR_FULL;
}
int gs_function_scheduler_execute_ms(gs_function_scheduler_t * scheduler)
{
uint32_t timeout_ms = 5000; // max timeout to ensure gs_time_rel_ms() works correctly (wrapping more than once is bad)
if (scheduler) {
timeout_ms = scheduler->max_timeout_ms;
uint32_t now_ms = gs_time_rel_ms();
gs_function_scheduler_entry_t * entry = scheduler->entries;
for (unsigned int i = 0; i < scheduler->max_entries; ++i, ++entry) {
if (entry->function) {
uint32_t elapsed = gs_time_diff_ms(entry->last_exec_ms, now_ms);
if (elapsed >= entry->timeout_ms) {
entry->timeout_ms = (entry->function)(entry->user_data);
entry->last_exec_ms = now_ms = gs_time_rel_ms();
elapsed = 0;
}
timeout_ms = gs_min(timeout_ms, (entry->timeout_ms - elapsed));
}
}
}
return (int)((timeout_ms < INT_MAX) ? timeout_ms : INT_MAX);
}

754
gomspace/libutil/src/gosh/command.c Normal file
View File

@ -0,0 +1,754 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include "command_local.h"
#include <stdlib.h>
#include <ctype.h>
#include <gs/util/vmem.h> // register commands
#include <gs/util/log.h> // register commands
#include <gs/util/string.h>
#include <gs/util/clock.h>
#include "../lock.h"
#define MAX_ARGC 30
#ifdef __AVR__
#include <avr/pgmspace.h>
#define cmd_strcmp strcmp_P
#define cmd_strncmp strncmp_P
#define cmd_strlen strlen_P
#define cmd_strcpy strcpy_P
#define cmd_read_ptr(ptr) ((void *) pgm_read_word(ptr))
#define cmd_read_int(ptr) pgm_read_word(ptr)
#else
#define cmd_strcmp strcmp
#define cmd_strncmp strncmp
#define cmd_strlen strlen
#define cmd_strcpy strcpy
#define cmd_read_ptr(ptr) *ptr
#define cmd_read_int(ptr) *ptr
#endif
// define common command log group.
static GS_LOG_GROUP(gs_command_log, "command", GS_LOG_CAT_COMMAND, LOG_DEFAULT_MASK | LOG_INFO_MASK);
#define LOG_DEFAULT gs_command_log
/**
Compile check that size of gs_command_t is multiplum of 4.
*/
GS_STATIC_ASSERT((sizeof(gs_command_t) % 4) == 0, gs_command_t_is_not_a_multiplum_of_4);
// Private context
typedef struct process_context {
// command context - must be first, as it is used to access private context (same address)
gs_command_context_t context;
// process function
gs_error_t (*process)(const gs_command_t * const cmds, int cmd_count, int arg_offset, struct process_context * pc);
// command error
gs_error_t error;
// only exact match (space after last argument)
bool requires_exact_match;
// first command match
const gs_command_t * cmd;
// number of hits when hunting commands, completion etc.
unsigned int hits;
// complete result
struct {
char * line;
size_t token_start;
} complete;
} private_context_t;
// command block
typedef struct gs_command_block {
//! Pointer to command block.
const gs_command_t * commands;
//! Number of commands in command block.
size_t count;
//! Reference to next command block.
struct gs_command_block * next;
} gs_command_block_t;
// commands
static gs_command_block_t g_commands;
// minimum stack size in bytes.
static size_t g_stack_size;
// command logger callback
static gs_command_log_t g_command_logger = NULL;
static void * g_command_logger_ctx = NULL;
static gs_error_t command_stdio_set_result(gs_command_context_t *ctx, const char *group, const char *key, const char *value)
{
static const char* printed_group_header = NULL;
/* Print Group header if Group string is non-empty */
if ((group != NULL) && (group[0] != '\0')) {
if (printed_group_header != group) {
fprintf(ctx->out, "%s:\r\n", group);
printed_group_header = group;
}
}
/* Print "<key>: <value>" if key string is non-empty */
if (key != NULL) {
if (key[0] != '\0') {
if ((group != NULL) && (group[0] != '\0')) {
fprintf(ctx->out, " %s: ", key);
} else {
fprintf(ctx->out, "%s: ", key);
}
}
}
fprintf(ctx->out, "%s\r\n", value);
return GS_OK;
}
gs_error_t gs_command_stdio_flush(gs_command_context_t *ctx)
{
fflush(ctx->out);
return GS_OK;
}
gs_error_t gs_command_stdio_wait_for_key(gs_command_context_t *ctx, int *ch, int timeout_ms)
{
return gs_stdio_getchar_timed(timeout_ms, ch);
}
static const gs_command_io_functions_t stdio_functions = {
.set_result = command_stdio_set_result,
.flush = gs_command_stdio_flush,
.wait_for_key = gs_command_stdio_wait_for_key
};
const char * gs_command_args(gs_command_context_t *ctx)
{
if (ctx->argc > 1) {
// find first matching argument (= starts with) - this is not 100% and doesn't handle arguments with spaces (quoted)
const char * arg = ctx->command_line;
while (arg && arg[0]) {
if (strncmp(arg, ctx->argv[1], strlen(ctx->argv[1])) == 0) {
return arg;
}
// skip argument
for (; *arg && (*arg != ' '); ++arg);
// skip spaces
// cppcheck-suppress redundantCondition
for (; *arg && (*arg == ' '); ++arg);
}
}
return "";
}
bool gs_command_build_argv(char *line, int *argc, char **argv, int max_argc)
{
// Skip spaces
for (; line && *line && isspace((unsigned int)*line); ++line);
*argc = 0;
argv[*argc] = line;
char quote = 0;
while (*line) {
// check for quote's: ' or "
if ((*line == '\'') || (*line == '\"')) {
if (quote == 0) {
quote = *line;
argv[*argc]++;
} else if (quote == *line) {
quote = 0;
*line = '\0';
}
}
// check for whitespace and no quotes active
else if (isspace((unsigned int)*line) && quote == 0) {
/* Delete space */
*line++ = '\0';
// skip spaces
for (; *line && isspace((unsigned int)*line); ++line);
/* If there is more data, we have another argument */
if (*line) {
(*argc)++;
if (*argc >= max_argc) {
return false;
}
argv[*argc] = line;
}
continue;
}
line++;
}
(*argc)++;
if (*argc >= max_argc) {
return false;
}
// According to C11 section 5.1.2.2.1, argv[argc] must be NULL
argv[*argc] = NULL;
// Check for invalid number of quotes
return (quote == 0) ? true : false;
}
static inline gs_error_t command_logger(const char *cmd_line, gs_error_t ret, gs_error_t cmd_ret, gs_timestamp_t ts, gs_timestamp_t te)
{
gs_lock_lock();
gs_command_log_t logger = g_command_logger;
void * log_ctx = g_command_logger_ctx;
gs_lock_unlock();
if (logger) {
return logger(cmd_line, ret, cmd_ret, ts, te, log_ctx);
}
return GS_OK;
}
static gs_error_t command_execute(const gs_command_t * const cmds, int cmd_count, int arg_offset, private_context_t * pc)
{
for (int i = 0; i < cmd_count; i++) {
const gs_command_t * cmd = &cmds[i];
if (cmd_strcmp(pc->context.argv[arg_offset], cmd->name) == 0) {
// check for sub-commands
const gs_command_t * list = (void*) cmd_read_ptr(&cmd->chain.list);
if (list) {
++arg_offset;
if (arg_offset >= pc->context.argc) {
return GS_ERROR_TYPE;
}
return command_execute(list, cmd_read_int(&cmd->chain.count), arg_offset, pc);
}
gs_command_handler_t handler = (void *) cmd_read_ptr(&cmd->handler);
if (handler == NULL) {
return GS_ERROR_NOT_IMPLEMENTED;
}
pc->context.argc -= arg_offset;
pc->context.argv = &pc->context.argv[arg_offset];
pc->context.command = cmd;
// check arguments - if specified
if (cmd->mandatory_args || cmd->optional_args) {
const int min_args = (cmd->mandatory_args == GS_COMMAND_NO_ARGS) ? 0 : cmd->mandatory_args;
const int args = (pc->context.argc - 1);
if (args < min_args) {
return GS_ERROR_ARG;
}
if (args > (min_args + cmd->optional_args)) {
return GS_ERROR_ARG;
}
}
pc->error = handler(&pc->context);
return GS_OK; // command was excecuted
}
}
return GS_ERROR_NOT_FOUND;
}
static gs_error_t command_process(private_context_t * pc)
{
const char * command = gs_string_skip_leading_spaces(pc->context.command_line);
// Make copy of string, because command parser mangles destroys it
const size_t command_len = strlen(command);
char command_copy[command_len + 1];
strcpy(command_copy, command);
if (command_len && command[command_len-1] == ' ') {
pc->requires_exact_match = true;
}
pc->context.optsp = 1;
pc->context.optind = 1;
pc->context.optopt = '?';
pc->context.command_line = command;
// split into arguments
char *argv[MAX_ARGC + 1];
if (gs_command_build_argv(command_copy, &pc->context.argc, argv, MAX_ARGC + 1) == false) {
return GS_ERROR_ARG;
}
pc->context.argv = argv;
gs_error_t error = GS_ERROR_NOT_FOUND;
for (const gs_command_block_t * block = &g_commands; block && (error == GS_ERROR_NOT_FOUND); block = block->next) {
if (block->commands) {
error = (pc->process)(block->commands, block->count, 0, pc);
}
}
return error;
}
gs_error_t gs_command_run(const char * command, gs_error_t * return_command_result)
{
return gs_command_execute_stdio(command, return_command_result);
}
gs_error_t gs_command_execute_stdio(const char * command, gs_error_t * return_command_result)
{
return gs_command_execute(command, return_command_result, stdout, &stdio_functions, NULL);
}
gs_error_t gs_command_execute(const char * command, gs_error_t * return_command_result, FILE *out,
const gs_command_io_functions_t *iof, void *iof_ctx)
{
command = gs_string_skip_leading_spaces(command);
GS_CHECK_ARG(gs_string_empty(command) == false);
private_context_t pc = {
.process = command_execute,
.error = GS_OK,
.context = {
.command_line = command,
.out = out,
.io_functions = iof,
.io_ctx = iof_ctx,
}
};
gs_timestamp_t tm_start, tm_end;
gs_clock_get_time(&tm_start);
gs_error_t error = command_process(&pc);
gs_clock_get_time(&tm_end);
command_logger(pc.context.command_line, error, pc.error, tm_start, tm_end);
if ((error == GS_OK) && return_command_result) {
*return_command_result = pc.error;
}
return error;
}
gs_error_t gs_command_set_output(gs_command_context_t *ctx, const char* group, const char* key, const char* value)
{
GS_CHECK_ARG(ctx);
if (ctx->io_functions && ctx->io_functions->set_result) {
return ctx->io_functions->set_result(ctx, group, key, value);
}
/* If no IO-function set - ignore the data and send Success */
return GS_OK;
}
gs_error_t gs_command_set_output_printf(gs_command_context_t *ctx, const char* group, const char* key, const char * format, ...)
{
GS_CHECK_ARG(ctx);
if (ctx->io_functions && ctx->io_functions->set_result)
{
va_list args;
va_start(args, format);
char value[256];
int size = vsnprintf(value, sizeof(value), format, args);
va_end(args);
/* Don't allow to set truncated results - Return error in this case */
if (size >= (int)sizeof(value)) {
return GS_ERROR_ALLOC;
}
return ctx->io_functions->set_result(ctx, group, key, value);
}
/* If no IO-function set - ignore the data and send Success */
return GS_OK;
}
gs_error_t gs_command_flush_output(gs_command_context_t *ctx)
{
GS_CHECK_ARG(ctx);
if (ctx->io_functions && ctx->io_functions->flush) {
return ctx->io_functions->flush(ctx);
}
/* If no IO-function set - ignore the data and send Success */
return GS_OK;
}
bool gs_command_wait_any_key(gs_command_context_t *ctx, int timeout_ms)
{
int ch;
gs_error_t ret = gs_command_wait_key(ctx, &ch, timeout_ms);
if (ret == GS_ERROR_TIMEOUT) {
return false;
}
/* Ensure that a commands handler will not stall if IO function if not available etc.
False will only be returned in case of a positive timeout */
return true;
}
gs_error_t gs_command_wait_key(gs_command_context_t *ctx, int* ch, int timeout_ms)
{
if (ctx && ctx->io_functions && ctx->io_functions->wait_for_key)
{
return ctx->io_functions->wait_for_key(ctx, ch, timeout_ms);
}
/* If no IO-function set set return GS_ERROR_HANDLE */
return GS_ERROR_HANDLE;
}
unsigned int gs_command_completer_add_token(gs_command_context_t * ctx, const char * token, bool exact)
{
private_context_t * pc = (private_context_t *) ctx;
char * line = &pc->complete.line[pc->complete.token_start];
if (token == NULL) {
// mark any pending partial token as exact
if ((line[0] == 0) || (pc->hits != 1)) {
return pc->hits;
}
exact = true;
}
if (exact) {
if (token) {
strcpy(line, token);
}
strcat(line, " ");
pc->complete.token_start = strlen(pc->complete.line);
pc->hits = 1;
} else {
if (pc->hits == 0) {
strcpy(line, token);
} else {
for (; *line && *token && (*line == *token); ++line, ++token);
*line = 0;
}
++pc->hits;
}
return pc->hits;
}
static unsigned int command_complete_add(private_context_t * pc, const gs_command_t * cmd, bool exact)
{
if (cmd) {
pc->cmd = cmd;
return gs_command_completer_add_token(&pc->context, cmd->name, exact);
} else {
return gs_command_completer_add_token(&pc->context, NULL, exact);
}
}
static gs_error_t command_complete(const gs_command_t * const cmds, int cmd_count, int arg_offset, private_context_t * pc)
{
if (arg_offset > 0) {
// command we are looking for must be in this block
pc->hits = 0;
}
pc->cmd = NULL;
bool exact_match = false;
for (int i = 0; i < cmd_count; i++) {
const gs_command_t * cmd = &cmds[i];
if (cmd_read_int(&cmd->mode) & GS_COMMAND_FLAG_HIDDEN) {
continue;
}
if (gs_string_empty(pc->context.argv[arg_offset])) {
// exceeding known token(s) - partial match
command_complete_add(pc, cmd, false);
continue;
}
if (pc->requires_exact_match || ((arg_offset+1) < pc->context.argc)) {
// must be an exact match
if (cmd_strcmp(pc->context.argv[arg_offset], cmd->name) == 0) {
command_complete_add(pc, cmd, true);
exact_match = true;
break;
}
} else if (cmd_strncmp(pc->context.argv[arg_offset], cmd->name,
strlen(pc->context.argv[arg_offset])) == 0) {
// partial match
command_complete_add(pc, cmd, false);
}
}
if (exact_match || ((arg_offset > 0) && (pc->hits == 1))) {
command_complete_add(pc, NULL, true);
if (strlen(pc->complete.line) > strlen(pc->context.command_line)) {
return GS_OK;
}
if (pc->cmd->chain.list) {
return command_complete(pc->cmd->chain.list, pc->cmd->chain.count, arg_offset+1, pc);
}
// command arguments
pc->context.argc -= arg_offset;
pc->context.argv = &pc->context.argv[arg_offset];
pc->context.command = pc->cmd;
// add the "already" completed ones
int arg_to_complete = 1;
for (; arg_to_complete < (pc->context.argc - 1); ++arg_to_complete) {
gs_command_completer_add_token(&pc->context, pc->context.argv[arg_to_complete], true);
}
// add the last - if its completed (space after)
if ((arg_to_complete < pc->context.argc) && pc->requires_exact_match) {
// cppcheck-suppress unreadVariable - not used on __AVR__ because it doesn't support 'completer'
gs_command_completer_add_token(&pc->context, pc->context.argv[arg_to_complete], true);
++arg_to_complete;
}
#if (__AVR__ == 0)
if (pc->cmd->completer) {
pc->hits = 0;
(pc->cmd->completer)(&pc->context, arg_to_complete);
} else
#endif
{
pc->hits = 1; // no completer - assume single hit
}
return GS_OK; // only used for breaking loop
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_command_complete(char *line, size_t max_line_length, FILE* out)
{
const size_t line_len = strlen(line);
char buffer[max_line_length];
buffer[0] = 0;
private_context_t pc = {
.process = command_complete,
.context = {
.command_line = line,
.out = out,
},
.complete = {
.line = buffer,
},
};
command_process(&pc);
gs_command_completer_add_token(&pc.context, NULL, true);
if (strlen(buffer) > line_len ) {
strcpy(line, buffer);
}
switch (pc.hits) {
case 0:
return GS_ERROR_NOT_FOUND;
case 1:
return GS_OK;
default:
return GS_ERROR_AMBIGUOUS;
}
}
static void command_help_print(const gs_command_t * const cmd, private_context_t * pc)
{
if (pc->hits == 1) {
if (cmd->help) {
fprintf(pc->context.out, "%s\r\n", cmd->help);
}
if (cmd->chain.count == 0) {
fprintf(pc->context.out, "usage: %s %s\r\n", cmd->name, cmd->usage ? cmd->usage : "");
} else {
for (unsigned int i = 0; i < cmd->chain.count; ++i) {
const gs_command_t * scmd = &cmd->chain.list[i];
if (scmd->mode & GS_COMMAND_FLAG_HIDDEN) {
continue;
}
fprintf(pc->context.out, " %-19s %s\r\n", scmd->name, scmd->help ? scmd->help : "");
}
}
} else {
fprintf(pc->context.out, " %-19s %s\r\n", cmd->name, cmd->help ? cmd->help : "");
}
}
static void command_help_hit(const gs_command_t * const cmd, private_context_t * pc)
{
pc->error = GS_OK;
++pc->hits;
if (pc->hits == 1) {
// single hit so far - hold off printing until we know if we get more
pc->cmd = cmd;
} else {
if (pc->cmd) {
command_help_print(pc->cmd, pc);
pc->cmd = NULL;
}
command_help_print(cmd, pc);
}
}
static gs_error_t command_help(const gs_command_t * const cmds, int cmd_count, int arg_offset, private_context_t * pc)
{
for (int i = 0; i < cmd_count; i++) {
const gs_command_t * cmd = &cmds[i];
if (cmd_read_int(&cmd->mode) & GS_COMMAND_FLAG_HIDDEN) {
continue;
}
if (pc->requires_exact_match || ((arg_offset+1) < pc->context.argc)) {
// must be an exact match
if (cmd_strcmp(pc->context.argv[arg_offset], cmd->name) == 0) {
const gs_command_t * list = (void*) cmd_read_ptr(&cmd->chain.list);
if (list && ((arg_offset+1) < pc->context.argc)) {
return command_help(list, cmd_read_int(&cmd->chain.count), arg_offset+1, pc);
}
command_help_hit(cmd, pc);
}
} else if (cmd_strncmp(pc->context.argv[arg_offset], cmd->name,
strlen(pc->context.argv[arg_offset])) == 0) {
command_help_hit(cmd, pc);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_command_show_help(const char * command, FILE* out)
{
private_context_t pc = {
.process = command_help,
.error = GS_ERROR_NOT_FOUND,
.context = {
.command_line = command,
.out = out,
}
};
gs_error_t error = command_process(&pc);
if (pc.cmd) {
command_help_print(pc.cmd, &pc);
error = GS_OK;
} else if ((error == GS_ERROR_NOT_FOUND) && pc.hits) {
error = GS_OK;
}
return error;
}
gs_error_t gs_command_register(const gs_command_t * commands, size_t count)
{
GS_CHECK_ARG(commands != NULL);
GS_CHECK_ARG(count > 0);
gs_error_t error = GS_OK;
gs_lock_lock();
{
// check if command block already installed
gs_command_block_t * last_block = NULL;
for (gs_command_block_t * block = &g_commands; block; block = block->next) {
if (block->commands) {
const gs_command_t * cmd = block->commands;
// loop through because it may be in the linked blocks
for (size_t i = 0; i < block->count; ++i, ++cmd) {
if (cmd == commands) {
error = GS_ERROR_EXIST;
break;
}
}
}
last_block = block;
}
if (error == GS_OK) {
gs_command_block_t * block = calloc(1, sizeof(*block));
if (block) {
// Insert command last, so lock isn't needed when accessing commands
block->commands = commands;
block->count = count;
block->next = NULL;
last_block->next = block;
} else {
error = GS_ERROR_ALLOC;
}
}
}
gs_lock_unlock();
return (error != GS_ERROR_EXIST) ? error : GS_OK;
}
size_t gs_command_get_stack_size(void)
{
return g_stack_size;
}
gs_error_t gs_command_init_no_commands(size_t stack_size)
{
g_stack_size = stack_size;
gs_error_t error = gs_lock_init();
if (error) {
return error;
}
gs_log_group_register(gs_command_log);
#if (__linux__ == 0)
// look up static linked commands - only embedded (= none linux) systems
gs_command_block_t * block = &g_commands;
extern volatile unsigned int __command_start __attribute__ ((__weak__));
extern volatile unsigned int __command_end __attribute__ ((__weak__));
if (&__command_start) {
block->count = ((ptrdiff_t)&__command_end - (ptrdiff_t)&__command_start) / sizeof(gs_command_t);
block->commands = (gs_command_t *) &__command_start;
}
#endif
return GS_OK;
}
gs_error_t gs_command_init(size_t stack_size)
{
gs_error_t error = gs_command_init_no_commands(stack_size);
if (error == GS_OK) {
// register default commands
gs_command_register_default_commands();
gs_log_register_commands();
}
return error;
}
gs_error_t gs_command_logger_default(const char* cmd_line, gs_error_t ret, gs_error_t cmd_ret, gs_timestamp_t t_start, gs_timestamp_t t_end, void *log_ctx)
{
(void)log_ctx;
timestamp_diff(&t_end, &t_start);
if (ret == GS_OK) {
log_info_group(gs_command_log, "'%s' returned '%s' ["
"t: <%04"PRIu32".%06"PRIu32">, dt: <%01"PRIu32".%06"PRIu32">]",
cmd_line, gs_error_string(cmd_ret),
t_start.tv_sec, t_start.tv_nsec/1000, t_end.tv_sec, t_end.tv_nsec/1000);
} else {
log_info_group(gs_command_log, "'%s' could not be run, returned '%s' ["
"t: <%04"PRIu32".%06"PRIu32">]",
cmd_line, gs_error_string(ret),
t_start.tv_sec, t_start.tv_nsec/1000);
}
return GS_OK;
}
gs_error_t gs_command_register_logger(gs_command_log_t log_cb, void *log_ctx)
{
gs_lock_lock();
g_command_logger = log_cb;
g_command_logger_ctx = log_ctx;
gs_lock_unlock();
return GS_OK;
}

35
gomspace/libutil/src/gosh/command_local.h Normal file
View File

@ -0,0 +1,35 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/gosh/command.h>
/**
Command I/O function - flush stdout.
*/
gs_error_t gs_command_stdio_flush(gs_command_context_t *ctx);
/**
Command I/O function - wait for a key.
*/
gs_error_t gs_command_stdio_wait_for_key(gs_command_context_t *ctx, int *ch, int timeout_ms);
/**
Complete command.
@param[in] line command line to complete
@param[in] max \a length (size)
@param[in] out output stream, e.g. stdout
*/
gs_error_t gs_command_complete(char *line, size_t max_line_length, FILE* out);
/**
Show help.
@param line command line to show help for.
@param out output stream, e.g. stdout
*/
gs_error_t gs_command_show_help(const char * command, FILE * out);
/**
Change console mode.
@param[in] mode console mode, 'cci'
@return_gs_error_t
*/
int gs_console_change_mode(const char * mode);

758
gomspace/libutil/src/gosh/console.c Normal file
View File

@ -0,0 +1,758 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
/**
@file
The console interface provides support for executing commands over stdout (typically a serial port).
The connection can run in 2 modes:
- normal, standard GOSH interface (Human Machine Interface), echo characters, prompt, etc.
- cci, Computer Computer Interface. Simple text interface, but with tagged output format - easier to parse by a computer.
*/
#include "console_local.h"
#include "command_local.h"
#include <gs/util/string.h>
#include <gs/util/stdio.h>
#include <gs/util/thread.h>
#include <gs/util/mutex.h>
#include <gs/util/time.h>
#include <gs/util/log/appender/console.h>
#include <stdlib.h>
#include <ctype.h>
#include <conf_util.h> // console defines set through Waf options
#if (__linux__)
#include <gs/util/linux/signal.h>
#include <gs/util/linux/command_line.h>
#include <unistd.h>
#include <termios.h>
#endif
/* Max history length (elements) */
#ifndef GS_CONSOLE_HISTORY_LEN
#define GS_CONSOLE_HISTORY_LEN 10
#endif
/* Max input length */
#ifndef GS_CONSOLE_INPUT_LEN
#define GS_CONSOLE_INPUT_LEN 100
#endif
#define CONTROL(X) ((X) - '@')
typedef enum {
CONSOLE_NORMAL = 0,
CONSOLE_ESCAPE = 1,
CONSOLE_PRE_ESCAPE = 2,
} console_escape_t;
static const char hash_prompt[] = "\033[1;30m # ";
static const char backspace_char = '\b';
static const char space_char = ' ';
static const char cr_char = '\r';
static const char nl_char = '\n';
static const char * user_prompt = "gosh";
static console_escape_t escape = CONSOLE_NORMAL;
#if (GS_CONSOLE_HISTORY_LEN > 0)
static int history_elements;
static int history_cur;
static int history_browse;
static char history[GS_CONSOLE_HISTORY_LEN][GS_CONSOLE_INPUT_LEN+1];
#endif
static int size;
static int pos;
static char buf[GS_CONSOLE_INPUT_LEN+1];
static gs_thread_t console_thread;
#if (__linux__)
static bool termios_changed;
static struct termios old_stdin;
static struct termios old_stdout;
#endif
static gs_mutex_t g_cci_lock; // Lock for protecting stdout for async output, e.g. log messages
static gs_error_t command_io_cci_set_result(gs_command_context_t *ctx, const char *group, const char *key, const char *value);
static const gs_command_io_functions_t cci_io_functions = {
.set_result = command_io_cci_set_result,
.flush = gs_command_stdio_flush,
.wait_for_key = gs_command_stdio_wait_for_key,
};
#define CCI_START_TAG "[X["
#define CCI_END_TAG "]X]"
static void gs_console_write(const char *str, int length)
{
for (int i = 0; i < length; i++) {
putchar(str[i]);
}
}
static void gs_console_prompt(void)
{
static const char col_start[] = "\033[1;32m";
static const char col_end[] = "\033[0m";
gs_console_write(col_start, sizeof(col_start) - 1);
gs_console_write(user_prompt, strlen(user_prompt));
gs_console_write(hash_prompt, sizeof(hash_prompt) - 1);
gs_console_write(col_end, sizeof(col_end) - 1);
}
void gs_console_set_prompt(const char * _prompt)
{
if (gs_string_empty(_prompt) == false) {
user_prompt = _prompt;
}
}
static void gs_console_reset(void)
{
pos = size = 0;
buf[pos] = 0;
gs_console_prompt();
}
static void gs_console_rewind(void)
{
int plen = strlen(hash_prompt) + strlen(user_prompt);
gs_console_write(&cr_char, 1);
while (size-- + plen) {
gs_console_write(&space_char, 1);
}
pos = size = 0;
gs_console_write(&cr_char, 1);
}
void gs_console_clear(void)
{
static const char clear[] = "\033[H\033[2J";
gs_console_write(clear, sizeof(clear) - 1);
gs_console_rewind();
gs_console_reset();
}
void gs_console_update(void)
{
gs_console_rewind();
gs_console_prompt();
pos = size = strlen(buf);
gs_console_write(buf, size);
}
#if (GS_CONSOLE_HISTORY_LEN > 0)
static void gs_console_history_add(void)
{
strncpy(history[history_cur], buf, GS_CONSOLE_INPUT_LEN);
history[history_cur][GS_CONSOLE_INPUT_LEN] = 0;
history_browse = 0;
history_cur = (history_cur + 1) % GS_CONSOLE_HISTORY_LEN;
if (history_elements < GS_CONSOLE_HISTORY_LEN) {
history_elements++;
}
}
static void gs_console_last_line(void)
{
if (history_elements < 1) {
return;
}
if (history_browse >= history_elements) {
return;
}
gs_console_rewind();
history_browse++;
strcpy(buf, history[(history_cur - history_browse + GS_CONSOLE_HISTORY_LEN) % GS_CONSOLE_HISTORY_LEN]);
gs_console_update();
}
static void gs_console_next_line(void)
{
if (history_elements < 1) {
return;
}
if (history_browse < 1) {
return;
}
gs_console_rewind();
history_browse--;
if (history_browse > 0) {
strcpy(buf, history[(history_cur - history_browse + GS_CONSOLE_HISTORY_LEN) % GS_CONSOLE_HISTORY_LEN]);
} else {
buf[0] = '\0';
}
gs_console_update();
}
#endif
static void gs_console_forward_char(void)
{
if (pos < size) {
gs_console_write(&buf[pos], 1);
pos++;
}
}
static void gs_console_end_of_line(void)
{
while (pos < size) {
gs_console_forward_char();
}
}
static void gs_console_backward_char(void)
{
if (pos > 0) {
pos--;
gs_console_write(&backspace_char, 1);
}
}
static void gs_console_beginning_of_line(void)
{
while (pos) {
gs_console_backward_char();
}
}
static void gs_console_newline(void)
{
gs_console_write(&cr_char, 1);
gs_console_write(&nl_char, 1);
}
static bool gs_command_not_empty(const char *ibuf)
{
while (*ibuf) {
if (!isblank((int) *ibuf++)) {
return true;
}
}
return false;
}
static void show_help(const char * command)
{
gs_error_t error = gs_command_show_help(command, stdout);
if (error) {
printf("Could not show help for \'%s\': %s (%d)\r\n", command, gs_error_string(error), error);
}
}
static void gs_console_execute(void)
{
gs_console_newline();
buf[GS_CONSOLE_INPUT_LEN] = 0; // ensure 0 termination
if (size > 0 && gs_command_not_empty(buf)) {
#if (GS_CONSOLE_HISTORY_LEN > 0)
gs_console_history_add();
#endif
gs_error_t result = GS_OK;
gs_error_t error = gs_command_execute_stdio(buf, &result);
if (error == GS_ERROR_TYPE) {
show_help(buf);
} else if (error == GS_ERROR_NOT_FOUND) {
printf("Unknown command \'%s\'\r\n", buf);
} else if (error == GS_ERROR_ARG) {
show_help(buf);
} else if (error) {
printf("Command \'%s\' did not execute: %s (%d)\r\n", buf, gs_error_string(error), error);
} else if (result == GS_ERROR_ARG) {
show_help(buf);
} else if (result) {
printf("Command \'%s\' executed, but returned error: %s (%d)\r\n", buf, gs_error_string(result), result);
}
}
gs_console_reset();
}
static void gs_console_complete(void)
{
/* We don't expand in the middle of a line */
if (size != pos) {
return;
}
const size_t old_buf_len = strlen(buf);
gs_error_t ret = gs_command_complete(buf, sizeof(buf), stdout);
if ((ret == GS_OK) && (old_buf_len == strlen(buf))) {
// completed (again) and no change - show help
ret = GS_ERROR_AMBIGUOUS;
}
switch (ret) {
case GS_ERROR_AMBIGUOUS:
gs_console_newline();
show_help(buf);
gs_console_update();
break;
case GS_OK:
gs_console_update();
break;
default:
case GS_ERROR_NOT_FOUND:
break;
}
}
static void gs_console_insert(char c)
{
int i;
int diff = size - pos;
if (size >= GS_CONSOLE_INPUT_LEN) {
return;
}
memmove(&buf[pos + 1], &buf[pos], diff);
buf[pos] = c;
gs_console_write(&buf[pos], diff + 1);
for (i = 0; i < diff; i++) {
gs_console_write(&backspace_char, 1);
}
size++;
pos++;
buf[size] = '\0';
}
static void gs_console_insert_overwrite(char c)
{
buf[pos++] = c;
if (pos > size) {
size++;
}
gs_console_write(&c, 1);
}
static void gs_console_delete(void)
{
int i;
int diff = size - pos;
/* Nothing to delete */
if (size == pos) {
return;
}
size--;
memmove(&buf[pos], &buf[pos + 1], diff - 1);
buf[size] = '\0';
gs_console_write(&buf[pos], diff - 1);
gs_console_write(&space_char, 1);
for (i = 0; i < diff; i++) {
gs_console_write(&backspace_char, 1);
}
}
static void gs_console_backspace(void)
{
if (pos < 1) {
return;
}
gs_console_backward_char();
gs_console_delete();
}
static void gs_console_kill_line(void)
{
int i;
int diff;
diff = size - pos;
if (diff == 0) {
return;
}
for (i = 0; i < diff; i++) {
gs_console_write(&space_char, 1);
}
for (i = 0; i < diff; i++) {
gs_console_write(&backspace_char, 1);
}
memset(&buf[pos], 0, diff);
size = pos;
}
static void gs_console_kill_line_from_beginning(void)
{
gs_console_beginning_of_line();
gs_console_kill_line();
}
static void gs_console_backward_kill_word(void)
{
while (pos > 0 && buf[pos - 1] == ' ') {
gs_console_backspace();
}
while (pos > 0 && buf[pos - 1] != ' ') {
gs_console_backspace();
}
}
static void gs_console_transpose_chars(void)
{
char c1, c2;
if (size < 2 || pos < 1) {
return;
}
if (pos == size) {
c1 = buf[pos - 1];
c2 = buf[pos - 2];
gs_console_backward_char();
gs_console_backward_char();
gs_console_insert_overwrite(c1);
gs_console_insert_overwrite(c2);
} else {
c1 = buf[pos];
c2 = buf[pos - 1];
gs_console_backward_char();
gs_console_insert_overwrite(c1);
gs_console_insert_overwrite(c2);
}
}
static void gs_console_normal(char c)
{
switch (c) {
case CONTROL('A'):
gs_console_beginning_of_line();
break;
case CONTROL('B'):
gs_console_backward_char();
break;
case CONTROL('C'):
// Either ignored or handled through signals
break;
case CONTROL('D'):
gs_console_delete();
break;
case CONTROL('E'):
gs_console_end_of_line();
break;
case CONTROL('F'):
gs_console_forward_char();
break;
case CONTROL('K'):
gs_console_kill_line();
break;
case CONTROL('L'):
gs_console_clear();
break;
#if (GS_CONSOLE_HISTORY_LEN > 0)
case CONTROL('N'):
gs_console_next_line();
break;
case CONTROL('P'):
gs_console_last_line();
break;
#endif
case CONTROL('T'):
gs_console_transpose_chars();
break;
case CONTROL('U'):
gs_console_kill_line_from_beginning();
break;
case CONTROL('W'):
gs_console_backward_kill_word();
break;
case CONTROL('Z'):
// We cannot suspend
break;
case CONTROL('H'):
case 0x7f:
gs_console_backspace();
break;
case '\r':
case '\n':
gs_console_execute();
break;
case '\t':
gs_console_complete();
break;
case '\033':
escape = CONSOLE_ESCAPE;
break;
default:
if (escape == CONSOLE_ESCAPE) {
if ((c == '[') || (c == 'O')) {
c = getchar();
if (c == 'F')
gs_console_end_of_line();
if (c == 'H')
gs_console_beginning_of_line();
#if (GS_CONSOLE_HISTORY_LEN > 0)
if (c == 'A')
gs_console_last_line();
if (c == 'B')
gs_console_next_line();
#endif
if (c == 'C')
gs_console_forward_char();
if (c == 'D')
gs_console_backward_char();
if (c == '1')
if (getchar() == '~')
gs_console_beginning_of_line();
if (c == '3')
if (getchar() == '~')
gs_console_delete();
}
escape = CONSOLE_NORMAL;
break;
}
if (isprint((unsigned char) c)) {
gs_console_insert(c);
}
break;
}
}
static gs_error_t command_io_cci_set_result(gs_command_context_t *ctx, const char *group, const char *key, const char *value)
{
gs_mutex_lock(g_cci_lock);
{
printf(CCI_START_TAG "cmd_res,%s,%s,%s" CCI_END_TAG, group, key, value);
}
gs_mutex_unlock(g_cci_lock);
return GS_OK;
}
static void gs_console_cci_log(gs_log_appender_t *appender, gs_log_level_t level, const gs_log_group_t *group, const gs_timestamp_t * ts, const char * format, va_list va)
{
va_list my_va;
va_copy(my_va, va);
gs_mutex_lock(g_cci_lock);
{
printf(CCI_START_TAG "log,%04"PRIu32".%06"PRIu32",%c,%s,", ts->tv_sec, ts->tv_nsec / 1000, gs_log_level_to_char(level), group->name);
vprintf(format, my_va);
printf(CCI_END_TAG "\r\n");
}
gs_mutex_unlock(g_cci_lock);
va_end(my_va);
}
static void gs_console_cci(char c)
{
switch (c) {
case CONTROL('C'):
case CONTROL('L'):
size = 0;
buf[0] = 0;
break;
case '\r':
case '\n':
buf[GS_CONSOLE_INPUT_LEN] = 0; // ensure 0 termination
if (size > 0 && gs_command_not_empty(buf)) {
static unsigned int seq; // simple sequence number keep incrementing
gs_mutex_lock(g_cci_lock);
++seq;
printf(CCI_START_TAG "cmd_exec_begin,%u,%s" CCI_END_TAG "\r\n", seq, buf);
gs_mutex_unlock(g_cci_lock);
gs_error_t result = GS_OK;
gs_error_t error = gs_command_execute(buf, &result, stdout, &cci_io_functions, NULL);
gs_mutex_lock(g_cci_lock);
printf(CCI_START_TAG "cmd_exec_end,%u,%d,%d" CCI_END_TAG "\r\n", seq, error, result);
gs_mutex_unlock(g_cci_lock);
}
size = 0;
buf[0] = 0;
break;
default:
if (isprint((unsigned char) c) && (size < GS_CONSOLE_INPUT_LEN)) {
buf[size++] = c;
buf[size] = 0;
}
break;
}
}
// Currrent mode handler, switch by sending command
static void (*console_handler)(char c) = gs_console_normal;
int gs_console_change_mode(const char * mode)
{
if (strcasecmp(mode, "cci") == 0) {
gs_error_t error = GS_OK;
if (console_handler != gs_console_cci) {
error = gs_mutex_create(&g_cci_lock);
if (error == GS_OK) {
gs_log_appender_console_set_cb(gs_console_cci_log);
console_handler = gs_console_cci; // change console handler
}
}
return error;
}
return GS_ERROR_NOT_SUPPORTED;
}
static void * gs_console_thread(void * param)
{
gs_console_reset();
while (1) {
char c = getchar();
console_handler(c);
}
gs_thread_exit(NULL);
}
gs_error_t gs_console_exit(void)
{
#if (__linux__)
if (termios_changed) {
tcsetattr(STDIN_FILENO, TCSANOW, &old_stdin);
tcsetattr(STDOUT_FILENO, TCSANOW, &old_stdout);
}
#endif
return GS_OK;
}
#if (__linux__)
static inline void exithandler(void)
{
printf("\n");
gs_console_exit();
}
#endif
static gs_error_t gs_console_init2(uint32_t flags)
{
#if (__linux__)
// save current stdio setting, for restoring when terminating process
tcgetattr(STDIN_FILENO, &old_stdin);
tcgetattr(STDOUT_FILENO, &old_stdout);
// change stdin settings
{
struct termios new = old_stdin;
new.c_iflag &= ~(IGNCR | ICRNL);
new.c_lflag &= ~(ECHO | ICANON | IEXTEN);
new.c_cc[VTIME]=0;
new.c_cc[VMIN]=1;
tcsetattr(STDIN_FILENO, TCSANOW, &new);
}
// change stdout settings
{
struct termios new = old_stdout;
new.c_iflag &= ~(IGNCR | ICRNL);
new.c_lflag &= ~(ECHO | ICANON | IEXTEN);
new.c_cc[VTIME]=0;
new.c_cc[VMIN]=1;
tcsetattr(STDOUT_FILENO, TCSANOW, &new);
}
termios_changed = true;
// add exit-handler to restore original termianl settings
atexit(exithandler);
// install signal handlers to ensure terminal settings are restored
if ((flags & GS_CONSOLE_F_NO_SIGNAL_HANDLER) == 0) {
// install signal handler(s) to ensure atexit() is called
gs_signal_catch(SIGTERM, NULL);
if (gs_command_line_ignore_ctrlc() == false) {
gs_signal_catch(SIGINT, NULL);
}
}
#endif
#if (__AVR__ == 0)
/** This is very important on AVR32 */
setvbuf(stdin, NULL, _IONBF, 0);
setvbuf(stdout, NULL, _IONBF, 0);
setvbuf(stderr, NULL, _IONBF, 0);
#endif
return GS_OK;
}
gs_error_t gs_console_init()
{
return gs_console_init2(0);
}
static gs_error_t _console_create_thread(gs_thread_priority_t priority, gs_thread_t * handle, uint32_t thread_create_flags)
{
gs_error_t error = gs_thread_create("CONSOLE",
gs_console_thread, NULL,
gs_command_get_stack_size(),
priority,
thread_create_flags,
handle);
if (error == GS_OK) {
// give thread a few moments to print prompt
gs_time_sleep_ms(20);
}
return error;
}
gs_error_t gs_console_create_thread(gs_thread_t * handle)
{
return _console_create_thread(GS_THREAD_PRIORITY_LOW, handle, 0);
}
gs_error_t gs_console_create_thread_with_priority(gs_thread_priority_t priority, gs_thread_t * handle)
{
return _console_create_thread(priority, handle, 0);
}
gs_error_t gs_console_start(const char * prompt, uint32_t flags)
{
if (console_thread) {
return GS_ERROR_EXIST;
}
gs_console_init2(flags);
gs_console_set_prompt(prompt);
return _console_create_thread(GS_THREAD_PRIORITY_LOW, &console_thread, GS_THREAD_CREATE_JOINABLE);
}
gs_error_t gs_console_stop(void)
{
if (console_thread == 0) {
return GS_ERROR_HANDLE;
}
#if (__linux__)
if (pthread_cancel(console_thread) != 0) {
return GS_ERROR_IO;
}
gs_error_t error = gs_thread_join(console_thread, NULL);
if (error == GS_OK) {
console_thread = 0;
}
return error;
#else
return GS_ERROR_NOT_SUPPORTED;
#endif
}

10
gomspace/libutil/src/gosh/console_local.h Normal file
View File

@ -0,0 +1,10 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/gosh/console.h>
/**
Change console mode.
@param[in] mode console mode, 'rgosh', 'normal'
@return_gs_error_t
*/
int gs_console_change_mode(const char * mode);

277
gomspace/libutil/src/gosh/default_commands.c Normal file
View File

@ -0,0 +1,277 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include "command_local.h"
#include "console_local.h"
#include <stdlib.h>
#if defined(__linux__)
#include <unistd.h>
#include <termios.h>
#endif
#include <gs/util/stdio.h>
#include <gs/util/clock.h>
#include <gs/util/time.h>
#include <gs/util/string.h>
#include <conf_util.h>
static int cmd_help(gs_command_context_t * context)
{
return gs_command_show_help(gs_command_args(context), context->out);
}
static int cmd_sleep(gs_command_context_t * context)
{
uint32_t sleep_ms;
gs_error_t error = gs_string_to_uint32(context->argv[1], &sleep_ms);
if (error) {
return error;
}
gs_time_sleep_ms(sleep_ms);
return GS_OK;
}
static int cmd_watch(gs_command_context_t * context, bool check_error)
{
uint32_t sleep_ms;
gs_error_t error = gs_string_to_uint32(context->argv[1], &sleep_ms);
if (error) {
return error;
}
fprintf(context->out, "Execution delay: %" PRIu32 "\r\n", sleep_ms);
char * new_command = strstr(gs_command_args(context), " ");
if (new_command == NULL) {
return GS_ERROR_ARG;
} else {
new_command = new_command + 1;
}
fprintf(context->out, "Command: %s\r\n", new_command);
while(1) {
gs_error_t cmd_result;
error = gs_command_execute(new_command, &cmd_result, context->out, context->io_functions, context->io_ctx);
if (error) {
return error;
}
if (check_error && cmd_result) {
return cmd_result;
}
if (gs_stdio_getchar_timed(sleep_ms, NULL) != GS_ERROR_TIMEOUT) {
break;
}
}
return GS_OK;
}
static int cmd_watch_nocheck(gs_command_context_t * context)
{
return cmd_watch(context, false);
}
static int cmd_watch_check(gs_command_context_t * context)
{
return cmd_watch(context, true);
}
#define CONTROL(X) ((X) - '@')
static int cmd_batch(gs_command_context_t * ctx)
{
char c;
int quit = 0, execute = 0;
unsigned int batch_size = 100;
unsigned int batch_input = 0;
unsigned int batch_count = 0;
char * batch[20] = {};
printf("Type each command followed by enter, hit ctrl+e to end typing, ctrl+x to cancel:\r\n");
/* Wait for ^q to quit. */
while (quit == 0) {
/* Get character */
c = getchar();
switch (c) {
/* CTRL + X */
case 0x18:
quit = 1;
break;
/* CTRL + E */
case 0x05:
execute = 1;
quit = 1;
break;
/* Backspace */
case CONTROL('H'):
case 0x7f:
if (batch_input > 0) {
putchar('\b');
putchar(' ');
putchar('\b');
batch_input--;
}
break;
case '\r':
putchar('\r');
putchar('\n');
if ((batch[batch_count] != NULL) && (batch_input < batch_size))
batch[batch_count][batch_input++] = '\r';
if ((batch[batch_count] != NULL) && (batch_input < batch_size))
batch[batch_count][batch_input++] = '\0';
batch_count++;
batch_input = 0;
if (batch_count == 20)
quit = 1;
break;
default:
putchar(c);
if (batch[batch_count] == NULL) {
batch[batch_count] = calloc(GS_CONSOLE_INPUT_LEN+1, 1);
}
if ((batch[batch_count] != NULL) && (batch_input < batch_size))
batch[batch_count][batch_input++] = c;
break;
}
}
if (execute) {
printf("\r\n");
for (unsigned int i = 0; i <= batch_count; i++) {
if (batch[i])
printf("[%02u] %s\r\n", i, batch[i]);
}
printf("Press ctrl+e to execute, or any key to abort\r\n");
c = getchar();
if (c != 0x05)
execute = 0;
}
/* Run/Free batch job */
for (unsigned int i = 0; i <= batch_count; i++) {
if (execute && batch[i]) {
printf("EXEC [%02u] %s\r\n", i, batch[i]);
gs_command_run(batch[i], NULL);
}
free(batch[i]);
}
return GS_OK;
}
#if defined(__linux__)
static int cmd_exit(gs_command_context_t * context)
{
gs_console_exit();
exit(EXIT_SUCCESS);
return GS_OK;
}
#endif
static int cmd_clock(gs_command_context_t * ctx)
{
if (ctx->argc > 1) {
gs_timestamp_t ts;
gs_error_t error = gs_clock_from_string(ctx->argv[1], &ts);
if (error) {
return GS_ERROR_ARG;
}
error = gs_clock_set_time(&ts);
if (error) {
fprintf(ctx->out, "Failed to set time, error=%s\r\n", gs_error_string(error));
return GS_ERROR_DATA;
}
}
timestamp_t clock;
gs_clock_get_monotonic(&clock);
fprintf(ctx->out, "monotonic: %10"PRIu32".%09"PRIu32" sec\r\n", clock.tv_sec, clock.tv_nsec);
gs_command_set_output_printf(ctx, "", "monotonic", "%10"PRIu32".%09"PRIu32"", clock.tv_sec, clock.tv_nsec);
char tbuf[25];
gs_clock_get_time(&clock);
gs_clock_to_iso8601_string(&clock, tbuf, sizeof(tbuf));
fprintf(ctx->out, "realtime: %10"PRIu32".%09"PRIu32" sec -> %s\r\n", clock.tv_sec, clock.tv_nsec, tbuf);
gs_command_set_output_printf(ctx, "", "realtime", "%10"PRIu32".%09"PRIu32"", clock.tv_sec, clock.tv_nsec);
return GS_OK;
}
static int cmd_console_mode(gs_command_context_t * ctx)
{
return gs_console_change_mode(ctx->argv[1]);
}
static const gs_command_t GS_COMMAND_ROOT cmd_default[] = {
{
.name = "help",
.help = "Show help",
.usage = "[command[ subcommand[ arg ...]]]",
.handler = cmd_help,
.optional_args = 100,
},{
.name = "sleep",
.help = "Sleep X ms",
.usage = "<mS>",
.handler = cmd_sleep,
.mandatory_args = 1,
},{
.name = "watch",
.help = "Run commands at intervals (abort with key)",
.usage = "<interval mS> <command> [arg ...]",
.handler = cmd_watch_nocheck,
.mandatory_args = 2,
.optional_args = 100,
},{
.name = "watch_check",
.help = "Like 'watch', but abort if command fails",
.usage = "<interval mS> <command [arg ...]>",
.handler = cmd_watch_check,
.mandatory_args = 2,
.optional_args = 100,
},{
.name = "batch",
.help = "Run multiple commands",
.handler = cmd_batch,
.mode = GS_COMMAND_FLAG_HIDDEN,
},{
.name = "clock",
.help = "Get/set system clock",
.usage = "[<sec.nsec> | <YYYY-MM-DDTHH:MM:SSZ>]",
.handler = cmd_clock,
.optional_args = 1,
},{
.name = "console_mode",
.help = "Console mode(s): cci",
.usage = "<mode>",
.handler = cmd_console_mode,
.mode = GS_COMMAND_FLAG_HIDDEN,
.mandatory_args = 1,
},
#if defined(__linux__)
{
.name = "exit",
.help = "Exit program",
.handler = cmd_exit,
},
#endif
};
gs_error_t gs_command_register_default_commands(void)
{
return GS_COMMAND_REGISTER(cmd_default);
}

55
gomspace/libutil/src/gosh/getopt.c Normal file
View File

@ -0,0 +1,55 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/gosh/command.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
int gs_command_getopt(gs_command_context_t *ctx, const char *opts)
{
int c;
char *cp;
if (ctx->optsp == 1) {
if (ctx->optind >= ctx->argc ||
ctx->argv[ctx->optind][0] != '-' ||
ctx->argv[ctx->optind][1] == '\0') {
return EOF;
} else if (!strcmp(ctx->argv[ctx->optind], "--")) {
ctx->optind++;
return EOF;
}
}
ctx->optopt = c = ctx->argv[ctx->optind][ctx->optsp];
if (c == ':' || (cp = strchr(opts, c)) == NULL) {
printf("illegal option -- %c\r\n", c);
if (ctx->argv[ctx->optind][++ctx->optsp] == '\0') {
ctx->optind++;
ctx->optsp = 1;
}
return '?';
}
if (*++cp == ':') {
if (ctx->argv[ctx->optind][ctx->optsp+1] != '\0') {
ctx->optarg = &ctx->argv[ctx->optind++][ctx->optsp+1];
} else if(++ctx->optind >= ctx->argc) {
printf("option requires an argument -- %c\r\n", c);
ctx->optsp = 1;
return '?';
} else {
ctx->optarg = ctx->argv[ctx->optind++];
}
ctx->optsp = 1;
} else {
if (ctx->argv[ctx->optind][++ctx->optsp] == '\0') {
ctx->optsp = 1;
ctx->optind++;
}
ctx->optarg = NULL;
}
return c;
}

92
gomspace/libutil/src/hexdump.c Normal file
View File

@ -0,0 +1,92 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/hexdump.h>
#include <gs/util/string.h>
#include <gs/util/stdio.h>
#include <gs/util/log.h>
static void print_disp_addr02(FILE * out, uintptr_t disp_addr)
{
fprintf(out, "0x%02"PRIx32" : ", (uint32_t) disp_addr);
}
static void print_disp_addr04(FILE * out, uintptr_t disp_addr)
{
fprintf(out, "0x%04"PRIx32" : ", (uint32_t) disp_addr);
}
static void print_disp_addrxx(FILE * out, uintptr_t disp_addr)
{
#if defined(PRIx64)
fprintf(out, "0x%08"PRIx64" : ", (uint64_t) disp_addr);
#else
fprintf(out, "0x%08"PRIx32" : ", (uint32_t) disp_addr);
#endif
}
void gs_hexdump_to_stream(const void * in_src, size_t len, const void * in_disp_addr, FILE* out)
{
volatile const uint8_t * src = in_src;
uintptr_t disp_addr = GS_TYPES_PTR2UINT(in_disp_addr);
const uintptr_t end_disp_addr = disp_addr + len;
// work-rounds for not printing NIL (if address 0), align addresses, not supporting %zx, %*x or %08p on all platforms
void (*print_addr)(FILE * out, uintptr_t disp_addr);
if (end_disp_addr <= 0xff) {
print_addr = print_disp_addr02;
} else if (end_disp_addr <= 0xffff) {
print_addr = print_disp_addr04;
} else {
print_addr = print_disp_addrxx;
}
print_addr(out, disp_addr);
size_t i = 0;
size_t j = 0;
size_t k = 0;
char text[17];
for(; i < len; ++i) {
const uint8_t ch = *src++;
++disp_addr;
// hex
fprintf(out, "%02x ", ch);
++j;
if (j == 8) {
fprintf(out, " ");
}
// printable
if ((ch < 32) || (ch > 126)) {
text[k] = '.';
} else {
text[k] = (char) ch;
}
++k;
text[k] = 0;
// newline?
if(j >= 16) {
fprintf(out, "|%-16.16s|\r\n", text);
j = 0;
k = 0;
text[k] = 0;
if (i < (len - 1)) {
print_addr(out, disp_addr);
}
}
}
if ((i == 0) || (i % 16)) {
if (j) {
// something was printed - show textual
for (; j < 16; j++) {
if (j == 7) {
fprintf(out, " ");
}
fprintf(out, " ");
}
fprintf(out, "|%-16.16s|", text);
}
fprintf(out, "\r\n");
}
}

34
gomspace/libutil/src/linux/argp.c Normal file
View File

@ -0,0 +1,34 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/linux/argp.h>
#include <gs/util/linux/exitcode.h>
#include <gs/util/string.h>
void gs_argp_parse(const struct argp * argp,
int argc, char ** argv,
unsigned int flags, int * return_arg_index,
const char * revision)
{
if (gs_string_empty(revision) == false) {
argp_program_version = revision;
}
int arg_index = 0;
int res = argp_parse(argp, argc, argv, 0, &arg_index, 0);
if (res) {
printf("Failed to parse argument/option (result: %d)\n", res);
exit(GS_EXITCODE_USAGE);
}
if ((return_arg_index == NULL) && (arg_index < argc)) {
// application doesn't expect unhandled arguments
for (int i = arg_index; i < argc; ++i) {
printf("Unhandled/unknown argument: [%s]\n", argv[i]);
}
exit(GS_EXITCODE_USAGE);
}
if (return_arg_index) {
*return_arg_index = arg_index;
}
}

68
gomspace/libutil/src/linux/clock.c Normal file
View File

@ -0,0 +1,68 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/clock.h>
#include <gs/util/rtc.h>
#include <time.h>
#include <errno.h>
void gs_clock_get_time(gs_timestamp_t * time)
{
struct timespec now;
clock_gettime(CLOCK_REALTIME, &now);
time->tv_sec = (uint32_t) now.tv_sec;
time->tv_nsec = (uint32_t) now.tv_nsec;
}
gs_error_t gs_clock_set_time(const gs_timestamp_t * time)
{
struct timespec now;
now.tv_sec = time->tv_sec;
now.tv_nsec = time->tv_nsec;
int res = clock_settime(CLOCK_REALTIME, &now);
if (res != 0) {
return gs_error(errno);
}
gs_error_t error = GS_OK;
if (gs_rtc_supported() == GS_OK) {
error = gs_rtc_set_time(time);
}
return error;
}
void gs_clock_get_monotonic(gs_timestamp_t * time)
{
struct timespec now;
clock_gettime(CLOCK_MONOTONIC, &now);
time->tv_sec = (uint32_t) now.tv_sec;
time->tv_nsec = (uint32_t) now.tv_nsec;
}
uint64_t gs_clock_get_nsec(void)
{
struct timespec now;
clock_gettime(CLOCK_MONOTONIC, &now);
return (((uint64_t)now.tv_sec) * GS_TIMESTAMP_NSEC_PER_SEC) + ((uint64_t)now.tv_nsec);
}
/**
Required by libcsp.
Proto-typed in ./libcsp/include/csp/arch/csp_clock.h, but with different argumet!
__attribute__((weak)) extern void clock_get_time(csp_timestamp_t * time);
__attribute__((weak)) extern void clock_set_time(csp_timestamp_t * time);
*/
void clock_get_time(gs_timestamp_t * time)
{
gs_clock_get_time(time);
}
void clock_set_time(const gs_timestamp_t * time)
{
gs_clock_set_time(time);
}

76
gomspace/libutil/src/linux/command_line.c Normal file
View File

@ -0,0 +1,76 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/linux/command_line.h>
#include <gs/util/linux/signal.h>
#include <gs/util/string.h>
#define KEY_IGNORE_CTRLC 200
static bool ignore_ctrlc;
static int parser(int key, char *arg, struct argp_state *state)
{
switch (key) {
case KEY_IGNORE_CTRLC:
ignore_ctrlc = true;
gs_signal_ignore(SIGINT);
break;
case 'h':
argp_help(state->root_argp, state->out_stream, ARGP_HELP_STD_HELP, state->name);
exit(0);
break;
default:
return ARGP_ERR_UNKNOWN;
}
return 0;
}
static const struct argp_option options[] = {
{
.name = "ignore-ctrlc",
.key = KEY_IGNORE_CTRLC,
.doc = "Ignore/disable CTRL-C"
},
{0}
};
static const struct argp argp_console = {.options = options, .parser = parser};
const struct argp_child gs_console_command_line_ignore_ctrlc_argp = {.argp = &argp_console};
bool gs_command_line_ignore_ctrlc(void)
{
return ignore_ctrlc;
}
static const struct argp_option help_options[] = {
{
.name = "help",
.key = 'h',
.doc = "Give this help list"
},
{0}
};
static const struct argp gs_argp_help = {.options = help_options, .parser = parser};
const struct argp_child gs_help_command_line_argp = {.argp = &gs_argp_help};
const char * gs_command_line_program_name(const char * argv)
{
if (gs_string_empty(argv) == false) {
const char * name = strrchr(argv, '/');
if (name) {
// skip slash
++name;
if (gs_string_empty(name) == false) {
return name;
}
} else {
return argv;
}
}
return "<program>";
}

28
gomspace/libutil/src/linux/cwd.c Normal file
View File

@ -0,0 +1,28 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/unistd.h>
#include <unistd.h>
#include <errno.h>
gs_error_t gs_getcwd(char * buf, size_t bufsize)
{
if (buf && bufsize) {
char * wd = getcwd(buf, bufsize);
if (wd) {
return GS_OK;
}
switch(errno) {
case ENAMETOOLONG:
case ERANGE:
return GS_ERROR_RANGE;
case EACCES:
case ENOENT:
return GS_ERROR_NOT_FOUND;
default:
break;
}
}
return GS_ERROR_ARG;
}

22
gomspace/libutil/src/linux/delay.c Normal file
View File

@ -0,0 +1,22 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/delay.h>
#include <unistd.h>
#include <gs/util/time.h>
void gs_delay_us(uint32_t time_us)
{
uint64_t ns = time_us;
ns *= 1000LL;
gs_time_sleep_ns(ns);
}
uint16_t gs_delay_ts_get(void)
{
return 0;
}
void gs_delay_from_ts(uint16_t ts, uint16_t delay)
{
}

308
gomspace/libutil/src/linux/drivers/can/can.c Normal file
View File

@ -0,0 +1,308 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/drivers/can/can.h>
#include <gs/util/thread.h>
#include <gs/util/log.h>
#include <gs/util/string.h>
#include <gs/util/time.h>
#include <unistd.h>
#include <errno.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
typedef struct {
// true if handle is in use
bool inuse;
// opened socket
int can_socket;
// receiver thread
gs_thread_t rxthread;
// received data callback
gs_can_rxdata_callback_t rx_callback;
void * user_data;
} gs_can_handle_t;
#define MAX_CAN_HANDLES 10
static gs_can_handle_t can_handles[MAX_CAN_HANDLES];
static int gs_can_alloc_handle(void)
{
int handle_id;
for (handle_id = 0; (handle_id < MAX_CAN_HANDLES) && (can_handles[handle_id].inuse == true); ++handle_id);
if (handle_id < MAX_CAN_HANDLES) {
gs_can_handle_t * handle = &can_handles[handle_id];
memset(handle, 0, sizeof(*handle));
handle->inuse = true;
handle->can_socket = -1;
}
return handle_id;
}
static inline gs_can_handle_t * gs_can_handle(uint8_t hdl)
{
if (hdl >= MAX_CAN_HANDLES) {
return NULL;
}
if (can_handles[hdl].inuse == false) {
return NULL;
}
return &can_handles[hdl];
}
static void * gs_can_rx_thread(void * parameter)
{
int hdl = (int) GS_TYPES_PTR2INT(parameter);
log_debug("%s: running, hdl: %d", __FUNCTION__, hdl);
gs_can_handle_t * handle = gs_can_handle(hdl);
if (handle == NULL) {
log_error("%s: CAN handle: %d is invalid or not opened", __FUNCTION__, hdl);
gs_thread_exit(NULL);
}
while (1) {
/* Read CAN frame */
struct can_frame frame;
ssize_t nbytes = read(handle->can_socket, &frame, sizeof(frame));
if (nbytes < 0) {
log_error("%s: read() on socket failed, error: %s", __FUNCTION__, strerror(errno));
continue;
}
if (nbytes != sizeof(frame)) {
log_warning("%s: read() returned incomplete CAN frame of %d bytes - ignoring frame", __FUNCTION__, (int) nbytes);
continue;
}
/* Frame type */
if (frame.can_id & (CAN_ERR_FLAG | CAN_RTR_FLAG)) {
/* Drop error and remote frames */
log_warning("%s: discarding ERR/RTR frame, can_id: 0x%x", __FUNCTION__, frame.can_id);
continue;
}
const bool extId = (frame.can_id & CAN_EFF_FLAG) ? true : false;
if (extId) {
frame.can_id &= CAN_EFF_MASK;
} else {
frame.can_id &= CAN_SFF_MASK;
}
handle->rx_callback(hdl, frame.can_id, extId, frame.data, frame.can_dlc, gs_time_rel_ms(), handle->user_data, false);
}
/* We should never reach this point */
return NULL;
}
static gs_error_t gs_can_send(uint8_t hdl, uint32_t canMsgId, bool extended, const void * data, size_t data_size, int timeout_ms)
{
gs_can_handle_t * handle = gs_can_handle(hdl);
if (handle == NULL) {
return GS_ERROR_HANDLE;
}
if ((data == NULL) || (data_size > 8)) {
log_error("%s: invalid data: %p, data_size: %u", __FUNCTION__, (void*) data, (unsigned int) data_size);
return GS_ERROR_ARG;
}
struct can_frame frame;
memset(&frame, 0, sizeof(frame));
frame.can_id = canMsgId;
if (extended) {
frame.can_id |= CAN_EFF_FLAG;
}
memcpy(frame.data, data, data_size);
frame.can_dlc = (uint8_t) data_size;
const int DELAY_MS = 10;
while (write(handle->can_socket, &frame, sizeof(frame)) != sizeof(frame)) {
if ((timeout_ms > 0) && (errno == ENOBUFS)) {
// Wait a bit and try again
gs_thread_sleep_ms(DELAY_MS);
timeout_ms -= DELAY_MS;
} else {
gs_error_t gserror = gs_error(errno);
log_error("%s: write() failed, error: %s", __FUNCTION__, gs_error_string(gserror));
return gserror;
}
}
return GS_OK;
}
gs_error_t gs_can_send_standard(uint8_t hdl, uint32_t canMsgId, const void * data, size_t data_size, int timeout_ms)
{
return gs_can_send(hdl, canMsgId, false, data, data_size, timeout_ms);
}
gs_error_t gs_can_send_extended(uint8_t hdl, uint32_t canExtMsgId, const void * data, size_t data_size, int timeout_ms)
{
return gs_can_send(hdl, canExtMsgId, true, data, data_size, timeout_ms);
}
static void gs_can_close(gs_can_handle_t * handle)
{
if (handle->can_socket >= 0) {
close(handle->can_socket);
}
// free instance - must be the last thing done, no lock needed
handle->inuse = false;
}
gs_error_t gs_can_open(const char * ifname, int * return_handle)
{
if ((ifname == NULL) || (ifname[0] == 0) || (return_handle == NULL)) {
log_error("%s: invalid CAN interface name", __FUNCTION__);
return GS_ERROR_ARG;
}
int handle_id = gs_can_alloc_handle();
if (handle_id >= MAX_CAN_HANDLES) {
log_error("%s: no free handles", __FUNCTION__);
return GS_ERROR_FULL;
}
gs_can_handle_t * handle = &can_handles[handle_id];
/* Create socket */
if ((handle->can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
gs_error_t gserror = gs_error(errno);
log_error("%s: socket() failed, error: %s", __FUNCTION__, gs_error_string(gserror));
gs_can_close(handle);
return gserror;
}
/* Locate interface */
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
strncpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name) - 1);
if (ioctl(handle->can_socket, SIOCGIFINDEX, &ifr) < 0) {
gs_error_t gserror = gs_error(errno);
log_error("%s: ioctl(ifname: [%s]) failed, error: %s", __FUNCTION__, ifr.ifr_name, gs_error_string(gserror));
gs_can_close(handle);
return gserror;
}
/* Bind the socket to CAN interface */
struct sockaddr_can addr;
memset(&addr, 0, sizeof(addr));
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(handle->can_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
gs_error_t gserror = gs_error(errno);
log_error("%s: bind() failed, error: %s", __FUNCTION__, gs_error_string(gserror));
gs_can_close(handle);
return gserror;
}
*return_handle = handle_id;
return GS_OK;
}
static gs_error_t gs_can_set_filter_mask(uint8_t hdl, bool extended, uint32_t canMsgId, uint32_t mask, gs_can_rxdata_callback_t rx_callback, void * user_data)
{
gs_can_handle_t * handle = gs_can_handle(hdl);
if (handle == NULL) {
return GS_ERROR_HANDLE;
}
if (extended) {
if ((canMsgId > CAN_EFF_MASK) || (mask > CAN_EFF_MASK)) {
return GS_ERROR_ARG;
}
} else {
if ((canMsgId > CAN_SFF_MASK) || (mask > CAN_SFF_MASK)) {
return GS_ERROR_ARG;
}
}
handle->rx_callback = rx_callback;
handle->user_data = user_data;
struct can_filter filter;
filter.can_id = canMsgId;
filter.can_mask = mask;
if (extended == false) {
filter.can_mask |= (CAN_EFF_MASK & ~CAN_SFF_MASK);
}
if (setsockopt(handle->can_socket, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
gs_error_t gserror = gs_error(errno);
log_error("%s: setsockopt(id: 0x%x, mask: 0x%x) failed, error: %s", __FUNCTION__, canMsgId, mask, gs_error_string(gserror));
return gserror;
}
return GS_OK;
}
gs_error_t gs_can_set_standard_filter_mask(uint8_t hdl, uint32_t canMsgId, uint32_t mask, gs_can_rxdata_callback_t rx_callback, void * user_data)
{
return gs_can_set_filter_mask(hdl, false, canMsgId, mask, rx_callback, user_data);
}
gs_error_t gs_can_set_extended_filter_mask(uint8_t hdl, uint32_t canExtMsgId, uint32_t mask, gs_can_rxdata_callback_t rx_callback, void * user_data)
{
return gs_can_set_filter_mask(hdl, true, canExtMsgId, mask, rx_callback, user_data);
}
gs_error_t gs_can_start(uint8_t hdl)
{
gs_can_handle_t * handle = gs_can_handle(hdl);
if (handle == NULL) {
return GS_ERROR_HANDLE;
}
if (handle->rxthread) {
return GS_OK;
}
/* Create receiver thread */
gs_error_t gserror = gs_thread_create("rxcan", gs_can_rx_thread, GS_TYPES_INT2PTR(hdl), 0, GS_THREAD_PRIORITY_HIGH, 0, &handle->rxthread);
if (gserror) {
log_error("s: gs_thread_create() failed, error: %s", gs_error_string(gserror));
return gserror;
}
return GS_OK;
}
gs_error_t gs_can_stop(uint8_t hdl)
{
gs_can_handle_t * handle = gs_can_handle(hdl);
if (handle == NULL) {
return GS_ERROR_HANDLE;
}
return GS_ERROR_NOT_IMPLEMENTED;
}
gs_error_t gs_can_error_state(uint8_t hdl, bool * restart_required)
{
gs_can_handle_t * handle = gs_can_handle(hdl);
if (handle == NULL) {
return GS_ERROR_HANDLE;
}
if (restart_required) {
*restart_required = false;
}
// missing error state check on CAN layer
return GS_OK;
}

102
gomspace/libutil/src/linux/drivers/gpio/gpio.c Normal file
View File

@ -0,0 +1,102 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/check.h>
#include <gs/util/linux/drivers/gpio/gpio.h>
#define MAX_DRIVERS 20
typedef struct {
gs_gpio_driver_entry_t entry;
bool in_use;
} gs_gpio_driver_handle_t;
static gs_gpio_driver_handle_t gpio_drivers[MAX_DRIVERS];
static uint8_t max_index_in_use = 0;
static inline gs_gpio_driver_entry_t * gs_find_driver_entry(gs_gpio_t * gpio)
{
gs_gpio_driver_handle_t * handle;
for (int i = max_index_in_use; i >= 0; i--) {
handle = &gpio_drivers[i];
if (((gpio->pin == handle->entry.pin) || (handle->entry.pin == GS_GPIO_ALL_PINS)) &&
((gpio->port == handle->entry.port) || (handle->entry.port == GS_GPIO_ALL_PORTS)) &&
(handle->in_use == true)) {
return &handle->entry;
}
}
return NULL;
}
gs_error_t gs_gpio_get(gs_gpio_t gpio, bool *value)
{
gs_gpio_driver_entry_t * driver_entry = gs_find_driver_entry(&gpio);
if (driver_entry) {
if (driver_entry->driver->get_handler) {
return driver_entry->driver->get_handler(gpio, value, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
bool gs_gpio_get_nc(gs_gpio_t gpio)
{
gs_gpio_driver_entry_t * driver_entry = gs_find_driver_entry(&gpio);
if (driver_entry) {
if (driver_entry->driver->get_nc_handler) {
return driver_entry->driver->get_nc_handler(gpio, driver_entry->driver_data);
}
}
return false;
}
gs_error_t gs_gpio_set(gs_gpio_t gpio, bool value)
{
gs_gpio_driver_entry_t * driver_entry = gs_find_driver_entry(&gpio);
if (driver_entry) {
if (driver_entry->driver->set_handler) {
return driver_entry->driver->set_handler(gpio, value, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
void gs_gpio_set_nc(gs_gpio_t gpio, bool value)
{
gs_gpio_driver_entry_t * driver_entry = gs_find_driver_entry(&gpio);
if (driver_entry) {
if (driver_entry->driver->set_nc_handler) {
driver_entry->driver->set_nc_handler(gpio, value, driver_entry->driver_data);
}
}
}
gs_error_t gs_gpio_init_as_interrupt(gs_gpio_t gpio, const gs_interrupt_conf_t * conf)
{
gs_gpio_driver_entry_t * driver_entry = gs_find_driver_entry(&gpio);
if (driver_entry) {
if (driver_entry->driver->init_as_interrupt_handler) {
return driver_entry->driver->init_as_interrupt_handler(gpio, conf, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_gpio_register_driver(const gs_gpio_driver_entry_t * driver_entry)
{
GS_CHECK_ARG(driver_entry != NULL);
GS_CHECK_ARG(driver_entry->driver != NULL);
gs_gpio_driver_handle_t * handle;
for (uint8_t i = 0; i < MAX_DRIVERS; i++) {
handle = &gpio_drivers[i];
if (handle->in_use == false) {
handle->entry = *driver_entry;
handle->in_use = true;
max_index_in_use = i;
return GS_OK;
}
}
/* Not enough space in buffer */
return GS_ERROR_FULL;
}

145
gomspace/libutil/src/linux/drivers/gpio/gpio_sysfs.c Normal file
View File

@ -0,0 +1,145 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
/**
@file
@brief GPIO Implementation for Linux of the GPIO API in libutil.
The GPIO driver provides a simple interface toward driving HW GPIO's.
*/
#include <gs/util/linux/drivers/gpio/gpio_sysfs.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <gs/util/log.h>
#include <gs/util/linux/sysfs_helper.h>
gs_error_t gs_gpio_sysfs_initialize(gs_gpio_t gpio, bool output,bool init_value, bool active_low)
{
char gpio_pin_str[6];
snprintf(gpio_pin_str, sizeof(gpio_pin_str), "%d", gpio.pin);
/* Try to unexport first */
gs_sysfs_write_file("/sys/class/gpio/unexport", gpio_pin_str);
if (gs_sysfs_write_file("/sys/class/gpio/export", gpio_pin_str) != GS_OK)
{
log_warning("failed to export GPIO %s: %s", gpio_pin_str, strerror(errno));
return GS_ERROR_NOT_SUPPORTED;
}
char gpio_sys_fname[128];
snprintf(gpio_sys_fname, sizeof(gpio_sys_fname), "/sys/class/gpio/gpio%d/active_low", gpio.pin);
const char * active_low_str = active_low ? "1" : "0";
if (gs_sysfs_write_file(gpio_sys_fname, active_low_str) != GS_OK)
{
log_warning("failed to set GPIO %d active_low: %s", gpio.pin, strerror(errno));
return GS_ERROR_NOT_SUPPORTED;
}
snprintf(gpio_sys_fname, sizeof(gpio_sys_fname), "/sys/class/gpio/gpio%d/direction", gpio.pin);
/* Glitch-free output set (high/low makes pin an output and sets value to 1/0 respectively)*/
const char * dir = output ? (init_value ? "high" : "low") : "in";
if (gs_sysfs_write_file(gpio_sys_fname, dir) != GS_OK)
{
log_warning("failed to set GPIO %d direction: %s", gpio.pin, strerror(errno));
return GS_ERROR_NOT_SUPPORTED;
}
return GS_OK;
}
gs_error_t gs_gpio_sysfs_get(gs_gpio_t gpio, bool *value, void * driver_data)
{
char gpio_sys_fname[128];
snprintf(gpio_sys_fname, sizeof(gpio_sys_fname), "/sys/class/gpio/gpio%d/value", gpio.pin);
if (access(gpio_sys_fname, R_OK) != 0)
{
log_error("GPIO %d not initialized - Can't read the input.", gpio.pin);
return GS_ERROR_ACCESS;
}
char value_str[10];
gs_error_t ret = gs_sysfs_read_file(gpio_sys_fname, value_str, sizeof(value_str));
if (ret == GS_OK)
{
if (strcmp(value_str, "1") == 0)
*value = true;
else
*value = false;
}
return ret;
}
bool gs_gpio_sysfs_get_nc(gs_gpio_t gpio, void * driver_data)
{
char gpio_sys_fname[128];
snprintf(gpio_sys_fname, sizeof(gpio_sys_fname), "/sys/class/gpio/gpio%d/value", gpio.pin);
if (access(gpio_sys_fname, R_OK) != 0)
{
log_error("GPIO %d not initialized - Can't read the input.", gpio.pin);
return 0;
}
char value_str[10];
gs_sysfs_read_file(gpio_sys_fname, value_str, sizeof(value_str));
if (strncmp(value_str, "1", 10) == 0) {
return true;
} else {
return false;
}
}
gs_error_t gs_gpio_sysfs_set(gs_gpio_t gpio, bool value, void * driver_data)
{
const char *value_str = value ? "1" : "0";
char gpio_sys_fname[128];
snprintf(gpio_sys_fname, sizeof(gpio_sys_fname), "/sys/class/gpio/gpio%d/value", gpio.pin);
if (access(gpio_sys_fname, W_OK) == 0)
{
return gs_sysfs_write_file(gpio_sys_fname, value_str);
}
log_error("GPIO %d not initialized - Can't set the output.", gpio.pin);
return GS_ERROR_ACCESS;
}
void gs_gpio_sysfs_set_nc(gs_gpio_t gpio, bool value, void * driver_data)
{
const char *value_str = value ? "1" : "0";
char gpio_sys_fname[128];
snprintf(gpio_sys_fname, sizeof(gpio_sys_fname), "/sys/class/gpio/gpio%d/value", gpio.pin);
if (access(gpio_sys_fname, W_OK) == 0)
{
gs_sysfs_write_file(gpio_sys_fname, value_str);
return;
}
log_error("GPIO %d not initialized - Can't set the output.", gpio.pin);
}
gs_error_t gs_gpio_sysfs_init_as_interrupt(gs_gpio_t gpio, const gs_interrupt_conf_t * conf, void * driver_data)
{
return GS_ERROR_NOT_IMPLEMENTED;
}
const gs_gpio_driver_t gs_gpio_sysfs_driver = {
.get_handler = gs_gpio_sysfs_get,
.get_nc_handler = gs_gpio_sysfs_get_nc,
.set_handler = gs_gpio_sysfs_set,
.set_nc_handler = gs_gpio_sysfs_set_nc,
.init_as_interrupt_handler = gs_gpio_sysfs_init_as_interrupt,
};

171
gomspace/libutil/src/linux/drivers/gpio/gpio_virtual.c Normal file
View File

@ -0,0 +1,171 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/linux/drivers/gpio/gpio_virtual.h>
#define MAX_VPINS 500
#define FALLING_EDGE_FLAG 0x1
#define RISING_EDGE_FLAG 0x2
typedef struct {
gs_gpio_t gpio;
bool output;
bool value;
bool in_use;
gs_gpio_isr_t isr;
uint8_t edge_flags;
uint32_t transistions;
} gs_gpio_virtual_t;
static gs_gpio_virtual_t vpins[MAX_VPINS];
gs_error_t gs_gpio_virtual_initialize(gs_gpio_t gpio, bool output, bool value)
{
gs_gpio_virtual_t * pin;
for (uint16_t i = 0; i < MAX_VPINS; i++) {
pin = &vpins[i];
if ((!pin->in_use) || ((pin->gpio.pin == gpio.pin) && (pin->gpio.port == gpio.port))) {
pin->gpio = gpio;
pin->output = output;
pin->value = value;
pin->in_use = true;
return GS_OK;
}
}
return GS_ERROR_FULL;
}
static gs_gpio_virtual_t * find_vpin(gs_gpio_t * gpio)
{
gs_gpio_virtual_t * pin;
for (uint16_t i = 0; i < MAX_VPINS; i++) {
pin = &vpins[i];
if (pin->gpio.pin == gpio->pin) {
if (pin->gpio.port == gpio->port) {
if (pin->in_use) {
return pin;
}
}
}
}
return NULL;
}
gs_error_t gs_gpio_virtual_get(gs_gpio_t gpio, bool *value, void * driver_data)
{
gs_gpio_virtual_t * pin = find_vpin(&gpio);
if (pin) {
*value = pin->value;
return GS_OK;
}
return GS_ERROR_NOT_FOUND;
}
bool gs_gpio_virtual_get_nc(gs_gpio_t gpio, void * driver_data)
{
gs_gpio_virtual_t * pin = find_vpin(&gpio);
if (pin) {
return pin->value;
}
return false;
}
gs_error_t gs_gpio_virtual_set(gs_gpio_t gpio, bool value, void * driver_data)
{
gs_gpio_virtual_t * pin = find_vpin(&gpio);
if (pin) {
if (pin->output) {
if (pin->value != value) {
pin->value = value;
pin->transistions++;
}
return GS_OK;
}
return GS_ERROR_PERM;
}
return GS_ERROR_NOT_FOUND;
}
void gs_gpio_virtual_set_nc(gs_gpio_t gpio, bool value, void * driver_data)
{
gs_gpio_virtual_t * pin = find_vpin(&gpio);
if (pin) {
if (pin->output) {
if (pin->value != value) {
pin->value = value;
pin->transistions++;
}
}
}
}
gs_error_t gs_gpio_virtual_init_as_interrupt(gs_gpio_t gpio, const gs_interrupt_conf_t * conf, void * driver_data)
{
gs_gpio_virtual_t * pin;
for (uint16_t i = 0; i < MAX_VPINS; i++) {
pin = &vpins[i];
if ((!pin->in_use) || ((pin->gpio.pin == gpio.pin) && (pin->gpio.port == gpio.port))) {
pin->gpio = gpio;
pin->output = false;
pin->value = 0;
pin->in_use = true;
pin->isr = conf->isr;
if (conf->falling_edge) {
pin->edge_flags |= FALLING_EDGE_FLAG;
}
if (conf->rising_edge) {
pin->edge_flags |= RISING_EDGE_FLAG;
}
return GS_OK;
}
}
return GS_ERROR_FULL;
}
gs_error_t gs_gpio_virtual_force_set(gs_gpio_t gpio, bool value)
{
gs_gpio_virtual_t * pin = find_vpin(&gpio);
if (pin) {
bool old_value = pin->value;
if (old_value != value) {
pin->value = value;
pin->transistions++;
if (pin->isr) {
if ((old_value == false) && (pin->edge_flags & RISING_EDGE_FLAG)) {
pin->isr(NULL);
} else if ((old_value == true) && (pin->edge_flags & FALLING_EDGE_FLAG)) {
pin->isr(NULL);
}
}
}
return GS_OK;
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_gpio_virtual_get_transistions(gs_gpio_t gpio, uint32_t * transitions)
{
gs_gpio_virtual_t * pin = find_vpin(&gpio);
if (pin) {
*transitions = pin->transistions;
pin->transistions = 0;
return GS_OK;
}
return GS_ERROR_NOT_FOUND;
}
const gs_gpio_driver_t gs_gpio_virtual_driver = {
.get_handler = gs_gpio_virtual_get,
.get_nc_handler = gs_gpio_virtual_get_nc,
.set_handler = gs_gpio_virtual_set,
.set_nc_handler = gs_gpio_virtual_set_nc,
.init_as_interrupt_handler = gs_gpio_virtual_init_as_interrupt,
};
const gs_gpio_driver_entry_t gs_gpio_virtual_driver_entry_all = {
.port = GS_GPIO_ALL_PORTS,
.pin = GS_GPIO_ALL_PINS,
.driver = &gs_gpio_virtual_driver,
.driver_data = NULL,
};

144
gomspace/libutil/src/linux/drivers/i2c/i2c.c Normal file
View File

@ -0,0 +1,144 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <stdio.h>
#include <gs/util/check.h>
#include <gs/util/linux/drivers/i2c/i2c.h>
#define MAX_DRIVERS 20
#define HIGHEST_I2C_ADDR 127
typedef struct {
gs_i2c_master_driver_entry_t entry;
bool in_use;
} gs_i2c_master_driver_handle_t;
typedef struct {
gs_i2c_slave_driver_entry_t entry;
bool in_use;
} gs_i2c_slave_driver_handle_t;
static gs_i2c_master_driver_handle_t master_drivers[MAX_DRIVERS];
static gs_i2c_slave_driver_handle_t slave_drivers[MAX_DRIVERS];
static uint8_t max_index_master_in_use = 0;
static uint8_t max_index_slave_in_use = 0;
gs_error_t gs_i2c_master_transaction(uint8_t device, uint8_t addr, const void * tx,
size_t txlen,
void * rx,
size_t rxlen,
int timeout_ms)
{
GS_CHECK_RANGE(addr <= HIGHEST_I2C_ADDR);
gs_i2c_master_driver_handle_t * handle;
for (int i = max_index_master_in_use; i >= 0; i--) {
handle = &master_drivers[i];
if (((device == handle->entry.device) || (handle->entry.device == GS_I2C_ALL_DEVICES))
&& ((addr == handle->entry.addr) || (handle->entry.addr == GS_I2C_ALL_ADDR))
&& (handle->in_use == true)) {
if (handle->entry.driver->master_transaction_handler) {
return handle->entry.driver->master_transaction_handler(device, addr, tx, txlen, rx, rxlen, timeout_ms, handle->entry.driver_data);
}
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_i2c_master_register_driver(const gs_i2c_master_driver_entry_t * driver_entry)
{
GS_CHECK_ARG(driver_entry != NULL);
GS_CHECK_ARG(driver_entry->driver != NULL);
GS_CHECK_RANGE((driver_entry->addr == GS_I2C_ALL_ADDR) || (driver_entry->addr <= HIGHEST_I2C_ADDR));
gs_i2c_master_driver_handle_t * handle;
for (uint8_t i = 0; i < MAX_DRIVERS; i++) {
handle = &master_drivers[i];
if (handle->in_use == false) {
handle->entry = *driver_entry;
handle->in_use = true;
max_index_master_in_use = i;
return GS_OK;
}
}
/* Not enough space in buffer */
return GS_ERROR_FULL;
}
static inline gs_i2c_slave_driver_entry_t * gs_slave_find_driver_entry(uint8_t device)
{
gs_i2c_slave_driver_handle_t * handle;
for (int i = max_index_slave_in_use; i >= 0; i--) {
handle = &slave_drivers[i];
if (((device == handle->entry.device) || (handle->entry.device == GS_I2C_ALL_DEVICES))
&& (handle->in_use == true)) {
return &handle->entry;
}
}
return NULL;
}
gs_error_t gs_i2c_slave_start(uint8_t device)
{
gs_i2c_slave_driver_entry_t * driver_entry = gs_slave_find_driver_entry(device);
if (driver_entry) {
if (driver_entry->driver->start_handler) {
return driver_entry->driver->start_handler(device, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_i2c_slave_set_rx(uint8_t device, gs_i2c_slave_receive_t rx)
{
gs_i2c_slave_driver_entry_t * driver_entry = gs_slave_find_driver_entry(device);
if (driver_entry) {
if (driver_entry->driver->set_rx_handler) {
return driver_entry->driver->set_rx_handler(device, rx, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_i2c_slave_set_get_rx_buf(uint8_t device, gs_i2c_slave_get_rx_buf_t get_rx_buf, size_t buf_length)
{
gs_i2c_slave_driver_entry_t * driver_entry = gs_slave_find_driver_entry(device);
if (driver_entry) {
if (driver_entry->driver->set_get_rx_buf_handler) {
return driver_entry->driver->set_get_rx_buf_handler(device, get_rx_buf, buf_length, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_i2c_slave_set_response(uint8_t device, const uint8_t * tx, size_t tx_length)
{
gs_i2c_slave_driver_entry_t * driver_entry = gs_slave_find_driver_entry(device);
if (driver_entry) {
if (driver_entry->driver->set_response_handler) {
return driver_entry->driver->set_response_handler(device, tx, tx_length, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_i2c_slave_register_driver(const gs_i2c_slave_driver_entry_t * driver_entry)
{
GS_CHECK_ARG(driver_entry != NULL);
GS_CHECK_ARG(driver_entry->driver != NULL);
gs_i2c_slave_driver_handle_t * handle;
for (uint8_t i = 0; i < MAX_DRIVERS; i++) {
handle = &slave_drivers[i];
if (handle->in_use == false) {
handle->entry = *driver_entry;
handle->in_use = true;
max_index_slave_in_use = i;
return GS_OK;
}
}
/* Not enough space in buffer */
return GS_ERROR_FULL;
}

137
gomspace/libutil/src/linux/drivers/spi/spi.c Normal file
View File

@ -0,0 +1,137 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <stdio.h>
#include <gs/util/check.h>
#include <gs/util/linux/drivers/spi/spi.h>
#define MAX_DRIVERS 20
typedef struct {
gs_spi_master_driver_entry_t entry;
bool in_use;
} gs_spi_master_driver_handle_t;
typedef struct {
gs_spi_slave_driver_entry_t entry;
bool in_use;
} gs_spi_slave_driver_handle_t;
static gs_spi_master_driver_handle_t master_drivers[MAX_DRIVERS];
static gs_spi_slave_driver_handle_t slave_drivers[MAX_DRIVERS];
static uint8_t max_index_master_in_use = 0;
static uint8_t max_index_slave_in_use = 0;
static inline gs_spi_master_driver_entry_t * gs_master_find_driver_entry(uint8_t slave)
{
gs_spi_master_driver_handle_t * handle;
for (int i = max_index_master_in_use; i >= 0; i--) {
handle = &master_drivers[i];
if (((slave == handle->entry.slave) || (handle->entry.slave == GS_SPI_ALL_SLAVES)) && (handle->in_use == true)) {
return &handle->entry;
}
}
return NULL;
}
gs_error_t gs_spi_master_transaction(uint8_t slave, const void * tx, void * rx, size_t size, int timeout_ms)
{
gs_spi_master_trans_t trans = {.tx = tx, .rx = rx, .size = size};
return gs_spi_master_transactions(slave, &trans, 1, timeout_ms);
}
gs_error_t gs_spi_master_transactions(uint8_t slave, gs_spi_master_trans_t *trans, size_t count, int timeout_ms)
{
gs_spi_master_driver_entry_t * driver_entry = gs_master_find_driver_entry(slave);
if (driver_entry) {
if (driver_entry->driver->master_transactions_handler) {
return driver_entry->driver->master_transactions_handler(slave, trans, count, timeout_ms, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_spi_master_register_driver(const gs_spi_master_driver_entry_t * driver_entry)
{
GS_CHECK_ARG(driver_entry != NULL);
GS_CHECK_ARG(driver_entry->driver != NULL);
gs_spi_master_driver_handle_t * handle;
for (uint8_t i = 0; i < MAX_DRIVERS; i++) {
handle = &master_drivers[i];
if (handle->in_use == false) {
handle->entry = *driver_entry;
handle->in_use = true;
max_index_master_in_use = i;
return GS_OK;
}
}
/* Not enough space in buffer */
return GS_ERROR_FULL;
}
static inline gs_spi_slave_driver_entry_t * gs_slave_find_driver_entry(uint8_t device)
{
gs_spi_slave_driver_handle_t * handle;
for (int i = max_index_slave_in_use; i >= 0; i--) {
handle = &slave_drivers[i];
if (((device == handle->entry.device) || (handle->entry.device == GS_SPI_ALL_DEVICES))
&& (handle->in_use == true)) {
return &handle->entry;
}
}
return NULL;
}
gs_error_t gs_spi_slave_start(uint8_t device)
{
gs_spi_slave_driver_entry_t * driver_entry = gs_slave_find_driver_entry(device);
if (driver_entry) {
if (driver_entry->driver->start_handler) {
return driver_entry->driver->start_handler(device, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_spi_slave_set_rx(uint8_t device, gs_spi_slave_receive_t rx)
{
gs_spi_slave_driver_entry_t * driver_entry = gs_slave_find_driver_entry(device);
if (driver_entry) {
if (driver_entry->driver->set_rx_handler) {
return driver_entry->driver->set_rx_handler(device, rx, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_spi_slave_set_response(uint8_t device, size_t offset, const uint8_t * tx, size_t size)
{
gs_spi_slave_driver_entry_t * driver_entry = gs_slave_find_driver_entry(device);
if (driver_entry) {
if (driver_entry->driver->set_response_handler) {
return driver_entry->driver->set_response_handler(device, offset, tx, size, driver_entry->driver_data);
}
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_spi_slave_register_driver(const gs_spi_slave_driver_entry_t * driver_entry)
{
GS_CHECK_ARG(driver_entry != NULL);
GS_CHECK_ARG(driver_entry->driver != NULL);
gs_spi_slave_driver_handle_t * handle;
for (uint8_t i = 0; i < MAX_DRIVERS; i++) {
handle = &slave_drivers[i];
if (handle->in_use == false) {
handle->entry = *driver_entry;
handle->in_use = true;
max_index_slave_in_use = i;
return GS_OK;
}
}
/* Not enough space in buffer */
return GS_ERROR_FULL;
}

30
gomspace/libutil/src/linux/drivers/sys/memory.c Normal file
View File

@ -0,0 +1,30 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <sys/sysinfo.h>
#include <gs/util/drivers/sys/memory.h>
gs_error_t gs_mem_get_int_ram_stat(gs_mem_ram_stat_t * ram_stat)
{
return GS_ERROR_NOT_SUPPORTED;
}
gs_error_t gs_mem_get_ext_ram_stat(gs_mem_ram_stat_t * ram_stat)
{
struct sysinfo info;
int res = sysinfo(&info);
if (res != GS_OK) {
return res;
}
ram_stat->total = info.totalram;
ram_stat->max_available = -1;
ram_stat->min_available = -1;
ram_stat->available = info.freeram;
return GS_OK;
}
gs_mem_ram_type_t gs_mem_get_ram_default()
{
return GS_MEM_RAM_TYPE_EXTERNAL;
}

41
gomspace/libutil/src/linux/function.c Normal file
View File

@ -0,0 +1,41 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/linux/function.h>
#include <gs/util/string.h>
typedef struct {
const char * short_name;
const char * long_name;
gs_function_t function;
} gs_function_register_t;
static gs_function_register_t registry[10];
gs_error_t gs_function_register(const char * short_name, const char * long_name, gs_function_t function)
{
for (unsigned int i = 0; i < GS_ARRAY_SIZE(registry); ++i) {
gs_function_register_t * cb = &registry[i];
if ((cb->short_name == NULL) && (cb->long_name == NULL)) {
cb->short_name = short_name;
cb->long_name = long_name;
cb->function = function;
return GS_OK;
}
}
return GS_ERROR_FULL;
}
gs_error_t gs_function_invoke(const char * name, void * arg)
{
for (unsigned int i = 0; i < GS_ARRAY_SIZE(registry); ++i) {
gs_function_register_t * cb = &registry[i];
if ((gs_string_empty(cb->short_name) == false) && (strcasecmp(cb->short_name, name) == 0)) {
return (cb->function)(arg);
}
if ((gs_string_empty(cb->long_name) == false) && (strcasecmp(cb->long_name, name) == 0)) {
return (cb->function)(arg);
}
}
return GS_ERROR_NOT_IMPLEMENTED;
}

59
gomspace/libutil/src/linux/mutex.c Normal file
View File

@ -0,0 +1,59 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/mutex.h>
#include <errno.h>
#include <stdlib.h>
gs_error_t gs_mutex_create(gs_mutex_t * mutex)
{
if (mutex == NULL) {
return GS_ERROR_ARG;
}
*mutex = malloc(sizeof(pthread_mutex_t));
if (*mutex == NULL) {
return GS_ERROR_ALLOC;
}
pthread_mutexattr_t attr;
pthread_mutexattr_init(&attr);
pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
int res = pthread_mutex_init(*mutex, &attr);
if (res < 0) {
res = gs_error(errno);
free(*mutex);
}
return res;
}
gs_error_t gs_mutex_destroy(gs_mutex_t mutex)
{
int res = GS_OK;
if (mutex) {
res = pthread_mutex_destroy(mutex);
if (res < 0) {
res = gs_error(errno);
}
free(mutex);
}
return res;
}
gs_error_t gs_mutex_lock(gs_mutex_t mutex)
{
int res = pthread_mutex_lock(mutex);
if (res < 0) {
res = gs_error(errno);
}
return res;
}
gs_error_t gs_mutex_unlock(gs_mutex_t mutex)
{
int res = pthread_mutex_unlock(mutex);
if (res < 0) {
res = gs_error(errno);
}
return res;
}

217
gomspace/libutil/src/linux/queue.c Normal file
View File

@ -0,0 +1,217 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
/**
* Inspired by c-pthread-queue by Matthew Dickinson
* http://code.google.com/p/c-pthread-queue/
*/
#include <gs/util/queue.h>
#include <pthread.h>
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <limits.h>
#define PTHREAD_QUEUE_ARG GS_ERROR_ARG
#define PTHREAD_QUEUE_EMPTY GS_ERROR_NOT_FOUND
#define PTHREAD_QUEUE_FULL GS_ERROR_FULL
#define PTHREAD_QUEUE_TIMEOUT GS_ERROR_TIMEOUT
#define PTHREAD_QUEUE_OK GS_OK
typedef struct gs_pthread_queue {
uint8_t * buffer;
size_t size;
size_t item_size;
size_t items;
size_t in;
size_t out;
pthread_mutex_t mutex;
pthread_cond_t cond_full;
pthread_cond_t cond_empty;
} pthread_queue_t;
static pthread_queue_t * pthread_queue_create(size_t length, size_t item_size)
{
pthread_queue_t * q = malloc(sizeof(pthread_queue_t));
if (q != NULL) {
q->buffer = malloc(length*item_size);
if (q->buffer != NULL) {
q->size = length;
q->item_size = item_size;
q->items = 0;
q->in = 0;
q->out = 0;
if (pthread_mutex_init(&(q->mutex), NULL) || pthread_cond_init(&(q->cond_full), NULL) || pthread_cond_init(&(q->cond_empty), NULL)) {
free(q->buffer);
free(q);
q = NULL;
}
} else {
free(q);
q = NULL;
}
}
return q;
}
static void pthread_queue_delete(pthread_queue_t * q)
{
if (q) {
free(q->buffer);
free(q);
}
}
static int pthread_queue_enqueue(pthread_queue_t * queue, const void * value, uint32_t timeout)
{
/* Calculate timeout */
struct timespec ts;
if (clock_gettime(CLOCK_REALTIME, &ts)) {
return PTHREAD_QUEUE_ARG;
}
uint32_t sec = timeout / 1000;
uint32_t nsec = (timeout - 1000 * sec) * 1000000;
ts.tv_sec += sec;
if (ts.tv_nsec + nsec > 1000000000)
ts.tv_sec++;
ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;
/* Get queue lock */
pthread_mutex_lock(&(queue->mutex));
while (queue->items == queue->size) {
int ret = -1;
if (timeout) {
ret = pthread_cond_timedwait(&(queue->cond_full), &(queue->mutex), &ts);
}
if (ret) {
pthread_mutex_unlock(&(queue->mutex));
return PTHREAD_QUEUE_TIMEOUT;
}
}
/* Coby object from input buffer */
memcpy(queue->buffer+(queue->in * queue->item_size), value, queue->item_size);
queue->items++;
queue->in = (queue->in + 1) % queue->size;
pthread_mutex_unlock(&(queue->mutex));
/* Nofify blocked threads */
pthread_cond_broadcast(&(queue->cond_empty));
return PTHREAD_QUEUE_OK;
}
static int pthread_queue_dequeue(pthread_queue_t * queue, void * buf, uint32_t timeout)
{
/* Calculate timeout */
struct timespec ts;
if (clock_gettime(CLOCK_REALTIME, &ts)) {
return PTHREAD_QUEUE_ARG;
}
uint32_t sec = timeout / 1000;
uint32_t nsec = (timeout - 1000 * sec) * 1000000;
ts.tv_sec += sec;
if (ts.tv_nsec + nsec > 1000000000)
ts.tv_sec++;
ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;
/* Get queue lock */
pthread_mutex_lock(&(queue->mutex));
while (queue->items == 0) {
int ret = -1;
if (timeout) {
ret = pthread_cond_timedwait(&(queue->cond_empty), &(queue->mutex), &ts);
}
if (ret) {
pthread_mutex_unlock(&(queue->mutex));
return PTHREAD_QUEUE_TIMEOUT;
}
}
/* Coby object to output buffer */
memcpy(buf, queue->buffer+(queue->out * queue->item_size), queue->item_size);
queue->items--;
queue->out = (queue->out + 1) % queue->size;
pthread_mutex_unlock(&(queue->mutex));
/* Nofify blocked threads */
pthread_cond_broadcast(&(queue->cond_full));
return PTHREAD_QUEUE_OK;
}
static size_t pthread_queue_items(pthread_queue_t * queue)
{
pthread_mutex_lock(&(queue->mutex));
size_t items = queue->items;
pthread_mutex_unlock(&(queue->mutex));
return items;
}
gs_error_t gs_queue_create(size_t items, size_t item_size, gs_queue_t * queue)
{
if (queue == NULL) {
return GS_ERROR_ARG;
}
pthread_queue_t * q = pthread_queue_create(items, item_size);
if (q == NULL) {
return GS_ERROR_ALLOC;
}
*queue = q;
return GS_OK;
}
gs_error_t gs_queue_destroy(gs_queue_t queue)
{
pthread_queue_delete(queue);
return GS_OK;
}
gs_error_t gs_queue_enqueue(gs_queue_t queue, const void *value, int timeout_ms)
{
return pthread_queue_enqueue(queue, value, (timeout_ms >= 0) ? timeout_ms : INT_MAX);
}
gs_error_t gs_queue_enqueue_isr(gs_queue_t queue, const void * value, gs_context_switch_t * cswitch)
{
(void) cswitch;
gs_error_t error = gs_queue_enqueue(queue, value, 0);
return (error != GS_ERROR_TIMEOUT) ? error : GS_ERROR_FULL;
}
gs_error_t gs_queue_dequeue(gs_queue_t queue, int timeout_ms, void *buf)
{
return pthread_queue_dequeue(queue, buf, (timeout_ms >= 0) ? timeout_ms : INT_MAX);
}
gs_error_t gs_queue_dequeue_isr(gs_queue_t queue, gs_context_switch_t * cswitch, void *buf)
{
(void) cswitch;
gs_error_t error = gs_queue_dequeue(queue, 0, buf);
return (error != GS_ERROR_TIMEOUT) ? error : GS_ERROR_NOT_FOUND;
}
unsigned int gs_queue_size(gs_queue_t queue)
{
if (queue) {
return pthread_queue_items(queue);
}
return 0;
}
unsigned int gs_queue_size_isr(gs_queue_t queue)
{
return gs_queue_size(queue);
}

78
gomspace/libutil/src/linux/rtc.c Normal file
View File

@ -0,0 +1,78 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/linux/rtc.h>
#include <time.h>
#include <linux/rtc.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <string.h>
static gs_error_t gs_rtc_get(void * driver_data, gs_timestamp_t * return_time)
{
if (return_time == NULL) {
return GS_ERROR_ARG;
}
return_time->tv_sec = 0;
return_time->tv_nsec = 0;
int fd = open("/dev/rtc", O_RDONLY | O_CLOEXEC);
if (fd < 0) {
return gs_error(errno);
}
struct tm tm;
memset(&tm, 0, sizeof(tm));
int res = ioctl(fd, RTC_RD_TIME, &tm);
close(fd);
if (res < 0) {
return gs_error(errno);
}
time_t time = mktime(&tm);
if (time < 0) {
return GS_ERROR_DATA;
}
return_time->tv_sec = (uint32_t) time;
return GS_OK;
}
static gs_error_t gs_rtc_set(void * driver_data, const gs_timestamp_t * set_time)
{
if (set_time == NULL) {
return GS_ERROR_ARG;
}
int fd = open("/dev/rtc", O_RDONLY | O_CLOEXEC);
if (fd < 0) {
return gs_error(errno);
}
const time_t now = set_time->tv_sec;
struct tm tm;
gmtime_r(&now, &tm);
int res = ioctl(fd, RTC_SET_TIME, &tm);
close(fd);
if (res < 0) {
return gs_error(errno);
}
return GS_OK;
}
gs_error_t gs_rtc_register_linux(bool get, bool set)
{
static gs_rtc_driver_t rtc_driver;
if (get) {
rtc_driver.get_time = gs_rtc_get;
}
if (set) {
rtc_driver.set_time = gs_rtc_set;
}
return gs_rtc_register(&rtc_driver, NULL);
}

89
gomspace/libutil/src/linux/sem.c Normal file
View File

@ -0,0 +1,89 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/sem.h>
#include <errno.h>
#include <time.h>
#include <stdlib.h>
gs_error_t gs_sem_create(unsigned int initialValue, gs_sem_t * sem)
{
if (sem == NULL) {
return GS_ERROR_ARG;
}
*sem = malloc(sizeof(sem_t));
if (*sem == NULL) {
return GS_ERROR_ALLOC;
}
int res = sem_init(*sem, 0, initialValue);
if (res < 0) {
res = gs_error(errno);
free(*sem);
}
return res;
}
gs_error_t gs_sem_destroy(gs_sem_t sem)
{
int res = GS_OK;
if (sem) {
res = sem_destroy(sem);
if (res < 0) {
res = gs_error(errno);
}
free(sem);
}
return res;
}
gs_error_t gs_sem_wait(gs_sem_t sem, int timeout_ms)
{
int res;
if (timeout_ms < 0) {
res = sem_wait(sem);
} else {
struct timespec ts;
res = clock_gettime(CLOCK_REALTIME, &ts);
if (res == 0) {
const uint32_t ms = (uint32_t)timeout_ms;
uint32_t sec = ms / 1000;
uint32_t nsec = (ms - (1000 * sec)) * 1000000;
ts.tv_sec += sec;
if (ts.tv_nsec + nsec >= 1000000000) {
ts.tv_sec++;
}
ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;
res = sem_timedwait(sem, &ts);
}
}
if (res < 0) {
res = gs_error(errno);
}
return res;
}
gs_error_t gs_sem_post(gs_sem_t sem)
{
int res = sem_post(sem);
if (res < 0) {
res = gs_error(errno);
}
return res;
}
gs_error_t gs_sem_post_isr(gs_sem_t sem, gs_context_switch_t * cswitch)
{
(void) cswitch;
int res = sem_post(sem);
if (res < 0) {
res = gs_error(errno);
}
return res;
}

38
gomspace/libutil/src/linux/signal.c Normal file
View File

@ -0,0 +1,38 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/linux/signal.h>
#include <gs/util/linux/exitcode.h>
static void gs_signal_default_handler(int signo, siginfo_t *si, void *context)
{
exit(GS_EXITCODE_SIGNAL(signo)); // ensure atexit are invoked
}
gs_error_t gs_signal_catch(int signal, gs_signal_handler handler)
{
if (handler == NULL) {
handler = gs_signal_default_handler;
}
struct sigaction sa = { .sa_flags = SA_SIGINFO,
.sa_sigaction = handler};
if (sigemptyset(&sa.sa_mask)) {
return GS_ERROR_UNKNOWN;
}
if (sigaction(signal, &sa, NULL)) {
return GS_ERROR_UNKNOWN;
}
return GS_OK;
}
gs_error_t gs_signal_ignore(int signal)
{
struct sigaction sa = { .sa_flags = 0,
.sa_handler = SIG_IGN}; // handle signal by ignoring
if (sigemptyset(&sa.sa_mask)) {
return GS_ERROR_UNKNOWN;
}
if (sigaction(signal, &sa, NULL)) {
return GS_ERROR_UNKNOWN;
}
return GS_OK;
}

36
gomspace/libutil/src/linux/stdio.c Normal file
View File

@ -0,0 +1,36 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/stdio.h>
#include <unistd.h>
#include <poll.h>
gs_error_t gs_stdio_putchar(int ch)
{
const int res = putchar(ch);
if (res < 0) {
return GS_ERROR_IO;
}
return GS_OK;
}
gs_error_t gs_stdio_getchar_timed(int timeout_ms, int * ch)
{
struct pollfd fds = {STDIN_FILENO, POLLIN, 0};
const int res = poll(&fds, 1, timeout_ms);
if (res == 0) {
return GS_ERROR_TIMEOUT;
}
if ((res > 0) && (fds.revents & POLLIN)) {
int tmp = getchar();
if (tmp >= 0) {
if (ch) {
*ch = tmp;
}
return GS_OK;
}
}
return GS_ERROR_IO;
}

48
gomspace/libutil/src/linux/sysfs_helper.c Normal file
View File

@ -0,0 +1,48 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/linux/sysfs_helper.h>
#include <gs/util/log.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
gs_error_t gs_sysfs_write_file(const char *path, const char *value)
{
log_trace("sysfs: write %s to %s", value, path);
int fd = open(path, O_WRONLY);
if (fd < 0) {
return GS_ERROR_HANDLE;
}
size_t len = strlen(value);
ssize_t bytes = write(fd, value, len);
close(fd);
if (bytes < 0) {
return GS_ERROR_NO_DATA;
}
return (len == (size_t)bytes) ? GS_OK : GS_ERROR_NO_DATA;
}
gs_error_t gs_sysfs_read_file(const char *path, char *value, size_t len)
{
log_trace("sysfs: read %s", path);
int fd = open(path, O_RDONLY);
if (fd < 0) {
return GS_ERROR_HANDLE;
}
ssize_t bytes = read(fd, value, len);
close(fd);
if (bytes < 0) {
return GS_ERROR_DATA;
}
return GS_OK;
}

89
gomspace/libutil/src/linux/thread.c Normal file
View File

@ -0,0 +1,89 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/thread.h>
#include <unistd.h>
#include <gs/util/time.h>
gs_error_t gs_thread_create(const char * const name,
gs_thread_func_t func,
void * parameter,
size_t stack_size,
gs_thread_priority_t priority,
uint32_t flags,
gs_thread_t * return_handle)
{
gs_time_uptime(); // force initialize of static offset
pthread_attr_t attr;
int res = pthread_attr_init(&attr);
if (res) {
return GS_ERROR_ALLOC;
}
if (flags & GS_THREAD_CREATE_JOINABLE) {
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
} else {
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
}
gs_thread_t handle;
res = pthread_create(&handle, &attr, func, parameter);
pthread_attr_destroy(&attr);
if (res) {
return GS_ERROR_ALLOC;
}
if (return_handle) {
*return_handle = handle;
}
return GS_OK;
}
gs_error_t gs_thread_create_with_stack(const char * const name,
gs_thread_func_t func,
void * parameter,
size_t stack_size,
gs_stack_type_t *stack,
gs_thread_priority_t priority,
uint32_t flags,
gs_thread_t * return_handle)
{
return gs_thread_create(name, func, parameter, stack_size, priority, flags, return_handle);
}
void gs_thread_exit(void * exitValue)
{
pthread_exit(exitValue);
}
void gs_thread_sleep_ms(uint32_t time_ms)
{
gs_time_sleep_ms(time_ms);
}
gs_error_t gs_thread_join(gs_thread_t thread, void ** return_retval)
{
gs_error_t error = GS_ERROR_ARG;
void * retval = 0;
if (thread) {
int res = pthread_join(thread, &retval);
if (res == 0) {
error = GS_OK;
} else {
retval = 0;
}
}
if (return_retval) {
*return_retval = retval;
}
return error;
}
void gs_thread_block(void)
{
/* Wait here forever */
for (;;) {
gs_time_sleep_ms(10000);
}
}

53
gomspace/libutil/src/linux/time.c Normal file
View File

@ -0,0 +1,53 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/time.h>
#include <time.h>
#include <gs/util/timestamp.h>
uint32_t gs_time_rel_ms(void)
{
struct timespec ts;
if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) {
return 0;
}
return (uint32_t)((ts.tv_sec * 1000) + (ts.tv_nsec/1000000));
}
uint32_t gs_time_rel_ms_isr(void)
{
return gs_time_rel_ms();
}
static uint32_t uptime_offset = 0;
uint32_t gs_time_uptime(void)
{
uint32_t seconds;
struct timespec ts;
if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) {
seconds = 0;
} else {
seconds = (uint32_t) ts.tv_sec;
}
if (uptime_offset == 0) {
uptime_offset = seconds;
}
return (seconds - uptime_offset);
}
void gs_time_sleep_ns(uint64_t time_ns)
{
struct timespec ts;
ts.tv_sec = (time_ns / GS_TIMESTAMP_NSEC_PER_SEC);
ts.tv_nsec = (time_ns % GS_TIMESTAMP_NSEC_PER_SEC);
// improvement: check return code (INTR) and use remaining.
nanosleep(&ts, NULL);
}
void gs_time_sleep_ms(uint32_t time_ms)
{
uint64_t ns = time_ms;
ns *= 1000000LL;
gs_time_sleep_ns( ns);
}

30
gomspace/libutil/src/lock.c Normal file
View File

@ -0,0 +1,30 @@
/* Copyright (c) 2013-2019 GomSpace A/S. All rights reserved. */
#include "lock.h"
#include <gs/util/mutex.h>
static gs_mutex_t gs_lock;
gs_error_t gs_lock_init(void)
{
if (gs_lock == NULL) {
return gs_mutex_create(&gs_lock);
}
return GS_OK;
}
gs_error_t gs_lock_lock(void)
{
if (gs_lock == NULL) {
gs_error_t error = gs_lock_init();
if (error) {
return error;
}
}
return gs_mutex_lock(gs_lock);
}
gs_error_t gs_lock_unlock(void)
{
return gs_mutex_unlock(gs_lock);
}

14
gomspace/libutil/src/lock.h Normal file
View File

@ -0,0 +1,14 @@
/* Copyright (c) 2013-2019 GomSpace A/S. All rights reserved. */
/**
@file
Basic/core locking.
Use for rare read/write locking, e.g. protecting register/de-regsiter functions.
*/
#include <gs/util/error.h>
gs_error_t gs_lock_init(void);
gs_error_t gs_lock_lock(void);
gs_error_t gs_lock_unlock(void);

88
gomspace/libutil/src/log/appender/console.c Normal file
View File

@ -0,0 +1,88 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/log/appender/console.h>
#include <gs/util/mutex.h>
#include <stdio.h>
// generated by waf configure -> GS_LOG_ENABLE_ISR_LOGS
#include <conf_util.h>
static gs_log_appender_console_cb_t g_console_log_cb = NULL;
static gs_mutex_t g_log_console_mutex = NULL;
gs_error_t gs_log_console_append_init(gs_log_appender_t *appender)
{
gs_error_t ret = GS_OK;
if (g_log_console_mutex == NULL) {
ret = gs_mutex_create(&g_log_console_mutex);
if (ret != GS_OK) {
g_log_console_mutex = NULL;
}
}
return ret;
}
static void gs_log_console_append_isr(gs_log_appender_t *appender, gs_log_level_t level, const gs_log_group_t *group, const gs_timestamp_t * ts, const char * format, va_list va)
{
va_list my_va;
va_copy(my_va, va);
const char * color = gs_log_level_to_color_begin(level);
const char * end_color = gs_log_level_to_color_end();
const char clevel = gs_log_level_to_char(level);
// print log
printf("%s%04"PRIu32".%06"PRIu32" %c %s: ", color, ts->tv_sec, ts->tv_nsec / 1000, clevel, group->name);
GS_PGM_VPRINTF(format, my_va);
printf("%s\r\n", end_color);
va_end(my_va);
}
static void gs_log_console_append(gs_log_appender_t *appender, gs_log_level_t level, const gs_log_group_t *group, const gs_timestamp_t * ts, const char * format, va_list va)
{
if (g_console_log_cb)
return g_console_log_cb(appender, level, group, ts, format, va);
if (g_log_console_mutex) {
gs_mutex_lock(g_log_console_mutex);
}
gs_log_console_append_isr(appender, level, group, ts, format, va);
if (g_log_console_mutex) {
gs_mutex_unlock(g_log_console_mutex);
}
}
static void gs_log_console_append_get_info(gs_log_appender_t *appender, char *info_str, uint8_t str_size)
{
if (!info_str) {
return;
}
snprintf(info_str, str_size, "Prints on stdout");
}
gs_error_t gs_log_appender_console_set_cb(gs_log_appender_console_cb_t cb)
{
g_console_log_cb = cb;
return GS_OK;
}
static const gs_log_appender_driver_t console_appender_driver = {
.init = gs_log_console_append_init,
.append = gs_log_console_append,
#ifdef GS_LOG_ENABLE_ISR_LOGS
.append_isr = gs_log_console_append_isr,
#else
.append_isr = 0,
#endif
.info = gs_log_console_append_get_info,
};
gs_log_appender_t gs_log_appender_console = {
.name = "console",
.drv = &console_appender_driver,
.drv_config = 0,
.mask = LOG_ALL_MASK,
};

117
gomspace/libutil/src/log/appender/simple_file.c Normal file
View File

@ -0,0 +1,117 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#ifndef __AVR__
#include <gs/util/log/appender/simple_file.h>
#include <gs/util/string.h>
#include <gs/util/mutex.h>
#include <stdlib.h>
#include <time.h>
typedef struct simple_file_drv_data {
FILE *fp;
gs_mutex_t mutex;
} simple_file_drv_data_t;
static gs_error_t gs_log_simple_file_init(gs_log_appender_t *appender)
{
const gs_log_appender_simple_file_config_t *config = appender->drv_config;
if (config == NULL || gs_string_empty(config->filename)) {
return GS_ERROR_ARG;
}
simple_file_drv_data_t *drv_data = appender->drv_data;
if (drv_data == NULL) {
drv_data = calloc(1, sizeof(*drv_data));
if (drv_data == NULL) {
return GS_ERROR_ALLOC;
}
}
/* If file is already open - Close it first */
if (drv_data->fp) {
gs_mutex_lock(drv_data->mutex);
fclose(drv_data->fp);
drv_data->fp = NULL;
gs_mutex_unlock(drv_data->mutex);
gs_mutex_destroy(drv_data->mutex);
}
const char * mode = config->truncate ? "w" : "a";
drv_data->fp = fopen(config->filename, mode);
if (drv_data->fp == NULL) {
log_error("%s: failed to open log-file: [%s], mode: %s", __FUNCTION__, config->filename, mode);
free(drv_data);
drv_data = 0;
return GS_ERROR_IO;
}
gs_mutex_create(&drv_data->mutex);
appender->drv_data = drv_data; /* Set driver data on appender */
return GS_OK;
}
static void gs_log_simple_file_append(gs_log_appender_t *appender, gs_log_level_t level, const gs_log_group_t *group, const gs_timestamp_t * ts, const char * format, va_list va)
{
va_list my_va;
va_copy(my_va, va);
const gs_log_appender_simple_file_config_t *config = appender->drv_config;
simple_file_drv_data_t *drv_data = appender->drv_data;
if (drv_data == 0) {
va_end(my_va);
return;
}
const char clevel = gs_log_level_to_char(level);
const time_t t = ts->tv_sec;
struct tm result;
const char * tzone;
if (config->use_local_time) {
localtime_r(&t, &result);
tzone = "";
} else {
gmtime_r(&t, &result);
tzone = "Z";
}
if (drv_data->mutex) {
gs_mutex_lock(drv_data->mutex);
}
{
fprintf(drv_data->fp, "%04d-%02d-%02d %02d:%02d:%02d.%06"PRIu32"%s %c %s: ",
result.tm_year + 1900, result.tm_mon + 1, result.tm_mday,
result.tm_hour, result.tm_min, result.tm_sec,
ts->tv_nsec / 1000, tzone, clevel, group->name);
vfprintf(drv_data->fp, format, my_va);
fprintf(drv_data->fp, "\r\n");
fflush(drv_data->fp);
}
if (drv_data->mutex) {
gs_mutex_unlock(drv_data->mutex);
}
va_end(my_va);
}
static void gs_log_simple_file_append_info(gs_log_appender_t *appender, char *info_str, uint8_t str_size)
{
if (!info_str) {
return;
}
const gs_log_appender_simple_file_config_t *config = appender->drv_config;
snprintf(info_str, str_size, "Writes to file \"%s\"", config->filename);
}
const gs_log_appender_driver_t gs_log_appender_simple_file_driver = {
.init = gs_log_simple_file_init,
.append = gs_log_simple_file_append,
.append_isr = 0,
.info = gs_log_simple_file_append_info,
};
#endif

392
gomspace/libutil/src/log/commands.c Normal file
View File

@ -0,0 +1,392 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/bytebuffer.h>
#include <gs/util/gosh/command.h>
#include <gs/util/log/log.h>
#include <gs/util/log/appender/appender.h>
#include "local.h"
#include <stdlib.h>
// iterator context
typedef struct {
gs_command_context_t * ctx;
gs_log_group_t * first;
gs_log_appender_t * first_appender;
bool completer;
bool detailed;
} iter_group_t;
#define FORMAT_BUF_SIZE 10
static const char * format_mask(uint8_t mask, char * buf)
{
snprintf(buf, FORMAT_BUF_SIZE, "%c%c%c%c%c%c",
(mask & LOG_ERROR_MASK) ? 'E' : '.',
(mask & LOG_WARNING_MASK) ? 'W' : '.',
(mask & LOG_NOTICE_MASK) ? 'N' : '.',
(mask & LOG_INFO_MASK) ? 'I' : '.',
(mask & LOG_DEBUG_MASK) ? 'D' : '.',
(mask & LOG_TRACE_MASK) ? 'T' : '.');
return buf;
}
static bool iter_print_group_appenders(void * ctx_in, gs_log_appender_t * appender)
{
gs_bytebuffer_t *bb = ctx_in;
gs_bytebuffer_printf(bb, "%s,", appender->name);
return true;
}
static bool iter_print_group(void * ctx_in, gs_log_group_t * group)
{
iter_group_t * ctx = ctx_in;
char level_mask[FORMAT_BUF_SIZE];
if (!ctx->completer) {
char appender_str[128] = "\0";
gs_bytebuffer_t bb;
gs_bytebuffer_init(&bb, appender_str, sizeof(appender_str));
gs_log_group_appender_iterate(group, &bb, iter_print_group_appenders);
if (ctx->detailed) {
gs_command_set_output_printf(ctx->ctx, group->name, "category", "0x%08x", group->category);
gs_command_set_output_printf(ctx->ctx, group->name, "mask", "%-6s (0x%02x)", format_mask(group->mask, level_mask), group->mask);
gs_command_set_output_printf(ctx->ctx, group->name, "appenders", appender_str);
} else {
gs_command_set_output_printf(ctx->ctx, NULL, NULL, "%-15s %-6s %s", group->name, format_mask(group->mask, level_mask), appender_str);
}
} else {
fprintf(ctx->ctx->out, " %-15s %-6s\r\n",
group->name,
format_mask(group->mask, level_mask));
}
return true;
}
static int cmd_log_group_list(gs_command_context_t * ctx)
{
iter_group_t iter = {.ctx = ctx, .completer = false};
if (ctx->argc > 1) {
iter.detailed = true;
gs_log_group_iterate(ctx->argv[1], &iter, iter_print_group);
} else {
fprintf(ctx->out, "Group Mask Appenders\r\n");
gs_log_group_iterate("*", &iter, iter_print_group);
}
return GS_OK;
}
static bool iter_print_appender(void * ctx_in, gs_log_appender_t * appender)
{
iter_group_t * ctx = ctx_in;
char level_mask[FORMAT_BUF_SIZE];
if (!ctx->completer) {
if (ctx->detailed) {
gs_command_set_output_printf(ctx->ctx, appender->name, "mask", "%-6s (0x%02x)", format_mask(appender->mask, level_mask), appender->mask);
if (appender->drv->info) {
char info_str[100];
appender->drv->info(appender, info_str, sizeof(info_str));
gs_command_set_output(ctx->ctx, appender->name, "info", info_str);
}
} else {
gs_command_set_output_printf(ctx->ctx, NULL, NULL, "%-15s %-6s", appender->name, format_mask(appender->mask, level_mask));
}
} else {
fprintf(ctx->ctx->out, " %-15s %-6s\r\n",
appender->name,
format_mask(appender->mask, level_mask));
}
return true;
}
static int cmd_log_appender_list(gs_command_context_t * ctx)
{
iter_group_t iter = {.ctx = ctx, .completer = false};
if (ctx->argc > 1) {
iter.detailed = true;
gs_log_appender_iterate(ctx->argv[1], &iter, iter_print_appender);
} else {
fprintf(ctx->out, "Appender Mask\r\n");
gs_log_appender_iterate("*", &iter, iter_print_appender);
}
return GS_OK;
}
typedef gs_error_t (*log_get_mask_t)(const char *name, uint8_t* mask);
typedef gs_error_t (*log_set_mask_t)(const char *name, uint8_t mask);
static int cmd_log_mask_handler(gs_command_context_t * ctx, log_get_mask_t get_mask, log_set_mask_t set_mask)
{
/* strtok writes to the string, so we need to duplicate it to avoid writing to read-only memory */
char strbuf[100];
GS_STRNCPY(strbuf, ctx->argv[1]);
char * saveptr = NULL;
char * token = strtok_r(strbuf, ",", &saveptr);
gs_error_t error = GS_OK;
while (token && (error == GS_OK)) {
uint8_t old_mask = 0;
if (gs_log_is_group_all(token) == false) {
error = get_mask(token, &old_mask);
}
if (error == GS_OK) {
uint8_t new_mask = 0;
error = gs_log_string_to_mask(ctx->argv[2], old_mask, &new_mask);
if (error == GS_OK) {
error = set_mask(token, new_mask);
}
}
token = strtok_r(NULL, ",", &saveptr);
}
return error;
}
static int cmd_log_group_mask(gs_command_context_t * ctx)
{
return cmd_log_mask_handler(ctx, gs_log_group_get_level_mask, gs_log_group_set_level_mask);
}
static int cmd_log_appender_mask(gs_command_context_t * ctx)
{
return cmd_log_mask_handler(ctx, gs_log_appender_get_level_mask, gs_log_appender_set_level_mask);
}
#ifndef __AVR__
static bool iter_log_completer(void *ctx_in, gs_log_group_t * group)
{
iter_group_t * ctx = ctx_in;
unsigned int hits = gs_command_completer_add_token(ctx->ctx, group->name, false);
if (hits == 1) {
ctx->first = group;
} else {
if (hits == 2) {
fprintf(ctx->ctx->out, "\r\n");
iter_print_group(ctx, ctx->first);
}
iter_print_group(ctx, group);
}
return true;
}
static gs_error_t cmd_log_group_completer(gs_command_context_t * ctx, int arg_to_complete)
{
if (arg_to_complete == 1) {
iter_group_t iter = {.ctx = ctx, .completer = true};
char name[50];
snprintf(name, sizeof(name), "%s*", (ctx->argc > 1) ? ctx->argv[1] : "");
gs_log_group_iterate(name, &iter, iter_log_completer);
return GS_OK;
}
return GS_ERROR_AMBIGUOUS;
}
static bool iter_log_appender_completer(void *ctx_in, gs_log_appender_t * appender)
{
iter_group_t * ctx = ctx_in;
unsigned int hits = gs_command_completer_add_token(ctx->ctx, appender->name, false);
if (hits == 1) {
ctx->first_appender = appender;
} else {
if (hits == 2) {
fprintf(ctx->ctx->out, "\r\n");
iter_print_appender(ctx, ctx->first_appender);
}
iter_print_appender(ctx, appender);
}
return true;
}
static gs_error_t cmd_log_appender_completer(gs_command_context_t * ctx, int arg_to_complete)
{
if (arg_to_complete == 1) {
iter_group_t iter = {.ctx = ctx, .completer = true};
char name[50];
snprintf(name, sizeof(name), "%s*", (ctx->argc > 1) ? ctx->argv[1] : "");
gs_log_appender_iterate(name, &iter, iter_log_appender_completer);
return GS_OK;
}
return GS_ERROR_AMBIGUOUS;
}
#endif
typedef struct {
gs_command_context_t *cmd_ctx;
unsigned int count;
} hist_ctx_t;
static bool appender_history_iter(void *ctx, gs_log_level_t level, const gs_timestamp_t *ts, const char *group, const char *msg)
{
hist_ctx_t * hist_ctx = ctx;
/* Break iteration if history record count is reached. */
if (hist_ctx->count-- == 0) {
return false;
}
gs_command_set_output_printf(hist_ctx->cmd_ctx, NULL, NULL,
"%s%04"PRIu32".%06"PRIu32" %c %s: %s%s",
gs_log_level_to_color_begin(level),
ts->tv_sec, ts->tv_nsec/1000,
gs_log_level_to_char(level),
group,
msg,
gs_log_level_to_color_end());
return true;
}
static int cmd_log_appender_hist(gs_command_context_t * ctx)
{
hist_ctx_t hist_ctx = {.cmd_ctx = ctx, .count = 20};
if (ctx->argc == 3) {
hist_ctx.count = atoi(ctx->argv[2]);
}
return gs_log_appender_history_iterate(ctx->argv[1], &hist_ctx, appender_history_iter);
}
static bool iter_log_group_find(void* ctx_in, gs_log_group_t *group)
{
gs_log_group_t **grp = ctx_in;
*grp = group;
return false;
}
static int cmd_log_insert(gs_command_context_t * ctx)
{
gs_log_group_t *log_group = NULL;
gs_error_t error = gs_log_group_iterate(ctx->argv[1], &log_group, iter_log_group_find);
if (error != GS_OK) {
return error;
}
gs_log_level_t level;
error = gs_log_string_to_level(ctx->argv[2], &level);
if (error == GS_OK) {
gs_log(level, log_group, GS_PGM_STR("%s"), ctx->argv[3]);
}
return error;
}
static int cmd_log_color(gs_command_context_t * ctx)
{
bool color;
gs_error_t error = gs_string_to_bool(ctx->argv[1], &color);
if (error == GS_OK) {
gs_log_set_print_color(color);
}
return error;
}
static const gs_command_t GS_COMMAND_SUB cmd_log_group_cmds[] = {
{
.name = "list",
.help = "list log groups",
.usage = "[group]",
#ifndef __AVR__
.completer = cmd_log_group_completer,
#endif
.handler = cmd_log_group_list,
.mandatory_args = GS_COMMAND_NO_ARGS,
.optional_args = 1,
},{
.name = "mask",
.help = "Set log group mask(s): e|w|i|d|t|stand|all|non",
.usage = "<group>[,group] <[+-]level>[,level]",
#ifndef __AVR__
.completer = cmd_log_group_completer,
#endif
.handler = cmd_log_group_mask,
.mandatory_args = 2,
},{
.name = "insert",
.help = "Log message",
.usage = "<group> <level> <message>",
#ifndef __AVR__
.completer = cmd_log_group_completer,
#endif
.handler = cmd_log_insert,
.mandatory_args = 3,
},{
.name = "color",
.help = "Enable/disable color logs (stdout)",
.usage = "<true|false>",
.handler = cmd_log_color,
.mandatory_args = 1,
}
};
static const gs_command_t GS_COMMAND_SUB cmd_log_appender_cmds[] = {
{
.name = "list",
.help = "list log appenders",
.usage = "[appender]",
#ifndef __AVR__
.completer = cmd_log_appender_completer,
#endif
.handler = cmd_log_appender_list,
.mandatory_args = GS_COMMAND_NO_ARGS,
.optional_args = 1,
}, {
.name = "mask",
.help = "Set log appender mask(s): e|w|i|d|t|stand|all|non",
.usage = "<appender>[,appender] <[+-]level>[,level]",
#ifndef __AVR__
.completer = cmd_log_appender_completer,
#endif
.handler = cmd_log_appender_mask,
.mandatory_args = 2,
}, {
.name = "hist",
.help = "Show log appender history",
.usage = "<appender> [cnt]",
#ifndef __AVR__
.completer = cmd_log_appender_completer,
#endif
.handler = cmd_log_appender_hist,
.mandatory_args = 1,
.optional_args = 1,
}
};
static const gs_command_t GS_COMMAND_SUB cmd_log_cmds[] = {
{
.name = "group",
.help = "log group commands",
.chain = GS_COMMAND_INIT_CHAIN(cmd_log_group_cmds),
}, {
.name = "appender",
.help = "log appender commands",
.chain = GS_COMMAND_INIT_CHAIN(cmd_log_appender_cmds),
}
};
static const gs_command_t GS_COMMAND_ROOT cmd_log[] = {
{
.name = "log",
.help = "log: Log system",
.chain = GS_COMMAND_INIT_CHAIN(cmd_log_cmds)
},{
.name = "debug",
.help = "Set Log group mask(s): e|w|n|i|d|t|stand|all|off",
.usage = "<group>[,group] <[+-]level>[,level]",
#ifndef __AVR__
.completer = cmd_log_group_completer,
#endif
.handler = cmd_log_group_mask,
.mandatory_args = 2,
},
};
gs_error_t gs_log_register_commands(void)
{
return GS_COMMAND_REGISTER(cmd_log);
}

27
gomspace/libutil/src/log/local.h Normal file
View File

@ -0,0 +1,27 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/log/log.h>
#include <gs/util/log/appender/appender.h>
struct gs_log_list {
union {
gs_log_group_t * group;
gs_log_appender_t *appender;
} data;
struct gs_log_list * next;
};
/**
De-register appender for the given log group.
@note The de-register function is not safe when logging is active, this function
is mostly for test and should only be used in product code with extreme caution.
If logging is still not active, this function can be used safely.
@param[in] group_name Name of the group.
@param[in] appender_name Name of appender to de-register for this group.
@return gs_error_t
*/
gs_error_t gs_log_group_deregister_appender(const char * group_name, const char * appender_name);
bool gs_log_is_group_all(const char * name);

705
gomspace/libutil/src/log/log.c Normal file
View File

@ -0,0 +1,705 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include "local.h"
#include <gs/util/string.h>
#include <gs/util/clock.h>
#include <gs/util/mutex.h>
#include <gs/util/stdio.h>
#include <gs/util/error.h>
#include <gs/util/log/log.h>
#include <gs/util/log/appender/console.h>
#include <gs/util/check.h>
#include "../lock.h"
#include <stdlib.h>
#define MASK_SET 0
#define MASK_AND 1
#define MASK_OR 2
// use color in log print
static bool g_print_no_color;
// Log Group list
GS_STATIC gs_log_list_t g_log_groups = { .data = { .group = 0} };
// Log Appender list
GS_STATIC gs_log_list_t g_log_appenders = { .data = { .appender = 0} };
// Root Log Appenders - used for holding a appender list
GS_STATIC gs_log_group_t g_log_group_root = {.name = GS_LOG_GROUP_ROOT};
// Default log group - always present.
GS_LOG_GROUP(LOG_DEFAULT, "default", GS_LOG_CAT_DEFAULT, LOG_ERROR_MASK | LOG_WARNING_MASK | LOG_NOTICE_MASK | LOG_INFO_MASK);
bool gs_log_is_group_all(const char * name)
{
return (name && ((strcasecmp(name, "*") == 0) || (strcasecmp(name, "all") == 0)));
}
static bool iter_set_level_mask(void * ctx_in, gs_log_group_t * group)
{
uint8_t * level_mask = ctx_in;
group->mask = *level_mask;
return true;
}
gs_error_t gs_log_group_set_level_mask(const char * group_name, uint8_t mask)
{
if (gs_string_empty(group_name)) {
return GS_ERROR_HANDLE;
}
return gs_log_group_iterate(group_name, &mask, iter_set_level_mask);
}
static bool iter_get_level_mask(void * ctx_in, gs_log_group_t * group)
{
uint8_t * level_mask = ctx_in;
*level_mask = group->mask;
return true;
}
gs_error_t gs_log_group_get_level_mask(const char * group_name, uint8_t *mask)
{
if (gs_string_empty(group_name)) {
return GS_ERROR_HANDLE;
}
gs_error_t error = gs_log_group_iterate(group_name, mask, iter_get_level_mask);
return error;
}
gs_error_t gs_log_string_to_level(const char * str, gs_log_level_t * return_level)
{
if (gs_string_empty(str)) {
return GS_ERROR_ARG;
}
const size_t len = strlen(str);
gs_log_level_t level;
if (strncasecmp(str, "trace", len) == 0) {
level = LOG_TRACE;
} else if (strncasecmp(str, "debug", len) == 0) {
level = LOG_DEBUG;
} else if (strncasecmp(str, "informational", len) == 0) {
level = LOG_INFO;
} else if (strncasecmp(str, "notice", len) == 0) {
level = LOG_NOTICE;
} else if (strncasecmp(str, "warning", len) == 0) {
level = LOG_WARNING;
} else if (strncasecmp(str, "error", len) == 0) {
level = LOG_ERROR;
} else {
return GS_ERROR_DATA;
}
if (return_level) {
*return_level = level;
}
return GS_OK;
}
char gs_log_level_to_char(gs_log_level_t level)
{
switch (level) {
case LOG_TRACE: return 'T';
case LOG_DEBUG: return 'D';
case LOG_INFO: return 'I';
case LOG_NOTICE: return 'N';
case LOG_WARNING: return 'W';
case LOG_ERROR: return 'E';
default: return '?';
}
}
gs_error_t gs_log_string_to_mask(const char *str, uint8_t old, uint8_t * return_mask)
{
GS_CHECK_ARG(gs_string_empty(str) == false);
char strbuf[50]; // copy buf, coz strtok will mess it up
GS_STRNCPY(strbuf, str);
char *saveptr = NULL;
char *token = strtok_r(strbuf, ",", &saveptr);
while (token) {
// check for +xxx (add), -xxxx (remove), xxxx (set)
int op = MASK_SET;
if (*token == '+') {
op = MASK_OR;
token++;
} else if (*token == '-') {
op = MASK_AND;
token++;
}
const unsigned int token_length = strlen(token);
if (token_length < 1) {
return GS_ERROR_DATA;
}
/* Check mask */
uint8_t mask;
gs_log_level_t level;
if (gs_log_string_to_level(token, &level) == GS_OK) {
// actual level
if (op == MASK_SET) {
// set all level bits equal or lover
mask = LOG_ALL_MASK & ~((1 << level) - 1);
} else {
mask = (1 << level);
}
} else if (!strncasecmp(token, "default", token_length)) { // legacy - conflicts with 'de(bug)'
mask = LOG_DEFAULT_MASK;
op = MASK_SET;
} else if (!strncasecmp(token, "standard", token_length)) {
mask = LOG_DEFAULT_MASK;
op = MASK_SET;
} else if (!strncasecmp(token, "all", token_length)) {
mask = LOG_ALL_MASK;
op = MASK_SET;
} else if (!strncasecmp(token, "off", token_length)) {
mask = 0;
op = MASK_SET;
} else if (!strncasecmp(token, "none", token_length)) { // legacy - conflicts with 'no(tice)'
mask = 0;
op = MASK_SET;
} else if (gs_string_to_uint8(token, &mask) == GS_OK) {
op = MASK_SET;
} else {
return GS_ERROR_DATA;
}
/* Apply operation */
if (op == MASK_OR) {
old |= mask;
} else if (op == MASK_AND) {
old &= ~mask;
} else if (op == MASK_SET) {
old = mask;
}
token = strtok_r(NULL, ",", &saveptr);
}
if (return_mask) {
*return_mask = old;
}
return GS_OK;
}
/**
All functions must call this initialization function, to ensure log is initialized.
*/
static gs_error_t gs_log_init_internal(void)
{
if (g_log_groups.data.group == NULL) {
return gs_log_init(true);
}
return GS_OK;
}
gs_error_t gs_log_group_iterate(const char * group_name, void * ctx, gs_log_group_iterator_t iter)
{
const bool all = (gs_string_empty(group_name) || gs_log_is_group_all(group_name));
bool found = false;
for (gs_log_list_t *node = &g_log_groups; node; node = node->next) {
if (node->data.group) {
if (all || gs_string_match(group_name, node->data.group->name)) {
found = true;
bool cont = iter(ctx, node->data.group);
if (cont == false) {
return GS_OK;
}
}
}
}
return found ? GS_OK : GS_ERROR_NOT_FOUND;
}
static gs_error_t gs_log_group_register_internal(gs_log_group_t *group)
{
// check if appender is already in the list and find last node
gs_log_list_t * parent = &g_log_groups; // there will always be at least 1 group -> default
for (; parent; parent = parent->next) {
if ((parent->data.group == group) || (strcasecmp(group->name, parent->data.group->name) == 0)) {
return GS_ERROR_EXIST;
}
if (parent->next == NULL) {
break;
}
}
gs_log_list_t * new_group_node = calloc(1, sizeof(*new_group_node));
if (new_group_node == NULL) {
return GS_ERROR_ALLOC;
}
new_group_node->data.group = group;
// add to list - must be done last, iterating list can be done without locking
parent->next = new_group_node;
return GS_OK;
}
gs_error_t gs_log_group_register(gs_log_group_t *group)
{
GS_CHECK_ARG(group != NULL);
GS_CHECK_ARG(gs_string_empty(group->name) == false);
gs_log_init_internal();
gs_lock_lock();
gs_error_t error = gs_log_group_register_internal(group);
gs_lock_unlock();
return error;
}
bool gs_log_group_is_level_enabled(gs_log_group_t *group, gs_log_level_t level)
{
return ((group->mask & level) > 0);
}
gs_error_t gs_log_appender_iterate(const char * name, void * ctx, gs_log_appender_iterator_t iter)
{
const bool all = (gs_string_empty(name) || gs_log_is_group_all(name));
bool found = false;
/* Iterate the dynamically registered log appenders: */
for (gs_log_list_t *node = &g_log_appenders; node; node = node->next) {
if (node->data.appender) {
if (all || gs_string_match(name, node->data.appender->name)) {
found = true;
bool cont = iter(ctx, node->data.appender);
if (cont == false) {
return GS_OK;
}
}
}
}
return found ? GS_OK : GS_ERROR_NOT_FOUND;
}
struct gs_log_history_ctx {
gs_log_record_iterator_t iter;
void *ctx;
};
static bool gs_log_history_iterator(void* ctx, gs_log_appender_t *appender)
{
struct gs_log_history_ctx *hist_ctx = ctx;
if (appender->drv->hist) {
appender->drv->hist(appender, hist_ctx->ctx, hist_ctx->iter);
}
return true;
}
gs_error_t gs_log_appender_history_iterate(const char * name, void * ctx, gs_log_record_iterator_t iter)
{
struct gs_log_history_ctx hist_ctx = {.iter=iter, .ctx = ctx};
return gs_log_appender_iterate(name, &hist_ctx, gs_log_history_iterator);
}
static gs_error_t gs_log_appender_register_internal(gs_log_appender_t *appender)
{
if (g_log_appenders.data.appender == NULL) {
// first appender
g_log_appenders.data.appender = appender;
if (appender->drv->init) {
gs_error_t error = appender->drv->init(appender);
if (error) {
g_log_appenders.data.appender = NULL;
return error;
}
}
return GS_OK;
}
// check if appender is already in the list and find last node
gs_log_list_t * parent = &g_log_appenders;
for (; parent; parent = parent->next) {
if ((parent->data.appender == appender) || (strcasecmp(parent->data.appender->name, appender->name) == 0)) {
return GS_ERROR_EXIST;
}
if (parent->next == NULL) {
break;
}
}
gs_log_list_t *new_appender = calloc(1, sizeof(*new_appender));
if (new_appender == NULL) {
return GS_ERROR_ALLOC;
}
new_appender->data.appender = appender;
if (appender->drv->init) {
gs_error_t error = appender->drv->init(appender);
if (error) {
free(new_appender);
return error;
}
}
// add to list - must be done last, iterating list can be done without locking
parent->next = new_appender;
return GS_OK;
}
gs_error_t gs_log_appender_register(gs_log_appender_t *appender)
{
GS_CHECK_ARG(appender != NULL);
GS_CHECK_ARG(gs_string_empty(appender->name) == false);
GS_CHECK_ARG(appender->drv != NULL);
GS_CHECK_ARG(appender->drv->append != NULL);
gs_log_init_internal();
gs_lock_lock();
gs_error_t error = gs_log_appender_register_internal(appender);
gs_lock_unlock();
return error;
}
gs_error_t gs_log_appender_add(gs_log_appender_t *appender, uint16_t count)
{
GS_CHECK_ARG(appender != NULL);
GS_CHECK_ARG(count != 0);
gs_error_t error = GS_OK;
for (uint16_t i = 0; i < count; i++) {
gs_error_t tmp_error = gs_log_appender_register(&appender[i]);
if ((error == GS_OK) && tmp_error) {
error = tmp_error;
}
}
return error;
}
static bool iter_set_appender_level_mask(void * ctx_in, gs_log_appender_t * appender)
{
uint8_t * level_mask = ctx_in;
appender->mask = *level_mask;
return true;
}
gs_error_t gs_log_appender_set_level_mask(const char * appender_name, uint8_t mask)
{
if (gs_string_empty(appender_name)) {
return GS_ERROR_HANDLE;
}
return gs_log_appender_iterate(appender_name, &mask, iter_set_appender_level_mask);
}
static bool iter_get_appender_level_mask(void * ctx_in, gs_log_appender_t * appender)
{
uint8_t * level_mask = ctx_in;
*level_mask = appender->mask;
return true;
}
gs_error_t gs_log_appender_get_level_mask(const char * appender_name, uint8_t *mask)
{
if (gs_string_empty(appender_name)) {
return GS_ERROR_HANDLE;
}
return gs_log_appender_iterate(appender_name, mask, iter_get_appender_level_mask);
}
// Appender register/de-register iterator context
typedef struct {
gs_log_appender_t *appender;
gs_error_t ret;
} iter_group_appender_t;
static bool iter_log_appender_add(void *ctx, gs_log_group_t *group)
{
iter_group_appender_t* in = ctx;
gs_log_list_t * last_elem = group->appenders;
for (gs_log_list_t *elem = group->appenders; elem; elem = elem->next) {
last_elem = elem;
if (elem->data.appender == in->appender) {
in->ret = GS_ERROR_EXIST;
return true;
}
}
in->ret = GS_ERROR_ALLOC;
gs_log_list_t *new_appender = calloc(1, sizeof(*new_appender));
if (new_appender) {
new_appender->data.appender = in->appender;
new_appender->next = 0;
if (last_elem != NULL) {
last_elem->next = new_appender;
} else {
group->appenders = new_appender;
}
in->ret = GS_OK;
}
return true;
}
gs_error_t gs_log_group_register_appender(const char * group_name, const char * appender_name)
{
gs_log_appender_t *appender = NULL;
for (gs_log_list_t *elem = &g_log_appenders; elem; elem = elem->next) {
if (elem->data.appender) {
if (strcasecmp(elem->data.appender->name, appender_name) == 0) {
appender = elem->data.appender;
break;
}
}
}
if (NULL == appender) {
return GS_ERROR_NOT_FOUND;
}
iter_group_appender_t ctx = {.appender = appender, .ret = GS_OK};
gs_error_t ret = GS_OK;
if (strcasecmp(group_name, GS_LOG_GROUP_ROOT) == 0) {
iter_log_appender_add(&ctx, &g_log_group_root);
} else {
ret = gs_log_group_iterate(group_name, &ctx, iter_log_appender_add);
}
if (ret == GS_OK) {
ret = ctx.ret;
}
return ret;
}
static bool iter_log_appender_remove(void *ctx, gs_log_group_t *group)
{
iter_group_appender_t* in = ctx;
in->ret = GS_ERROR_NOT_FOUND;
gs_log_list_t * last_elem = group->appenders;
for (gs_log_list_t *elem = group->appenders; elem; elem = elem->next) {
if (elem->data.appender == in->appender) {
if (elem == group->appenders) {
group->appenders = elem->next;
}
last_elem->next = elem->next;
free(elem);
in->ret = GS_OK;
break;
}
last_elem = elem;
}
return true;
}
gs_error_t gs_log_group_deregister_appender(const char * group_name, const char * appender_name)
{
gs_log_appender_t *appender = NULL;
for (gs_log_list_t *elem = &g_log_appenders; elem; elem = elem->next) {
if (elem->data.appender) {
if (strcasecmp(elem->data.appender->name, appender_name) == 0) {
appender = elem->data.appender;
break;
}
}
}
if (NULL == appender) {
return GS_ERROR_NOT_FOUND;
}
iter_group_appender_t ctx = {.appender = appender, .ret = GS_OK};
gs_error_t ret;
if (strcasecmp(group_name, GS_LOG_GROUP_ROOT) == 0) {
ret = iter_log_appender_remove(&ctx, &g_log_group_root);
} else {
ret = gs_log_group_iterate(group_name, &ctx, iter_log_appender_remove);
}
if (ret == GS_OK) {
ret = ctx.ret;
}
return ret;
}
gs_error_t gs_log_group_appender_iterate(gs_log_group_t * group, void * ctx, gs_log_appender_iterator_t iter)
{
GS_CHECK_ARG(group != NULL);
bool found = false;
for (gs_log_list_t *elem = group->appenders; elem; elem = elem->next) {
found = true;
iter(ctx, elem->data.appender);
}
/* Iterate root appenders */
for (gs_log_list_t *elem = g_log_group_root.appenders; elem; elem = elem->next) {
found = true;
iter(ctx, elem->data.appender);
}
return found ? GS_OK : GS_ERROR_NOT_FOUND;
}
static inline void gs_log_process_appenders(const gs_log_list_t * it, gs_log_level_t level,
const gs_log_group_t * group, gs_timestamp_t* ts, bool from_isr, const char * format, va_list va)
{
for (; it; it = it->next) {
gs_log_appender_t* appender = it->data.appender;
if ((appender->mask & (1 << level)) == 0) {
continue;
}
if (from_isr == false) {
// log from none ISR context
appender->drv->append(appender, level, group, ts, format, va);
} else if (appender->drv->append_isr) {
// log from ISR (Interrupt Service Routine) context
appender->drv->append_isr(appender, level, group, ts, format, va);
}
}
}
static inline void gs_log_common_va(gs_log_level_t level, gs_log_group_t * group, bool from_isr, const char * format, va_list va)
{
// get time as soon as possible
gs_timestamp_t ts;
gs_clock_get_time(&ts);
// only needed if someone call function directly - otherwise the log macro has set it to a valid group
if (group == NULL) {
group = LOG_DEFAULT;
}
// check level mask for current group (this will nearly always be true, because the log macro has done the checking
if (group->mask & (1 << level)) {
// legacy - if log hasn't been initialized, this will initialize with console output enabled.
gs_log_init_internal();
if (group->appenders) {
gs_log_process_appenders(group->appenders, level, group, &ts, from_isr, format, va);
}
if (group->additivity) {
/* Call root appenders */
gs_log_process_appenders(g_log_group_root.appenders, level, group, &ts, from_isr, format, va);
}
}
}
void gs_log(gs_log_level_t level, gs_log_group_t * group, const char * format, ...)
{
va_list va_args;
va_start(va_args, format);
gs_log_common_va(level, group, false, format, va_args);
va_end(va_args);
}
void gs_log_isr(gs_log_level_t level, gs_log_group_t * group, const char * format, ...)
{
va_list va_args;
va_start(va_args, format);
gs_log_common_va(level, group, true, format, va_args);
va_end(va_args);
}
void gs_log_va(gs_log_level_t level, gs_log_group_t * group, const char * format, va_list args)
{
gs_log_common_va(level, group, false, format, args);
}
void gs_log_set_print_color(bool color)
{
g_print_no_color = (color == false);
}
const char * gs_log_level_to_color_begin(gs_log_level_t level)
{
if (g_print_no_color) {
return "";
}
switch (level) {
case LOG_ERROR: return "\E[1;31m"; // Red
case LOG_WARNING: return "\E[0;33m"; // Yellow
case LOG_NOTICE:
case LOG_INFO: return "\E[0;32m"; // Green
case LOG_DEBUG: return "\E[0;34m"; // Blue
default:
case LOG_TRACE: return "\E[0;35m"; // Magenta
}
}
const char * gs_log_level_to_color_end(void)
{
if (g_print_no_color) {
return "";
}
return "\E[0m";
}
uint8_t gs_log_level_to_mask(gs_log_level_t level)
{
/* Enable all levels with priority above the set level */
uint8_t level_mask = (0xFF << level) & LOG_ALL_MASK;
return level_mask;
}
static bool iter_flush_appender(void * ctx_in, gs_log_appender_t * appender)
{
if (appender->drv->flush) {
appender->drv->flush(appender);
}
return true;
}
gs_error_t gs_log_appender_flush_all()
{
return gs_log_appender_iterate("", NULL, iter_flush_appender);
}
gs_error_t gs_log_init(bool with_console_appender)
{
gs_error_t error = GS_OK;
gs_lock_init(); // ignore result, this is the log system
if (g_log_groups.data.group == NULL) {
// default log group -> mark log as initialized
g_log_groups.data.group = LOG_DEFAULT;
// register console log appender
if (with_console_appender) {
error = gs_log_appender_register(&gs_log_appender_console);
if (error == GS_OK) {
error = gs_log_group_register_appender(GS_LOG_GROUP_ROOT, gs_log_appender_console.name);
}
}
}
return error;
}

42
gomspace/libutil/src/rtc.c Normal file
View File

@ -0,0 +1,42 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/rtc.h>
static const gs_rtc_driver_t * rtc_driver;
static void * rtc_driver_data;
gs_error_t gs_rtc_register(const gs_rtc_driver_t * driver, void * driver_data)
{
rtc_driver = driver;
rtc_driver_data = driver_data;
return GS_OK;
}
gs_error_t gs_rtc_supported(void)
{
return rtc_driver ? GS_OK : GS_ERROR_NOT_SUPPORTED;
}
gs_error_t gs_rtc_get_time(gs_timestamp_t * time)
{
if (time == NULL) {
return GS_ERROR_ARG;
}
if (rtc_driver && rtc_driver->get_time) {
return rtc_driver->get_time(rtc_driver_data, time);
}
return GS_ERROR_NOT_SUPPORTED;
}
gs_error_t gs_rtc_set_time(const gs_timestamp_t * time)
{
if (time == NULL) {
return GS_ERROR_ARG;
}
if (rtc_driver && rtc_driver->set_time) {
return rtc_driver->set_time(rtc_driver_data, time);
}
return GS_ERROR_NOT_SUPPORTED;
}

81
gomspace/libutil/src/stdio.c Normal file
View File

@ -0,0 +1,81 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/stdio.h>
#include <stdarg.h>
gs_error_t gs_stdio_get(char * buf, size_t len)
{
while (len > 0) {
int ch;
gs_error_t error = gs_stdio_getchar(&ch);
if (error) {
return error;
}
*buf++ = ch;
--len;
}
return GS_OK;
}
gs_error_t gs_stdio_put(const char * buf, size_t len, bool text)
{
while (len > 0) {
if ((*buf == '\n') && text) {
gs_stdio_putchar('\r');
}
gs_stdio_putchar(*buf++);
--len;
}
return GS_OK;
}
void gs_color_printf(gs_color_printf_t color_arg, const char * format, ...)
{
va_list args;
va_start(args, format);
if ((color_arg & GS_COLOR_ATTRS) == GS_COLOR_BOLD) {
printf("\033[1;");
} else {
printf("\033[0;");
}
switch(color_arg & GS_COLOR_COLORS) {
case GS_COLOR_NONE:
printf("0m");
break;
case GS_COLOR_BLACK:
printf("30m");
break;
case GS_COLOR_RED:
printf("31m");
break;
case GS_COLOR_GREEN:
printf("32m");
break;
case GS_COLOR_YELLOW:
printf("33m");
break;
case GS_COLOR_BLUE:
printf("34m");
break;
case GS_COLOR_MAGENTA:
printf("35m");
break;
case GS_COLOR_CYAN:
printf("36m");
break;
case GS_COLOR_WHITE:
printf("37m");
break;
default:
break;
}
vprintf(format, args);
printf("\033[0m");
va_end(args);
}

746
gomspace/libutil/src/string.c Normal file
View File

@ -0,0 +1,746 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/string.h>
#include <gs/util/minmax.h>
#include <gs/util/check.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#if (__AVR__ == 0)
#include <math.h>
#endif
#ifndef GS_STRING_GET_SUBOPTION_UNIT_TEST
#define GS_STRING_GET_SUBOPTION_UNIT_TEST 0
#endif
const char * gs_string_skip_leading_spaces(const char * string)
{
if (string) {
for (; *string == ' '; ++string);
}
return string;
}
gs_error_t gs_string_to_int32(const char * string, int32_t * return_value)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (!(isdigit((int)string[0]) || (string[0] == '-'))) {
return GS_ERROR_DATA;
}
int32_t tmp;
uint8_t base = 10;
// check for hexadecimal notation
if ((string[0] == '0') && ((string[1] == 'x') || (string[1] == 'X')))
{
base = 16;
}
const char *desired_end = string + strlen(string);
// The desired end should point to that last non-space char in the string
while ((isspace((int)desired_end[0]) || (desired_end[0] == '\0')) && (desired_end > string))
{
desired_end--;
}
char *end;
gs_error_t err = GS_OK;
tmp = gs_string_strto32int(string, &end, base, &err);
if (err != GS_OK)
{
return err;
}
if (desired_end != end-1)
{
return GS_ERROR_DATA;
}
if (return_value)
{
*return_value = tmp;
}
return GS_OK;
}
gs_error_t gs_string_to_int8(const char * string, int8_t * return_value)
{
int32_t value;
gs_error_t error = gs_string_to_int32(string, &value);
if (error == GS_OK) {
if ((value >= INT8_MIN) && (value <= INT8_MAX)) {
if (return_value) {
*return_value = (int8_t) value;
}
} else {
error = GS_ERROR_OVERFLOW;
}
}
return error;
}
gs_error_t gs_string_to_uint8(const char * string, uint8_t * return_value)
{
uint32_t value;
gs_error_t error = gs_string_to_uint32(string, &value);
if (error == GS_OK) {
if (value <= UINT8_MAX) {
if (return_value) {
*return_value = (uint8_t) value;
}
} else {
error = GS_ERROR_OVERFLOW;
}
}
return error;
}
gs_error_t gs_string_to_int16(const char * string, int16_t * return_value)
{
int32_t value;
gs_error_t error = gs_string_to_int32(string, &value);
if (error == GS_OK) {
if ((value >= INT16_MIN) && (value <= INT16_MAX)) {
if (return_value) {
*return_value = (int16_t) value;
}
} else {
error = GS_ERROR_OVERFLOW;
}
}
return error;
}
gs_error_t gs_string_to_uint16(const char * string, uint16_t * return_value)
{
uint32_t value;
gs_error_t error = gs_string_to_uint32(string, &value);
if (error == GS_OK) {
if (value <= UINT16_MAX) {
if (return_value) {
*return_value = (uint16_t) value;
}
} else {
error = GS_ERROR_OVERFLOW;
}
}
return error;
}
gs_error_t gs_string_to_uint32(const char * string, uint32_t * return_value)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (!isdigit((int)string[0])) {
return GS_ERROR_DATA;
}
uint32_t tmp;
uint8_t base = 10;
// check for hexadecimal notation
if ((string[0] == '0') && ((string[1] == 'x') || (string[1] == 'X')))
{
base = 16;
}
const char *desired_end = string + strlen(string);
// The desired end should point to that last non-space char in the string
while ((isspace((int)desired_end[0]) || desired_end[0] == 0) && (desired_end > string))
{
desired_end--;
}
char *end;
gs_error_t err = GS_OK;
tmp = gs_string_strto32uint(string, &end, base, &err);
if (err != GS_OK)
{
return err;
}
if (desired_end != end-1)
{
return GS_ERROR_DATA;
}
if (return_value) {
*return_value = tmp;
}
return GS_OK;
}
gs_error_t gs_string_to_uint64(const char * string, uint64_t * return_value)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (!isdigit((int)string[0]))
{
return GS_ERROR_DATA;
}
uint64_t tmp;
uint8_t base = 10;
// check for hexadecimal notation
if ((string[0] == '0') && ((string[1] == 'x') || (string[1] == 'X'))) {
base = 16;
}
const char *desired_end = string + strlen(string);
// The desired end should point to that last non-space char in the string
while ((isspace((int)desired_end[0]) || desired_end[0] == 0) && (desired_end > string))
{
desired_end--;
}
char *end;
gs_error_t err = GS_OK;
tmp = gs_string_strto64uint(string, &end, base, &err);
if (err != GS_OK)
{
return err;
}
if (desired_end != end-1)
{
return GS_ERROR_DATA;
}
if (return_value) {
*return_value = tmp;
}
return GS_OK;
}
gs_error_t gs_string_to_int64(const char * string, int64_t * return_value)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (!(isdigit((int)string[0]) || (string[0] == '-')))
{
return GS_ERROR_DATA;
}
int64_t tmp;
uint8_t base = 10;
// check for hexadecimal notation
if ((string[0] == '0') && ((string[1] == 'x') || (string[1] == 'X'))) {
base = 16;
}
const char *desired_end = string + strlen(string);
// The desired end should point to that last non-space char in the string
while ((isspace((int)desired_end[0]) || desired_end[0] == 0) && (desired_end > string))
{
desired_end--;
}
char *end;
gs_error_t err = GS_OK;
tmp = gs_string_strto64int(string, &end, base, &err);
if (err != GS_OK)
{
return err;
}
if (desired_end != end-1)
{
return GS_ERROR_DATA;
}
if (return_value) {
*return_value = tmp;
}
return GS_OK;
}
gs_error_t gs_string_hex_to_uint32(const char * string, uint32_t * return_value)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (!isxdigit((int)string[0])) {
return GS_ERROR_DATA;
}
const char *desired_end = string + strlen(string);
// The desired end should point to that last non-space char in the string
while ((isspace((int)desired_end[0]) || desired_end[0] == 0) && (desired_end > string))
{
desired_end--;
}
char *end;
gs_error_t err = GS_OK;
uint32_t tmp = gs_string_strto32uint(string, &end, 16, &err);
if (err != GS_OK)
{
return err;
}
if (desired_end != end-1)
{
return GS_ERROR_DATA;
}
if (return_value) {
*return_value = tmp;
}
return GS_OK;
}
gs_error_t gs_string_hex_to_uint64(const char * string, uint64_t * return_value)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (!isxdigit((int)string[0]))
{
return GS_ERROR_DATA;
}
const char *desired_end = string + strlen(string);
// The desired end should point to that last non-space char in the string
while ((isspace((int)desired_end[0]) || desired_end[0] == 0) && (desired_end > string))
{
desired_end--;
}
char *end;
gs_error_t err = GS_OK;
uint64_t tmp = gs_string_strto64uint(string, &end, 16, &err);
if (err != GS_OK)
{
return err;
}
if (desired_end != end-1)
{
return GS_ERROR_DATA;
}
if (return_value) {
*return_value = tmp;
}
return GS_OK;
}
#if (__AVR__ == 0)
gs_error_t gs_string_to_float(const char * string, float * pvalue)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (gs_string_empty(string)) {
return GS_ERROR_DATA;
}
// float strtof(const char *nptr, char **endptr);
char * endp = NULL;
float tmp = strtof(string, &endp);
//if ((endp == NULL) || (gs_string_empty(gs_string_skip_leading_spaces(endp)) == false) || (string == endp)) {
if ((endp == NULL) || (gs_string_empty(gs_string_skip_leading_spaces(endp)) == false) || (string == endp) || isinf(tmp)) {
return GS_ERROR_DATA;
}
if (pvalue) {
*pvalue = tmp;
}
return GS_OK;
}
gs_error_t gs_string_to_double(const char * string, double * pvalue)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (gs_string_empty(string)) {
return GS_ERROR_DATA;
}
// double strtod(const char *nptr, char **endptr);
char * endp = NULL;
double tmp = strtod(string, &endp);
if ((endp == NULL) || (gs_string_empty(gs_string_skip_leading_spaces(endp)) == false) || isinf(tmp)) {
return GS_ERROR_DATA;
}
if (pvalue) {
*pvalue = tmp;
}
return GS_OK;
}
#endif
#define GS_STRING_BOOL_TRUE "true"
#define GS_STRING_BOOL_FALSE "false"
const char * gs_string_from_bool(bool value)
{
if (value) {
return GS_STRING_BOOL_TRUE;
} else {
return GS_STRING_BOOL_FALSE;
}
}
gs_error_t gs_string_to_bool(const char * string, bool * return_value)
{
string = gs_string_skip_leading_spaces(string);
if (string == NULL) {
return GS_ERROR_ARG;
}
if (string[0] == 0) {
return GS_ERROR_DATA;
}
bool value = false;
if (strcasecmp(string, GS_STRING_BOOL_TRUE) == 0) {
value = true;
} else if (strcasecmp(string, GS_STRING_BOOL_FALSE) == 0) {
value = false;
} else if (strcasecmp(string, "on") == 0) {
value = true;
} else if (strcasecmp(string, "off") == 0) {
value = false;
} else if (strcasecmp(string, "1") == 0) {
value = true;
} else if (strcasecmp(string, "0") == 0) {
value = false;
} else if (strcasecmp(string, "yes") == 0) {
value = true;
} else if (strcasecmp(string, "no") == 0) {
value = false;
} else {
return GS_ERROR_DATA;
}
if (return_value) {
*return_value = value;
}
return GS_OK;
}
char * gs_string_bytesize(long n, char *buf, size_t buf_size)
{
char postfix = 'B';
double size = (double) n;
if (n >= 1048576) {
size /= 1048576.0;
postfix = 'M';
} else if (n >= 1024) {
size /= 1024.0;
postfix = 'K';
}
snprintf(buf, buf_size, "%.1f%c", size, postfix);
return buf;
}
gs_error_t gs_string_to_pointer(const char * string, void ** value)
{
#if __LP64__
uint64_t tmp;
gs_error_t error = gs_string_to_uint64(string, &tmp);
if ((error == GS_OK) && value) {
*value = GS_TYPES_UINT2PTR(tmp);
}
return error;
#else
uint32_t tmp;
gs_error_t error = gs_string_to_uint32(string, &tmp);
if ((error == GS_OK) && value) {
*value = GS_TYPES_UINT2PTR(tmp);
}
return error;
#endif
}
bool gs_string_empty(const char * string)
{
if ((string == NULL) || (string[0] == 0)) {
return true;
}
return false;
}
bool gs_string_match(const char * pattern, const char * string)
{
if (string && pattern) {
while (*string || *pattern) {
int p = tolower((int)*pattern);
int s = tolower((int)*string);
if (*pattern == '*') {
++pattern;
p = tolower((int)*pattern);
for (; *string && (tolower((int)*string) != p); ++string);
s = tolower((int)*string);
}
if (s != p) {
return false;
}
if (s) {
++string;
}
if (p) {
++pattern;
}
}
if ((*string == 0) && (*pattern == 0)) {
return true;
}
}
return false;
}
bool gs_string_has_wildcards(const char * string)
{
if (strchr(string, '*')) {
return true;
}
// future wildcard
//if (strchr(str, '?')) {
// return true;
//}
return false;
}
void gs_string_trim(char * buffer, size_t buffer_size)
{
// remove trailing stuff
int len = strnlen(buffer, buffer_size);
if (len) {
for (int i = (len - 1); i >= 0; --i) {
if (isspace((int)buffer[i])) {
buffer[i] = 0;
} else {
break;
}
}
}
char * start;
for (start = buffer; *start && isspace((int)*start); ++start);
if (*start && (start != buffer)) {
// move chars up
for (; *start; ++start) {
*buffer++ = *start;
}
*buffer = 0;
}
}
bool gs_string_endswith(const char * string, const char * endswith)
{
if (string == NULL || endswith == NULL) {
return false;
}
int str_len = strlen(string);
int endswith_len = strlen(endswith);
return (str_len >= endswith_len) &&
(0 == strcmp(string + (str_len-endswith_len), endswith));
}
static size_t suboption_len(const char * ref, const char * end)
{
if (ref) {
size_t len = (end) ? ((size_t)(end - ref)) : strlen(ref);
for (; len && (ref[len - 1] == ' '); --len);
return len;
}
return 0;
}
static gs_error_t suboption_copy(const char * data, size_t len, char * buf, size_t buf_size, gs_error_t error)
{
if (len >= buf_size) {
error = GS_ERROR_OVERFLOW;
len = (buf_size - 1);
}
if (data == NULL) {
len = 0;
} else {
strncpy(buf, data, len);
}
buf[len] = 0;
return error;
}
gs_error_t gs_string_get_suboption(const char * options, const char * suboption, char * buf, size_t buf_size)
{
GS_CHECK_ARG(options != NULL);
GS_CHECK_ARG((buf != NULL) && (buf_size > 0));
const char * next = options;
for (;next;) {
const char * key = next;
if (*key == ',') {
key = NULL; // no key-value
}
next = strchr(next, ',');
const char * value = NULL;
if (key) {
for (; *key == ' '; ++key);
value = strchr(key, '=');
if (value == NULL) {
// no value
} else if (next && (value >= next)) {
// no value
value = NULL;
}
}
const unsigned int key_len = suboption_len(key, value ? value : next);
if (value) {
if (*value == '=') {
++value;
}
for (; *value == ' '; ++value);
}
const unsigned int value_len = suboption_len(value, next);
if (GS_STRING_GET_SUBOPTION_UNIT_TEST) { // -> #define
printf(" key=[%.*s], len=%u, value=[%.*s], len=%u, next=[%s]\n",
key_len, key ? key : "", key_len,
value_len, value ? value : "", value_len,
next ? next : "");
}
// if suboption is empty, it means get value of first element - ignoring any key
if (gs_string_empty(suboption)) {
if (value) {
return suboption_copy(value, value_len, buf, buf_size, GS_OK);
}
if (key) {
return suboption_copy(key, key_len, buf, buf_size, GS_OK);
}
return suboption_copy(NULL, 0, buf, buf_size, GS_OK); // empty
}
if ((key_len == strlen(suboption)) && (strncasecmp(key, suboption, key_len) == 0)) {
return suboption_copy(value, value_len, buf, buf_size, GS_OK);
}
if (next) {
++next;
if (next[0] == 0) {
next = NULL;
}
}
}
// not found - return default
return suboption_copy(NULL, 0, buf, buf_size, GS_ERROR_NOT_FOUND);
}
static const char * _suboption_name(const char * suboption)
{
if (gs_string_empty(suboption)) {
return "first suboption";
}
return suboption;
}
#define _get_suboption(_type) \
char buf[20]; \
gs_error_t error = gs_string_get_suboption(options, suboption, buf, sizeof(buf)); \
if (error == GS_OK) { \
error = gs_string_to_##_type(buf, value); \
} \
if (error) { \
if (error == GS_ERROR_NOT_FOUND) { \
error = GS_OK; \
} else { \
log_error("Failed to extract suboption [%s] from [%s], error: %d", _suboption_name(suboption), options, error); \
} \
*value = def; \
} \
return error; \
gs_error_t gs_string_get_suboption_uint8(const char * options, const char * suboption, uint8_t def, uint8_t * value)
{
_get_suboption(uint8)
}
gs_error_t gs_string_get_suboption_uint16(const char * options, const char * suboption, uint16_t def, uint16_t * value)
{
_get_suboption(uint16)
}
gs_error_t gs_string_get_suboption_uint32(const char * options, const char * suboption, uint32_t def, uint32_t * value)
{
_get_suboption(uint32)
}
gs_error_t gs_string_get_suboption_string(const char * options, const char * suboption, const char * def, char * buf, size_t buf_size)
{
gs_error_t error = gs_string_get_suboption(options, suboption, buf, buf_size);
if (error) {
if (error == GS_ERROR_NOT_FOUND) {
error = GS_OK;
}
error = suboption_copy(def, def ? strlen(def) : 0, buf, buf_size, error);
}
return error;
}
gs_error_t gs_string_get_suboption_bool(const char * options, const char * suboption, bool def, bool * value)
{
char buf[20];
gs_error_t error = gs_string_get_suboption(options, suboption, buf, sizeof(buf));
if (error == GS_OK) {
if (gs_string_empty(buf) || (suboption && (strcasecmp(suboption, buf) == 0))) {
// this means 'true', a=21,active,a=22
*value = true;
} else {
error = gs_string_to_bool(buf, value);
}
}
if (error) {
if (error == GS_ERROR_NOT_FOUND) {
error = GS_OK;
} else {
log_error("Failed to extract suboption [%s] from [%s], error: %d", _suboption_name(suboption), options, error);
}
*value = def;
}
return error;
}

399
gomspace/libutil/src/strtoint.c Normal file
View File

@ -0,0 +1,399 @@
/*-
* Copyright (c) 1990 The Regents of the University of California.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. [rescinded 22 July 1999]
* 4. Neither the name of the University nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*/
/*
@deftypefn Supplemental {long int} strtol (const char *@var{string}, @
char **@var{endptr}, int @var{base})
@deftypefnx Supplemental {unsigned long int} strtoul (const char *@var{string}, @
char **@var{endptr}, int @var{base})
The @code{strtol} function converts the string in @var{string} to a
long integer value according to the given @var{base}, which must be
between 2 and 36 inclusive, or be the special value 0. If @var{base}
is 0, @code{strtol} will look for the prefixes @code{0} and @code{0x}
to indicate bases 8 and 16, respectively, else default to base 10.
When the base is 16 (either explicitly or implicitly), a prefix of
@code{0x} is allowed. The handling of @var{endptr} is as that of
@code{strtod} above. The @code{strtoul} function is the same, except
that the converted value is unsigned.
@end deftypefn
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#ifdef HAVE_LIMITS_H
#include <limits.h>
#endif
#ifdef HAVE_SYS_PARAM_H
#include <sys/param.h>
#endif
#include <ctype.h>
#include <gs/util/error.h>
#include <gs/util/string.h>
/*
* Convert a string to a long integer.
*
* Ignores `locale' stuff. Assumes that the upper and lower case
* alphabets and digits are each contiguous.
*/
int32_t gs_string_strto32int(const char *nptr, char **endptr, uint8_t base, gs_error_t * err)
{
register const char *s = nptr;
register uint32_t acc;
register int c;
register uint32_t cutoff;
register int neg = 0, any, cutlim;
/*
* Skip white space and pick up leading +/- sign if any.
* If base is 0, allow 0x for hex and 0 for octal, else
* assume decimal; if base is already 16, allow 0x.
*/
do
{
c = *s++;
} while (isspace(c));
if (c == '-')
{
neg = 1;
c = *s++;
} else if (c == '+')
{
c = *s++;
}
if ((base == 0 || base == 16) &&
c == '0' && (*s == 'x' || *s == 'X'))
{
c = s[1];
s += 2;
base = 16;
}
if (base == 0)
{
base = c == '0' ? 8 : 10;
}
/*
* Compute the cutoff value between legal numbers and illegal
* numbers. That is the largest legal value, divided by the
* base. An input number that is greater than this value, if
* followed by a legal input character, is too big. One that
* is equal to this value may be valid or not; the limit
* between valid and invalid numbers is then based on the last
* digit. For instance, if the range for longs is
* [-2147483648..2147483647] and the input base is 10,
* cutoff will be set to 214748364 and cutlim to either
* 7 (neg==0) or 8 (neg==1), meaning that if we have accumulated
* a value > 214748364, or equal but the next digit is > 7 (or 8),
* the number is too big, and we will return a range error.
*
* Set any if any `digits' consumed; make it negative to indicate
* overflow.
*/
cutoff = neg ? -(uint32_t)INT32_MIN : INT32_MAX;
cutlim = cutoff % (uint32_t)base;
cutoff /= (uint32_t)base;
for (acc = 0, any = 0;; c = *s++)
{
if (isdigit(c))
{
c -= '0';
}
else if (isalpha(c))
{
c -= isupper(c) ? 'A' - 10 : 'a' - 10;
}
else
{
break;
}
if (c >= base)
{
break;
}
if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
{
any = -1;
}
else
{
any = 1;
acc *= base;
acc += c;
}
}
if (any < 0)
{
acc = neg ? INT32_MIN : INT32_MAX;
*err = GS_ERROR_OVERFLOW;
} else if (neg)
{
acc = -acc;
}
if (endptr != 0)
{
*endptr = (char *) (any ? s - 1 : nptr);
}
return (acc);
}
/*
* Convert a string to an unsigned long integer.
*
* Ignores `locale' stuff. Assumes that the upper and lower case
* alphabets and digits are each contiguous.
*/
uint32_t gs_string_strto32uint(const char *nptr, char **endptr, uint8_t base, gs_error_t * err)
{
register const char *s = nptr;
register uint32_t acc;
register int32_t c;
register uint32_t cutoff;
register int32_t neg = 0, any, cutlim;
/*
* See strtol for comments as to the logic used.
*/
do
{
c = *s++;
} while (isspace(c));
if (c == '-')
{
neg = 1;
c = *s++;
} else if (c == '+')
{
c = *s++;
}
if ((base == 0 || base == 16) &&
c == '0' && (*s == 'x' || *s == 'X'))
{
c = s[1];
s += 2;
base = 16;
}
if (base == 0)
{
base = c == '0' ? 8 : 10;
}
cutoff = (uint32_t)UINT32_MAX / (uint32_t)base;
cutlim = (uint32_t)UINT32_MAX % (uint32_t)base;
for (acc = 0, any = 0;; c = *s++)
{
if (isdigit(c))
{
c -= '0';
}
else if (isalpha(c))
{
c -= isupper(c) ? 'A' - 10 : 'a' - 10;
}
else
{
break;
}
if (c >= base)
{
break;
}
if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
{
any = -1;
}
else
{
any = 1;
acc *= base;
acc += c;
}
}
if (any < 0)
{
acc = UINT32_MAX;
*err = GS_ERROR_OVERFLOW;
} else if (neg)
{
acc = -acc;
}
if (endptr != 0)
{
*endptr = (char *) (any ? s - 1 : nptr);
}
return (acc);
}
int64_t gs_string_strto64int(const char *nptr, char **endptr, uint8_t base, gs_error_t * err)
{
register const char *s = nptr;
register uint64_t acc;
register int32_t c;
register uint64_t cutoff;
register int32_t neg = 0, any, cutlim;
/*
* Skip white space and pick up leading +/- sign if any.
* If base is 0, allow 0x for hex and 0 for octal, else
* assume decimal; if base is already 16, allow 0x.
*/
do {
c = *s++;
} while (isspace(c));
if (c == '-') {
neg = 1;
c = *s++;
} else if (c == '+')
c = *s++;
if ((base == 0 || base == 16) &&
c == '0' && (*s == 'x' || *s == 'X')) {
c = s[1];
s += 2;
base = 16;
}
if (base == 0)
base = c == '0' ? 8 : 10;
/*
* Compute the cutoff value between legal numbers and illegal
* numbers. That is the largest legal value, divided by the
* base. An input number that is greater than this value, if
* followed by a legal input character, is too big. One that
* is equal to this value may be valid or not; the limit
* between valid and invalid numbers is then based on the last
* digit. For instance, if the range for longs is
* [-2147483648..2147483647] and the input base is 10,
* cutoff will be set to 214748364 and cutlim to either
* 7 (neg==0) or 8 (neg==1), meaning that if we have accumulated
* a value > 214748364, or equal but the next digit is > 7 (or 8),
* the number is too big, and we will return a range error.
*
* Set any if any `digits' consumed; make it negative to indicate
* overflow.
*/
cutoff = neg ? -(uint64_t)INT64_MIN : INT64_MAX;
cutlim = cutoff % (uint64_t)base;
cutoff /= (uint64_t)base;
for (acc = 0, any = 0;; c = *s++) {
if (isdigit(c))
c -= '0';
else if (isalpha(c))
c -= isupper(c) ? 'A' - 10 : 'a' - 10;
else
break;
if (c >= base)
break;
if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
any = -1;
else {
any = 1;
acc *= base;
acc += c;
}
}
if (any < 0) {
acc = neg ? INT64_MIN : INT64_MAX;
*err = GS_ERROR_OVERFLOW;
} else if (neg)
acc = -acc;
if (endptr != 0)
*endptr = (char *) (any ? s - 1 : nptr);
return (acc);
}
/*
* Convert a string to an unsigned long integer.
*
* Ignores `locale' stuff. Assumes that the upper and lower case
* alphabets and digits are each contiguous.
*/
uint64_t gs_string_strto64uint(const char *nptr, char **endptr, uint8_t base, gs_error_t * err)
{
register const char *s = nptr;
register uint64_t acc;
register int32_t c;
register uint64_t cutoff;
register int32_t neg = 0, any, cutlim;
/*
* See strtol for comments as to the logic used.
*/
do {
c = *s++;
} while (isspace(c));
if (c == '-') {
neg = 1;
c = *s++;
} else if (c == '+')
c = *s++;
if ((base == 0 || base == 16) &&
c == '0' && (*s == 'x' || *s == 'X')) {
c = s[1];
s += 2;
base = 16;
}
if (base == 0)
base = c == '0' ? 8 : 10;
cutoff = (uint64_t)UINT64_MAX / (uint64_t)base;
cutlim = (uint64_t)UINT64_MAX % (uint64_t)base;
for (acc = 0, any = 0;; c = *s++) {
if (isdigit(c))
c -= '0';
else if (isalpha(c))
c -= isupper(c) ? 'A' - 10 : 'a' - 10;
else
break;
if (c >= base)
break;
if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
any = -1;
else {
any = 1;
acc *= base;
acc += c;
}
}
if (any < 0) {
acc = UINT64_MAX;
*err = GS_ERROR_OVERFLOW;
} else if (neg)
acc = -acc;
if (endptr != 0)
*endptr = (char *) (any ? s - 1 : nptr);
return (acc);
}

63
gomspace/libutil/src/test/cmocka.c Normal file
View File

@ -0,0 +1,63 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/test/cmocka.h>
#include <gs/util/error.h>
#include <math.h>
void cm_print_error(const char * const format, ...) CMOCKA_PRINTF_ATTRIBUTE(1, 2);
#define EQUAL_TO_STRING(equal) (equal ? "!=" : "==")
void _gs_assert_int_equal(const intptr_t left, const intptr_t right, bool equal, const char * const file, const int line)
{
const bool cmp_equal = left == right;
if (cmp_equal != equal) {
cm_print_error("%ld %s %ld\n", left, EQUAL_TO_STRING(equal), right);
_fail(file, line);
}
}
void _gs_assert_uint_equal(const uintptr_t left, const uintptr_t right, bool equal, bool hex, const char * const file, const int line)
{
const bool cmp_equal = left == right;
if (cmp_equal != equal) {
if(hex) {
cm_print_error("0x%lx %s 0x%lx\n", left, EQUAL_TO_STRING(equal), right);
} else {
cm_print_error("%lu %s %lu\n", left, EQUAL_TO_STRING(equal), right);
}
_fail(file, line);
}
}
void _gs_assert_error_equal(const int left, const int right, bool equal, const char * const file, const int line)
{
const bool cmp_equal = left == right;
if (cmp_equal != equal) {
cm_print_error("%d(%s) %s %d(%s)\n", left, gs_error_string(left), EQUAL_TO_STRING(equal), right, gs_error_string(right));
_fail(file, line);
}
}
void _gs_assert_float_equal(const float left, const float right, const float diff, bool equal, const char * const file, const int line)
{
const bool cmp_equal = (fabsf(left - right) < diff);
if (cmp_equal != equal) {
cm_print_error("%e %s %e\n", left, EQUAL_TO_STRING(equal), right);
_fail(file, line);
}
}
void _gs_assert_double_equal(const double left, const double right, const double diff, bool equal, const char * const file, const int line)
{
const bool cmp_equal = (fabsf(left - right) < diff);
if (cmp_equal != equal) {
cm_print_error("%e %s %e\n", left, EQUAL_TO_STRING(equal), right);
_fail(file, line);
}
}

176
gomspace/libutil/src/test/command.c Normal file
View File

@ -0,0 +1,176 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/test/command.h>
#include <gs/util/test/cmocka.h>
#include <gs/util/error.h>
#include <gs/util/time.h>
#include <fnmatch.h>
#include <stdio.h>
void cm_print_error(const char * const format, ...) CMOCKA_PRINTF_ATTRIBUTE(1, 2);
struct results {
char group[32];
char key[32];
char value[128];
};
static FILE* g_output_file = 0;
static char g_stdout_buf[10000];
static char g_stdin_buf[1000];
static uint16_t g_stdin_idx;
static struct results g_results[100];
static uint32_t g_results_count = 0;
static gs_error_t test_set_result(gs_command_context_t *ctx, const char *group, const char *key, const char *value)
{
if (g_results_count >= GS_ARRAY_SIZE(g_results)) {
return GS_ERROR_ALLOC;
}
if (group) {
strncpy(g_results[g_results_count].group, group, sizeof(g_results[0].group));
}
if (key) {
strncpy(g_results[g_results_count].key, key, sizeof(g_results[0].key));
}
if (value) {
strncpy(g_results[g_results_count].value, value, sizeof(g_results[0].value));
}
g_results_count++;
return GS_OK;
}
static gs_error_t test_flush(gs_command_context_t *ctx)
{
fflush(ctx->out);
return GS_OK;
}
static gs_error_t test_wait_for_key(gs_command_context_t *ctx, int *ch, int timeout_ms)
{
if (g_stdin_buf[g_stdin_idx] == 0) {
gs_time_sleep_ms(timeout_ms);
return GS_ERROR_TIMEOUT;
}
*ch = g_stdin_buf[g_stdin_idx];
g_stdin_idx++;
return GS_OK;
}
static gs_command_io_functions_t g_test_io_functions = {
.set_result = test_set_result,
.flush = test_flush,
.wait_for_key = test_wait_for_key,
};
static gs_error_t prepare_validation(const char *input)
{
g_results_count = 0;
memset(g_stdout_buf, 0, sizeof(g_stdout_buf));
memset(g_stdin_buf, 0, sizeof(g_stdin_buf));
g_stdin_idx = 0;
if (input != NULL) {
memcpy(g_stdin_buf, input, strlen(input));
}
g_output_file = fmemopen(g_stdout_buf, sizeof(g_stdout_buf) - 1, "w");
if (!g_output_file) {
return GS_ERROR_ALLOC;
}
return GS_OK;
}
static bool match(const char *first, const char * second)
{
return (fnmatch(first, second, 0) == 0);
}
static gs_error_t do_validation(const char *expected)
{
fclose(g_output_file);
if (expected == NULL)
return GS_OK;
if (!match(expected, g_stdout_buf))
{
return GS_ERROR_DATA;
}
return GS_OK;
}
void _gs_assert_command_validate(const char *cmd, gs_error_t ret, gs_error_t cmd_ret, const char *std_in, const char *std_out,
const char * const file, const int line)
{
if (prepare_validation(std_in) != GS_OK)
{
cm_print_error("Validation function failed to allocate it's resources\n");
_fail(file, line);
}
gs_error_t _ret;
gs_error_t _command_ret = GS_OK;
_ret = gs_command_execute(cmd, &_command_ret, g_output_file, &g_test_io_functions, NULL);
if (_ret != ret) {
cm_print_error("Return: %d(%s) != %d(%s)\n", _ret, gs_error_string(_ret), ret, gs_error_string(ret));
_fail(file, line);
}
if (_ret == GS_OK) /* Only check CMD return if command execution succeeded */
{
if (_command_ret != cmd_ret) {
cm_print_error("Command return: %d(%s) != %d(%s)\n", _command_ret, gs_error_string(_command_ret),
cmd_ret, gs_error_string(cmd_ret));
_fail(file, line);
}
}
if (do_validation(std_out) != GS_OK)
{
//cm_print_error("Stdout <actual> != <expected>: \n%s\n!=\n%s\n", g_stdout_buf, std_out);
printf("Stdout <actual> != <expected>: \n%s\n!=\n%s\n", g_stdout_buf, std_out);
_fail(file, line);
}
return;
}
void _gs_assert_command_validate_last_result(unsigned int no, const char *group, const char *key, const char *value,
const char * const file, const int line)
{
if (no >= g_results_count)
{
cm_print_error("Result no: %d not available. Only %d results returned from command\n", no, g_results_count);
_fail(file, line);
}
if (group) {
if (!match(group, g_results[no].group)) {
cm_print_error("group: %s != %s\n", group, g_results[no].group);
_fail(file, line);
}
}
if (key) {
if (!match(key, g_results[no].key)) {
cm_print_error("key: %s != %s\n", key, g_results[no].key);
_fail(file, line);
}
}
if (value) {
if (!match(value, g_results[no].value)) {
cm_print_error("value: %s != %s\n", value, g_results[no].value);
_fail(file, line);
}
}
return;
}

165
gomspace/libutil/src/test/log.c Normal file
View File

@ -0,0 +1,165 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/test/log.h>
#include <gs/util/test/cmocka.h>
#include <gs/util/log/log.h>
#include <gs/util/log/appender/appender.h>
#include <gs/util/mutex.h>
#include <gs/util/bytebuffer.h>
#include <fnmatch.h>
#include <stdio.h>
#include <memory.h>
#include <fnmatch.h>
// cmocka
void cm_print_error(const char * const format, ...) CMOCKA_PRINTF_ATTRIBUTE(1, 2);
static gs_mutex_t g_lock;
#define GS_TEST_LOG_MAX_LEVELS 6
#define GS_TEST_LOG_MAX_LOG_MESSAGES 200
typedef struct {
// Overall count per log level.
unsigned int count[GS_TEST_LOG_MAX_LEVELS];
// Log messages.
struct {
// Level.
gs_log_level_t level;
// Format log message.
char msg[150];
} logs[GS_TEST_LOG_MAX_LOG_MESSAGES];
// Index of next entry in \a logs.
unsigned int next_log;
} gs_test_log_stats_t;
static gs_test_log_stats_t gs_test_log_stats;
static void log_callback(gs_log_appender_t *appender, gs_log_level_t level, const gs_log_group_t * group, const gs_timestamp_t * ts, const char * format, va_list va)
{
gs_mutex_lock(g_lock);
gs_test_log_stats.count[level]++;
{
gs_bytebuffer_t bb;
gs_bytebuffer_init(&bb,
gs_test_log_stats.logs[gs_test_log_stats.next_log].msg,
sizeof(gs_test_log_stats.logs[gs_test_log_stats.next_log].msg));
va_list my_va;
va_copy(my_va, va);
gs_bytebuffer_printf(&bb, "%s: ", group->name);
gs_bytebuffer_vprintf(&bb, format, my_va);
va_end(my_va);
gs_bytebuffer_get_as_string(&bb, NULL); // ensure NUL termination
gs_test_log_stats.logs[gs_test_log_stats.next_log].level = level;
++gs_test_log_stats.next_log;
if (gs_test_log_stats.next_log >= GS_TEST_LOG_MAX_LOG_MESSAGES) {
gs_test_log_stats.next_log = 0;
}
gs_test_log_stats.logs[gs_test_log_stats.next_log].msg[0] = 0; // clear next entry
gs_test_log_stats.logs[gs_test_log_stats.next_log].level = GS_TEST_LOG_MAX_LEVELS;
}
gs_mutex_unlock(g_lock);
}
static gs_log_appender_driver_t g_log_test_appender_driver = {
.init = 0,
.append = log_callback,
.append_isr = log_callback,
};
static gs_log_appender_t g_log_test_appender = {
.name = "test_appender",
.drv = &g_log_test_appender_driver,
.drv_config = NULL,
.drv_data = NULL,
.mask = LOG_ALL_MASK,
};
void gs_test_log_init(bool verbose)
{
gs_log_init(true);
if (g_lock == NULL) {
GS_ASSERT_ERROR_EQUAL(gs_mutex_create(&g_lock), GS_OK);
/* Add/Register appender - Only first time that init is called. */
gs_log_appender_add(&g_log_test_appender, 1);
gs_log_group_register_appender("root", "test_appender");
}
if (verbose) {
gs_log_appender_set_level_mask("console", LOG_ALL_MASK);
} else {
gs_log_appender_set_level_mask("console", 0);
}
gs_test_log_clear();
}
void gs_test_log_clear(void)
{
gs_mutex_lock(g_lock);
memset(&gs_test_log_stats, 0, sizeof(gs_test_log_stats));
gs_mutex_unlock(g_lock);
}
void gs_assert_log_count(int level, unsigned int count, const char * file, int line)
{
if (level < 0) {
unsigned int tot = 0;
for (int i = 0; i < GS_TEST_LOG_MAX_LEVELS; ++i) {
tot += gs_test_log_stats.count[i];
}
if (tot != count) {
cm_print_error("Unexpected total log count: %u != %u\n", tot, count);
_fail(file, line);
}
} else if (level >= GS_TEST_LOG_MAX_LEVELS) {
cm_print_error("Unknown log level: %d - valid levels 0 - %d\n", level, GS_TEST_LOG_MAX_LEVELS - 1);
_fail(file, line);
} else if (gs_test_log_stats.count[level] != count) {
cm_print_error("Unexpected log count: %u != %u\n", gs_test_log_stats.count[level], count);
_fail(file, line);
}
}
void gs_assert_log(unsigned int stack_index, unsigned int count, gs_log_level_t level, const char * pattern, const char * file, int line)
{
if (stack_index == UINT32_MAX) {
// loop through all logs
unsigned int next = gs_test_log_stats.next_log;
unsigned int hits = 0;
for (unsigned int i = next - 1; i != next; --i) {
if (i >= GS_TEST_LOG_MAX_LOG_MESSAGES) {
i = (GS_TEST_LOG_MAX_LOG_MESSAGES - 1);
}
if ((gs_test_log_stats.logs[i].level == level) && (gs_test_log_stats.logs[i].msg[0])) {
if (fnmatch(pattern, gs_test_log_stats.logs[i].msg, 0) == 0) {
++hits;
}
}
}
if (hits != count) {
cm_print_error("Unexpected log count: %u != %u\n", hits, count);
_fail(file, line);
}
} else if (stack_index >= GS_TEST_LOG_MAX_LOG_MESSAGES) {
cm_print_error("Invalid stack_index: %u - valid 0 - %d\n", stack_index, GS_TEST_LOG_MAX_LOG_MESSAGES - 1);
_fail(file, line);
} else {
unsigned int i = (((gs_test_log_stats.next_log + GS_TEST_LOG_MAX_LOG_MESSAGES) - 1 - stack_index) % GS_TEST_LOG_MAX_LOG_MESSAGES);
if ((gs_test_log_stats.logs[i].level == level) &&
(gs_test_log_stats.logs[i].msg[0]) &&
(fnmatch(pattern, gs_test_log_stats.logs[i].msg, 0) == 0)) {
// match
} else {
cm_print_error("[%c][%s] != [%c][%s]\n",
gs_log_level_to_char(gs_test_log_stats.logs[i].level), gs_test_log_stats.logs[i].msg,
gs_log_level_to_char(level), pattern);
_fail(file, line);
}
}
}

28
gomspace/libutil/src/time.c Normal file
View File

@ -0,0 +1,28 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/time.h>
uint32_t gs_time_diff_ms(uint32_t ref_ms, uint32_t now_ms)
{
if (now_ms >= ref_ms) {
return (now_ms - ref_ms);
}
// assuming time wrapped at max uint32_t
return ((UINT32_MAX - ref_ms) + now_ms);
}
bool gs_time_sleep_until_ms(uint32_t * ref_ms, uint32_t sleep_ms)
{
const uint32_t now = gs_time_rel_ms();
*ref_ms += sleep_ms; // this is expected to be in the future
uint32_t ms = gs_time_diff_ms(now, *ref_ms);
if (ms > sleep_ms) {
// we are behind - catch up, could be bad seed or too long processing
*ref_ms = now;
gs_time_sleep_ms(0); // yield - let others have a go
return true;
}
gs_time_sleep_ms(ms);
return false;
}

61
gomspace/libutil/src/timestamp.c Normal file
View File

@ -0,0 +1,61 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/timestamp.h>
int timestamp_add(gs_timestamp_t * base, const gs_timestamp_t * add)
{
if (!base || !add)
return -1;
base->tv_sec += add->tv_sec;
if (base->tv_nsec + add->tv_nsec >= GS_TIMESTAMP_NSEC_PER_SEC) {
base->tv_sec++;
base->tv_nsec = (base->tv_nsec + add->tv_nsec) % GS_TIMESTAMP_NSEC_PER_SEC;
} else {
base->tv_nsec += add->tv_nsec;
}
return 0;
}
int timestamp_diff(gs_timestamp_t * base, const gs_timestamp_t * diff)
{
if (!base || !diff)
return -1;
base->tv_sec -= diff->tv_sec;
if (base->tv_nsec >= diff->tv_nsec) {
base->tv_nsec -= diff->tv_nsec;
} else {
base->tv_sec--;
base->tv_nsec = (base->tv_nsec + GS_TIMESTAMP_NSEC_PER_SEC) - diff->tv_nsec;
}
return 0;
}
/* Test is timestamp is greater or equal */
int timestamp_ge(const gs_timestamp_t * base, const gs_timestamp_t * test)
{
if (!base || !test)
return -1;
if (test->tv_sec > base->tv_sec ||
(test->tv_sec == base->tv_sec &&
test->tv_nsec > base->tv_nsec)) {
return 1;
}
return 0;
}
int timestamp_copy(const gs_timestamp_t * from, gs_timestamp_t * to)
{
if (!from || !to)
return -1;
to->tv_sec = from->tv_sec;
to->tv_nsec = from->tv_nsec;
return 0;
}

123
gomspace/libutil/src/vmem/commands.c Normal file
View File

@ -0,0 +1,123 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/gosh/command.h>
#include <gs/util/vmem.h>
#include <gs/util/hexdump.h>
static int cmd_vmem_read(gs_command_context_t * ctx)
{
if (gs_vmem_get_map() == NULL) {
return GS_ERROR_NOT_FOUND;
}
void * addr;
if (gs_string_to_pointer(ctx->argv[1], &addr)) {
return GS_ERROR_ARG;
}
uint32_t length;
if (gs_string_to_uint32(ctx->argv[2], &length)) {
return GS_ERROR_ARG;
}
char data[length];
void* to = gs_vmem_cpy(data, addr, length);
if (to == NULL) {
return GS_ERROR_ARG;
}
gs_hexdump_to_stream(data, length, addr, ctx->out);
return GS_OK;
}
static unsigned int to_int(char c)
{
if (c >= '0' && c <= '9') return c - '0';
if (c >= 'A' && c <= 'F') return 10 + c - 'A';
if (c >= 'a' && c <= 'f') return 10 + c - 'a';
return -1;
}
static int cmd_vmem_write(gs_command_context_t * ctx)
{
if (gs_vmem_get_map() == NULL) {
return GS_ERROR_NOT_FOUND;
}
void * addr;
if (gs_string_to_pointer(ctx->argv[1], &addr)) {
return GS_ERROR_ARG;
}
int len = strlen(ctx->argv[2]) / 2;
char data[len];
for (int i = 0; (i < len); ++i) {
data[i] = 16 * to_int(ctx->argv[2][2*i]) + to_int(ctx->argv[2][2*i+1]);
}
gs_vmem_cpy(addr, data, len);
return GS_OK;
}
static int cmd_vmem_list(gs_command_context_t * ctx)
{
return gs_vmem_list(ctx->out);
}
static int cmd_vmem_lock(gs_command_context_t * context)
{
return gs_vmem_lock_by_name(context->argv[1], true);
}
static int cmd_vmem_unlock(gs_command_context_t * context)
{
return gs_vmem_lock_by_name(context->argv[1], false);
}
static const gs_command_t GS_COMMAND_SUB cmd_vmem_sub[] = {
{
.name = "read",
.help = "Read from virtual memory",
.usage = "<addr> <length>",
.handler = cmd_vmem_read,
.mandatory_args = 2,
},{
.name = "write",
.help = "Write to virtual memory",
.usage = "<addr> <data>",
.handler = cmd_vmem_write,
.mandatory_args = 2,
},{
.name = "lock",
.help = "Lock the virtual memory",
.usage = "<entry>",
.handler = cmd_vmem_lock,
.mandatory_args = 1,
},{
.name = "unlock",
.help = "Unlock the virtual memory",
.usage = "<entry>",
.handler = cmd_vmem_unlock,
.mandatory_args = 1,
},{
.name = "list",
.help = "Show virtual memory mappings",
.handler = cmd_vmem_list,
.mandatory_args = GS_COMMAND_NO_ARGS,
}
};
static const gs_command_t GS_COMMAND_ROOT cmd_vmem[] = {
{
.name = "vmem",
.help = "Virtual memory",
.chain = GS_COMMAND_INIT_CHAIN(cmd_vmem_sub),
},
};
gs_error_t gs_vmem_register_commands(void)
{
return GS_COMMAND_REGISTER(cmd_vmem);
}

143
gomspace/libutil/src/vmem/vmem.c Normal file
View File

@ -0,0 +1,143 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include <gs/util/vmem.h>
#include <gs/util/string.h>
#include <gs/util/log.h>
#include <gs/util/stdio.h>
#include <gs/util/check.h>
static const gs_vmem_t * g_vmem_map;
gs_error_t gs_vmem_set_map(const gs_vmem_t * map)
{
g_vmem_map = map;
return GS_OK;
}
const gs_vmem_t * gs_vmem_get_map(void)
{
return g_vmem_map;
}
gs_error_t gs_vmem_list(FILE * out)
{
const gs_vmem_t * mem = g_vmem_map;
if (mem) {
unsigned int found = 0;
for (; mem->name; ++mem) {
if (found == 0) {
fprintf(out, "%-20s %-10s %-6s %-6s %s\r\n", "name", "virt.", "phys.", "size", "size");
}
fprintf(out, "%-20s %p 0x%04x 0x%04x %5u\r\n", mem->name, mem->virtmem.p, (unsigned int) mem->physmem.u, (unsigned int) mem->size, (unsigned int) mem->size);
++found;
}
if (found) {
return GS_OK;
}
}
return GS_ERROR_NOT_FOUND;
}
const gs_vmem_t * gs_vmem_get_by_name(const char * name)
{
if (name) {
const gs_vmem_t * mem = g_vmem_map;
if (mem) {
for (; mem->name; ++mem) {
if (strcasecmp(name, mem->name) == 0) {
return mem;
}
}
}
}
return NULL;
}
gs_error_t gs_vmem_lock_by_name(const char * name, bool on)
{
const gs_vmem_t * mem = gs_vmem_get_by_name(name);
if (mem) {
if (mem->drv && mem->drv->lock) {
return (mem->drv->lock)(mem, on);
}
return GS_ERROR_NOT_SUPPORTED;
}
return GS_ERROR_NOT_FOUND;
}
gs_error_t gs_vmem_lock_all(bool on)
{
const gs_vmem_t * mem = g_vmem_map;
if (mem) {
unsigned int locked = 0;
for (; mem->name; ++mem) {
if (mem->drv && mem->drv->lock) {
(mem->drv->lock)(mem, on);
++locked;
}
}
if (locked) {
return GS_OK;
}
}
return GS_ERROR_NOT_FOUND;
}
/**
@note NO LOGGING - currently the log system uses this interface, and logging can therefor create circular/forever loops.
*/
void * gs_vmem_cpy(void* to, const void* from, size_t size)
{
/* Search memories */
const gs_vmem_t *vmem_from = NULL;
const gs_vmem_t *vmem_to = NULL;
const gs_vmem_t *mem = g_vmem_map;
const gs_address_t _to = {.p = to};
const gs_address_t _from = {.p = (void*) from};
if (mem) {
while(mem->size != 0) {
//printf("0x%lx 0x%lx %"PRIu32" %lu %lu\r\n", mem->start, mem->physmem_start, mem->size, to, from);
if ((_to.u >= mem->virtmem.u) && (_to.u < mem->virtmem.u + mem->size)) {
vmem_to = mem;
}
if ((_from.u >= mem->virtmem.u) && (_from.u < mem->virtmem.u + mem->size)) {
vmem_from = mem;
}
mem++;
}
}
// VMEM -> VMEM
if (vmem_to && vmem_from) {
printf("%s: VMEM to VMEM is not supported\r\n", __FUNCTION__);
return NULL;
}
// RAM -> VMEM
if (vmem_to) {
if ((vmem_to->drv == NULL) || (vmem_to->drv->write == NULL)) {
printf("%s: Writting to VMEM %p is not supported\r\n", __FUNCTION__, to);
return NULL;
}
gs_address_t physaddr = {.u = (_to.u - vmem_to->virtmem.u) + vmem_to->physmem.u};
//printf("Copying from ram 0x%lx to physaddr 0x%lx %u\r\n", from, physaddr, (unsigned int) size);
vmem_to->drv->write(vmem_to, physaddr.p, from, size);
return to;
}
// VMEM -> RAM
if (vmem_from) {
if (vmem_from->drv == NULL || (vmem_from->drv->read == NULL)) {
printf("%s: Reading from VMEM %p is not supported\r\n", __FUNCTION__, from);
return NULL;
}
gs_address_t physaddr = {.u = (_from.u - vmem_from->virtmem.u) + vmem_from->physmem.u};
//printf("Copying from mem physaddr 0x%lx to 0x%lx %u\r\n", physaddr, to, (unsigned int) size);
vmem_from->drv->read(vmem_from, to, physaddr.p, size);
return to;
}
// RAM -> RAM (no VMEM mapping found)
return memcpy(to, from, size);
}

18
gomspace/libutil/src/watchdog/local.h Normal file
View File

@ -0,0 +1,18 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include <gs/util/watchdog/watchdog.h>
#include <gs/util/watchdog/watchdog_task.h>
#include <gs/util/thread.h>
#include <gs/util/log.h>
typedef struct {
gs_thread_t task;
bool is_running;
bool do_stop;
} gs_swwd_monitor_task_t;
// Return monitor task instance
gs_swwd_monitor_task_t * gs_swwd_get_monitor_task(void);
GS_LOG_GROUP_EXTERN(gs_swwd_log);
#define LOG_DEFAULT gs_swwd_log

68
gomspace/libutil/src/watchdog/monitor_task.c Normal file
View File

@ -0,0 +1,68 @@
/* Copyright (c) 2013-2018 GomSpace A/S. All rights reserved. */
#include "local.h"
#include <gs/util/watchdog/watchdog_task.h>
#include <gs/util/check.h>
#include <gs/util/time.h>
#define GS_SWWD_CHECK_INTERVAL_MS 1000 /* Check every 1 sec. */
static void * gs_swwd_monitor_task(void* parameter)
{
gs_swwd_monitor_task_t * monitor = parameter;
log_info("monitor task started");
monitor->is_running = true;
while(!monitor->do_stop) {
gs_time_sleep_ms(GS_SWWD_CHECK_INTERVAL_MS);
gs_swwd_check_expired_clients(NULL);
}
monitor->is_running = false;
log_info("monitor task exiting");
return NULL;
}
gs_error_t gs_swwd_monitor_task_start(void)
{
gs_swwd_monitor_task_t * monitor = gs_swwd_get_monitor_task();
GS_CHECK_SUPPORTED(monitor != NULL); /* SWWD must be initialized */
GS_CHECK_SUPPORTED(monitor->is_running == false); /* SWWD task must not already be running */
/* Start the monitor task */
gs_error_t error = gs_thread_create("SWWD", gs_swwd_monitor_task, monitor, 4000, GS_THREAD_PRIORITY_HIGH, 0, &monitor->task);
if (error) {
log_error("%s: gs_thread_create() failed, error: %s", __FUNCTION__, gs_error_string(error));
}
return error;
}
gs_error_t gs_swwd_monitor_task_stop(uint32_t timeout_s)
{
gs_swwd_monitor_task_t * monitor = gs_swwd_get_monitor_task();
GS_CHECK_SUPPORTED(monitor != NULL); /* SWWD must be initialized */
/* Signal the task to stop */
monitor->do_stop = true;
/* Wait for the task to stop */
const uint32_t timeout = GS_SWWD_CHECK_INTERVAL_MS + (timeout_s * 1000);
uint32_t tm = 0;
while(monitor->is_running && (tm < timeout)) {
gs_thread_sleep_ms(100);
tm += 100;
}
if (monitor->is_running) {
return GS_ERROR_BUSY;
}
return GS_OK;
}

292
gomspace/libutil/src/watchdog/watchdog.c Normal file
View File

@ -0,0 +1,292 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include "local.h"
#include <gs/util/check.h>
#include <gs/util/log.h>
#include <gs/util/time.h>
#include <gs/util/mutex.h>
#include <stdlib.h>
#define GS_SWWD_DEFAULT_TIMEOUT 30 /* 30 second timeout */
// define log group and make it default
GS_LOG_GROUP(gs_swwd_log, "swwd", GS_LOG_CAT_DEFAULT, GS_LOG_DEFAULT_MASK);
// watchdog state
typedef enum {
GS_SWWD_STATE_FREE = 0,
GS_SWWD_STATE_ACTIVE = 1,
GS_SWWD_STATE_EXPIRED = 2,
} gs_swwd_state_t;
// watchdog client instance
struct gs_swwd_hdl {
// State
gs_swwd_state_t state;
// Last 'user' touch value - used for detecting if watchdog has been touched (avoid race condition)
uint32_t last_touch;
// Last detected touch time
uint32_t last_touch_ms;
// User 'set' attributes
struct {
// Name
const char* name;
// Timeout - converted from seconds (given to API) to mS
uint32_t timeout_ms;
// Action when/if timeout occurs
gs_swwd_timeout_action_t action;
// callback
gs_swwd_callback_function_t cb;
// user data (for callback)
void * cb_userdata;
// Touch - incremented on each touch
uint32_t touch;
} user;
};
typedef struct gs_swwd {
gs_watchdog_device_t *wdev;
gs_mutex_t lock;
gs_swwd_monitor_task_t monitor;
uint32_t num_clients;
gs_swwd_hdl_t clients[0];
} gs_swwd_t;
static gs_swwd_t* g_swwd = NULL;
gs_swwd_monitor_task_t * gs_swwd_get_monitor_task(void)
{
if (g_swwd) {
return &g_swwd->monitor;
}
return NULL;
}
gs_error_t gs_swwd_create(uint32_t max_clients, gs_watchdog_device_t * wdev)
{
GS_CHECK_SUPPORTED(g_swwd == NULL); /* SWWD must not be initialized more than once */
GS_CHECK_ARG(max_clients > 0);
if (wdev) {
// ping is the only mandatory
GS_CHECK_ARG((wdev->ops != NULL) && (wdev->ops->ping != NULL));
}
gs_log_group_register(LOG_DEFAULT);
gs_swwd_t *swwd_obj = calloc(1, sizeof(*swwd_obj) + (sizeof(swwd_obj->clients[0]) * max_clients));
if (swwd_obj == NULL) {
return GS_ERROR_ALLOC;
}
if (gs_mutex_create(&(swwd_obj->lock))) {
free(swwd_obj);
return GS_ERROR_ALLOC;
}
swwd_obj->num_clients = max_clients;
swwd_obj->wdev = wdev;
if (wdev) {
if (wdev->ops->set_timeout) {
wdev->ops->set_timeout(wdev, (wdev->timeout > 0) ? wdev->timeout : GS_SWWD_DEFAULT_TIMEOUT);
}
if (wdev->ops->set_pretimeout) {
wdev->ops->set_pretimeout(wdev, (wdev->pretimeout > 0) ? wdev->pretimeout : (GS_SWWD_DEFAULT_TIMEOUT - (GS_SWWD_DEFAULT_TIMEOUT/10)));
}
if (wdev->ops->start) {
wdev->ops->start(wdev);
}
wdev->ops->ping(wdev);
} else {
log_warning("%s: no watchdog device specifed - cannot reset system!", __FUNCTION__);
}
g_swwd = swwd_obj; /* Set the task handle as the last operation */
return GS_OK;
}
gs_error_t gs_swwd_destroy(uint32_t timeout_s)
{
GS_CHECK_SUPPORTED(g_swwd != NULL);
if (gs_swwd_monitor_task_stop(timeout_s) != GS_OK) {
return GS_ERROR_BUSY;
}
if (g_swwd->wdev && g_swwd->wdev->ops->stop) {
g_swwd->wdev->ops->stop(g_swwd->wdev);
}
gs_mutex_destroy(g_swwd->lock);
free(g_swwd);
g_swwd = NULL;
return GS_OK;
}
static const char * get_action(gs_swwd_timeout_action_t action)
{
switch (action) {
case GS_SWWD_TIMEOUT_ACTION_RESET:
return "reset";
case GS_SWWD_TIMEOUT_ACTION_LOG:
return "log";
}
return "unknown";
}
gs_error_t gs_swwd_check_expired_clients(uint32_t *num_expired)
{
GS_CHECK_SUPPORTED(g_swwd != NULL); /* SWWD must be initialized */
uint32_t expired_clients_reset = 0;
uint32_t expired_clients_log = 0;
gs_mutex_lock(g_swwd->lock);
for (uint32_t idx = 0; idx < g_swwd->num_clients; idx++) {
gs_swwd_hdl_t * wd = &g_swwd->clients[idx];
uint32_t now_ms = gs_time_rel_ms();
if (wd->state == GS_SWWD_STATE_ACTIVE) {
if (wd->last_touch != wd->user.touch) {
// watchdog has been touched since last we checked - update touch time
wd->last_touch = wd->user.touch;
wd->last_touch_ms = now_ms;
} else {
const uint32_t elapsed_ms = gs_time_diff_ms(wd->last_touch_ms, now_ms);
if (elapsed_ms >= wd->user.timeout_ms) {
wd->state = GS_SWWD_STATE_EXPIRED;
char logbuf[100];
snprintf(logbuf, sizeof(logbuf),
"[%s] expired -> %s (elapsed %"PRIu32" mS, timeout %"PRIu32" mS)",
wd->user.name,
get_action(wd->user.action),
elapsed_ms,
wd->user.timeout_ms);
gs_swwd_callback_function_t cb = wd->user.cb;
void * cb_userdata = wd->user.cb_userdata;
// Unlock while doing log and callback
// - we accept the tiny risk, that client has deregistered and will be called with invalid data
gs_mutex_unlock(g_swwd->lock);
{
log_error("%s", logbuf);
if (cb) {
(cb)(cb_userdata);
}
}
gs_mutex_lock(g_swwd->lock);
}
}
}
if (wd->state == GS_SWWD_STATE_EXPIRED) {
switch (wd->user.action) {
case GS_SWWD_TIMEOUT_ACTION_RESET:
++expired_clients_reset;
break;
case GS_SWWD_TIMEOUT_ACTION_LOG:
if (wd->last_touch != wd->user.touch) {
// its alive - reactive watchdog
wd->state = GS_SWWD_STATE_ACTIVE;
wd->last_touch = wd->user.touch;
wd->last_touch_ms = now_ms;
} else {
++expired_clients_log;
}
break;
}
}
}
gs_mutex_unlock(g_swwd->lock);
if ((expired_clients_reset == 0) && g_swwd->wdev) {
g_swwd->wdev->ops->ping(g_swwd->wdev);
}
if (num_expired) {
*num_expired = (expired_clients_reset + expired_clients_log);
}
return GS_OK;
}
gs_error_t gs_swwd_register_with_action(gs_swwd_hdl_t ** user_wd,
uint32_t timeout_seconds,
gs_swwd_callback_function_t callback, void * userdata,
const char * name,
gs_swwd_timeout_action_t action)
{
GS_CHECK_ARG(gs_string_empty(name) == false);
GS_CHECK_ARG(timeout_seconds > 0);
GS_CHECK_ARG(user_wd != NULL);
GS_CHECK_SUPPORTED(g_swwd != NULL); /* SWWD must be initialized */
gs_swwd_hdl_t * wd = NULL;
gs_mutex_lock(g_swwd->lock);
{
for (unsigned int idx = 0; idx < g_swwd->num_clients; idx++) {
if (g_swwd->clients[idx].state == GS_SWWD_STATE_FREE) {
wd = &g_swwd->clients[idx];
memset(wd, 0, sizeof(*wd));
// set user stuff
wd->user.name = name;
wd->user.timeout_ms = (timeout_seconds * 1000);
wd->user.cb = callback;
wd->user.cb_userdata = userdata;
wd->user.action = action;
// set internal stuff
wd->state = GS_SWWD_STATE_ACTIVE;
wd->last_touch_ms = gs_time_rel_ms();
break;
}
}
}
gs_mutex_unlock(g_swwd->lock);
*user_wd = wd;
if (wd == NULL) {
log_error("[%s] cannot create instance due to no available handles", name);
return GS_ERROR_FULL;
}
return GS_OK;
}
gs_error_t gs_swwd_deregister(gs_swwd_hdl_t ** wd)
{
GS_CHECK_SUPPORTED(g_swwd != NULL); /* SWWD must be initialized */
GS_CHECK_ARG(wd != NULL);
GS_CHECK_HANDLE(*wd != NULL);
gs_mutex_lock(g_swwd->lock);
memset((*wd), 0, sizeof(**wd));
gs_mutex_unlock(g_swwd->lock);
*wd = NULL;
return GS_OK;
}
gs_error_t gs_swwd_touch(gs_swwd_hdl_t * wd)
{
GS_CHECK_HANDLE(wd != NULL);
++wd->user.touch;
return GS_OK;
}
gs_error_t gs_swwd_set_timeout(gs_swwd_hdl_t * wd, uint32_t timeout_seconds)
{
GS_CHECK_ARG(timeout_seconds > 0);
GS_CHECK_HANDLE(wd != NULL);
++wd->user.touch;
wd->user.timeout_ms = (timeout_seconds * 1000);
return GS_OK;
}

5
gomspace/libutil/src/zip/cppcheck-suppress.txt Normal file
View File

@ -0,0 +1,5 @@
// we don't wanna change 3rd part code for none-critical issue
localtimeCalled:src/zip/miniz/miniz.c
utimeCalled:src/zip/miniz/miniz.c
assignIfError:src/zip/miniz/miniz.c
unreadVariable:src/zip/miniz/miniz.c

9429
gomspace/libutil/src/zip/miniz/LIST_OF_CHANGES Normal file
View File

File diff suppressed because it is too large Load Diff

7572
gomspace/libutil/src/zip/miniz/miniz.c Normal file
View File

File diff suppressed because it is too large Load Diff

1329
gomspace/libutil/src/zip/miniz/miniz.h Normal file
View File

File diff suppressed because it is too large Load Diff

357
gomspace/libutil/src/zip/zip.c Normal file
View File

@ -0,0 +1,357 @@
/* Copyright (c) 2013-2017 GomSpace A/S. All rights reserved. */
#include "gs/util/zip/zip.h"
#include "miniz/miniz.h"
#include <gs/util/minmax.h>
#include <gs/util/log.h>
#include <gs/util/error.h>
#include <limits.h>
static void cleanup(FILE *pInfile, FILE *pOutfile, uint8_t *stream_inbuf, uint8_t *stream_outbuf)
{
if(pInfile)
fclose(pInfile);
if(pOutfile)
fclose(pOutfile);
if(stream_inbuf)
free(stream_inbuf);
if(stream_outbuf)
free(stream_outbuf);
}
int gs_zip_compress_file(const char *src, const char *dest)
{
FILE *pInfile, *pOutfile;
uint32_t infile_size;
long file_loc;
// Open input file.
pInfile = fopen(src, "rb");
if (!pInfile)
{
log_error("Zip compress: Failed opening input file!");
return GS_ERROR_IO;
}
// Determine input file's size.
fseek(pInfile, 0, SEEK_END);
file_loc = ftell(pInfile);
fseek(pInfile, 0, SEEK_SET);
if((file_loc < 0) || ((unsigned long)file_loc > UINT_MAX))
{
log_error("Zip compress: File is too large to be processed.");
fclose(pInfile);
return GS_ERROR_IO;
}
infile_size = (uint32_t)file_loc;
uint32_t buffer_size = infile_size;
// Allocate input buffer memory
uint8_t *stream_inbuf = malloc(buffer_size);
if (stream_inbuf == NULL)
{
log_error("Zip compress: Failed to allocate input buffer memory");
fclose(pInfile);
return GS_ERROR_IO;
}
// Allocate output buffer memory
uint8_t *stream_outbuf = malloc(buffer_size);
if (stream_outbuf == NULL)
{
log_error("Zip compress: Failed to allocate output buffer memory");
cleanup(pInfile, NULL, stream_inbuf, NULL);
return GS_ERROR_IO;
}
// Open output file.
pOutfile = fopen(dest, "wb");
if (!pOutfile)
{
log_error("Zip compress: Failed opening output file!");
cleanup(pInfile, NULL, stream_inbuf, stream_outbuf);
return GS_ERROR_IO;
}
// Init the z_stream
z_stream stream;
memset(&stream, 0, sizeof(stream));
stream.next_in = stream_inbuf;
stream.avail_in = 0;
stream.next_out = stream_outbuf;
stream.avail_out = buffer_size;
// Compression.
uint32_t infile_remaining = infile_size;
if(deflateInit(&stream, Z_BEST_COMPRESSION) != Z_OK)
{
log_error("Zip compress: deflateInit() failed!");
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_DATA;
}
for( ; ; )
{
int status;
if(!stream.avail_in)
{
// Input buffer is empty, so read more bytes from input file.
uint32_t n = gs_min((uint32_t)buffer_size, infile_remaining);
if (fread(stream_inbuf, 1, n, pInfile) != n)
{
log_error("Zip compress: Failed reading from input file!");
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_IO;
}
stream.next_in = stream_inbuf;
stream.avail_in = n;
infile_remaining -= n;
}
status = deflate(&stream, infile_remaining ? Z_NO_FLUSH : Z_FINISH);
if((status == Z_STREAM_END) || (!stream.avail_out))
{
// Output buffer is full, or compression is done, so write buffer to output file.
uint32_t n = buffer_size - stream.avail_out;
if (fwrite(stream_outbuf, 1, n, pOutfile) != n)
{
log_error("Zip compress: Failed writing to output file!");
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_IO;
}
stream.next_out = stream_outbuf;
stream.avail_out = buffer_size;
}
if(status == Z_STREAM_END)
{
break;
}
else if(status != Z_OK)
{
log_error("Zip compress: deflate() failed with status %i!", status);
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_DATA;
}
}
if(deflateEnd(&stream) != Z_OK)
{
log_error("Zip compress: deflateEnd() failed!");
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_DATA;
}
cleanup(pInfile, NULL, stream_inbuf, stream_outbuf);
if(EOF == fclose(pOutfile))
{
log_error("Zip compress: Failed writing to output file!");
return GS_ERROR_IO;
}
log_debug("Total input bytes: %u\n", (mz_uint32)stream.total_in);
log_debug("Total output bytes: %u\n", (mz_uint32)stream.total_out);
log_debug("Success.\n");
return GS_OK;
}
int gs_zip_decompress_file(const char *src, const char *dest)
{
FILE *pInfile, *pOutfile;
uint32_t infile_size;
long file_loc;
// Open input file.
pInfile = fopen(src, "rb");
if (!pInfile)
{
log_error("Zip decompress: Failed opening input file!");
return GS_ERROR_IO;
}
// Determine input file's size.
fseek(pInfile, 0, SEEK_END);
file_loc = ftell(pInfile);
fseek(pInfile, 0, SEEK_SET);
if((file_loc < 0) || ((unsigned long)file_loc > UINT_MAX))
{
log_error("Zip decompress: File is too large to be processed.");
fclose(pInfile);
return GS_ERROR_IO;
}
infile_size = (uint32_t)file_loc;
uint32_t buffer_size = infile_size;
// Allocate input buffer memory
uint8_t *stream_inbuf = malloc(buffer_size);
if (stream_inbuf == NULL)
{
log_error("Zip decompress: Failed to allocate input buffer memory");
fclose(pInfile);
return GS_ERROR_IO;
}
// Allocate output buffer memory
uint8_t *stream_outbuf = malloc(buffer_size);
if (stream_outbuf == NULL)
{
log_error("Zip decompress: Failed to allocate output buffer memory");
cleanup(pInfile, NULL, stream_inbuf, NULL);
return GS_ERROR_IO;
}
// Open output file.
pOutfile = fopen(dest, "wb");
if (!pOutfile)
{
log_error("Zip decompress: Failed opening output file!");
cleanup(pInfile, NULL, stream_inbuf, stream_outbuf);
return GS_ERROR_IO;
}
// Init the z_stream
z_stream stream;
memset(&stream, 0, sizeof(stream));
stream.next_in = stream_inbuf;
stream.avail_in = 0;
stream.next_out = stream_outbuf;
stream.avail_out = buffer_size;
// Decompression.
uint32_t infile_remaining = infile_size;
if(inflateInit(&stream))
{
log_error("Zip decompress: inflateInit() failed!");
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_DATA;
}
for( ; ; )
{
int status;
if(!stream.avail_in)
{
// Input buffer is empty, so read more bytes from input file.
uint32_t n = gs_min((uint32_t)buffer_size, infile_remaining);
if(fread(stream_inbuf, 1, n, pInfile) != n)
{
log_error("Zip decompress: Failed reading from input file!");
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_IO;
}
stream.next_in = stream_inbuf;
stream.avail_in = n;
infile_remaining -= n;
}
status = inflate(&stream, Z_SYNC_FLUSH);
if((status == Z_STREAM_END) || (!stream.avail_out))
{
// Output buffer is full, or decompression is done, so write buffer to output file.
uint32_t n = buffer_size - stream.avail_out;
if(fwrite(stream_outbuf, 1, n, pOutfile) != n)
{
log_error("Zip decompress: Failed writing to output file!");
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_IO;
}
stream.next_out = stream_outbuf;
stream.avail_out = buffer_size;
}
if(status == Z_STREAM_END)
{
break;
}
else if(status != Z_OK)
{
log_error("Zip decompress: inflate() failed with status %i!", status);
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_DATA;
}
}
if(inflateEnd(&stream) != Z_OK)
{
log_error("Zip decompress: inflateEnd() failed!");
cleanup(pInfile, pOutfile, stream_inbuf, stream_outbuf);
return GS_ERROR_DATA;
}
cleanup(pInfile, NULL, stream_inbuf, stream_outbuf);
if(EOF == fclose(pOutfile))
{
log_error("Zip decompress: Failed writing to output file!");
return GS_ERROR_IO;
}
log_debug("Total input bytes: %u", (mz_uint32)stream.total_in);
log_debug("Total output bytes: %u", (mz_uint32)stream.total_out);
log_debug("Success.\n");
return GS_OK;
}
int gs_zip_compress_data(const unsigned char *src, uint32_t src_len, unsigned char *dest, uint32_t *dest_len)
{
mz_ulong cmp_len = src_len;
if(compress(dest, &cmp_len, src, (mz_ulong)src_len) != MZ_OK)
{
return GS_ERROR_DATA;
}
*dest_len = cmp_len;
return GS_OK;
}
int gs_zip_decompress_data(const unsigned char *src, uint32_t src_len, unsigned char *dest, uint32_t dest_len, uint32_t *decomp_len)
{
mz_ulong tmp = dest_len;
if(uncompress(dest, &tmp, src, (mz_ulong)src_len) != MZ_OK)
return GS_ERROR_DATA;
*decomp_len = tmp;
return GS_OK;
}