167 lines
3.4 KiB
C
167 lines
3.4 KiB
C
#pragma once
|
|
#include <stddef.h>
|
|
#include <stdcountof.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#define SYS_write 1
|
|
#define SYS_exit_group 231
|
|
|
|
#define STDOUT_FILENO 1
|
|
|
|
static
|
|
void write(uint64_t fileno, void *buffer, size_t len) {
|
|
uint64_t a,d;
|
|
asm volatile("syscall"
|
|
:"=a"(a),"=d"(d):"a"(SYS_write),"D"(fileno),"S"(buffer),"d"(len):"memory");
|
|
}
|
|
|
|
[[noreturn]] static
|
|
void exit_group(uint64_t code) {
|
|
asm volatile(
|
|
"syscall\n"
|
|
"ud2"
|
|
::"a"(SYS_exit_group),"D"(code));
|
|
for (;;);
|
|
}
|
|
|
|
#define xstr(s) str(s)
|
|
#define str(s) #s
|
|
|
|
typedef uint64_t bcdint;
|
|
|
|
static
|
|
void printns(size_t len, unsigned char chars[len]) {
|
|
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;
|
|
}
|
|
write(STDOUT_FILENO, p, countof(buf));
|
|
}
|
|
|
|
static
|
|
void print_many_char(size_t len, char chars[len]) {
|
|
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++;
|
|
}
|
|
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++;
|
|
}
|
|
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);
|
|
}
|
|
|
|
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 long grabnum(unsigned char *s, unsigned char **end) {
|
|
unsigned long r = 0;
|
|
for (; *s >= '0' && *s <= '9'; s++)
|
|
r = r * 10 + (*s - '0');
|
|
if (end) *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;
|
|
}
|