148 lines
3.1 KiB
C
148 lines
3.1 KiB
C
#pragma once
|
|
#include <stdlib.h>
|
|
#include <stdcountof.h>
|
|
#include <sys/syscall.h>
|
|
#include <unistd.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#define xstr(s) str(s)
|
|
#define str(s) #s
|
|
|
|
typedef uint64_t bcdint;
|
|
|
|
static
|
|
void printns(size_t len, unsigned char chars[len]) {
|
|
syscall(SYS_write, STDOUT_FILENO, chars, len);
|
|
}
|
|
|
|
static
|
|
void printptr(uintptr_t v) {
|
|
char buf[sizeof(uintptr_t)*2] = { };
|
|
char *p = buf+countof(buf);
|
|
for (size_t i = 0; i < countof(buf); i++) {
|
|
p--;
|
|
if ((v & 0x0F) < 0xA) {
|
|
*p = '0' + (v & 0x0F);
|
|
} else {
|
|
*p = 'A' - 0xA + (v & 0x0F);
|
|
}
|
|
v >>= 4;
|
|
}
|
|
syscall(SYS_write, STDOUT_FILENO, p, countof(buf));
|
|
}
|
|
|
|
static
|
|
void print_many_char(size_t len, char chars[len]) {
|
|
syscall(SYS_write, STDOUT_FILENO, chars, len);
|
|
}
|
|
|
|
#define print(string) print_many_char(sizeof(string)-1, string)
|
|
|
|
static
|
|
void printd(unsigned long v) {
|
|
if (!v) {
|
|
print("0");
|
|
return;
|
|
}
|
|
char buf[32];
|
|
char *p = buf+32;
|
|
unsigned count = 0;
|
|
while (v) {
|
|
p--;
|
|
*p = '0' + (v % 10);
|
|
v /= 10;
|
|
count++;
|
|
}
|
|
syscall(SYS_write, STDOUT_FILENO, p, count);
|
|
}
|
|
|
|
static
|
|
void printh(unsigned long v) {
|
|
if (!v) {
|
|
print("0");
|
|
return;
|
|
}
|
|
char buf[32];
|
|
char *p = buf+32;
|
|
unsigned count = 0;
|
|
while (v) {
|
|
p--;
|
|
if ((v & 0x0F) < 0xA) {
|
|
*p = '0' + (v & 0x0F);
|
|
} else {
|
|
*p = 'A' - 0xA + (v & 0x0F);
|
|
}
|
|
v >>= 4;
|
|
count++;
|
|
}
|
|
syscall(SYS_write, STDOUT_FILENO, p, count);
|
|
}
|
|
|
|
static
|
|
void print_hex_char(unsigned char v) {
|
|
char buf[2];
|
|
|
|
if ((v >> 4) < 0x0A) {
|
|
buf[0] = '0' + (v >> 4);
|
|
} else {
|
|
buf[0] = 'A' - 0xA + (v >> 4);
|
|
}
|
|
|
|
if ((v & 0x0F) < 0xA) {
|
|
buf[1] = '0' + (v & 0x0F);
|
|
} else {
|
|
buf[1] = 'A' - 0xA + (v & 0x0F);
|
|
}
|
|
|
|
syscall(SYS_write, STDOUT_FILENO, buf, sizeof(buf));
|
|
}
|
|
|
|
static
|
|
void print_pre_hex(size_t len, unsigned char prefix[len], unsigned long v) {
|
|
printns(len, prefix);
|
|
printh(v);
|
|
}
|
|
|
|
static
|
|
void print_pre_ptr(size_t len, unsigned char prefix[len], void *ptr) {
|
|
printns(len, prefix);
|
|
printptr((uintptr_t)ptr);
|
|
}
|
|
|
|
#define print_watch_ptr(expr) print_pre_ptr(\
|
|
sizeof(#expr ": ")-1, (unsigned char*)(#expr ": "), expr)
|
|
#define print_watch_hex(expr) print_pre_hex(\
|
|
sizeof(#expr ": ")-1, (unsigned char*)(#expr ": "), expr)
|
|
|
|
static
|
|
bcdint grabbcd(unsigned char *s, unsigned char **end) {
|
|
bcdint r = 0;
|
|
for (; *s >= '0' && *s <= '9'; s++)
|
|
r = (r << 4) + (*s - '0');
|
|
if (end) *end = s;
|
|
return r;
|
|
}
|
|
|
|
static
|
|
unsigned grabnum(const char *s, const char **end) {
|
|
unsigned r = 0;
|
|
for (; *s >= '0' && *s <= '9'; s++)
|
|
r = r * 10 + (*s - '0');
|
|
*end = s;
|
|
return r;
|
|
}
|
|
|
|
// from wikipedia, the free encyclopedia
|
|
// https://en.wikipedia.org/wiki/Binary-coded_decimal
|
|
static bcdint bcdadd(bcdint a, bcdint b) {
|
|
bcdint t1, t2;
|
|
t1 = a + 0x0666666666666666;
|
|
t2 = t1 ^ b;
|
|
t1 = t1 + b;
|
|
t2 = t1 ^ t2;
|
|
t2 = ~t2 & 0x1111111111111110;
|
|
t2 = (t2 >> 2) | (t2 >> 3);
|
|
return t1 - t2;
|
|
}
|