515 lines
16 KiB
C
515 lines
16 KiB
C
#include "libc.h"
|
|
#include "minmax.h"
|
|
#include "swintr.h"
|
|
#include "syscall.h"
|
|
|
|
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;
|
|
long *aligned_dst;
|
|
const long *aligned_src;
|
|
|
|
/* 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)) {
|
|
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 *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; \
|
|
}
|
|
|
|
PRINT_INT(Int, int);
|
|
PRINT_INT(Int64, long long int);
|
|
|
|
int vsnprintf(char *str, size_t size, const char *format, va_list ap)
|
|
{
|
|
int ret = 0;
|
|
int i = 0;
|
|
int c = 0;
|
|
|
|
while (format[i] != '\0' && (size|| !str)) {
|
|
switch (format[i]) {
|
|
case '%':
|
|
switch (format[i + 1]) {
|
|
case 'i':
|
|
case 'd': {
|
|
int s;
|
|
int d = va_arg(ap, int);
|
|
if (str)
|
|
s = printInt(d, &str[c], size);
|
|
else
|
|
s = printInt(d, NULL, size);
|
|
|
|
size -= s;
|
|
c += s;
|
|
ret += s;
|
|
break;
|
|
}
|
|
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]) {
|
|
case 'd': {
|
|
int s;
|
|
long long int d = va_arg(ap, long long int);
|
|
if (str)
|
|
s = printInt64(d, &str[c], size);
|
|
else
|
|
s = printInt64(d, NULL, size);
|
|
|
|
size -= s;
|
|
c += s;
|
|
ret += s;
|
|
break;
|
|
}
|
|
case 'p':
|
|
case 'x': {
|
|
char val[sizeof(long long int) * 2];
|
|
unsigned int valIdx = 0;
|
|
long long int d = va_arg(ap, long 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);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
i++;
|
|
break;
|
|
case 'i':
|
|
case 'd': {
|
|
int s;
|
|
long int d = va_arg(ap, long int);
|
|
if (str)
|
|
s = printInt64(d, &str[c], size);
|
|
else
|
|
s = printInt64(d, NULL, size);
|
|
|
|
size -= s;
|
|
c += s;
|
|
ret += s;
|
|
break;
|
|
}
|
|
}
|
|
i++;
|
|
break;
|
|
}
|
|
i++;
|
|
break;
|
|
|
|
default:
|
|
if (str) {
|
|
str[c++] = format[i];
|
|
size--;
|
|
}
|
|
ret++;
|
|
}
|
|
i++;
|
|
}
|
|
|
|
if (str) {
|
|
if (size) {
|
|
str[c++] = '\0';
|
|
size--;
|
|
} 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;
|
|
char p[256]; //TODO replace by malloc
|
|
|
|
/* Determine required size */
|
|
|
|
n = vsnprintf(p, size, fmt, ap);
|
|
|
|
if (n < 0)
|
|
return -1;
|
|
|
|
/* One extra byte for '\0' */
|
|
|
|
size = min(256U,(size_t)n + 1);
|
|
|
|
n = vsnprintf(p, size, fmt, ap);
|
|
|
|
if (n < 0) {
|
|
return -1;
|
|
}
|
|
*strp = p;
|
|
|
|
return size;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
void yolo()
|
|
{
|
|
syscall0(SYSCALL_ID_YOLO);
|
|
}
|