matos/userspace/libc.c

821 lines
26 KiB
C
Raw Normal View History

2021-11-05 23:02:23 +01:00
#include "libc.h"
#include "errno.h"
2024-01-29 21:47:24 +01:00
#include "list.h"
2021-11-05 23:02:23 +01:00
#include "minmax.h"
#include "swintr.h"
#include "sys/mman.h"
2021-11-05 23:02:23 +01:00
#include "syscall.h"
2024-02-14 23:20:46 +01:00
#include "thread.h"
2024-02-14 18:43:47 +01:00
#include "unistd.h"
2021-11-05 23:02:23 +01:00
int errno = 0;
2021-11-05 23:02:23 +01:00
int memcmp(const void *aptr, const void *bptr, size_t size)
{
const unsigned char *a = (const unsigned char *)aptr;
const unsigned char *b = (const unsigned char *)bptr;
for (size_t i = 0; i < size; i++) {
if (a[i] < b[i])
return -1;
else if (b[i] < a[i])
return 1;
}
return 0;
}
// Inspirated by https://interrupt.memfault.com/blog/memcpy-newlib-nano
/* Nonzero if either X or Y is not aligned on a "long" boundary. */
#define UNALIGNED(X, Y) \
(((long)X & (sizeof (long) - 1)) | ((long)Y & (sizeof (long) - 1)))
/* How many bytes are copied each iteration of the 4X unrolled loop. */
#define BIGBLOCKSIZE (sizeof (long) << 2)
/* How many bytes are copied each iteration of the word copy loop. */
#define LITTLEBLOCKSIZE (sizeof (long))
/* Threshhold for punting to the byte copier. */
#define TOO_SMALL(LEN) ((LEN) < BIGBLOCKSIZE)
void *memcpy(void *dst0, const void *src0, size_t len0)
{
#if 0
char *dstChar = dst0;
const char *srcChar = src0;
for (size_t i = 0; i < len0; i++) {
*(dstChar++) = *(srcChar++);
}
return dst0;
#else
char *dst = dst0;
const char *src = src0;
/* If the size is small, or either SRC or DST is unaligned,
then punt into the byte copy loop. This should be rare. */
if (!TOO_SMALL(len0) && !UNALIGNED(src, dst)) {
2023-11-11 00:09:00 +01:00
long *aligned_dst;
const long *aligned_src;
2021-11-05 23:02:23 +01:00
aligned_dst = (long *)dst;
aligned_src = (long *)src;
/* Copy 4X long words at a time if possible. */
while (len0 >= BIGBLOCKSIZE) {
*aligned_dst++ = *aligned_src++;
*aligned_dst++ = *aligned_src++;
*aligned_dst++ = *aligned_src++;
*aligned_dst++ = *aligned_src++;
len0 -= BIGBLOCKSIZE;
}
/* Copy one long word at a time if possible. */
while (len0 >= LITTLEBLOCKSIZE) {
*aligned_dst++ = *aligned_src++;
len0 -= LITTLEBLOCKSIZE;
}
/* Pick up any residual with a byte copier. */
dst = (char *)aligned_dst;
src = (char *)aligned_src;
}
while (len0--)
*dst++ = *src++;
return dst0;
#endif
}
void *memmove(void *dst, const void *src, size_t n)
{
char *dstChar = dst;
const char *srcChar = src;
for (size_t i = 0; i < n; i++) {
*(dstChar++) = *(srcChar++);
}
return dst;
}
2021-11-05 23:02:23 +01:00
void *memset(void *src, int c, size_t n)
{
for (char *ptr = (char *)src; n > 0; n--, ptr++) {
*ptr = (char)c;
}
return src;
}
char *itoa(long long int value, char *str, int base)
{
char *rc;
char *ptr;
char *low;
// Check for supported base.
if (base < 2 || base > 36) {
*str = '\0';
return str;
}
rc = ptr = str;
// Set '-' for negative decimals.
if (value < 0 && base == 10) {
*ptr++ = '-';
}
// Remember where the numbers start.
low = ptr;
// The actual conversion.
do {
// Modulo is negative for negative value. This trick makes abs() unnecessary.
*ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz"
[35 + value % base];
value /= base;
} while (value);
// Terminating the string.
*ptr-- = '\0';
// Invert the numbers.
while (low < ptr) {
char tmp = *low;
*low++ = *ptr;
*ptr-- = tmp;
}
return rc;
}
/* K&R */
void reverse(char s[])
{
int c, i, j;
for (i = 0, j = strlen(s) - 1; i < j; i++, j--) {
c = s[i];
s[i] = s[j];
s[j] = c;
}
}
/* K&R */
int strlen(const char s[])
{
int i = 0;
while (s[i] != '\0')
++i;
return i;
}
/* K&R
* Returns <0 if s1<s2, 0 if s1==s2, >0 if s1>s2 */
int strcmp(const char s1[], const char s2[])
{
int i;
for (i = 0; s1[i] == s2[i]; i++) {
if (s1[i] == '\0')
return 0;
}
return s1[i] - s2[i];
}
unsigned int strnlen(const char *s, size_t count)
{
const char *sc;
for (sc = s; count-- && *sc != '\0'; ++sc)
/* nothing */ continue;
return sc - s;
}
char *strzcpy(register char *dst, register const char *src, register int len)
{
int i;
if (len <= 0)
return dst;
for (i = 0; i < len; i++) {
dst[i] = src[i];
if (src[i] == '\0')
return dst;
}
dst[len - 1] = '\0';
return dst;
}
int printf(const char *format, ...)
{
int ret;
va_list ap;
va_start(ap, format);
ret = vprintf(format, ap);
va_end(ap);
return ret;
}
int puts(const char *str)
{
int ret = 0;
while (*str) {
putc(*(str++));
ret++;
}
return ret;
}
// int max is 2^(sizeof(int)*8) which is (2^3)^(sizeof(int)*8/3)
// = 8^(sizeof(int)*8/3) ~ 10^(sizeof(int)*8/3)
#define PRINT_INT(name, type) \
int print##name(type integer, char *str, size_t size) \
{ \
char num[sizeof(integer) * 3]; \
int i = 0; \
int c = 0; \
int ret = 0; \
\
if (integer < 0) { \
if (str) { \
if (size) { \
str[c++] = '-'; \
size--; \
ret++; \
} else { \
return ret; \
} \
} else { \
ret++; \
} \
} \
\
do { \
int digit = integer % 10; \
num[i++] = (digit > 0) ? digit : -digit; \
integer = integer / 10; \
} while (integer != 0); \
\
for (i = i - 1; i >= 0; i--) { \
if (str) { \
if (size) { \
str[c++] = num[i] + '0'; \
size--; \
ret++; \
} else { \
return ret; \
} \
} else { \
ret++; \
} \
} \
return ret; \
}
2024-02-14 23:20:46 +01:00
#define PRINT_UINT(name, type) \
int print##name(type integer, char *str, size_t size) \
{ \
char num[sizeof(integer) * 3]; \
int i = 0; \
int c = 0; \
int ret = 0; \
\
do { \
int digit = integer % 10; \
num[i++] = (digit > 0) ? digit : -digit; \
integer = integer / 10; \
} while (integer != 0); \
\
for (i = i - 1; i >= 0; i--) { \
if (str) { \
if (size) { \
str[c++] = num[i] + '0'; \
size--; \
ret++; \
} else { \
return ret; \
} \
} else { \
ret++; \
} \
} \
return ret; \
}
2021-11-05 23:02:23 +01:00
PRINT_INT(Int, int);
2024-02-14 23:20:46 +01:00
PRINT_INT(Lint, long int);
PRINT_INT(Llint, long long int);
PRINT_UINT(Uint, unsigned int);
PRINT_UINT(Luint, long unsigned int);
PRINT_UINT(Lluint, long long unsigned int);
#define PRINT_PART(func, type, str, size, c, ret) \
{ \
int s; \
type d = va_arg(ap, type); \
if (str) \
s = func(d, &str[c], size); \
else \
s = func(d, NULL, size); \
\
size -= s; \
c += s; \
ret += s; \
break; \
}
2021-11-05 23:02:23 +01:00
int vsnprintf(char *str, size_t size, const char *format, va_list ap)
{
int ret = 0;
int i = 0;
int c = 0;
2024-02-14 23:20:46 +01:00
while (format[i] != '\0' && (size|| !str)) {
2021-11-05 23:02:23 +01:00
switch (format[i]) {
case '%':
switch (format[i + 1]) {
case 'i':
2024-02-14 23:20:46 +01:00
case 'd': PRINT_PART(printInt, int, str, size, c, ret)
case 'u': PRINT_PART(printUint, uint, str, size, c, ret)
2021-11-05 23:02:23 +01:00
case 'p':
case 'x': {
char val[sizeof(int) * 2];
unsigned int valIdx = 0;
int d = va_arg(ap, int);
itoa(d, val, 16);
if (str) {
while (val[valIdx]) {
if (size) {
str[c++] = val[valIdx++];
size--;
ret++;
} else {
return ret;
}
}
} else {
ret += strlen(val);
}
break;
}
case 'c': {
if (str) {
int ch = va_arg(ap, int);
str[c++] = ch;
size--;
}
ret++;
break;
}
case 's': {
char *stri = va_arg(ap, char *);
if (!stri)
stri = "[NULL STR]";
if (str) {
while (*stri) {
if (size) {
str[c++] = *(stri++);
size--;
ret++;
} else {
return ret;
}
}
} else {
ret += strlen(stri);
}
break;
}
case '%':
if (str) {
str[c++] = '%';
size--;
}
ret++;
break;
case 'l':
switch (format[i + 2]) {
case 'l':
switch (format[i + 3]) {
2024-02-14 23:20:46 +01:00
case 'i':
case 'd': PRINT_PART(printLlint, long long int, str, size, c, ret)
case 'u': PRINT_PART(printLluint, long long unsigned int, str, size, c, ret)
2021-11-05 23:02:23 +01:00
case 'p':
case 'x': {
char val[sizeof(long long int) * 2];
unsigned int valIdx = 0;
2024-02-14 23:20:46 +01:00
unsigned long long int d =
va_arg(ap, unsigned long long int);
2021-11-05 23:02:23 +01:00
itoa(d, val, 16);
if (str) {
while (val[valIdx]) {
if (size) {
str[c++] = val[valIdx++];
size--;
ret++;
} else {
return ret;
}
}
} else {
ret += strlen(val);
}
break;
}
}
i++;
break;
case 'i':
2024-02-14 23:20:46 +01:00
case 'd': PRINT_PART(printLint, long int, str, size, c, ret)
case 'u':
PRINT_PART(printLuint, long unsigned int, str, size, c, ret)
case 'p':
case 'x': {
char val[sizeof(int) * 2];
unsigned int valIdx = 0;
unsigned long int d = va_arg(ap, unsigned long int);
itoa(d, val, 16);
if (str) {
while (val[valIdx]) {
if (size) {
str[c++] = val[valIdx++];
size--;
ret++;
} else {
return ret;
}
}
} else {
ret += strlen(val);
}
2021-11-05 23:02:23 +01:00
break;
}
}
i++;
break;
}
i++;
break;
default:
if (str) {
str[c++] = format[i];
size--;
}
ret++;
}
i++;
}
if (str) {
if (size) {
str[c++] = '\0';
} else {
if (c > 0) {
str[c - 1] = '\0';
}
}
}
return ret;
}
int vprintf(const char *fmt, va_list ap)
{
char tmp[256];
int idx = 0;
int ret = vsnprintf(tmp, sizeof(tmp), fmt, ap);
while (ret > 0 && tmp[idx]) {
putc(tmp[idx++]);
}
return ret;
}
int asprintf(char **strp, const char *fmt, ...)
{
int ret;
va_list ap;
va_start(ap, fmt);
ret = vasprintf(strp, fmt, ap);
va_end(ap);
return ret;
}
int vasprintf(char **strp, const char *fmt, va_list ap)
{
int n = 0;
size_t size = 0;
2023-11-11 00:08:08 +01:00
char *p = malloc(256);
2021-11-05 23:02:23 +01:00
/* Determine required size */
n = vsnprintf(p, size, fmt, ap);
2023-11-11 00:08:08 +01:00
if (n < 0){
free(p);
2021-11-05 23:02:23 +01:00
return -1;
2023-11-11 00:08:08 +01:00
}
2021-11-05 23:02:23 +01:00
/* One extra byte for '\0' */
2023-11-11 00:08:08 +01:00
size = min(256U, (size_t)n + 1);
2021-11-05 23:02:23 +01:00
n = vsnprintf(p, size, fmt, ap);
if (n < 0) {
2023-11-11 00:08:08 +01:00
free(p);
2021-11-05 23:02:23 +01:00
return -1;
}
2023-11-11 00:08:08 +01:00
*strp = p;
2021-11-05 23:02:23 +01:00
return size;
}
int syscall5(int id, unsigned int arg1, unsigned int arg2, unsigned int arg3,
unsigned int arg4, unsigned int arg5)
{
unsigned int args[] = {arg3, arg4, arg5};
return syscall3(id, arg1, arg2, (unsigned)args);
}
int syscall4(int id, unsigned int arg1, unsigned int arg2, unsigned int arg3,
unsigned int arg4)
{
unsigned int args[] = {arg3, arg4};
return syscall3(id, arg1, arg2, (unsigned)args);
}
2021-11-05 23:02:23 +01:00
int syscall3(int id, unsigned int arg1, unsigned int arg2, unsigned int arg3)
{
int ret;
asm volatile("movl %1,%%eax \n"
"movl %2,%%ebx \n"
"movl %3,%%ecx \n"
"movl %4,%%edx \n"
"int %5\n"
"movl %%eax, %0"
: "=g"(ret)
: "g"(id), "g"(arg1), "g"(arg2), "g"(arg3), "i"(SYSCALL_INTR_NB)
: "eax", "ebx", "ecx", "edx");
return ret;
}
int syscall2(int id, unsigned int arg1, unsigned int arg2)
{
return syscall3(id, arg1, arg2, 0);
}
int syscall1(int id, unsigned int arg1)
{
return syscall3(id, arg1, 0, 0);
}
int syscall0(int id)
{
return syscall3(id, 0, 0, 0);
}
void _exit(int status)
{
syscall1(SYSCALL_ID_EXIT, status);
}
int putc(const int c){
return syscall1(SYSCALL_ID_PUTC, c);
}
2023-11-11 00:07:26 +01:00
void helo()
2021-11-05 23:02:23 +01:00
{
2023-11-11 00:07:26 +01:00
syscall0(SYSCALL_ID_HELO);
2021-11-05 23:02:23 +01:00
}
2021-11-06 00:13:40 +01:00
int testSycall5(uint arg1, uint arg2, uint arg3, uint arg4, uint arg5)
{
return syscall5(SYSCALL_ID_TEST, arg1, arg2, arg3, arg4, arg5);
}
2021-11-06 00:13:40 +01:00
char readc()
{
return syscall0(SYSCALL_ID_READ);
}
2023-03-21 22:41:17 +01:00
char getchar()
2021-11-06 00:13:40 +01:00
{
char c = 0;
do {
c = readc();
} while (c == 0);
return c;
}
int readline(char *buf, int size)
{
int i = 0;
for (; i < size - 1; i++) {
2023-03-21 22:41:17 +01:00
char key = getchar();
2021-11-06 00:13:40 +01:00
if (key == '\n')
break;
if(key == '\b' && i>=1){
buf[i-1] = '\0';
i-=2;
2021-11-06 00:23:45 +01:00
continue;
2021-11-06 00:13:40 +01:00
}
buf[i] = key;
}
buf[i] = '\0';
return i == (size-1);
}
int brk(void *addr)
2022-08-09 16:15:00 +02:00
{
uintptr_t new = syscall1(SYSCALL_ID_BRK, (unsigned int)addr);
//errno = ENOMEM
return (new >= (uintptr_t)addr)?0:-1;
}
void *sbrk(intptr_t increment)
{
void *current = (void *)syscall1(SYSCALL_ID_BRK, 0);
if (increment == 0) {
return current;
}
2024-01-29 21:47:24 +01:00
if ((uintptr_t)syscall1(SYSCALL_ID_BRK, (uintptr_t)current + increment) < ((uintptr_t)current + increment)) {
// errno = ENOMEM
return (void *)-1;
}
return current;
2022-08-09 16:15:00 +02:00
}
2023-03-21 22:41:17 +01:00
2024-01-29 21:47:24 +01:00
// Malloc internal
struct heapBlock {
size_t size;
struct heapBlock *next, *prev;
int free;
};
2023-11-11 00:08:08 +01:00
2024-01-29 21:47:24 +01:00
static struct heapBlock *heapBlkList = NULL;
static struct heapBlock *findFreeBlock(size_t size)
{
struct heapBlock *cur = NULL;
2024-01-29 21:47:24 +01:00
struct heapBlock *found = NULL;
int idx;
list_foreach(heapBlkList, cur, idx)
{
if (cur->size >= size && cur->free) {
2024-01-29 21:47:24 +01:00
found = cur;
break;
}
2023-03-21 22:41:17 +01:00
}
2024-01-29 21:47:24 +01:00
return found;
}
2023-03-21 22:41:17 +01:00
static struct heapBlock *allocNewBlock(size_t size)
{
2024-01-29 21:47:24 +01:00
struct heapBlock *blk = sbrk(size + sizeof(struct heapBlock));
struct heapBlock *head = sbrk(0);
size_t blkSize = (intptr_t)head - (intptr_t)blk - sizeof(struct heapBlock);
if (blk == (void *)-1) {
return NULL;
2023-03-21 22:41:17 +01:00
}
2024-01-29 21:47:24 +01:00
blk->size = blkSize;
blk->free = 1;
list_add_tail(heapBlkList, blk);
2024-01-29 21:47:24 +01:00
return blk;
}
2023-03-21 22:41:17 +01:00
static struct heapBlock *splitBlock(struct heapBlock *blk, size_t neededSize)
2024-01-29 21:47:24 +01:00
{
if (blk->size < neededSize + sizeof(struct heapBlock) + 1) {
return NULL;
}
struct heapBlock *newBlk = (struct heapBlock *)((uintptr_t)(blk + 1) + neededSize);
newBlk->free = 1;
newBlk->size = blk->size - sizeof(struct heapBlock) - neededSize;
blk->size = neededSize;
2024-01-29 21:47:24 +01:00
return newBlk;
2023-11-11 00:08:08 +01:00
}
2024-01-29 21:47:24 +01:00
void *malloc(size_t size)
2023-11-11 00:08:08 +01:00
{
2024-01-29 21:47:24 +01:00
if (size == 0)
return NULL;
struct heapBlock *blk = findFreeBlock(size);
if (!blk)
blk = allocNewBlock(size);
if (!blk)
return NULL;
struct heapBlock *remainBlock = splitBlock(blk, size);
if (remainBlock) {
list_add_head(heapBlkList, remainBlock);
2023-11-11 00:08:08 +01:00
}
2024-01-29 21:47:24 +01:00
blk->free = 0;
2024-01-29 21:47:24 +01:00
return blk + 1; // return the area after the blk description
}
static struct heapBlock *getHeapBlock(void *ptr)
{
2024-01-29 21:47:24 +01:00
return (struct heapBlock *)ptr - 1;
}
2023-11-11 00:08:08 +01:00
void free(void *ptr)
{
if (!ptr)
2024-01-29 21:47:24 +01:00
return;
struct heapBlock *blk = getHeapBlock(ptr);
assert(blk->free == 0);
blk->free = 1;
2023-03-21 22:41:17 +01:00
}
2024-01-29 21:47:24 +01:00
void *calloc(size_t nmemb, size_t size)
{
size_t allocSize = nmemb * size;
void *ptr = malloc(allocSize);
if (ptr != NULL)
memset(ptr, 0, allocSize);
return ptr;
}
void *realloc(void *ptr, size_t size)
{
if (!ptr) {
return malloc(size);
}
struct heapBlock *blk = getHeapBlock(ptr);
if (blk->size >= size) {
return ptr;
}
void *new_ptr;
new_ptr = malloc(size);
if (!new_ptr) {
return NULL;
}
memmove(new_ptr, ptr, blk->size);
free(ptr);
return new_ptr;
}
void *mmap(void *addr, size_t len, int prot, int flags, char *path) {
2023-03-24 23:43:09 +01:00
int ret = syscall5(SYSCALL_ID_MMAP, (unsigned int)&addr, len, prot, flags, (unsigned int)path);
if(!ret)
return addr;
errno = ret;
return MAP_FAILED;
}
int munmap(void *addr, size_t len)
{
if (len == 0)
return -EINVAL;
return syscall2(SYSCALL_ID_MUNMAP, (unsigned int)addr, len);
2023-03-24 23:43:09 +01:00
}
2024-02-13 00:19:39 +01:00
/* As when a new thread is run, the params are passed by register (See cpu_ustate_init), use
* this function to simplify new thread usage*/
static void thread_runner()
{
register unsigned long int reg_arg1 asm("%eax");
register unsigned long int reg_arg2 asm("%ebx");
start_routine *func = (start_routine *)reg_arg1;
void *arg = (void *)reg_arg2;
func(arg);
_exit(0);
}
2024-02-14 23:20:46 +01:00
int thread_create(pthread_t *thread, start_routine *func, void *arg, size_t stackSize) {
return syscall5(SYSCALL_ID_NEW_THREAD, (unsigned int)thread, (unsigned int)thread_runner, (unsigned int)func,
2024-02-13 00:19:39 +01:00
(unsigned int)arg, stackSize);
}
2024-02-14 18:43:47 +01:00
int usleep(useconds_t usec) {
return syscall1(SYSCALL_ID_USLEEP, (unsigned int)usec);
}
2024-02-14 23:20:46 +01:00
pid_t gettid(void) {
return syscall0(SYSCALL_ID_GETTID);
}
pid_t getpid(void) {
return syscall0(SYSCALL_ID_GETPID);
}