jesus/tinyusb
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add BSP support for F1C100s

Browse Source
This commit is contained in:
Yunhao Tian
2021-12-04 18:02:07 +08:00
parent dff54d854d
commit 68ca62dfd7
33 changed files with 5043 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hw/mcu/allwinner/f1c100s/lib/malloc.c
+834
View File
@@ -0,0 +1,834 @@
/*
* lib/libc/malloc/malloc.c
*/
#include <malloc.h>
static void * __heap_pool = NULL;
/*
* Some macros.
*/
#define tlsf_cast(t, exp) ((t)(exp))
#define tlsf_min(a, b) ((a) < (b) ? (a) : (b))
#define tlsf_max(a, b) ((a) > (b) ? (a) : (b))
#define tlsf_assert assert
#define tlsf_insist(x) { tlsf_assert(x); if (!(x)) { status--; } }
#if defined(__ARM64__) || defined(__X64__)
# define TLSF_64BIT
#else
# undef TLSF_64BIT
#endif
/*
* Public constants
*/
enum tlsf_public
{
/*
* log2 of number of linear subdivisions of block sizes
*/
SL_INDEX_COUNT_LOG2 = 5,
};
/*
* Private constants
*/
enum tlsf_private
{
#if defined(TLSF_64BIT)
/*
* All allocation sizes and addresses are aligned to 8 bytes
*/
ALIGN_SIZE_LOG2 = 3,
#else
/*
* All allocation sizes and addresses are aligned to 4 bytes
*/
ALIGN_SIZE_LOG2 = 2,
#endif
ALIGN_SIZE = (1 << ALIGN_SIZE_LOG2),
#if defined(TLSF_64BIT)
FL_INDEX_MAX = 32,
#else
FL_INDEX_MAX = 30,
#endif
SL_INDEX_COUNT = (1 << SL_INDEX_COUNT_LOG2),
FL_INDEX_SHIFT = (SL_INDEX_COUNT_LOG2 + ALIGN_SIZE_LOG2),
FL_INDEX_COUNT = (FL_INDEX_MAX - FL_INDEX_SHIFT + 1),
SMALL_BLOCK_SIZE = (1 << FL_INDEX_SHIFT),
};
/*
* Block header structure
*/
typedef struct block_header_t
{
/*
* Points to the previous physical block
*/
struct block_header_t * prev_phys_block;
/*
* The size of this block, excluding the block header
*/
size_t size;
/*
* Next and previous free blocks
*/
struct block_header_t * next_free;
struct block_header_t * prev_free;
} block_header_t;
/*
* The TLSF control structure.
*/
typedef struct control_t
{
/*
* Empty lists point at this block to indicate they are free.
*/
block_header_t block_null;
/*
* Bitmaps for free lists.
*/
unsigned int fl_bitmap;
unsigned int sl_bitmap[FL_INDEX_COUNT];
/*
* Head of free lists.
*/
block_header_t * blocks[FL_INDEX_COUNT][SL_INDEX_COUNT];
} control_t;
/*
* A type used for casting when doing pointer arithmetic.
*/
typedef ptrdiff_t tlsfptr_t;
/*
* Associated constants
*/
static const size_t block_header_free_bit = 1 << 0;
static const size_t block_header_prev_free_bit = 1 << 1;
static const size_t block_header_overhead = sizeof(size_t);
static const size_t block_start_offset = offsetof(block_header_t, size) + sizeof(size_t);
static const size_t block_size_min = sizeof(block_header_t) - sizeof(block_header_t *);
static const size_t block_size_max = tlsf_cast(size_t, 1) << FL_INDEX_MAX;
#if defined(__GNUC__) && (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) && defined(__GNUC_PATCHLEVEL__)
static int tlsf_ffs(unsigned int word)
{
return __builtin_ffs(word) - 1;
}
static int tlsf_fls(unsigned int word)
{
const int bit = word ? 32 - __builtin_clz(word) : 0;
return bit - 1;
}
#else
static int tlsf_fls_generic(unsigned int word)
{
int bit = 32;
if (!word) bit -= 1;
if (!(word & 0xffff0000)) { word <<= 16; bit -= 16; }
if (!(word & 0xff000000)) { word <<= 8; bit -= 8; }
if (!(word & 0xf0000000)) { word <<= 4; bit -= 4; }
if (!(word & 0xc0000000)) { word <<= 2; bit -= 2; }
if (!(word & 0x80000000)) { word <<= 1; bit -= 1; }
return bit;
}
static int tlsf_ffs(unsigned int word)
{
return tlsf_fls_generic(word & (~word + 1)) - 1;
}
static int tlsf_fls(unsigned int word)
{
return tlsf_fls_generic(word) - 1;
}
#endif
#if defined(TLSF_64BIT)
static int tlsf_fls_sizet(size_t size)
{
int high = (int)(size >> 32);
int bits = 0;
if(high)
{
bits = 32 + tlsf_fls(high);
}
else
{
bits = tlsf_fls((int)size & 0xffffffff);
}
return bits;
}
#else
#define tlsf_fls_sizet tlsf_fls
#endif
static size_t block_get_size(const block_header_t * block)
{
return block->size & ~(block_header_free_bit | block_header_prev_free_bit);
}
static void block_set_size(block_header_t * block, size_t size)
{
const size_t oldsize = block->size;
block->size = size | (oldsize & (block_header_free_bit | block_header_prev_free_bit));
}
static int block_is_last(const block_header_t * block)
{
return (0 == block_get_size(block));
}
static int block_is_free(const block_header_t * block)
{
return tlsf_cast(int, block->size & block_header_free_bit);
}
static void block_set_free(block_header_t * block)
{
block->size |= block_header_free_bit;
}
static void block_set_used(block_header_t * block)
{
block->size &= ~block_header_free_bit;
}
static int block_is_prev_free(const block_header_t * block)
{
return tlsf_cast(int, block->size & block_header_prev_free_bit);
}
static void block_set_prev_free(block_header_t * block)
{
block->size |= block_header_prev_free_bit;
}
static void block_set_prev_used(block_header_t * block)
{
block->size &= ~block_header_prev_free_bit;
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-align"
static block_header_t * block_from_ptr(void * ptr)
{
return tlsf_cast(block_header_t *, tlsf_cast(unsigned char*, ptr) - block_start_offset);
}
#pragma GCC diagnostic pop
static void * block_to_ptr(block_header_t * block)
{
return tlsf_cast(void *, tlsf_cast(unsigned char*, block) + block_start_offset);
}
static block_header_t * offset_to_block(void * ptr, size_t size)
{
return tlsf_cast(block_header_t *, tlsf_cast(tlsfptr_t, ptr) + size);
}
static block_header_t * block_prev(block_header_t * block)
{
return block->prev_phys_block;
}
static block_header_t * block_next(block_header_t * block)
{
block_header_t * next = offset_to_block(block_to_ptr(block), block_get_size(block) - block_header_overhead);
tlsf_assert(!block_is_last(block));
return next;
}
static block_header_t * block_link_next(block_header_t * block)
{
block_header_t * next = block_next(block);
next->prev_phys_block = block;
return next;
}
static void block_mark_as_free(block_header_t * block)
{
block_header_t * next = block_link_next(block);
block_set_prev_free(next);
block_set_free(block);
}
static void block_mark_as_used(block_header_t * block)
{
block_header_t * next = block_next(block);
block_set_prev_used(next);
block_set_used(block);
}
static size_t align_up(size_t x, size_t align)
{
tlsf_assert(0 == (align & (align - 1)) && "must align to a power of two");
return (x + (align - 1)) & ~(align - 1);
}
static size_t align_down(size_t x, size_t align)
{
tlsf_assert(0 == (align & (align - 1)) && "must align to a power of two");
return x - (x & (align - 1));
}
static void * align_ptr(const void * ptr, size_t align)
{
const tlsfptr_t aligned = (tlsf_cast(tlsfptr_t, ptr) + (align - 1)) & ~(align - 1);
tlsf_assert(0 == (align & (align - 1)) && "must align to a power of two");
return tlsf_cast(void*, aligned);
}
static size_t adjust_request_size(size_t size, size_t align)
{
size_t adjust = 0;
if (size && size < block_size_max)
{
const size_t aligned = align_up(size, align);
adjust = tlsf_max(aligned, block_size_min);
}
return adjust;
}
static void mapping_insert(size_t size, int * fli, int * sli)
{
int fl, sl;
if (size < SMALL_BLOCK_SIZE)
{
fl = 0;
sl = tlsf_cast(int, size) / (SMALL_BLOCK_SIZE / SL_INDEX_COUNT);
}
else
{
fl = tlsf_fls_sizet(size);
sl = tlsf_cast(int, size >> (fl - SL_INDEX_COUNT_LOG2)) ^ (1 << SL_INDEX_COUNT_LOG2);
fl -= (FL_INDEX_SHIFT - 1);
}
*fli = fl;
*sli = sl;
}
static void mapping_search(size_t size, int * fli, int * sli)
{
if (size >= (1 << SL_INDEX_COUNT_LOG2))
{
const size_t round = (1 << (tlsf_fls_sizet(size) - SL_INDEX_COUNT_LOG2)) - 1;
size += round;
}
mapping_insert(size, fli, sli);
}
static block_header_t * search_suitable_block(control_t * control, int * fli, int * sli)
{
int fl = *fli;
int sl = *sli;
unsigned int sl_map = control->sl_bitmap[fl] & (~0U << sl);
if (!sl_map)
{
const unsigned int fl_map = control->fl_bitmap & (~0U << (fl + 1));
if (!fl_map)
{
return 0;
}
fl = tlsf_ffs(fl_map);
*fli = fl;
sl_map = control->sl_bitmap[fl];
}
tlsf_assert(sl_map && "internal error - second level bitmap is null");
sl = tlsf_ffs(sl_map);
*sli = sl;
return control->blocks[fl][sl];
}
static void remove_free_block(control_t * control, block_header_t * block, int fl, int sl)
{
block_header_t * prev = block->prev_free;
block_header_t * next = block->next_free;
tlsf_assert(prev && "prev_free field can not be null");
tlsf_assert(next && "next_free field can not be null");
next->prev_free = prev;
prev->next_free = next;
if (control->blocks[fl][sl] == block)
{
control->blocks[fl][sl] = next;
if (next == &control->block_null)
{
control->sl_bitmap[fl] &= ~(1 << sl);
if (!control->sl_bitmap[fl])
{
control->fl_bitmap &= ~(1 << fl);
}
}
}
}
static void insert_free_block(control_t * control, block_header_t * block, int fl, int sl)
{
block_header_t * current = control->blocks[fl][sl];
tlsf_assert(current && "free list cannot have a null entry");
tlsf_assert(block && "cannot insert a null entry into the free list");
block->next_free = current;
block->prev_free = &control->block_null;
current->prev_free = block;
tlsf_assert(block_to_ptr(block) == align_ptr(block_to_ptr(block), ALIGN_SIZE) && "block not aligned properly");
control->blocks[fl][sl] = block;
control->fl_bitmap |= (1 << fl);
control->sl_bitmap[fl] |= (1 << sl);
}
static void block_remove(control_t * control, block_header_t * block)
{
int fl, sl;
mapping_insert(block_get_size(block), &fl, &sl);
remove_free_block(control, block, fl, sl);
}
static void block_insert(control_t * control, block_header_t * block)
{
int fl, sl;
mapping_insert(block_get_size(block), &fl, &sl);
insert_free_block(control, block, fl, sl);
}
static int block_can_split(block_header_t * block, size_t size)
{
return block_get_size(block) >= sizeof(block_header_t) + size;
}
static block_header_t * block_split(block_header_t * block, size_t size)
{
block_header_t* remaining = offset_to_block(block_to_ptr(block), size - block_header_overhead);
const size_t remain_size = block_get_size(block) - (size + block_header_overhead);
tlsf_assert(block_to_ptr(remaining) == align_ptr(block_to_ptr(remaining), ALIGN_SIZE) && "remaining block not aligned properly");
tlsf_assert(block_get_size(block) == remain_size + size + block_header_overhead);
block_set_size(remaining, remain_size);
tlsf_assert(block_get_size(remaining) >= block_size_min && "block split with invalid size");
block_set_size(block, size);
block_mark_as_free(remaining);
return remaining;
}
static block_header_t * block_absorb(block_header_t * prev, block_header_t * block)
{
tlsf_assert(!block_is_last(prev) && "previous block can't be last!");
prev->size += block_get_size(block) + block_header_overhead;
block_link_next(prev);
return prev;
}
static block_header_t * block_merge_prev(control_t * control, block_header_t * block)
{
if (block_is_prev_free(block))
{
block_header_t* prev = block_prev(block);
tlsf_assert(prev && "prev physical block can't be null");
tlsf_assert(block_is_free(prev) && "prev block is not free though marked as such");
block_remove(control, prev);
block = block_absorb(prev, block);
}
return block;
}
static block_header_t * block_merge_next(control_t * control, block_header_t * block)
{
block_header_t* next = block_next(block);
tlsf_assert(next && "next physical block can't be null");
if (block_is_free(next))
{
tlsf_assert(!block_is_last(block) && "previous block can't be last!");
block_remove(control, next);
block = block_absorb(block, next);
}
return block;
}
static void block_trim_free(control_t * control, block_header_t * block, size_t size)
{
tlsf_assert(block_is_free(block) && "block must be free");
if (block_can_split(block, size))
{
block_header_t* remaining_block = block_split(block, size);
block_link_next(block);
block_set_prev_free(remaining_block);
block_insert(control, remaining_block);
}
}
static void block_trim_used(control_t * control, block_header_t * block, size_t size)
{
tlsf_assert(!block_is_free(block) && "block must be used");
if (block_can_split(block, size))
{
block_header_t* remaining_block = block_split(block, size);
block_set_prev_used(remaining_block);
remaining_block = block_merge_next(control, remaining_block);
block_insert(control, remaining_block);
}
}
static block_header_t * block_trim_free_leading(control_t * control, block_header_t * block, size_t size)
{
block_header_t * remaining_block = block;
if (block_can_split(block, size))
{
remaining_block = block_split(block, size - block_header_overhead);
block_set_prev_free(remaining_block);
block_link_next(block);
block_insert(control, block);
}
return remaining_block;
}
static block_header_t * block_locate_free(control_t * control, size_t size)
{
int fl = 0, sl = 0;
block_header_t * block = 0;
if (size)
{
mapping_search(size, &fl, &sl);
block = search_suitable_block(control, &fl, &sl);
}
if (block)
{
tlsf_assert(block_get_size(block) >= size);
remove_free_block(control, block, fl, sl);
}
return block;
}
static void * block_prepare_used(control_t * control, block_header_t * block, size_t size)
{
void* p = 0;
if (block)
{
block_trim_free(control, block, size);
block_mark_as_used(block);
p = block_to_ptr(block);
}
return p;
}
static void control_construct(control_t * control)
{
int i, j;
control->block_null.next_free = &control->block_null;
control->block_null.prev_free = &control->block_null;
control->fl_bitmap = 0;
for (i = 0; i < FL_INDEX_COUNT; ++i)
{
control->sl_bitmap[i] = 0;
for (j = 0; j < SL_INDEX_COUNT; ++j)
{
control->blocks[i][j] = &control->block_null;
}
}
}
static inline void * tlsf_add_pool(void * tlsf, void * mem, size_t bytes)
{
block_header_t * block;
block_header_t * next;
const size_t pool_overhead = 2 * block_header_overhead;
const size_t pool_bytes = align_down(bytes - pool_overhead, ALIGN_SIZE);
if (((ptrdiff_t)mem % ALIGN_SIZE) != 0)
return 0;
if (pool_bytes < block_size_min || pool_bytes > block_size_max)
return 0;
block = offset_to_block(mem, -(tlsfptr_t)block_header_overhead);
block_set_size(block, pool_bytes);
block_set_free(block);
block_set_prev_used(block);
block_insert(tlsf_cast(control_t*, tlsf), block);
next = block_link_next(block);
block_set_size(next, 0);
block_set_used(next);
block_set_prev_free(next);
return mem;
}
static inline void tlsf_remove_pool(void * tlsf, void * pool)
{
control_t * control = tlsf_cast(control_t *, tlsf);
block_header_t * block = offset_to_block(pool, -(int)block_header_overhead);
int fl = 0, sl = 0;
tlsf_assert(block_is_free(block) && "block should be free");
tlsf_assert(!block_is_free(block_next(block)) && "next block should not be free");
tlsf_assert(block_get_size(block_next(block)) == 0 && "next block size should be zero");
mapping_insert(block_get_size(block), &fl, &sl);
remove_free_block(control, block, fl, sl);
}
static inline void * tlsf_create(void * mem)
{
if (((tlsfptr_t)mem % ALIGN_SIZE) != 0)
return 0;
control_construct(tlsf_cast(control_t *, mem));
return tlsf_cast(void *, mem);
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
static inline void * tlsf_create_with_pool(void * mem, size_t bytes)
{
void * tlsf = tlsf_create(mem);
tlsf_add_pool(tlsf, (char *)mem + sizeof(control_t), bytes - sizeof(control_t));
return tlsf;
}
#pragma GCC diagnostic pop
static inline void tlsf_destroy(void * tlsf)
{
(void)tlsf;
}
static inline void * tlsf_get_pool(void * tlsf)
{
return tlsf_cast(void *, (char *)tlsf + sizeof(control_t));
}
static inline void * tlsf_malloc(void * tlsf, size_t size)
{
control_t * control = tlsf_cast(control_t *, tlsf);
const size_t adjust = adjust_request_size(size, ALIGN_SIZE);
block_header_t * block = block_locate_free(control, adjust);
return block_prepare_used(control, block, adjust);
}
static inline void * tlsf_memalign(void * tlsf, size_t align, size_t size)
{
control_t * control = tlsf_cast(control_t *, tlsf);
const size_t adjust = adjust_request_size(size, ALIGN_SIZE);
const size_t gap_minimum = sizeof(block_header_t);
const size_t size_with_gap = adjust_request_size(adjust + align + gap_minimum, align);
const size_t aligned_size = (align <= ALIGN_SIZE) ? adjust : size_with_gap;
block_header_t* block = block_locate_free(control, aligned_size);
tlsf_assert(sizeof(block_header_t) == block_size_min + block_header_overhead);
if (block)
{
void * ptr = block_to_ptr(block);
void * aligned = align_ptr(ptr, align);
size_t gap = tlsf_cast(size_t, tlsf_cast(tlsfptr_t, aligned) - tlsf_cast(tlsfptr_t, ptr));
if (gap && gap < gap_minimum)
{
const size_t gap_remain = gap_minimum - gap;
const size_t offset = tlsf_max(gap_remain, align);
const void * next_aligned = tlsf_cast(void *, tlsf_cast(tlsfptr_t, aligned) + offset);
aligned = align_ptr(next_aligned, align);
gap = tlsf_cast(size_t, tlsf_cast(tlsfptr_t, aligned) - tlsf_cast(tlsfptr_t, ptr));
}
if (gap)
{
tlsf_assert(gap >= gap_minimum && "gap size too small");
block = block_trim_free_leading(control, block, gap);
}
}
return block_prepare_used(control, block, adjust);
}
static inline void tlsf_free(void * tlsf, void * ptr)
{
if (ptr)
{
control_t * control = tlsf_cast(control_t *, tlsf);
block_header_t * block = block_from_ptr(ptr);
tlsf_assert(!block_is_free(block) && "block already marked as free");
block_mark_as_free(block);
block = block_merge_prev(control, block);
block = block_merge_next(control, block);
block_insert(control, block);
}
}
static inline void * tlsf_realloc(void * tlsf, void * ptr, size_t size)
{
control_t * control = tlsf_cast(control_t *, tlsf);
void * p = 0;
if (ptr && size == 0)
{
tlsf_free(tlsf, ptr);
}
else if (!ptr)
{
p = tlsf_malloc(tlsf, size);
}
else
{
block_header_t * block = block_from_ptr(ptr);
block_header_t * next = block_next(block);
const size_t cursize = block_get_size(block);
const size_t combined = cursize + block_get_size(next) + block_header_overhead;
const size_t adjust = adjust_request_size(size, ALIGN_SIZE);
tlsf_assert(!block_is_free(block) && "block already marked as free");
if (adjust > cursize && (!block_is_free(next) || adjust > combined))
{
p = tlsf_malloc(tlsf, size);
if (p)
{
const size_t minsize = tlsf_min(cursize, size);
memcpy(p, ptr, minsize);
tlsf_free(tlsf, ptr);
}
}
else
{
if (adjust > cursize)
{
block_merge_next(control, block);
block_mark_as_used(block);
}
block_trim_used(control, block, adjust);
p = ptr;
}
}
return p;
}
void * mm_create(void * mem, size_t bytes)
{
return tlsf_create_with_pool(mem, bytes);
}
void mm_destroy(void * mm)
{
tlsf_destroy(mm);
}
void * mm_get_pool(void * mm)
{
return tlsf_get_pool(mm);
}
void * mm_add_pool(void * mm, void * mem, size_t bytes)
{
return tlsf_add_pool(mm, mem, bytes);
}
void mm_remove_pool(void * mm, void * pool)
{
tlsf_remove_pool(mm, pool);
}
void * mm_malloc(void * mm, size_t size)
{
return tlsf_malloc(mm, size);
}
void * mm_memalign(void * mm, size_t align, size_t size)
{
return tlsf_memalign(mm, align, size);
}
void * mm_realloc(void * mm, void * ptr, size_t size)
{
return tlsf_realloc(mm, ptr, size);
}
void mm_free(void * mm, void * ptr)
{
tlsf_free(mm, ptr);
}
void * malloc(size_t size)
{
return tlsf_malloc(__heap_pool, size);
}
void * memalign(size_t align, size_t size)
{
return tlsf_memalign(__heap_pool, align, size);
}
void * realloc(void * ptr, size_t size)
{
return tlsf_realloc(__heap_pool, ptr, size);
}
void * calloc(size_t nmemb, size_t size)
{
void * ptr;
if((ptr = malloc(nmemb * size)))
memset(ptr, 0, nmemb * size);
return ptr;
}
void free(void * ptr)
{
tlsf_free(__heap_pool, ptr);
}
void do_init_mem_pool(void)
{
#ifndef __SANDBOX__
extern unsigned char __heap_start;
extern unsigned char __heap_end;
__heap_pool = tlsf_create_with_pool((void *)&__heap_start, (size_t)(&__heap_end - &__heap_start));
#else
static char __heap_buf[SZ_16M];
__heap_pool = tlsf_create_with_pool((void *)__heap_buf, (size_t)(sizeof(__heap_buf)));
#endif
}
hw/mcu/allwinner/f1c100s/lib/memcpy.S
+404
View File
@@ -0,0 +1,404 @@
/*
* memcpy.S
*/
.text
.global memcpy
.type memcpy, %function
.align 4
memcpy:
/* determine copy direction */
cmp r1, r0
bcc .Lmemcpy_backwards
moveq r0, #0 /* quick abort for len=0 */
moveq pc, lr
stmdb sp!, {r0, lr} /* memcpy() returns dest addr */
subs r2, r2, #4
blt .Lmemcpy_fl4 /* less than 4 bytes */
ands r12, r0, #3
bne .Lmemcpy_fdestul /* oh unaligned destination addr */
ands r12, r1, #3
bne .Lmemcpy_fsrcul /* oh unaligned source addr */
.Lmemcpy_ft8:
/* we have aligned source and destination */
subs r2, r2, #8
blt .Lmemcpy_fl12 /* less than 12 bytes (4 from above) */
subs r2, r2, #0x14
blt .Lmemcpy_fl32 /* less than 32 bytes (12 from above) */
stmdb sp!, {r4} /* borrow r4 */
/* blat 32 bytes at a time */
.Lmemcpy_floop32:
ldmia r1!, {r3, r4, r12, lr}
stmia r0!, {r3, r4, r12, lr}
ldmia r1!, {r3, r4, r12, lr}
stmia r0!, {r3, r4, r12, lr}
subs r2, r2, #0x20
bge .Lmemcpy_floop32
cmn r2, #0x10
ldmgeia r1!, {r3, r4, r12, lr} /* blat a remaining 16 bytes */
stmgeia r0!, {r3, r4, r12, lr}
subge r2, r2, #0x10
ldmia sp!, {r4} /* return r4 */
.Lmemcpy_fl32:
adds r2, r2, #0x14
/* blat 12 bytes at a time */
.Lmemcpy_floop12:
ldmgeia r1!, {r3, r12, lr}
stmgeia r0!, {r3, r12, lr}
subges r2, r2, #0x0c
bge .Lmemcpy_floop12
.Lmemcpy_fl12:
adds r2, r2, #8
blt .Lmemcpy_fl4
subs r2, r2, #4
ldrlt r3, [r1], #4
strlt r3, [r0], #4
ldmgeia r1!, {r3, r12}
stmgeia r0!, {r3, r12}
subge r2, r2, #4
.Lmemcpy_fl4:
/* less than 4 bytes to go */
adds r2, r2, #4
ldmeqia sp!, {r0, pc} /* done */
/* copy the crud byte at a time */
cmp r2, #2
ldrb r3, [r1], #1
strb r3, [r0], #1
ldrgeb r3, [r1], #1
strgeb r3, [r0], #1
ldrgtb r3, [r1], #1
strgtb r3, [r0], #1
ldmia sp!, {r0, pc}
/* erg - unaligned destination */
.Lmemcpy_fdestul:
rsb r12, r12, #4
cmp r12, #2
/* align destination with byte copies */
ldrb r3, [r1], #1
strb r3, [r0], #1
ldrgeb r3, [r1], #1
strgeb r3, [r0], #1
ldrgtb r3, [r1], #1
strgtb r3, [r0], #1
subs r2, r2, r12
blt .Lmemcpy_fl4 /* less the 4 bytes */
ands r12, r1, #3
beq .Lmemcpy_ft8 /* we have an aligned source */
/* erg - unaligned source */
/* This is where it gets nasty ... */
.Lmemcpy_fsrcul:
bic r1, r1, #3
ldr lr, [r1], #4
cmp r12, #2
bgt .Lmemcpy_fsrcul3
beq .Lmemcpy_fsrcul2
cmp r2, #0x0c
blt .Lmemcpy_fsrcul1loop4
sub r2, r2, #0x0c
stmdb sp!, {r4, r5}
.Lmemcpy_fsrcul1loop16:
mov r3, lr, lsr #8
ldmia r1!, {r4, r5, r12, lr}
orr r3, r3, r4, lsl #24
mov r4, r4, lsr #8
orr r4, r4, r5, lsl #24
mov r5, r5, lsr #8
orr r5, r5, r12, lsl #24
mov r12, r12, lsr #8
orr r12, r12, lr, lsl #24
stmia r0!, {r3-r5, r12}
subs r2, r2, #0x10
bge .Lmemcpy_fsrcul1loop16
ldmia sp!, {r4, r5}
adds r2, r2, #0x0c
blt .Lmemcpy_fsrcul1l4
.Lmemcpy_fsrcul1loop4:
mov r12, lr, lsr #8
ldr lr, [r1], #4
orr r12, r12, lr, lsl #24
str r12, [r0], #4
subs r2, r2, #4
bge .Lmemcpy_fsrcul1loop4
.Lmemcpy_fsrcul1l4:
sub r1, r1, #3
b .Lmemcpy_fl4
.Lmemcpy_fsrcul2:
cmp r2, #0x0c
blt .Lmemcpy_fsrcul2loop4
sub r2, r2, #0x0c
stmdb sp!, {r4, r5}
.Lmemcpy_fsrcul2loop16:
mov r3, lr, lsr #16
ldmia r1!, {r4, r5, r12, lr}
orr r3, r3, r4, lsl #16
mov r4, r4, lsr #16
orr r4, r4, r5, lsl #16
mov r5, r5, lsr #16
orr r5, r5, r12, lsl #16
mov r12, r12, lsr #16
orr r12, r12, lr, lsl #16
stmia r0!, {r3-r5, r12}
subs r2, r2, #0x10
bge .Lmemcpy_fsrcul2loop16
ldmia sp!, {r4, r5}
adds r2, r2, #0x0c
blt .Lmemcpy_fsrcul2l4
.Lmemcpy_fsrcul2loop4:
mov r12, lr, lsr #16
ldr lr, [r1], #4
orr r12, r12, lr, lsl #16
str r12, [r0], #4
subs r2, r2, #4
bge .Lmemcpy_fsrcul2loop4
.Lmemcpy_fsrcul2l4:
sub r1, r1, #2
b .Lmemcpy_fl4
.Lmemcpy_fsrcul3:
cmp r2, #0x0c
blt .Lmemcpy_fsrcul3loop4
sub r2, r2, #0x0c
stmdb sp!, {r4, r5}
.Lmemcpy_fsrcul3loop16:
mov r3, lr, lsr #24
ldmia r1!, {r4, r5, r12, lr}
orr r3, r3, r4, lsl #8
mov r4, r4, lsr #24
orr r4, r4, r5, lsl #8
mov r5, r5, lsr #24
orr r5, r5, r12, lsl #8
mov r12, r12, lsr #24
orr r12, r12, lr, lsl #8
stmia r0!, {r3-r5, r12}
subs r2, r2, #0x10
bge .Lmemcpy_fsrcul3loop16
ldmia sp!, {r4, r5}
adds r2, r2, #0x0c
blt .Lmemcpy_fsrcul3l4
.Lmemcpy_fsrcul3loop4:
mov r12, lr, lsr #24
ldr lr, [r1], #4
orr r12, r12, lr, lsl #8
str r12, [r0], #4
subs r2, r2, #4
bge .Lmemcpy_fsrcul3loop4
.Lmemcpy_fsrcul3l4:
sub r1, r1, #1
b .Lmemcpy_fl4
.Lmemcpy_backwards:
add r1, r1, r2
add r0, r0, r2
subs r2, r2, #4
blt .Lmemcpy_bl4 /* less than 4 bytes */
ands r12, r0, #3
bne .Lmemcpy_bdestul /* oh unaligned destination addr */
ands r12, r1, #3
bne .Lmemcpy_bsrcul /* oh unaligned source addr */
.Lmemcpy_bt8:
/* we have aligned source and destination */
subs r2, r2, #8
blt .Lmemcpy_bl12 /* less than 12 bytes (4 from above) */
stmdb sp!, {r4, lr}
subs r2, r2, #0x14 /* less than 32 bytes (12 from above) */
blt .Lmemcpy_bl32
/* blat 32 bytes at a time */
.Lmemcpy_bloop32:
ldmdb r1!, {r3, r4, r12, lr}
stmdb r0!, {r3, r4, r12, lr}
ldmdb r1!, {r3, r4, r12, lr}
stmdb r0!, {r3, r4, r12, lr}
subs r2, r2, #0x20
bge .Lmemcpy_bloop32
.Lmemcpy_bl32:
cmn r2, #0x10
ldmgedb r1!, {r3, r4, r12, lr} /* blat a remaining 16 bytes */
stmgedb r0!, {r3, r4, r12, lr}
subge r2, r2, #0x10
adds r2, r2, #0x14
ldmgedb r1!, {r3, r12, lr} /* blat a remaining 12 bytes */
stmgedb r0!, {r3, r12, lr}
subge r2, r2, #0x0c
ldmia sp!, {r4, lr}
.Lmemcpy_bl12:
adds r2, r2, #8
blt .Lmemcpy_bl4
subs r2, r2, #4
ldrlt r3, [r1, #-4]!
strlt r3, [r0, #-4]!
ldmgedb r1!, {r3, r12}
stmgedb r0!, {r3, r12}
subge r2, r2, #4
.Lmemcpy_bl4:
/* less than 4 bytes to go */
adds r2, r2, #4
moveq pc, lr
/* copy the crud byte at a time */
cmp r2, #2
ldrb r3, [r1, #-1]!
strb r3, [r0, #-1]!
ldrgeb r3, [r1, #-1]!
strgeb r3, [r0, #-1]!
ldrgtb r3, [r1, #-1]!
strgtb r3, [r0, #-1]!
mov pc, lr
/* erg - unaligned destination */
.Lmemcpy_bdestul:
cmp r12, #2
/* align destination with byte copies */
ldrb r3, [r1, #-1]!
strb r3, [r0, #-1]!
ldrgeb r3, [r1, #-1]!
strgeb r3, [r0, #-1]!
ldrgtb r3, [r1, #-1]!
strgtb r3, [r0, #-1]!
subs r2, r2, r12
blt .Lmemcpy_bl4 /* less than 4 bytes to go */
ands r12, r1, #3
beq .Lmemcpy_bt8 /* we have an aligned source */
/* erg - unaligned source */
/* This is where it gets nasty ... */
.Lmemcpy_bsrcul:
bic r1, r1, #3
ldr r3, [r1, #0]
cmp r12, #2
blt .Lmemcpy_bsrcul1
beq .Lmemcpy_bsrcul2
cmp r2, #0x0c
blt .Lmemcpy_bsrcul3loop4
sub r2, r2, #0x0c
stmdb sp!, {r4, r5, lr}
.Lmemcpy_bsrcul3loop16:
mov lr, r3, lsl #8
ldmdb r1!, {r3-r5, r12}
orr lr, lr, r12, lsr #24
mov r12, r12, lsl #8
orr r12, r12, r5, lsr #24
mov r5, r5, lsl #8
orr r5, r5, r4, lsr #24
mov r4, r4, lsl #8
orr r4, r4, r3, lsr #24
stmdb r0!, {r4, r5, r12, lr}
subs r2, r2, #0x10
bge .Lmemcpy_bsrcul3loop16
ldmia sp!, {r4, r5, lr}
adds r2, r2, #0x0c
blt .Lmemcpy_bsrcul3l4
.Lmemcpy_bsrcul3loop4:
mov r12, r3, lsl #8
ldr r3, [r1, #-4]!
orr r12, r12, r3, lsr #24
str r12, [r0, #-4]!
subs r2, r2, #4
bge .Lmemcpy_bsrcul3loop4
.Lmemcpy_bsrcul3l4:
add r1, r1, #3
b .Lmemcpy_bl4
.Lmemcpy_bsrcul2:
cmp r2, #0x0c
blt .Lmemcpy_bsrcul2loop4
sub r2, r2, #0x0c
stmdb sp!, {r4, r5, lr}
.Lmemcpy_bsrcul2loop16:
mov lr, r3, lsl #16
ldmdb r1!, {r3-r5, r12}
orr lr, lr, r12, lsr #16
mov r12, r12, lsl #16
orr r12, r12, r5, lsr #16
mov r5, r5, lsl #16
orr r5, r5, r4, lsr #16
mov r4, r4, lsl #16
orr r4, r4, r3, lsr #16
stmdb r0!, {r4, r5, r12, lr}
subs r2, r2, #0x10
bge .Lmemcpy_bsrcul2loop16
ldmia sp!, {r4, r5, lr}
adds r2, r2, #0x0c
blt .Lmemcpy_bsrcul2l4
.Lmemcpy_bsrcul2loop4:
mov r12, r3, lsl #16
ldr r3, [r1, #-4]!
orr r12, r12, r3, lsr #16
str r12, [r0, #-4]!
subs r2, r2, #4
bge .Lmemcpy_bsrcul2loop4
.Lmemcpy_bsrcul2l4:
add r1, r1, #2
b .Lmemcpy_bl4
.Lmemcpy_bsrcul1:
cmp r2, #0x0c
blt .Lmemcpy_bsrcul1loop4
sub r2, r2, #0x0c
stmdb sp!, {r4, r5, lr}
.Lmemcpy_bsrcul1loop32:
mov lr, r3, lsl #24
ldmdb r1!, {r3-r5, r12}
orr lr, lr, r12, lsr #8
mov r12, r12, lsl #24
orr r12, r12, r5, lsr #8
mov r5, r5, lsl #24
orr r5, r5, r4, lsr #8
mov r4, r4, lsl #24
orr r4, r4, r3, lsr #8
stmdb r0!, {r4, r5, r12, lr}
subs r2, r2, #0x10
bge .Lmemcpy_bsrcul1loop32
ldmia sp!, {r4, r5, lr}
adds r2, r2, #0x0c
blt .Lmemcpy_bsrcul1l4
.Lmemcpy_bsrcul1loop4:
mov r12, r3, lsl #24
ldr r3, [r1, #-4]!
orr r12, r12, r3, lsr #8
str r12, [r0, #-4]!
subs r2, r2, #4
bge .Lmemcpy_bsrcul1loop4
.Lmemcpy_bsrcul1l4:
add r1, r1, #1
b .Lmemcpy_bl4
hw/mcu/allwinner/f1c100s/lib/memset.S
+79
View File
@@ -0,0 +1,79 @@
/*
* memcpy.S
*/
.text
.global memset
.type memset, %function
.align 4
memset:
stmfd sp!, {r0} /* remember address for return value */
and r1, r1, #0x000000ff /* we write bytes */
cmp r2, #0x00000004 /* do we have less than 4 bytes */
blt .Lmemset_lessthanfour
/* first we will word align the address */
ands r3, r0, #0x00000003 /* get the bottom two bits */
beq .Lmemset_addraligned /* the address is word aligned */
rsb r3, r3, #0x00000004
sub r2, r2, r3
cmp r3, #0x00000002
strb r1, [r0], #0x0001 /* set 1 byte */
strgeb r1, [r0], #0x0001 /* set another byte */
strgtb r1, [r0], #0x0001 /* and a third */
cmp r2, #0x00000004
blt .Lmemset_lessthanfour
/* now we must be word aligned */
.Lmemset_addraligned:
orr r3, r1, r1, lsl #8 /* repeat the byte into a word */
orr r3, r3, r3, lsl #16
/* we know we have at least 4 bytes ... */
cmp r2, #0x00000020 /* if less than 32 then use words */
blt .Lmemset_lessthan32
/* we have at least 32 so lets use quad words */
stmfd sp!, {r4-r6} /* store registers */
mov r4, r3 /* duplicate data */
mov r5, r3
mov r6, r3
.Lmemset_loop16:
stmia r0!, {r3-r6} /* store 16 bytes */
sub r2, r2, #0x00000010 /* adjust count */
cmp r2, #0x00000010 /* still got at least 16 bytes ? */
bgt .Lmemset_loop16
ldmfd sp!, {r4-r6} /* restore registers */
/* do we need to set some words as well ? */
cmp r2, #0x00000004
blt .Lmemset_lessthanfour
/* have either less than 16 or less than 32 depending on route taken */
.Lmemset_lessthan32:
/* we have at least 4 bytes so copy as words */
.Lmemset_loop4:
str r3, [r0], #0x0004
sub r2, r2, #0x0004
cmp r2, #0x00000004
bge .Lmemset_loop4
.Lmemset_lessthanfour:
cmp r2, #0x00000000
ldmeqfd sp!, {r0}
moveq pc, lr /* zero length so exit */
cmp r2, #0x00000002
strb r1, [r0], #0x0001 /* set 1 byte */
strgeb r1, [r0], #0x0001 /* set another byte */
strgtb r1, [r0], #0x0001 /* and a third */
ldmfd sp!, {r0}
mov pc, lr /* exit */
hw/mcu/allwinner/f1c100s/lib/printf.c
+757
View File
@@ -0,0 +1,757 @@
///////////////////////////////////////////////////////////////////////////////
// \author (c) Marco Paland (info@paland.com)
// 2014-2018, PALANDesign Hannover, Germany
//
// \license The MIT License (MIT)
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// \brief Tiny printf, sprintf and (v)snprintf implementation, optimized for speed on
// embedded systems with a very limited resources. These routines are thread
// safe and reentrant!
// Use this instead of the bloated standard/newlib printf cause these use
// malloc for printf (and may not be thread safe).
//
///////////////////////////////////////////////////////////////////////////////
#include <stdint.h>
#include "printf.h"
// ntoa conversion buffer size, this must be big enough to hold
// one converted numeric number including padded zeros (dynamically created on stack)
// 32 byte is a good default
#define PRINTF_NTOA_BUFFER_SIZE 32U
// ftoa conversion buffer size, this must be big enough to hold
// one converted float number including padded zeros (dynamically created on stack)
// 32 byte is a good default
#define PRINTF_FTOA_BUFFER_SIZE 32U
// define this to support floating point (%f)
#define PRINTF_SUPPORT_FLOAT
// define this to support long long types (%llu or %p)
#define PRINTF_SUPPORT_LONG_LONG
// define this to support the ptrdiff_t type (%t)
// ptrdiff_t is normally defined in <stddef.h> as long or long long type
#define PRINTF_SUPPORT_PTRDIFF_T
///////////////////////////////////////////////////////////////////////////////
// internal flag definitions
#define FLAGS_ZEROPAD (1U << 0U)
#define FLAGS_LEFT (1U << 1U)
#define FLAGS_PLUS (1U << 2U)
#define FLAGS_SPACE (1U << 3U)
#define FLAGS_HASH (1U << 4U)
#define FLAGS_UPPERCASE (1U << 5U)
#define FLAGS_CHAR (1U << 6U)
#define FLAGS_SHORT (1U << 7U)
#define FLAGS_LONG (1U << 8U)
#define FLAGS_LONG_LONG (1U << 9U)
#define FLAGS_PRECISION (1U << 10U)
typedef unsigned char bool;
#ifndef false
#define false 0
#endif
#ifndef true
#define true (!false)
#endif
extern void sys_uart_putc(char c);
static char last_ch;
void _putchar(char character)
{
if(character == 0x0a && last_ch != 0x0d)
{
sys_uart_putc(0x0d);
}
sys_uart_putc(character);
}
// output function type
typedef void (*out_fct_type)(char character, void* buffer, size_t idx, size_t maxlen);
// wrapper (used as buffer) for output function type
typedef struct {
void (*fct)(char character, void* arg);
void* arg;
} out_fct_wrap_type;
// internal buffer output
static inline void _out_buffer(char character, void* buffer, size_t idx, size_t maxlen)
{
if (idx < maxlen) {
((char*)buffer)[idx] = character;
}
}
// internal null output
static inline void _out_null(char character, void* buffer, size_t idx, size_t maxlen)
{
(void)character; (void)buffer; (void)idx; (void)maxlen;
}
// internal _putchar wrapper
static inline void _out_char(char character, void* buffer, size_t idx, size_t maxlen)
{
(void)buffer; (void)idx; (void)maxlen;
if (character) {
_putchar(character);
}
}
// internal output function wrapper
static inline void _out_fct(char character, void* buffer, size_t idx, size_t maxlen)
{
(void)idx; (void)maxlen;
// buffer is the output fct pointer
((out_fct_wrap_type*)buffer)->fct(character, ((out_fct_wrap_type*)buffer)->arg);
}
// internal strlen
// \return The length of the string (excluding the terminating 0)
static inline unsigned int _strlen(const char* str)
{
const char* s;
for (s = str; *s; ++s);
return (unsigned int)(s - str);
}
// internal test if char is a digit (0-9)
// \return true if char is a digit
static inline bool _is_digit(char ch)
{
return (ch >= '0') && (ch <= '9');
}
// internal ASCII string to unsigned int conversion
static unsigned int _atoi(const char** str)
{
unsigned int i = 0U;
while (_is_digit(**str)) {
i = i * 10U + (unsigned int)(*((*str)++) - '0');
}
return i;
}
// internal itoa format
static size_t _ntoa_format(out_fct_type out, char* buffer, size_t idx, size_t maxlen, char* buf, size_t len, bool negative, unsigned int base, unsigned int prec, unsigned int width, unsigned int flags)
{
const size_t start_idx = idx;
// pad leading zeros
while (!(flags & FLAGS_LEFT) && (len < prec) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
buf[len++] = '0';
}
while (!(flags & FLAGS_LEFT) && (flags & FLAGS_ZEROPAD) && (len < width) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
buf[len++] = '0';
}
// handle hash
if (flags & FLAGS_HASH) {
if (!(flags & FLAGS_PRECISION) && len && ((len == prec) || (len == width))) {
len--;
if (len && (base == 16U)) {
len--;
}
}
if ((base == 16U) && !(flags & FLAGS_UPPERCASE) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
buf[len++] = 'x';
}
else if ((base == 16U) && (flags & FLAGS_UPPERCASE) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
buf[len++] = 'X';
}
else if ((base == 2U) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
buf[len++] = 'b';
}
if (len < PRINTF_NTOA_BUFFER_SIZE) {
buf[len++] = '0';
}
}
// handle sign
if (len && (len == width) && (negative || (flags & FLAGS_PLUS) || (flags & FLAGS_SPACE))) {
len--;
}
if (len < PRINTF_NTOA_BUFFER_SIZE) {
if (negative) {
buf[len++] = '-';
}
else if (flags & FLAGS_PLUS) {
buf[len++] = '+'; // ignore the space if the '+' exists
}
else if (flags & FLAGS_SPACE) {
buf[len++] = ' ';
}
}
// pad spaces up to given width
if (!(flags & FLAGS_LEFT) && !(flags & FLAGS_ZEROPAD)) {
for (size_t i = len; i < width; i++) {
out(' ', buffer, idx++, maxlen);
}
}
// reverse string
for (size_t i = 0U; i < len; i++) {
out(buf[len - i - 1U], buffer, idx++, maxlen);
}
// append pad spaces up to given width
if (flags & FLAGS_LEFT) {
while (idx - start_idx < width) {
out(' ', buffer, idx++, maxlen);
}
}
return idx;
}
// internal itoa for 'long' type
static size_t _ntoa_long(out_fct_type out, char* buffer, size_t idx, size_t maxlen, unsigned long value, bool negative, unsigned long base, unsigned int prec, unsigned int width, unsigned int flags)
{
char buf[PRINTF_NTOA_BUFFER_SIZE];
size_t len = 0U;
// no hash for 0 values
if (!value) {
flags &= ~FLAGS_HASH;
}
// write if precision != 0 and value is != 0
if (!(flags & FLAGS_PRECISION) || value) {
do {
const char digit = (char)(value % base);
buf[len++] = digit < 10 ? '0' + digit : (flags & FLAGS_UPPERCASE ? 'A' : 'a') + digit - 10;
value /= base;
} while (value && (len < PRINTF_NTOA_BUFFER_SIZE));
}
return _ntoa_format(out, buffer, idx, maxlen, buf, len, negative, (unsigned int)base, prec, width, flags);
}
// internal itoa for 'long long' type
#if defined(PRINTF_SUPPORT_LONG_LONG)
static size_t _ntoa_long_long(out_fct_type out, char* buffer, size_t idx, size_t maxlen, unsigned long long value, bool negative, unsigned long long base, unsigned int prec, unsigned int width, unsigned int flags)
{
char buf[PRINTF_NTOA_BUFFER_SIZE];
size_t len = 0U;
// no hash for 0 values
if (!value) {
flags &= ~FLAGS_HASH;
}
// write if precision != 0 and value is != 0
if (!(flags & FLAGS_PRECISION) || value) {
do {
const char digit = (char)(value % base);
buf[len++] = digit < 10 ? '0' + digit : (flags & FLAGS_UPPERCASE ? 'A' : 'a') + digit - 10;
value /= base;
} while (value && (len < PRINTF_NTOA_BUFFER_SIZE));
}
return _ntoa_format(out, buffer, idx, maxlen, buf, len, negative, (unsigned int)base, prec, width, flags);
}
#endif // PRINTF_SUPPORT_LONG_LONG
#if defined(PRINTF_SUPPORT_FLOAT)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
static size_t _ftoa(out_fct_type out, char* buffer, size_t idx, size_t maxlen, double value, unsigned int prec, unsigned int width, unsigned int flags)
{
const size_t start_idx = idx;
char buf[PRINTF_FTOA_BUFFER_SIZE];
size_t len = 0U;
double diff = 0.0;
// if input is larger than thres_max, revert to exponential
const double thres_max = (double)0x7FFFFFFF;
// powers of 10
static const double pow10[] = { 1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000 };
// test for negative
bool negative = false;
if (value < 0) {
negative = true;
value = 0 - value;
}
// set default precision to 6, if not set explicitly
if (!(flags & FLAGS_PRECISION)) {
prec = 6U;
}
// limit precision to 9, cause a prec >= 10 can lead to overflow errors
while ((len < PRINTF_FTOA_BUFFER_SIZE) && (prec > 9U)) {
buf[len++] = '0';
prec--;
}
int whole = (int)value;
double tmp = (value - whole) * pow10[prec];
unsigned long frac = (unsigned long)tmp;
diff = tmp - frac;
if (diff > 0.5l) {
++frac;
// handle rollover, e.g. case 0.99 with prec 1 is 1.0
if (frac >= pow10[prec]) {
frac = 0;
++whole;
}
}
else if ((diff == 0.5l) && ((frac == 0U) || (frac & 1U))) {
// if halfway, round up if odd, OR if last digit is 0
++frac;
}
// TBD: for very large numbers switch back to native sprintf for exponentials. Anyone want to write code to replace this?
// Normal printf behavior is to print EVERY whole number digit which can be 100s of characters overflowing your buffers == bad
if (value > thres_max) {
return 0U;
}
if (prec == 0U) {
diff = value - (double)whole;
if (diff > 0.5l) {
// greater than 0.5, round up, e.g. 1.6 -> 2
++whole;
}
else if ((diff == 0.5l) && (whole & 1)) {
// exactly 0.5 and ODD, then round up
// 1.5 -> 2, but 2.5 -> 2
++whole;
}
}
else {
unsigned int count = prec;
// now do fractional part, as an unsigned number
while (len < PRINTF_FTOA_BUFFER_SIZE) {
--count;
buf[len++] = (char)(48U + (frac % 10U));
if (!(frac /= 10U)) {
break;
}
}
// add extra 0s
while ((len < PRINTF_FTOA_BUFFER_SIZE) && (count-- > 0U)) {
buf[len++] = '0';
}
if (len < PRINTF_FTOA_BUFFER_SIZE) {
// add decimal
buf[len++] = '.';
}
}
// do whole part, number is reversed
while (len < PRINTF_FTOA_BUFFER_SIZE) {
buf[len++] = (char)(48 + (whole % 10));
if (!(whole /= 10)) {
break;
}
}
// pad leading zeros
while (!(flags & FLAGS_LEFT) && (flags & FLAGS_ZEROPAD) && (len < width) && (len < PRINTF_FTOA_BUFFER_SIZE)) {
buf[len++] = '0';
}
// handle sign
if ((len == width) && (negative || (flags & FLAGS_PLUS) || (flags & FLAGS_SPACE))) {
len--;
}
if (len < PRINTF_FTOA_BUFFER_SIZE) {
if (negative) {
buf[len++] = '-';
}
else if (flags & FLAGS_PLUS) {
buf[len++] = '+'; // ignore the space if the '+' exists
}
else if (flags & FLAGS_SPACE) {
buf[len++] = ' ';
}
}
// pad spaces up to given width
if (!(flags & FLAGS_LEFT) && !(flags & FLAGS_ZEROPAD)) {
for (size_t i = len; i < width; i++) {
out(' ', buffer, idx++, maxlen);
}
}
// reverse string
for (size_t i = 0U; i < len; i++) {
out(buf[len - i - 1U], buffer, idx++, maxlen);
}
// append pad spaces up to given width
if (flags & FLAGS_LEFT) {
while (idx - start_idx < width) {
out(' ', buffer, idx++, maxlen);
}
}
return idx;
}
#pragma GCC diagnostic pop
#endif // PRINTF_SUPPORT_FLOAT
// internal vsnprintf
static int _vsnprintf(out_fct_type out, char* buffer, const size_t maxlen, const char* format, va_list va)
{
unsigned int flags, width, precision, n;
size_t idx = 0U;
if (!buffer) {
// use null output function
out = _out_null;
}
while (*format)
{
// format specifier? %[flags][width][.precision][length]
if (*format != '%') {
// no
out(*format, buffer, idx++, maxlen);
format++;
continue;
}
else {
// yes, evaluate it
format++;
}
// evaluate flags
flags = 0U;
do {
switch (*format) {
case '0': flags |= FLAGS_ZEROPAD; format++; n = 1U; break;
case '-': flags |= FLAGS_LEFT; format++; n = 1U; break;
case '+': flags |= FLAGS_PLUS; format++; n = 1U; break;
case ' ': flags |= FLAGS_SPACE; format++; n = 1U; break;
case '#': flags |= FLAGS_HASH; format++; n = 1U; break;
default : n = 0U; break;
}
} while (n);
// evaluate width field
width = 0U;
if (_is_digit(*format)) {
width = _atoi(&format);
}
else if (*format == '*') {
const int w = va_arg(va, int);
if (w < 0) {
flags |= FLAGS_LEFT; // reverse padding
width = (unsigned int)-w;
}
else {
width = (unsigned int)w;
}
format++;
}
// evaluate precision field
precision = 0U;
if (*format == '.') {
flags |= FLAGS_PRECISION;
format++;
if (_is_digit(*format)) {
precision = _atoi(&format);
}
else if (*format == '*') {
const int prec = (int)va_arg(va, int);
precision = prec > 0 ? (unsigned int)prec : 0U;
format++;
}
}
// evaluate length field
switch (*format) {
case 'l' :
flags |= FLAGS_LONG;
format++;
if (*format == 'l') {
flags |= FLAGS_LONG_LONG;
format++;
}
break;
case 'h' :
flags |= FLAGS_SHORT;
format++;
if (*format == 'h') {
flags |= FLAGS_CHAR;
format++;
}
break;
#if defined(PRINTF_SUPPORT_PTRDIFF_T)
case 't' :
flags |= (sizeof(ptrdiff_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
format++;
break;
#endif
case 'j' :
flags |= (sizeof(intmax_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
format++;
break;
case 'z' :
flags |= (sizeof(size_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
format++;
break;
default :
break;
}
// evaluate specifier
switch (*format) {
case 'd' :
case 'i' :
case 'u' :
case 'x' :
case 'X' :
case 'o' :
case 'b' : {
// set the base
unsigned int base;
if (*format == 'x' || *format == 'X') {
base = 16U;
}
else if (*format == 'o') {
base = 8U;
}
else if (*format == 'b') {
base = 2U;
}
else {
base = 10U;
flags &= ~FLAGS_HASH; // no hash for dec format
}
// uppercase
if (*format == 'X') {
flags |= FLAGS_UPPERCASE;
}
// no plus or space flag for u, x, X, o, b
if ((*format != 'i') && (*format != 'd')) {
flags &= ~(FLAGS_PLUS | FLAGS_SPACE);
}
// ignore '0' flag when precision is given
if (flags & FLAGS_PRECISION) {
flags &= ~FLAGS_ZEROPAD;
}
// convert the integer
if ((*format == 'i') || (*format == 'd')) {
// signed
if (flags & FLAGS_LONG_LONG) {
#if defined(PRINTF_SUPPORT_LONG_LONG)
const long long value = va_arg(va, long long);
idx = _ntoa_long_long(out, buffer, idx, maxlen, (unsigned long long)(value > 0 ? value : 0 - value), value < 0, base, precision, width, flags);
#endif
}
else if (flags & FLAGS_LONG) {
const long value = va_arg(va, long);
idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned long)(value > 0 ? value : 0 - value), value < 0, base, precision, width, flags);
}
else {
const int value = (flags & FLAGS_CHAR) ? (char)va_arg(va, int) : (flags & FLAGS_SHORT) ? (short int)va_arg(va, int) : va_arg(va, int);
idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned int)(value > 0 ? value : 0 - value), value < 0, base, precision, width, flags);
}
}
else {
// unsigned
if (flags & FLAGS_LONG_LONG) {
#if defined(PRINTF_SUPPORT_LONG_LONG)
idx = _ntoa_long_long(out, buffer, idx, maxlen, va_arg(va, unsigned long long), false, base, precision, width, flags);
#endif
}
else if (flags & FLAGS_LONG) {
idx = _ntoa_long(out, buffer, idx, maxlen, va_arg(va, unsigned long), false, base, precision, width, flags);
}
else {
const unsigned int value = (flags & FLAGS_CHAR) ? (unsigned char)va_arg(va, unsigned int) : (flags & FLAGS_SHORT) ? (unsigned short int)va_arg(va, unsigned int) : va_arg(va, unsigned int);
idx = _ntoa_long(out, buffer, idx, maxlen, value, false, base, precision, width, flags);
}
}
format++;
break;
}
#if defined(PRINTF_SUPPORT_FLOAT)
case 'f' :
case 'F' :
idx = _ftoa(out, buffer, idx, maxlen, va_arg(va, double), precision, width, flags);
format++;
break;
#endif // PRINTF_SUPPORT_FLOAT
case 'c' : {
unsigned int l = 1U;
// pre padding
if (!(flags & FLAGS_LEFT)) {
while (l++ < width) {
out(' ', buffer, idx++, maxlen);
}
}
// char output
out((char)va_arg(va, int), buffer, idx++, maxlen);
// post padding
if (flags & FLAGS_LEFT) {
while (l++ < width) {
out(' ', buffer, idx++, maxlen);
}
}
format++;
break;
}
case 's' : {
char* p = va_arg(va, char*);
unsigned int l = _strlen(p);
// pre padding
if (flags & FLAGS_PRECISION) {
l = (l < precision ? l : precision);
}
if (!(flags & FLAGS_LEFT)) {
while (l++ < width) {
out(' ', buffer, idx++, maxlen);
}
}
// string output
while ((*p != 0) && (!(flags & FLAGS_PRECISION) || precision--)) {
out(*(p++), buffer, idx++, maxlen);
}
// post padding
if (flags & FLAGS_LEFT) {
while (l++ < width) {
out(' ', buffer, idx++, maxlen);
}
}
format++;
break;
}
case 'p' : {
width = sizeof(void*) * 2U;
flags |= FLAGS_ZEROPAD | FLAGS_UPPERCASE;
#if defined(PRINTF_SUPPORT_LONG_LONG)
const bool is_ll = sizeof(uintptr_t) == sizeof(long long);
if (is_ll) {
idx = _ntoa_long_long(out, buffer, idx, maxlen, (uintptr_t)va_arg(va, void*), false, 16U, precision, width, flags);
}
else {
#endif
idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned long)((uintptr_t)va_arg(va, void*)), false, 16U, precision, width, flags);
#if defined(PRINTF_SUPPORT_LONG_LONG)
}
#endif
format++;
break;
}
case '%' :
out('%', buffer, idx++, maxlen);
format++;
break;
default :
out(*format, buffer, idx++, maxlen);
format++;
break;
}
}
// termination
out((char)0, buffer, idx < maxlen ? idx : maxlen - 1U, maxlen);
// return written chars without terminating \0
return (int)idx;
}
///////////////////////////////////////////////////////////////////////////////
int printf(const char* format, ...)
{
va_list va;
va_start(va, format);
char buffer[1];
const int ret = _vsnprintf(_out_char, buffer, (size_t)-1, format, va);
va_end(va);
return ret;
}
int sprintf(char* buffer, const char* format, ...)
{
va_list va;
va_start(va, format);
const int ret = _vsnprintf(_out_buffer, buffer, (size_t)-1, format, va);
va_end(va);
return ret;
}
int snprintf(char* buffer, size_t count, const char* format, ...)
{
va_list va;
va_start(va, format);
const int ret = _vsnprintf(_out_buffer, buffer, count, format, va);
va_end(va);
return ret;
}
int vsnprintf(char* buffer, size_t count, const char* format, va_list va)
{
return _vsnprintf(_out_buffer, buffer, count, format, va);
}
int fctprintf(void (*out)(char character, void* arg), void* arg, const char* format, ...)
{
va_list va;
va_start(va, format);
out_fct_wrap_type out_fct_wrap = { out, arg };
const int ret = _vsnprintf(_out_fct, (char*)&out_fct_wrap, (size_t)-1, format, va);
va_end(va);
return ret;
}