diff --git a/inc/assert.h b/inc/assert.h new file mode 100644 index 0000000..01777fe --- /dev/null +++ b/inc/assert.h @@ -0,0 +1,35 @@ +#ifndef _ASSERT_H +#define _ASSERT_H + +#include +#include + + +#ifdef __cplusplus +extern "C" +{ +#endif + + +#ifdef _RTLIBC_ASSERT_ + +#define assert(condition) if (!(condition)) rtlibc_assert_failed_handler(#condition, __FILE__, __LINE__) +#define assert_msg(condition, message) if (!(condition)) rtlibc_assert_failed_handler(message, __FILE__, __LINE__) + +void rtlibc_assert_failed_handler(const char *message, const char *file_name, uint32_t line_no); + +#else + +#define assert(condition) (void)(condition) +#define assert_msg(condition, message) (void)(condition) + +#endif + + +#ifdef __cplusplus +} +#endif + + +#endif + diff --git a/inc/ctype.h b/inc/ctype.h new file mode 100644 index 0000000..2c6e5b7 --- /dev/null +++ b/inc/ctype.h @@ -0,0 +1,131 @@ +#ifndef _RTCTYPE_H +#define _RTCTYPE_H + +#include +#include + + +#ifdef __cplusplus +extern "C" +{ +#endif + + +static inline bool isspace(char c) +{ + return (c == ' ') || + (c == '\t') || + (c == '\n') || + (c == '\v') || + (c == '\f') || + (c == '\r'); +} + + +static inline bool isdigit(char c) +{ + return (c >= '0') && (c <= '9'); +} + + + +static inline bool isxdigit(char c) +{ + return ((c >= '0') && (c <= '9')) || + ((c >= 'a') && (c <= 'f')) || + ((c >= 'A') && (c <= 'F')); +} + + + +static inline bool islower(char c) +{ + return (c >= 'a') && (c <= 'z'); +} + + + +static inline bool isupper(char c) +{ + return (c >= 'A') && (c <= 'Z'); +} + + + +static inline bool isalpha(char c) +{ + return islower(c) || isupper(c); +} + + + +static inline bool isalnum(char c) +{ + return isalpha(c) || isdigit(c); +} + + + +static inline bool ispunct(char c) +{ + return ((c >= '!') && (c <= '/')) || + ((c >= ':') && (c <= '@')) || + ((c >= '[') && (c <= '`')) || + ((c >= '{') && (c <= '~')); +} + + + +static inline bool isgraph(char c) +{ + return isalnum(c) || ispunct(c); +} + + + +static inline bool isprint(char c) +{ + return isgraph(c) || isspace(c); +} + + + +static inline bool iscntrl(char c) +{ + return (c >= 0) && (c <= 37); +} + + + +static inline bool isblank(char c) +{ + return (c == ' ') || (c == '\t'); +} + + + +static inline char tolower(char c) +{ + if (isupper(c)) + return c + ('a' - 'A'); + else + return c; +} + + + +static inline char toupper(char c) +{ + if (islower(c)) + return c - ('a' - 'A'); + else + return c; +} + + +#ifdef __cplusplus +} +#endif + + +#endif diff --git a/inc/inttypes.h b/inc/inttypes.h new file mode 100644 index 0000000..c808c2e --- /dev/null +++ b/inc/inttypes.h @@ -0,0 +1 @@ +#include \ No newline at end of file diff --git a/inc/malloc.h b/inc/malloc.h new file mode 100644 index 0000000..c40007c --- /dev/null +++ b/inc/malloc.h @@ -0,0 +1,46 @@ +#ifndef _RTMALLOC_H +#define _RTMALLOC_H + +#include +#include + + +#ifdef __cplusplus +extern "C" +{ +#endif + + +typedef struct rtmalloc_block_link_s +{ + size_t size; + struct rtmalloc_block_link_s *next; +}rtmalloc_block_link_t; + + +typedef struct +{ + size_t alignment; + size_t alignment_mask; + size_t allocated_mask; + + size_t size_free; + size_t size_free_min; + size_t min_block_size; + + rtmalloc_block_link_t free_list_head; + rtmalloc_block_link_t *free_list_tail; +}rtmalloc_context_t; + + +void rtmalloc_init(rtmalloc_context_t *context, void *heap, size_t size, size_t alignment); +void* rtmalloc(rtmalloc_context_t *context, size_t size); +void rtfree(rtmalloc_context_t *context, void *p); +size_t rtmsize(rtmalloc_context_t *context, void *p); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/inc/math.h b/inc/math.h new file mode 100644 index 0000000..5bf3478 --- /dev/null +++ b/inc/math.h @@ -0,0 +1,295 @@ +#ifndef _RTLIBC_MATH_H +#define _RTLIBC_MATH_H + + +#include +#include + + +#if defined ARM_MATH_CM7 || defined ARM_MATH_CM4 || defined ARM_MATH_CM3 || defined ARM_MATH_CM0 +#include +#include +#endif + + +#define M_E 2.718281828459045235360287471352662498L //e +#define M_LOG2E 1.442695040888963407359924681001892137L //log_2 e +#define M_LOG10E 0.434294481903251827651128918916605082L //log_10 e +#define M_LN2 0.693147180559945309417232121458176568L //log_e 2 +#define M_LN10 2.302585092994045684017991454684364208L //log_e 10 +#define M_PI 3.141592653589793238462643383279502884L //pi +#define M_PI_2 1.570796326794896619231321691639751442L //pi/2 +#define M_PI_4 0.785398163397448309615660845819875721L //pi/4 +#define M_1_PI 0.318309886183790671537767526745028724L //1/pi +#define M_2_PI 0.636619772367581343075535053490057448L //2/pi +#define M_2_SQRTPI 1.128379167095512573896158903121545172L //2/sqrt(pi) +#define M_SQRT2 1.414213562373095048801688724209698079L //sqrt(2) +#define M_SQRT1_2 0.707106781186547524400844362104849039L //1/sqrt(2) + + +#define INFINITY (__builtin_inff()) +#define M_NAN (__builtin_nanf("0")) + + + +#ifdef __cplusplus +extern "C" +{ +#endif + + +static inline float round(float x) +{ +#ifdef ARM_MATH_CM7 + register float rv; + __asm volatile ("vrinta.f32 %0, %1" : "=w" (rv) : "w" (x)); + return rv; +#else + return (int32_t)(x + 0.5L); +#endif +} + + + +static inline float floor(float x) +{ +#ifdef ARM_MATH_CM7 + register float rv; + __asm volatile ("vrintm.f32 %0, %1" : "=w" (rv) : "w" (x)); + return rv; +#else + return (x >= 0) ? (int32_t)x : (int32_t)x - 1; +#endif +} + + +static inline float trunc(float x) +{ +#ifdef ARM_MATH_CM7 + register float rv; + __asm volatile ("vrintz.f32 %0, %1" : "=w" (rv) : "w" (x)); + return rv; +#else + return (int32_t)x; +#endif +} + + +static inline float ceil(float x) +{ +#ifdef ARM_MATH_CM7 + register float rv; + __asm volatile ("vrintp.f32 %0, %1" : "=w" (rv) : "w" (x)); + return rv; +#else + return (x >= 0) ? + (((x - (int32_t)x) > 0) ? (int32_t)(x + 1) : (int32_t)x) : + (((x - (int32_t)x) < 0) ? (int32_t)(x - 1) : (int32_t)x); +#endif +} + + +static inline float sqrt(float x) +{ +#if defined ARM_MATH_CM7 || defined ARM_MATH_CM4 + if (x >= 0) + { + register float rv; + __asm volatile ("vsqrt.f32 %0, %1" : "=w" (rv) : "w" (x)); + return rv; + } + else + { + return 0; + } +#else + + return 0; //TODO not implemented +#endif +} + + +static inline float sqrtf(float x) +{ + return sqrt(x); +} + + +static inline float fmod(float x, float y) +{ + return x - floor(x / y) * y; +} + + +static inline float frac(float x) +{ + return x - trunc(x); +} + + + +static inline float fabs(float x) +{ +#if defined(ARM_MATH_CM7) || defined(ARM_MATH_CM4) + register float rv; + __asm volatile ("vabs.f32 %0, %1" : "=w" (rv) : "w" (x)); + return rv; +#else + return (x >= 0) ? x : -x; +#endif +} + + +static inline float fmax(float a, float b) +{ +#ifdef ARM_MATH_CM7 + register float v; + __asm volatile ("vmaxnm.f32 %0, %1, %2" : "=w" (v) : "w" (a), "w" (b)); //max instruction + return v; +#else + return (a > b) ? a : b; +#endif +} + + +static inline float fmin(float a, float b) +{ +#ifdef ARM_MATH_CM7 + register float v; + __asm volatile ("vminnm.f32 %0, %1, %2" : "=w" (v) : "w" (a), "w" (b)); //min instruction + return v; +#else + return (a < b) ? a : b; +#endif +} + + +static inline float fsat(float a, float x, float b) +{ + return fmin(fmax(a, x), b); +} + + +static inline int32_t smin(int32_t a, int32_t b) +{ + return (a < b) ? a : b; +} + +static inline int32_t smax(int32_t a, int32_t b) +{ + return (a > b) ? a : b; +} + +static inline int32_t ssat(int32_t x, int32_t a, int32_t b) +{ + if (x < a) + return a; + else if (x > b) + return b; + else + return x; +} + + + +static inline uint32_t umin(uint32_t a, uint32_t b) +{ + return (a < b) ? a : b; +} + +static inline uint32_t umax(uint32_t a, uint32_t b) +{ + return (a > b) ? a : b; +} + +static inline uint32_t usat(uint32_t x, uint32_t a, uint32_t b) +{ + if (x < a) + return a; + else if (x > b) + return b; + else + return x; +} + + +static inline int32_t round_closest_signed(int32_t n, int32_t d) +{ + return (((n < 0) ^ (d < 0)) ? ((n - d / 2) / d) : ((n + d / 2) / d)) * d; +} + + +static inline uint32_t round_closest_unsigned(uint32_t n, uint32_t d) +{ + return ((n + d / 2) / d) * d; +} + + + +#if defined(ARM_MATH_CM7) || defined(ARM_MATH_CM4) + + +static inline float sin(float x) +{ + return arm_sin_f32(x); +} + + +static inline float cos(float x) +{ + return arm_cos_f32(x); +} + + +static inline float tan(float x) +{ + float s, c; + + arm_sin_cos_f32(x, &s, &c); + + return s / c; +} + + +static inline float cotan(float x) +{ + float s, c; + + arm_sin_cos_f32(x, &s, &c); + + return c / s; +} + +#else + +float sin(float x); +float cos(float x); + +static inline float tan(float x) +{ + return sin(x) / cos(x); +} + + +static inline float cotan(float x) +{ + return cos(x) / sin(x); +} + +#endif + + + + + +float exp(float x); +float log(float x); +float powf(float b, float x); + + +#ifdef __cplusplus +} +#endif + + +#endif diff --git a/inc/printf.h b/inc/printf.h new file mode 100644 index 0000000..afb6a3f --- /dev/null +++ b/inc/printf.h @@ -0,0 +1,65 @@ +#ifndef _RTPRINTF_H +#define _RTPRINTF_H + + +#include +#include + + +#ifdef __cplusplus +extern "C" +{ +#endif + +enum +{ + //do not change the order + FLAG_MINUS = (1 << 0), + FLAG_PLUS = (1 << 1), + FLAG_SPACE = (1 << 2), + FLAG_POUND = (1 << 3), + FLAG_ZERO = (1 << 4), + + NOT_SPECIFIED = -1, + SPECIFIED_AS_ARG = -2 +}; + + +typedef struct +{ + uint8_t flags; + int32_t width; + int32_t precision; + char length; + int32_t specifier_index; +}rtformat_specifier_t; + + +typedef int32_t (*rtprintf_output_handler_t)(void*, const char*, size_t); +typedef int32_t (*rtformat_specifier_handler_t)(rtprintf_output_handler_t, void*, va_list*, rtformat_specifier_t*); + + + +//management functions +void rtprintf_include_float(void); +int32_t rtprintf_add_format(char specifier, rtformat_specifier_handler_t handler); + + + +//full versions +int32_t rtprintf(rtprintf_output_handler_t output_handler, void *user, const char *format, va_list args); + + +//simplified versions +int32_t rtiprintf(rtprintf_output_handler_t output_handler, void *user, const char *format, va_list args); + + + + +#ifdef __cplusplus +} +#endif + + +#endif + diff --git a/inc/stdio.h b/inc/stdio.h new file mode 100644 index 0000000..4e5ab16 --- /dev/null +++ b/inc/stdio.h @@ -0,0 +1,43 @@ +#ifndef _RTSTDIO_H +#define _RTSTDIO_H + +#include + +#include +#include +#include +#include + +#include + + +#define printf(...) (-1) + + +#ifdef __cplusplus +extern "C" +{ +#endif + + +int32_t sprintf(char *str, const char *format, ...); +int32_t snprintf(char *str, size_t n, const char *format, ...); + +int32_t vsprintf(char *str, const char *format, va_list args); +int32_t vsnprintf(char *str, size_t n, const char *format, va_list args); + + +int32_t isprintf(char *str, const char *format, ...); +int32_t isnprintf(char *str, size_t n, const char *format, ...); + +int32_t ivsprintf(char *str, const char *format, va_list args); +int32_t ivsnprintf(char *str, size_t n, const char *format, va_list args); + + +#ifdef __cplusplus +} +#endif + + +#endif + diff --git a/inc/stdlib.h b/inc/stdlib.h new file mode 100644 index 0000000..71d324d --- /dev/null +++ b/inc/stdlib.h @@ -0,0 +1,63 @@ +#ifndef _RTSTDLIB_H +#define _RTSTDLIB_H + +#include + +#include +#include + + +#define alloca(...) __builtin_alloca(__VA_ARGS__) + + + +#ifdef __cplusplus +extern "C" +{ +#endif + + + +#ifdef _RTLIBC_MALLOC_FREERTOS_ + +#define malloc_init(...) while(0) +#define msize (0) + +void* malloc(size_t size); +void free(void *p); + +#endif + + +#ifdef _RTLIBC_MALLOC_NOOS_ + +void malloc_init(void *heap, size_t size, size_t alignment); +size_t msize(void *p); + +void* malloc(size_t size); +void free(void *p); + +#endif + + + +uint64_t strtoul(const char *str, char **endptr, int32_t base); +int64_t strtol(const char *str, char **endptr, int32_t base); + +float strtof(const char *str, char **endptr); + +void abort(void); + + +static inline int32_t abs(int32_t a) +{ + return (a < 0) ? -a : a; +} + + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/inc/string.h b/inc/string.h new file mode 100644 index 0000000..4e5c877 --- /dev/null +++ b/inc/string.h @@ -0,0 +1,96 @@ +#ifndef _RTSTRING_H +#define _RTSTRING_H + +#include + +#include +#include + + +#define memzero(ptr, size) memset(ptr, 0, size) + + +#ifdef __cplusplus +extern "C" +{ +#endif + +//String manipulation functions + +void strtrim(char **str, const char *trim); + +size_t strlen(const char *str); +size_t strnlen(const char *str, size_t num); +size_t wstrlen(const uint16_t *str); + +char* strcat(char *dest, const char *src); +char* strncat(char *dest, const char *src, size_t num); + + +char* strchr(const char *str, char c); +//char* strrchr(const char *str, char c); + + +//const char* strstr(const char *str1, const char *str2); +char* strstr(char *str1, const char *str2); +char* stristr(char *str1, const char *str2); + + + +int strcmp(const char *str1, const char *str2); +int strncmp(const char *str1, const char *str2, size_t num); + +int stricmp(const char *str1, const char *str2); +int strincmp(const char *str1, const char *str2, size_t num); + + +char* strcpy(char *dest, const char *src); +char* strncpy(char *dest, const char *src, size_t num); + +size_t strspn(const char *str1, const char *str2); +size_t strcspn(const char *str1, const char *str2); + + +char* strdup(const char *s1); + +char* strpbrk(char *str1, const char *str2); + + +char* strstr(char *str1, const char *str2); + +char* strtok(char *str, const char *delim); +char* strtok_r(char *str, const char *delim, char **saveptr); + +char* strtok_m(char *str, const char *delim, const char *token_markers); +char* strtok_rm(char *str, const char *delim, char **saveptr, const char *token_markers); + + + +//memory functions +void* memcpy(void *dest, const void *src, size_t num); +void* memmove(void *dest, const void *src, size_t num); + + +void* memchr(void *ptr, uint8_t value, size_t num); + +int memcmp(const void *ptr1, const void *ptr2, size_t num); + + +void* memset(void *ptr, uint8_t value, size_t num); + + +const char* strfile(const char *fullpath); + +//get the path part with specified number of levels +//(0 means full path) +//positive means that many levels from the left (1 means root level "/") +//negative means full path less that meny levels from the end (-1 means all levels except last) +char* strpath(const char *fullpath, int level); + + +#ifdef __cplusplus +} +#endif + + +#endif diff --git a/inc/types.h b/inc/types.h new file mode 100644 index 0000000..b62b174 --- /dev/null +++ b/inc/types.h @@ -0,0 +1,8 @@ +#ifndef _RTTYPES_H +#define _RTTYPES_H + +#include +#include + +#endif + diff --git a/readme.txt b/readme.txt new file mode 100644 index 0000000..a4e7514 --- /dev/null +++ b/readme.txt @@ -0,0 +1,16 @@ +The following macros can be defined: + + +To enable dynamic memory allocation define +_RTLIBC_MALLOC_FREERTOS_ or _RTLIBC_MALLOC_NOOS_ + +To enable asserts define _RTLIBC_ASSERT_ + + + + +The following functions are weak and can be defined by the user: + +void rtlibc_abort_handler(void); +void rtlibc_malloc_failed_handler(void); + diff --git a/rtlibc.mk b/rtlibc.mk new file mode 100644 index 0000000..defe42c --- /dev/null +++ b/rtlibc.mk @@ -0,0 +1,11 @@ +CSRCS += \ +$(TWAVE_BASE_PATH)/components/software/rtlibc/src/iprintf.c \ +$(TWAVE_BASE_PATH)/components/software/rtlibc/src/printf.c \ +$(TWAVE_BASE_PATH)/components/software/rtlibc/src/malloc.c \ +$(TWAVE_BASE_PATH)/components/software/rtlibc/src/stdio.c \ +$(TWAVE_BASE_PATH)/components/software/rtlibc/src/stdlib.c \ +$(TWAVE_BASE_PATH)/components/software/rtlibc/src/string.c \ +$(TWAVE_BASE_PATH)/components/software/rtlibc/src/math.c + +INCDIRS += $(TWAVE_BASE_PATH)/components/software/rtlibc/inc + diff --git a/src/iprintf.c b/src/iprintf.c new file mode 100644 index 0000000..61f5fa1 --- /dev/null +++ b/src/iprintf.c @@ -0,0 +1,223 @@ +#include + +#include +#include +#include + +#include <../src/stdio_internal.h> + + +static const char l_flags[] = "-+ #0"; +static const char l_length_chars[] = "hljzt"; +static const char l_fs_chars[] = "duxXocs"; + + +//print %u, %x or %X +static int32_t fsh_integer(rtprintf_output_handler_t output_handler, void *user, uint32_t val, const char *charset) +{ + int32_t rv = 0; + + char str[12]; //will fit a 32 bit number + char* const str_end = str + sizeof(str); + const uint32_t base = strlen(charset); + + char *ptr = str_end; + if (val) + { + while(val) + { + *--ptr = charset[val % base]; + val /= base; + } + } + else + *--ptr = '0'; + + OUTPUT_CHARS(user, ptr, str_end - ptr); + + return rv; +} + + + +//str - string to parse +//spec - specifier found +//index - the index fo the data to be printed (before the real data we might have the width or/and precision passed as arguments) +static inline const char* parse_format_specifier(const char *str, char *spec, uint32_t *index) +{ + const char *ptr = str + 1; //skip over the '%' character + *index = 0; + + //parse flag + while(*ptr && strchr(l_flags, *ptr)) + ptr++; + + //parse width + if (*ptr == 0) + return str; + else if (*ptr == '*') + { + (*index)++; + ptr++; + } + else + while (*ptr && strchr("0123456789", *ptr)) + ptr++; + + //parse precision + if (*ptr == '.') + { + ptr++; + + if (*ptr == 0) + return str; + else if (*ptr == '*') + { + (*index)++; + ptr++; + } + else + while (*ptr && strchr("0123456789", *ptr)) + ptr++; + } + + //parse length + if (*ptr && strchr(l_length_chars, *ptr)) + ptr++; + + //parse type + if (*ptr && strchr(l_fs_chars, *ptr)) + { + *spec = *ptr; + ptr++; + } + else + return str; + + return ptr; +} + + + +int32_t rtiprintf(rtprintf_output_handler_t output_handler, void *user, const char *format, va_list args) +{ + int32_t rv = 0; + + while(*format) + { + const char *specifier; + ptrdiff_t offset = 0; + + do + { + specifier = strchr(format + offset, '%'); + offset += 2; + }while(specifier && (specifier[1] == '%')); + + + if (specifier) //% char found + { + //print the string up to the % char + const size_t output_size = specifier - format; + if (output_size) + OUTPUT_CHARS(user, format, output_size); + + //process the specifier + char spec = 0; + uint32_t index; + const char *new_pos = parse_format_specifier(specifier, &spec, &index); + + if (new_pos != specifier) + { + while(index--) + va_arg(args, uint32_t); //skip width and/or precision specified as arguments + + //we have a valid specifier + switch(spec) + { + case 'c': + { + const char val = va_arg(args, uint32_t); + OUTPUT_CHARS(user, &val, 1); + }break; + + case 'd': + { + int32_t val_signed = va_arg(args, int32_t); + if (val_signed < 0) + { + OUTPUT_CHARS(user, "-", 1); + val_signed = -val_signed; + } + const int32_t rv_temp = fsh_integer(output_handler, user, val_signed, "0123456789"); + if (rv_temp < 0) + return rv_temp; + else + rv += rv_temp; + }break; + + case 'u': + { + const int32_t rv_temp = fsh_integer(output_handler, user, va_arg(args, uint32_t), "0123456789"); + if (rv_temp < 0) + return rv_temp; + else + rv += rv_temp; + }break; + + case 'x': + { + const int32_t rv_temp = fsh_integer(output_handler, user, va_arg(args, uint32_t), "0123456789abcdef"); + if (rv_temp < 0) + return rv_temp; + else + rv += rv_temp; + }break; + + case 'X': + { + const int32_t rv_temp = fsh_integer(output_handler, user, va_arg(args, uint32_t), "0123456789ABCDEF"); + if (rv_temp < 0) + return rv_temp; + else + rv += rv_temp; + }break; + + case 's': + { + const char *str = va_arg(args, const char*); + const char *ptr = str; + while(*ptr) + ptr++; + OUTPUT_CHARS(user, str, ptr - str); + }break; + + default: break; + + } + + format = new_pos; + } + else + { + //invalid specifier so print it + OUTPUT_CHARS(user, specifier, 1); + format = specifier + 1; + } + } + else + { + //no % char found + //print the remainder of the string + const char* str = format; + while(*format) + format++; + + OUTPUT_CHARS(user, str, format - str); + } + } + + return rv; +} + + diff --git a/src/malloc.c b/src/malloc.c new file mode 100644 index 0000000..f01b9cd --- /dev/null +++ b/src/malloc.c @@ -0,0 +1,173 @@ +#include + + +static void rtmalloc_insert_in_free_list(rtmalloc_context_t *context, rtmalloc_block_link_t *block) +{ + rtmalloc_block_link_t *i; + + for(i = &context->free_list_head; i->next < block; i = i->next); + + //check if the block and the previous one are contiguous + if (((uint8_t*)i + i->size) == (uint8_t*)block) + { + i->size += block->size; + block = i; + } + + //check if is contigous with the next block + if (((uint8_t*)block + block->size) == (uint8_t*)i->next) + { + if (i->next != context->free_list_tail) + { + block->size += i->next->size; + block->next = i->next->next; + } + else + { + block->next = context->free_list_tail; + } + } + else + { + block->next = i->next; + } + + if (i != block) + i->next = block; +} + + + +void rtmalloc_init(rtmalloc_context_t *context, void *heap, size_t size, size_t alignment) +{ + context->alignment = alignment; + context->alignment_mask = context->alignment - 1; + context->allocated_mask = (size_t)1 << ((sizeof(size_t) << 3) - 1); + context->min_block_size = (sizeof(rtmalloc_block_link_t) + context->alignment) & ~context->alignment_mask; + + //align properly + void *aligned_heap = (void*)(((size_t)heap + context->alignment_mask) & ~context->alignment_mask); + size -= (aligned_heap - heap); + + context->free_list_head.size = 0; + context->free_list_head.next = aligned_heap; + + context->free_list_tail = (rtmalloc_block_link_t*)(((size_t)aligned_heap + size - sizeof(rtmalloc_block_link_t)) & ~context->alignment_mask); + context->free_list_tail->size = 0; + context->free_list_tail->next = NULL; + + context->size_free = (uint8_t*)context->free_list_tail - (uint8_t*)context->free_list_head.next; + context->size_free_min = context->size_free; + + context->free_list_head.next->size = context->size_free; + context->free_list_head.next->next = context->free_list_tail; +} + + + + +void* rtmalloc(rtmalloc_context_t *context, size_t size) +{ + void *p = NULL; + + if ((size & context->allocated_mask) == 0) + { + if (size > 0) + { + size += sizeof(rtmalloc_block_link_t); + size = (size + context->alignment_mask) & ~context->alignment_mask; + } + + if ((size > 0) && (size <= context->size_free)) + { + rtmalloc_block_link_t *prev_block = &context->free_list_head; + rtmalloc_block_link_t *block = context->free_list_head.next; + + while((block->size < size) && block->next) + { + prev_block = block; + block = block->next; + } + + if (block != context->free_list_tail) + { + //get the memory pointer to return + p = (uint8_t*)block + sizeof(rtmalloc_block_link_t); + + //remove from list + prev_block->next = block->next; + + if ((block->size - size) > context->min_block_size) + { + //split a larger block + rtmalloc_block_link_t *new_block_link = (rtmalloc_block_link_t*)((uint8_t*)block + size); + new_block_link->size = block->size - size; + block->size = size; + + rtmalloc_insert_in_free_list(context, new_block_link); + } + + context->size_free -= block->size; + if (context->size_free < context->size_free_min) + context->size_free_min = context->size_free; + + block->size |= context->allocated_mask; + block->next = NULL; + } + } + } + + return p; +} + + +void rtfree(rtmalloc_context_t *context, void *p) +{ + if (p) + { + rtmalloc_block_link_t *block = (rtmalloc_block_link_t*)((uint8_t*)p - sizeof(rtmalloc_block_link_t)); + + if ((block->size & context->allocated_mask) && (block->next == NULL)) + { + block->size &= ~context->allocated_mask; + + context->size_free += block->size; + rtmalloc_insert_in_free_list(context, block); + } + } +} + + +size_t rtmsize(rtmalloc_context_t *context, void *p) +{ + size_t size = 0; + + if (p) + { + rtmalloc_block_link_t *block = (rtmalloc_block_link_t*)((uint8_t*)p - sizeof(rtmalloc_block_link_t)); + + if ((block->size & context->allocated_mask) && (block->next == NULL)) + size = block->size & ~context->allocated_mask; + } + + return size; +} + + +/* +void rtmalloc_dump(malloc_context_t *context) +{ + printf("\nFree block list (%lu)", context->size_free); + + malloc_block_link_t *block = context->free_list_head.next; + + while(block != context->free_list_tail) + { + printf("\n\t%p (%lu)", block, block->size); + block = block->next; + } + + printf("\n\n"); +} +*/ + diff --git a/src/math.c b/src/math.c new file mode 100644 index 0000000..703f4b4 --- /dev/null +++ b/src/math.c @@ -0,0 +1,190 @@ +#include + + +static float cos_taylor(float x); + + +static float agm(float x, float y) +{ + float a = (x + y) / 2.0; + float g = sqrt(x * y); + + while(fabs(a - g) > 1E-7) + { + float an = (a + g) / 2.0; + float gn = sqrt(a * g); + + a = an; + g = gn; + } + + return a; +} + + + +static float sin_taylor(float x) +{ + if (x > (M_PI / 4.0)) + return cos_taylor((M_PI / 2.0) - x); + + float rv = x; + const float xx = x * x; + + float power_term = x; + float factorial_term = 1; + uint32_t factorial_n = 1; + + for(uint32_t i = 0; i < 4; i++) + { + power_term *= xx; + factorial_term *= ++factorial_n; + factorial_term *= ++factorial_n; + + if (i & 0x1) + rv += power_term / factorial_term; + else + rv -= power_term / factorial_term; + } + + return rv; +} + + +static float cos_taylor(float x) +{ + if (x > (M_PI / 4.0)) + return sin_taylor((M_PI / 2.0) - x); + + float rv = 1; + const float xx = x * x; + + float power_term = 1; + float factorial_term = 1; + uint32_t factorial_n = 0; + + for(uint32_t i = 0; i < 4; i++) + { + power_term *= xx; + factorial_term *= ++factorial_n; + factorial_term *= ++factorial_n; + + if (i & 0x1) + rv += power_term / factorial_term; + else + rv -= power_term / factorial_term; + } + + return rv; +} + + + +#if defined(ARM_MATH_CM7) || defined(ARM_MATH_CM4) +#else + +float sin(float x) +{ + x = fmod(x, 2 * M_PI); + if (x < 0) + x += 2 * M_PI; + + if (x > M_PI) + return -sin(x - M_PI); + + if (x > (M_PI / 2.0)) + return cos_taylor(x - (M_PI / 2.0)); + + return sin_taylor(x); +} + + +float cos(float x) +{ + x = fmod(x, 2 * M_PI); + if (x < 0) + x += 2 * M_PI; + + if (x > M_PI) + return -cos(x - M_PI); + + if (x > (M_PI / 2.0)) + return -cos_taylor(M_PI - x); + + return cos_taylor(x); +} + +#endif + + +float exp(float x) +{ + int32_t neg = 0; + + if (x < 0) + { + neg = 1; + x = -x; + } + + float rv = 1; + + int32_t x_int = x; + x -= x_int; + + while(x_int--) + rv *= M_E; + + if (x > 0) + { + float rv_frac = 1; + + for(uint32_t i = 8; i > 0; i--) + rv_frac = 1 + x * rv_frac / (float)i; + + if (neg) + return 1 / (rv * rv_frac); + else + return rv * rv_frac; + + } + + if (neg) + return 1 / rv; + else + return rv; +} + + + +float log(float x) +{ + if (x < 0) + return 0.0/0.0; + + if (x == 0) + return -INFINITY; + + if (x == 1) + return 0; + + float rv1 = 0; + while(x < 1) + { + x *= 2; + rv1 += M_LN2; + } + + const int32_t m = 12; + float rv = M_PI / (2 * agm(1, 4 / (x * (float)(1 << m)))) - m * M_LN2; + + return rv - rv1; +} + + + +float powf(float b, float x) +{ + return exp(x * log(b)); +} + diff --git a/src/printf.c b/src/printf.c new file mode 100644 index 0000000..7e27958 --- /dev/null +++ b/src/printf.c @@ -0,0 +1,672 @@ +#include + +#include +#include +#include +#include +#include + +#include + +#include <../src/stdio_internal.h> + + +static const char l_flags[] = "-+ #0"; +static const char l_length_chars[] = "hljzt"; + +#define max(a, b) (((uint32_t)(a) > (uint32_t)(b)) ? (uint32_t)(a) : (uint32_t)(b)) +#define min(a, b) (((uint32_t)(a) < (uint32_t)(b)) ? (uint32_t)(a) : (uint32_t)(b)) + + +//printf %c +static int32_t fsh_c(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s) +{ + int32_t rv = 0; + char val = (char)va_arg(*arg, int32_t); + OUTPUT_CHARS(user, &val, 1); + return rv; +} + + +//printf %s +static int32_t fsh_s(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s) +{ + int32_t rv = 0; + + if (s->width == SPECIFIED_AS_ARG) + s->width = va_arg(*arg, uint32_t); + else if (s->width == NOT_SPECIFIED) + s->width = 0; + + if (s->precision == SPECIFIED_AS_ARG) + s->precision = va_arg(*arg, uint32_t); + + const char *str = va_arg(*arg, const char*); + + const size_t print_length = (s->precision == NOT_SPECIFIED) ? strlen(str) : strnlen(str, s->precision); + + const uint32_t width_total = max(s->width, print_length); + uint32_t width_padding = width_total - print_length; + + if (s->flags & FLAG_MINUS) + OUTPUT_CHARS(user, str, print_length); + + while(width_padding--) + OUTPUT_CHARS(user, " ", 1); + + if ((s->flags & FLAG_MINUS) == 0) + OUTPUT_CHARS(user, str, print_length); + + return rv; +} + +//print %f +static int32_t fsh_float(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s) +{ + int32_t rv = 0; + + if (s->width == SPECIFIED_AS_ARG) + s->width = va_arg(*arg, uint32_t); + else if (s->width == NOT_SPECIFIED) + s->width = 0; + + if (s->precision == SPECIFIED_AS_ARG) + s->precision = va_arg(*arg, uint32_t); + else if (s->precision == NOT_SPECIFIED) + s->precision = 6; + + double val = va_arg(*arg, double); + char sign = 0; + + if (val < 0) + { + sign = '-'; + val = -val; + } + + if ((s->flags & FLAG_PLUS) && (sign == 0)) + sign = '+'; + + //round the number to the precision specified + uint64_t p = 1; + for(int32_t i = 0; i < s->precision; i++) + p *= 10; + + + uint64_t integer = (uint64_t)val; + const double frac_d = (val - integer) * p; + uint64_t frac = (uint64_t)frac_d; + const double diff = frac_d - frac; + + if (diff > 0.5) + { + frac++; + + if (frac >= p) + { + frac = 0; + integer++; + } + } + else if (diff < 0.5) + { + } + else if ((frac == 0) || (frac & 1)) + { + //banker's rounding rule + frac++; + } + + + if (s->precision == 0) + { + const double diff = val - integer; + if (!((diff < 0.5) || (diff > 0.5)) && (integer & 1)) + integer++; + } + + + //get the digits + char str[40]; //will fit 10^38 (integer part only) + char *ptr = str + sizeof(str); + if (integer) + { + while(integer) + { + *--ptr = (integer % 10) + '0'; + integer /= 10; + } + } + else + *--ptr = '0'; + + //get the decimals + char decimals[s->precision + 1]; + if (s->precision) + { + char *p_decimal = decimals + sizeof(decimals); + + for(int32_t i = 0; i < s->precision; i++) + { + *--p_decimal = (frac % 10) + '0'; + frac /= 10; + } + + *--p_decimal = '.'; + } + + + const uint32_t significant_digits = str + sizeof(str) - ptr; //total number of significant digits (without the sign) + const uint32_t total_chars = significant_digits + (sign ? 1 : 0) + (s->precision ? s->precision + 1 : 0); + + const uint32_t width_total = max(s->width, total_chars); + uint32_t width_padding = width_total - total_chars; + + + if (s->flags & FLAG_MINUS) + { + //left alignment + if (sign) + OUTPUT_CHARS(user, &sign, 1); + + OUTPUT_CHARS(user, ptr, significant_digits); + + if (s->precision) + OUTPUT_CHARS(user, decimals, s->precision + 1); + + while(width_padding--) + OUTPUT_CHARS(user, " ", 1); + } + else + { + const char pad_char = (s->flags & FLAG_ZERO) ? '0' : ' '; + + //right alignment + if ((s->flags & FLAG_ZERO) && sign) + OUTPUT_CHARS(user, &sign, 1); + + while(width_padding--) + OUTPUT_CHARS(user, &pad_char, 1); + + if (!(s->flags & FLAG_ZERO) && sign) + OUTPUT_CHARS(user, &sign, 1); + + OUTPUT_CHARS(user, ptr, significant_digits); + + if (s->precision) + OUTPUT_CHARS(user, decimals, s->precision + 1); + } + + + return rv; +} + + + +//print integers%d, %u, %x, %X, %o, %p +static int32_t fsh_integer(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s, const char *charset, int32_t is_signed) +{ + int32_t rv = 0; + + const uint32_t base = strlen(charset); + + char str[32]; //will fit a 32 bit number represented in binary + char* const str_end = str + sizeof(str); + + if (s->width == SPECIFIED_AS_ARG) + s->width = va_arg(*arg, uint32_t); + else if (s->width == NOT_SPECIFIED) + s->width = 0; + + if (s->precision == SPECIFIED_AS_ARG) + s->precision = va_arg(*arg, uint32_t); + else if (s->precision == NOT_SPECIFIED) + s->precision = 1; + + uint64_t val; + char sign = 0; + + if (is_signed) + { + int64_t v; + + if (s->length == 'l') + v = va_arg(*arg, int64_t); + else + v = va_arg(*arg, int32_t); + + if (v < 0) + { + val = -v; + sign = '-'; + } + else + val = v; + } + else + { + if (s->length == 'l') + val = va_arg(*arg, uint64_t); + else + val = va_arg(*arg, uint32_t); + } + + + if ((s->flags & FLAG_PLUS) && (sign == 0)) + sign = '+'; + + char *ptr = str_end; + while(val) + { + *--ptr = charset[val % base]; + val /= base; + } + + const uint32_t significant_digits = str_end - ptr; //total number of significant digits (without the sign) + uint32_t precision_digits = max(s->precision, significant_digits); //total number of digits including the precision padding + uint32_t precision_padding = precision_digits - significant_digits; //how many zeros to pad with + const uint32_t precision_digits_sign = sign ? (precision_digits + 1) : precision_digits; //make space for sign + + const uint32_t width_total = max(s->width, precision_digits_sign); + uint32_t width_padding = width_total - precision_digits_sign; + + if (s->flags & FLAG_MINUS) + { + //left alignment + if (sign) + OUTPUT_CHARS(user, &sign, 1); + + while(precision_padding--) + OUTPUT_CHARS(user, "0", 1); + + OUTPUT_CHARS(user, ptr, significant_digits); + + while(width_padding--) + OUTPUT_CHARS(user, " ", 1); + } + else + { + const char pad_char = (s->flags & FLAG_ZERO) ? '0' : ' '; + + //right alignment + if ((s->flags & FLAG_ZERO) && sign) + OUTPUT_CHARS(user, &sign, 1); + + while(width_padding--) + OUTPUT_CHARS(user, &pad_char, 1); + + if (!(s->flags & FLAG_ZERO) && sign) + OUTPUT_CHARS(user, &sign, 1); + + while(precision_padding--) + OUTPUT_CHARS(user, "0", 1); + + OUTPUT_CHARS(user, ptr, significant_digits); + } + + return rv; +} + + +//print %u +static int32_t fsh_u(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s) +{ + return fsh_integer(output_handler, user, arg, s, "0123456789", 0); +} + +//print %d +static int32_t fsh_d(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s) +{ + return fsh_integer(output_handler, user, arg, s, "0123456789", 1); +} + + +//printf %x +static int32_t fsh_x(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s) +{ + return fsh_integer(output_handler, user, arg, s, "0123456789abcdef", 0); +} + + +//printf %X +static int32_t fsh_x_cap(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s) +{ + return fsh_integer(output_handler, user, arg, s, "0123456789ABCDEF", 0); +} + +//printf %o +static int32_t fsh_o(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s) +{ + return fsh_integer(output_handler, user, arg, s, "01234567", 0); +} + + + +/* +//print %r - fixed point positive (number of fraction bits precedes the value in the variable argument list) +static int fsh_ufxp_com(FILE *f, va_list *arg, format_specifier_t *s, uint8_t shortest, uint8_t signed_print) +{ + char integer_str[12]; //will fit a 32 bit number + char fraction_str[32]; //will fit a 32 bit fractional part + + if (s->width == SPECIFIED_AS_ARG) + s->width = va_arg(*arg, uint32_t); + else if (s->width == NOT_SPECIFIED) + s->width = 0; + if (s->precision == SPECIFIED_AS_ARG) + s->precision = va_arg(*arg, uint32_t); + else if (s->precision == NOT_SPECIFIED) + { + shortest = 1; + s->precision = sizeof(fraction_str); + } + + s->precision = min(s->precision, sizeof(fraction_str)); //TODO + + uint32_t frac_bits = va_arg(*arg, uint32_t); //this is also the maximum number of decimal fraction digits + uint32_t val; + char sign; + + + if (signed_print) + { + int32_t signed_val = va_arg(*arg, int32_t); + + if (signed_val < 0) + { + val = -signed_val; + sign = '-'; + } + else + { + val = signed_val; + sign = (s->flags & FLAG_PLUS) ? '+' : 0; + } + } + else + { + val = va_arg(*arg, uint32_t); + sign = (s->flags & FLAG_PLUS) ? '+' : 0; + } + + + uint32_t fraction = val & ((1 << frac_bits) - 1); + val >>= frac_bits; + + char *integer_ptr = integer_str + sizeof(integer_str); + if (val) + { + while(val) + { + *--integer_ptr = (val % 10) + '0'; + val /= 10; + } + } + else + *--integer_ptr = '0'; + + char *fraction_ptr = fraction_str; + const uint32_t mask = (1 << frac_bits) - 1; + while((fraction || !shortest) && s->precision) + { + s->precision--; + + fraction *= 10; + *fraction_ptr++ = (fraction >> frac_bits) + '0'; + fraction &= mask; + } + + const uint32_t integer_digits = integer_str + sizeof(integer_str) - integer_ptr; //total number of significant digits (without the sign) + const uint32_t fraction_digits = fraction_ptr - fraction_str; + + const uint32_t total_digits = integer_digits + + fraction_digits + ((fraction_digits) ? 1 : 0); //if we have fraction digits we need one space for '.' + + const uint32_t width_total = max(s->width, total_digits); + uint32_t width_padding = width_total - total_digits; + + if (s->flags & FLAG_MINUS) + { + //left alignment + if (sign) + if (fputc(sign, f) < 0) + return -1; + + if (fwrite(integer_ptr, integer_digits, 1, f) != 1) + return -1; + + if (fraction_digits) + { + if (fputc('.', f) < 0) + return -1; + + if (fwrite(fraction_str, fraction_digits, 1, f) != 1) + return -1; + } + + while(width_padding--) + if (fputc(' ', f) < 0) + return -1; + } + else + { + const char pad_char = (s->flags & FLAG_ZERO) ? '0' : ' '; + + //right alignment + if ((s->flags & FLAG_ZERO) && sign) + if (fputc(sign, f) < 0) + return -1; + + while(width_padding--) + if (fputc(pad_char, f) < 0) + return -1; + + if (!(s->flags & FLAG_ZERO) && sign) + if (fputc(sign, f) < 0) + return -1; + + if (fwrite(integer_ptr, integer_digits, 1, f) != 1) + return -1; + + if (fraction_digits) + { + if (fputc('.', f) < 0) + return -1; + + if (fwrite(fraction_str, fraction_digits, 1, f) != 1) + return -1; + } + } + + return width_total; +} + +static int fsh_r(FILE *f, va_list *arg, format_specifier_t *s) +{ + return fsh_ufxp_com(f, arg, s, 1, 0); +} + +static int fsh_R(FILE *f, va_list *arg, format_specifier_t *s) +{ + return fsh_ufxp_com(f, arg, s, 0, 0); +} + +static int fsh_q(FILE *f, va_list *arg, format_specifier_t *s) +{ + return fsh_ufxp_com(f, arg, s, 1, 1); +} + +static int fsh_Q(FILE *f, va_list *arg, format_specifier_t *s) +{ + return fsh_ufxp_com(f, arg, s, 0, 1); +} +*/ + + +static char l_fs_chars[16] = "duxXocs"; +static rtformat_specifier_handler_t l_fs_handlers[sizeof(l_fs_chars)] = {fsh_d, fsh_u, fsh_x, fsh_x_cap, fsh_o, fsh_c, fsh_s}; + + +static inline const char* parse_format_specifier(const char *str, rtformat_specifier_t *s) +{ + const char *ptr = str + 1; + + //parse flag + s->flags = 0; + + char *pos; + while(*ptr && (pos = strchr(l_flags, *ptr))) + { + s->flags |= (1 << (pos - l_flags)); + ptr++; + } + + //parse width + s->width = NOT_SPECIFIED; //default + + if (*ptr == 0) + return str; + else if (*ptr == '*') + { + s->width = SPECIFIED_AS_ARG; + ptr++; + } + else if (strchr("123456789", *ptr)) + { + char *end; + s->width = strtoul(ptr, &end, 10); + ptr = end; + } + + + //parse precision + s->precision = NOT_SPECIFIED; //default + + if (*ptr == '.') + { + ptr++; + + if (*ptr == 0) + return str; + else if (*ptr == '*') + { + s->precision = SPECIFIED_AS_ARG; + ptr++; + } + else if (strchr("0123456789", *ptr)) + { + char *end; + s->precision = strtoul(ptr, &end, 10); + ptr = end; + } + else + return str; + } + + + //parse length + s->length = 0; + + if (*ptr == 0) + return str; + else if (strchr(l_length_chars, *ptr)) + s->length = *ptr++; + + //parse type + const char *c; + + if (*ptr == 0) + return str; + else if ((c = strchr(l_fs_chars, *ptr))) + { + s->specifier_index = c - l_fs_chars; + ptr++; + } + else + return str; + + return ptr; +} + + + +int32_t rtprintf(rtprintf_output_handler_t output_handler, void *user, const char *format, va_list args) +{ + int32_t rv = 0; + rtformat_specifier_t spec; + + while(*format) + { + ptrdiff_t offset = 0; + const char *specifier = strchr(format + offset, '%'); + + if (specifier) //% char found + { + //print the string up to the % char + const size_t output_size = specifier - format; + if (output_size) + OUTPUT_CHARS(user, format, output_size); + + + const char *new_pos = parse_format_specifier(specifier, &spec); + + if (new_pos != specifier) + { + //we have a valid specifier + const int32_t rv_temp = l_fs_handlers[spec.specifier_index](output_handler, user, &args, &spec); + if (rv_temp < 0) + return rv_temp; + else + rv += rv_temp; + + format = new_pos; + } + else + { + //invalid specifier so print it + format = specifier + 1; + + if (*format) + { + OUTPUT_CHARS(user, format, 1); + format++; + } + } + } + else + { + //no % char found + //print the remainder of the string + const char* str = format; + while(*format) + format++; + + OUTPUT_CHARS(user, str, format - str); + } + } + + return rv; +} + + + +int32_t rtprintf_add_format(char specifier, rtformat_specifier_handler_t handler) +{ + const size_t i = strlen(l_fs_chars); + + if (i >= (sizeof(l_fs_chars) - 1)) + return -1; + + l_fs_chars[i] = specifier; + l_fs_chars[i + 1] = 0; + l_fs_handlers[i] = handler; + + return 0; +} + + +void rtprintf_include_float(void) +{ + assert(rtprintf_add_format('f', fsh_float) == 0); +} + diff --git a/src/stdio.c b/src/stdio.c new file mode 100644 index 0000000..0339185 --- /dev/null +++ b/src/stdio.c @@ -0,0 +1,120 @@ +#include + +#include +#include +#include +#include + +#include <../src/stdio_internal.h> + + + +static int32_t rtstring_output_handler(void *user, const char *data, size_t size) +{ + string_descriptor_t *str_descriptor = (string_descriptor_t*)user; + + size_t len = size; + + if (str_descriptor->len > 0) + { + if (len > (str_descriptor->len - 1)) + len = str_descriptor->len - 1; + + str_descriptor->len -= len; + } + + if (str_descriptor->ptr && len) + { + memcpy(str_descriptor->ptr, data, len); + str_descriptor->ptr += len; + *str_descriptor->ptr = 0; + } + + return size; +} + + + +int32_t sprintf(char *str, const char *format, ...) +{ + va_list args; + va_start(args, format); + + string_descriptor_t str_descriptor = {str, -1}; + const int32_t rv = rtprintf(rtstring_output_handler, &str_descriptor, format, args); + + va_end(args); + return rv; +} + + +int32_t snprintf(char *str, size_t n, const char *format, ...) +{ + va_list args; + va_start(args, format); + + string_descriptor_t str_descriptor = {str, n}; + const int32_t rv = rtprintf(rtstring_output_handler, &str_descriptor, format, args); + + va_end(args); + return rv; +} + + +int32_t vsprintf(char *str, const char *format, va_list args) +{ + string_descriptor_t str_descriptor = {str, -1}; + return rtprintf(rtstring_output_handler, &str_descriptor, format, args); +} + + + +int32_t vsnprintf(char *str, size_t n, const char *format, va_list args) +{ + string_descriptor_t str_descriptor = {str, n}; + return rtprintf(rtstring_output_handler, &str_descriptor, format, args); +} + + + +//simplified versions +int32_t isprintf(char *str, const char *format, ...) +{ + va_list args; + va_start(args, format); + + string_descriptor_t str_descriptor = {str, -1}; + const int32_t rv = rtiprintf(rtstring_output_handler, &str_descriptor, format, args); + + va_end(args); + return rv; +} + + +int32_t isnprintf(char *str, size_t n, const char *format, ...) +{ + va_list args; + va_start(args, format); + + string_descriptor_t str_descriptor = {str, n}; + int32_t rv = rtiprintf(rtstring_output_handler, &str_descriptor, format, args); + + va_end(args); + return rv; +} + + +int32_t ivsprintf(char *str, const char *format, va_list args) +{ + string_descriptor_t str_descriptor = {str, -1}; + return rtiprintf(rtstring_output_handler, &str_descriptor, format, args); +} + + + +int32_t ivsnprintf(char *str, size_t n, const char *format, va_list args) +{ + string_descriptor_t str_descriptor = {str, n}; + return rtiprintf(rtstring_output_handler, &str_descriptor, format, args); +} + diff --git a/src/stdio_internal.h b/src/stdio_internal.h new file mode 100644 index 0000000..48a76e8 --- /dev/null +++ b/src/stdio_internal.h @@ -0,0 +1,28 @@ +#ifndef _STDIO_INTERNAL_H +#define _STDIO_INTERNAL_H + +#include + +#include +#include + + +typedef struct +{ + char *ptr; + size_t len; +}string_descriptor_t; + + + +#define OUTPUT_CHARS(user, message, size) \ +{\ + const int32_t rv_temp = output_handler(user, message, size);\ + if (rv_temp < 0)\ + return rv_temp;\ + else\ + rv += rv_temp;\ +} + +#endif + diff --git a/src/stdlib.c b/src/stdlib.c new file mode 100644 index 0000000..f74c1c0 --- /dev/null +++ b/src/stdlib.c @@ -0,0 +1,344 @@ +#include +#include +#include + +#include + +#include + + +__attribute__((weak)) void rtlibc_abort_handler(void) +{ + +} + + +static inline uint8_t char2digit(char c) +{ + if (c >= 'a') + return c - 'a' + 10; + else if (c >= 'A') + return c - 'A' + 10; + else + return c - '0'; +} + + +uint64_t strtoul(const char *str, char **endptr, int32_t base) +{ + const int32_t max_base = 'Z' - 'A' + 11; + + if ((base > max_base) || (base < 0)) + { + *endptr = (char*)str; + return 0; + } + + while(*str && isspace(*str)) + str++; + + if (*str == 0) + { + *endptr = (char*)str; + return 0; + } + + if (base == 0) + { + //autodetect the base + if (*str == '0') + { + str++; + if (*str == 0) + { + *endptr = (char*)str; + return 0; + } + + if ((*str == 'x') || (*str == 'X')) + { + base = 16; + str++; + } + else + { + base = 8; + } + } + else if (strchr("123456789", *str)) + { + base = 10; + } + else + { + *endptr = (char*)str; + return 0; + } + } + + //build allowed char list + char charset[max_base + 1]; + char *ptr = charset; + for(int32_t i = 0; i < base; i++) + { + if (i < 10) + *ptr++ = i + '0'; + else + { + *ptr++ = i - 10 + 'A'; + *ptr++ = i - 10 + 'a'; + } + } + *ptr = 0; + + uint64_t val = 0; + + while(*str && strchr(charset, *str)) + { + val *= base; + val += char2digit(*str); + str++; + } + + *endptr = (char*)str; + + return val; +} + + + +int64_t strtol(const char *str, char **endptr, int32_t base) +{ + const int32_t max_base = 'Z' - 'A' + 11; + + if ((base > max_base) || (base < 0)) + { + *endptr = (char*)str; + return 0; + } + + while(*str && isspace(*str)) + str++; + + if (*str == 0) + { + *endptr = (char*)str; + return 0; + } + + bool negative = false; + + if (*str == '-') + { + negative = true; + str++; + } + else if (*str == '+') + str++; + + if (base == 0) + { + //autodetect the base + if (*str == '0') + { + str++; + if (*str == 0) + { + *endptr = (char*)str; + return 0; + } + + if ((*str == 'x') || (*str == 'X')) + { + base = 16; + str++; + } + else + { + base = 8; + } + } + else if (strchr("123456789", *str)) + { + base = 10; + } + else + { + *endptr = (char*)str; + return 0; + } + } + + //build allowed char list + char charset[max_base + 1]; + char *ptr = charset; + for(int32_t i = 0; i < base; i++) + { + if (i < 10) + *ptr++ = i + '0'; + else + { + *ptr++ = i - 10 + 'A'; + *ptr++ = i - 10 + 'a'; + } + } + *ptr = 0; + + int64_t val = 0; + + while(*str && strchr(charset, *str)) + { + val *= base; + val += char2digit(*str); + str++; + } + + *endptr = (char*)str; + + if (negative) + return -val; + else + return val; +} + + + +float strtof(const char *str, char **endptr) +{ + while(*str && isspace(*str)) + str++; + + if (*str == 0) + { + *endptr = (char*)str; + return 0; + } + + bool negative = false; + + if (*str == '-') + { + negative = true; + str++; + } + else if (*str == '+') + str++; + + + float val = 0; + + while(*str && strchr("0123456789", *str)) + { + val *= 10; + val += *str - '0'; + str++; + } + + + if (*str == '.') //decimals follow + { + str++; + + uint64_t frac_value = 0; + uint64_t frac_denominator = 1; + + while(*str && strchr("0123456789", *str)) + { + frac_denominator *= 10; + frac_value *= 10; + frac_value += *str - '0'; + + str++; + } + + val += frac_value / (float)frac_denominator; + + if (*str) + { + *endptr = (char*)str; + + if (negative) + return -val; + else + return val; + } + } + + + *endptr = (char*)str; + + if (negative) + return -val; + else + return val; +} + + + + +void abort(void) +{ + rtlibc_abort_handler(); +} + + + + +#ifdef _RTLIBC_MALLOC_FREERTOS_ + +#include + +void* malloc(size_t size) +{ + return pvPortMalloc(size); +} + +void free(void *p) +{ + vPortFree(p); +} + +#endif + + +#ifdef _RTLIBC_MALLOC_NOOS_ + +#include + +static rtmalloc_context_t l_malloc_context; + +__attribute__((weak)) void rtlibc_malloc_failed_handler(void) +{ +} + +void malloc_init(void *heap, size_t size, size_t alignment) +{ + assert(heap); + assert(size); + assert(alignment); + rtmalloc_init(&l_malloc_context, heap, size, alignment); +} + +void* malloc(size_t size) +{ + void *ptr = rtmalloc(&l_malloc_context, size); + + if (ptr == NULL) + rtlibc_malloc_failed_handler(); + + return ptr; +} + +void free(void *p) +{ + assert(p); + rtfree(&l_malloc_context, p); +} + +size_t msize(void *p) +{ + assert(p); + return rtmsize(&l_malloc_context, p); +} + +#endif diff --git a/src/string.c b/src/string.c new file mode 100644 index 0000000..35dcb6a --- /dev/null +++ b/src/string.c @@ -0,0 +1,659 @@ +#include + +#include +#include + + +//String manipulation functions + +void strtrim(char **str, const char *trim) +{ + //skip all the delimiter chars at the beginning + while(**str && strchr(trim, **str)) + (*str)++; + + //if we still have a non null string, trim from the back + if (**str) + { + char *end = *str + strlen(*str); + while(strchr(trim, *(--end))); + *(++end) = 0; + } +} + + +size_t strlen(const char *str) +{ + size_t s = 0; + + while(*str++) + s++; + + return s; +} + + +size_t strnlen(const char *str, size_t num) +{ + size_t s = 0; + + while(*str++ && num--) + s++; + + return s; +} + + +size_t wstrlen(const uint16_t *str) +{ + size_t s = 0; + + while(*str++) + s++; + + return s; +} + + +char* strcat(char *dest, const char *src) +{ + char *rv = dest; + + while(*dest) + dest++; + + do + { + *dest++ = *src; + }while(*src++); + + return rv; +} + + +char* strncat(char *dest, const char *src, size_t num) +{ + char *rv = dest; + + while(*dest) + dest++; + + while(*src && num--) + { + *dest++ = *src++; + } + + *dest = 0; + + return rv; +} + + +char* strchr(const char *str, char c) +{ + while(*str) + { + if (*str == c) + return (char*)str; + + str++; + } + + if (*str == c) + return (char*)str; + else + return 0; +} + + +int strcmp(const char *str1, const char *str2) +{ + while(*str1 && *str2) + { + if ((*str1) > (*str2)) + return 1; + else if ((*str1) < (*str2)) + return -1; + + str1++; + str2++; + } + + if (*str1) + return 1; + else if (*str2) + return -1; + + return 0; +} + + +int strncmp(const char *str1, const char *str2, size_t num) +{ + while(*str1 && *str2 && num) + { + if ((*str1) > (*str2)) + return 1; + else if ((*str1) < (*str2)) + return -1; + + str1++; + str2++; + + num--; + } + + if (num == 0) + return 0; + else if (*str1) + return 1; + else if (*str2) + return -1; + + return 0; +} + + +int stricmp(const char *str1, const char *str2) +{ + while(*str1 && *str2) + { + if (tolower(*str1) > tolower(*str2)) + return 1; + else if (tolower(*str1) < tolower(*str2)) + return -1; + + str1++; + str2++; + } + + if (*str1) + return 1; + else if (*str2) + return -1; + + return 0; +} + + + +int strincmp(const char *str1, const char *str2, size_t num) +{ + while(*str1 && *str2 && num) + { + if (tolower(*str1) > tolower(*str2)) + return 1; + else if (tolower(*str1) < tolower(*str2)) + return -1; + + str1++; + str2++; + + num--; + } + + if (num == 0) + return 0; + else if (*str1) + return 1; + else if (*str2) + return -1; + + return 0; +} + + + +char* strcpy(char *dest, const char *src) +{ + char *rv = dest; + do + { + *dest++ = *src; + }while(*src++); + + return rv; +} + + +char* strncpy(char *dest, const char *src, size_t num) +{ + char *rv = dest; + + while(*src && num) + { + *dest++ = *src++; + num--; + } + + while(num--) + *dest++ = 0; + + return rv; +} + + + +size_t strspn(const char *str1, const char *str2) +{ + size_t span = 0; + + while(*str1 && strchr(str2, *str1)) + { + str1++; + span++; + } + + return span; +} + + +size_t strcspn(const char *str1, const char *str2) +{ + size_t span = 0; + + while(*str1 && !strchr(str2, *str1)) + { + str1++; + span++; + } + + return span; +} + + + +//char *strdup(const char *s1); + + +//const char* strpbrk(const char *str1, const char *str2); +char* strpbrk(char *str1, const char *str2) +{ + while(*str1) + { + if (strchr(str2, *str1)) + return str1; + str1++; + } + return 0; +} + + +/* +const char* strstr(const char *str1, const char *str2) +{ + const char *ptr1 = str1; + const char *ptr2 = str2; + + while(*str1) + { + if (*ptr2 == 0) + return str1; + + if (*ptr1 == *ptr2) + { + ptr1++; + ptr2++; + } + else + { + ptr2 = str2; + str1++; + ptr1 = str1; + } + } + + return 0; +} +*/ + + +char* strstr(char *str1, const char *str2) +{ + char *ptr1 = str1; + const char *ptr2 = str2; + + while(*str1) + { + if (*ptr2 == 0) + return str1; + + if (*ptr1 == *ptr2) + { + ptr1++; + ptr2++; + } + else + { + ptr2 = str2; + str1++; + ptr1 = str1; + } + } + + return 0; +} + + +char* stristr(char *str1, const char *str2) +{ + char *ptr1 = str1; + const char *ptr2 = str2; + + while(*str1) + { + if (*ptr2 == 0) + return str1; + + if (tolower(*ptr1) == tolower(*ptr2)) + { + ptr1++; + ptr2++; + } + else + { + ptr2 = str2; + str1++; + ptr1 = str1; + } + } + + return 0; +} + + +char* strtok(char *str, const char *delim) +{ + static char *saveptr; + + if (str) + saveptr = str; + + if (*saveptr == 0) + return 0; + + //find first non-delimiter + while(*saveptr && strchr(delim, *saveptr)) + saveptr++; + + char *result = saveptr; + + while(*saveptr && !strchr(delim, *saveptr)) + saveptr++; + + if (*saveptr) + *saveptr++ = 0; + + return result; +} + + +char* strtok_r(char *str, const char *delim, char **saveptr) +{ + if (str) + *saveptr = str; + + if (**saveptr == 0) + return 0; + + //find first non-delimiter + while(**saveptr && strchr(delim, **saveptr)) + (*saveptr)++; + + char *result = *saveptr; + + while(**saveptr && !strchr(delim, **saveptr)) + (*saveptr)++; + + if (**saveptr) + *(*saveptr)++ = 0; + + return result; +} + + + +char* strtok_m(char *str, const char *delim, const char* token_markers) +{ + static char *saveptr; + + if (str) + saveptr = str; + + if (*saveptr == 0) + return 0; + + const size_t stm = strlen(token_markers); + uint8_t tm_count[stm]; + + memset(tm_count, 0, sizeof(tm_count)); + + //find first non-delimiter + while(*saveptr && strchr(delim, *saveptr)) + saveptr++; + + char *result = saveptr; + uint8_t inside = 0; + + while(*saveptr) + { + char *ptr; + + ptr = strchr(token_markers, *saveptr); + if (ptr) + { + ptrdiff_t i = ptr - token_markers; + tm_count[i] = !tm_count[i]; + + inside = 0; + for(uint32_t i = 0; i < stm; i++) + if (tm_count[i]) + { + inside = 1; + break; + } + } + else + { + //check if inside token marker + if (!inside && strchr(delim, *saveptr)) + break; + } + + saveptr++; + } + + if (*saveptr) + *saveptr++ = 0; + + return result; +} + + +char* strtok_rm(char *str, const char *delim, char **saveptr, const char* token_markers) +{ + if (str) + *saveptr = str; + + if (**saveptr == 0) + return 0; + + const size_t stm = strlen(token_markers); + uint8_t tm_count[stm]; + + memset(tm_count, 0, sizeof(tm_count)); + + //find first non-delimiter + while(**saveptr && strchr(delim, **saveptr)) + (*saveptr)++; + + char *result = *saveptr; + uint8_t inside = 0; + + while(**saveptr) + { + char *ptr; + + ptr = strchr(token_markers, **saveptr); + if (ptr) + { + ptrdiff_t i = ptr - token_markers; + tm_count[i] = !tm_count[i]; + + inside = 0; + for(uint32_t i = 0; i < stm; i++) + if (tm_count[i]) + { + inside = 1; + break; + } + } + else + { + if (!inside && strchr(delim, **saveptr)) + break; + } + + (*saveptr)++; + } + + if (**saveptr) + *(*saveptr)++ = 0; + + return result; +} + + +//memory functions +void* memcpy(void *dest, const void *src, size_t num) +{ + if (num) + { + const uint8_t *src_ptr8 = (const uint8_t*)src; + uint8_t *dest_ptr8 = (uint8_t*)dest; + uint8_t * const dest_ptr8_end = dest_ptr8 + num; + + if (((uintptr_t)src_ptr8 & 0x03) == ((uintptr_t)dest_ptr8 & 0x03)) //if we have the same alignment + { + uint8_t *end = (uint8_t*)(((uintptr_t)dest_ptr8 + 3) & ~0x3); + if (end > dest_ptr8_end) + end = dest_ptr8_end; + + while(dest_ptr8 < end) + *dest_ptr8++ = *src_ptr8++; + + uint32_t *dest_ptr32 = (uint32_t*)dest_ptr8; + const uint32_t *src_ptr32 = (const uint32_t*)src_ptr8; + uint32_t * const dest_ptr32_end = (uint32_t*)((uintptr_t)dest_ptr8_end & ~0x3); + while(dest_ptr32 < dest_ptr32_end) + *dest_ptr32++ = *src_ptr32++; + + dest_ptr8 = (uint8_t*)dest_ptr32; + src_ptr8 = (uint8_t*)src_ptr32; + while(dest_ptr8 < dest_ptr8_end) + *dest_ptr8++ = *src_ptr8++; + } + else + { + while(dest_ptr8 < dest_ptr8_end) + *dest_ptr8++ = *src_ptr8++; + } + } + + return dest; +} + +void* memmove(void *dest, const void *src, size_t num) +{ + if (dest == src) + return dest; + + uint8_t *src_p8 = (uint8_t*)src; + uint8_t *dest_p8 = (uint8_t*)dest; + + if (src < dest) + { + //copy from back + src_p8 += num - 1; + dest_p8 += num - 1; + + while(num--) + *dest_p8-- = *src_p8--; + } + else + { + //copy from front + while(num--) + *dest_p8++ = *src_p8++; + } + + return dest; +} + + +//const void* memchr(const void *ptr, uint8_t value, size_t num); +//void* memchr(void *ptr, uint8_t value, size_t num); + +int memcmp(const void *ptr1, const void *ptr2, size_t num) +{ + const uint8_t *l_ptr8 = (const uint8_t*)ptr1; + const uint8_t *r_ptr8 = (const uint8_t*)ptr2; + + while(num--) + { + if (*l_ptr8 > *r_ptr8) + return 1; + else if (*l_ptr8 < *r_ptr8) + return -1; + + l_ptr8++; + r_ptr8++; + } + + return 0; +} + + +void* memset(void *ptr, uint8_t value, size_t num) +{ + if (num) + { + uint8_t *dest_ptr8 = (uint8_t*)ptr; + uint8_t * const dest_ptr8_end = dest_ptr8 + num; + const uint32_t value32 = ((uint32_t)value << 24) + ((uint32_t)value << 16) + ((uint32_t)value << 8) + (uint32_t)value; + + uint8_t *end = (uint8_t*)(((uintptr_t)dest_ptr8 + 3) & ~0x3); + if (end > dest_ptr8_end) + end = dest_ptr8_end; + while(dest_ptr8 < end) + *dest_ptr8++ = value; + + uint32_t *dest_ptr32 = (uint32_t*)dest_ptr8; + uint32_t * const dest_ptr32_end = (uint32_t*)((uintptr_t)dest_ptr8_end & ~0x3); + while(dest_ptr32 < dest_ptr32_end) + *dest_ptr32++ = value32; + + dest_ptr8 = (uint8_t*)dest_ptr32; + while(dest_ptr8 < dest_ptr8_end) + *dest_ptr8++ = value; + } + + return ptr; +} + + +const char* strfile(const char *fullpath) +{ + const char *ptr = fullpath + strlen(fullpath); + + while(ptr >= fullpath) + { + if (*ptr == '/') + return ptr + 1; + + ptr--; + } + + return fullpath; +} + diff --git a/template/rtlibc_conf.h b/template/rtlibc_conf.h new file mode 100644 index 0000000..ec99291 --- /dev/null +++ b/template/rtlibc_conf.h @@ -0,0 +1,40 @@ +/****************************************************************************** +* File: rtlibc_conf.h +* +* Description: RTLIBC configuration include file +* Must be copied in the application directory +* and customized. +* +* (C) 2016 Thornwave Labs LLC. +* +* This unpublished material is property of Thornwave Labs LLC. +* All rights reserved. The methods and techniques described herein +* are considered trade secrets and/or confidential. +* Reproduction or distribution, in whole or in part, is forbidden +* except by express written permission of Thornwave Labs LLC. +* +******************************************************************************* +* Current version: 1.0 +* +* Change log +* ********** +* +* 1.0 Razvan Turiac 3/9/2016 Initial version +* +******************************************************************************/ +#ifndef _RTLIBC_CONF_H +#define _RTLIBC_CONF_H + + +//define this if the assert() code is needed +//if defined it will require the following hookups to be provided by the application +//void assert_failed_handler(const char *message, const char *file_name, uint32_t line_no); +#define _RTLIBC_ASSERT_ + + +//if defined it will use FreeRTOS allocator to provide malloc(), realloc() and free() +//#define _RTLIBC_MALLOC_FREERTOS_ + + +#endif +