From 6742aa38e30fa0c440ed5730916d7c783d9b6397 Mon Sep 17 00:00:00 2001 From: sys64738 Date: Sat, 29 Jan 2022 02:26:40 +0100 Subject: [PATCH] port stuff to rpi and zynq --- jazelle.c | 288 ++++++++++++++++++++++++++++++++----------- rpi/Makefile | 8 +- zynq/.gitignore | 1 + zynq/Makefile | 5 +- zynq/cache_cortexa.c | 176 ++++++++++++++++++++++++++ zynq/zynq_printf.h | 81 ++++++++++++ 6 files changed, 487 insertions(+), 72 deletions(-) create mode 100644 zynq/cache_cortexa.c create mode 100644 zynq/zynq_printf.h diff --git a/jazelle.c b/jazelle.c index c9a8f87..ccde2fe 100644 --- a/jazelle.c +++ b/jazelle.c @@ -1,9 +1,45 @@ +#if !defined(__arm__) || (__SIZE_WIDTH__ != 32) +#error "This code is 32-bit ARM only!" +#endif +#if defined(__ARM_ARCH_6M__) || defined(__ARM_ARCH_7EM__) || defined(__ARM_ARCH_8M_MAIN__) \ + || defined(__ARM_ARCH_7R__) || defined(__ARM_ARCH_8R__) +#error "This code is only available on ARM7, ARM9, ARM11 and Cortex-A devices, not on Cortex-M or Cortex-R." +#endif + #include #include #include + +#if defined(__linux__) && !defined(__KERNEL__) + +#include #include #include +#define iprintf(fmt, ...) printf((fmt), ##__VA_ARGS__) + +#elif defined(__KERNEL__) + +#include +#include +#define iprintf(fmt, ...) printk(KERN_ERR " " fmt, ##__VA_ARGS__) +// TODO: is a memcpy impl needed? + +#elif defined(ZYNQ) /* zynq baremetal */ + +#include +#include +#include "xil_printf.h" +#include "zynq_printf.h" +#define iprintf(fmt, ...) printf((fmt), ##__VA_ARGS__) + +#else + +// newlib assumed +#include +#include + +#endif // platform // known ID table: // Chip name | ARM core | ARM CPUID | JTAG IDCODE | Jazelle ID @@ -45,12 +81,66 @@ bytecode IDs that use a handler: */ -#if !defined(__linux__) || defined(_LINUX_KERNEL_H) +#ifdef __linux__ +static bool getstr(char** ap, const char* name, uint32_t* out) { + char* a = *ap, *aa = a; + a = strstr(a, name); if (!a) { /*printf("no name\n");*/ return false; } + a = strstr(a, ": "); if (!a) { /*printf("no colon\n");*/ return false; } + a = a + strlen(": "); + + *out = strtoul(a, &aa, 0); + if (a == aa) { /*printf("no int: %s\n", a);*/ return false; } + + *ap = aa; + return true; +} +#endif + +#if !defined(__linux__) || defined(__KERNEL__) inline #endif static uint32_t arm_get_id(void) { -#if defined(__linux__) && !defined(_LINUX_KERNEL_H) - // TODO: read out /sys/devices/system/cpu/cpu*/regs/identification/midr_el1 or /proc/cpuinfo +#if defined(__linux__) && !defined(__KERNEL__) + // NOTE: better to read out /sys/devices/system/cpu/cpu*/regs/identification/midr_el1 , + // but this one doesn't seem to be available on my rpi running 5.4.x, + // so, /proc/cpuinfo it is + uint32_t midr = 0; + + FILE* f = fopen("/proc/cpuinfo", "rb"); + if (!f) return 0; + void* dest = malloc(4096); // should be enough + size_t rrr = fread(dest, 1, 4096, f); + fclose(f); + if (rrr == 0) goto EXIT; + char* a = (char*)dest; + + uint32_t impl, arch, var, part, rev; + + if (!getstr(&a, "CPU implementer", &impl)) { printf("no impl\n"); goto EXIT; } + if (!getstr(&a, "CPU architecture", &arch)) { printf("no arch\n"); goto EXIT; } + if (!getstr(&a, "CPU variant", &var)) { printf("no var\n"); goto EXIT; } + if (!getstr(&a, "CPU part", &part)) { printf("no part\n"); goto EXIT; } + if (!getstr(&a, "CPU revision", &rev)) { printf("no rev\n"); goto EXIT; } + + midr = (impl << 24) | (arch << 20) | (var << 16) | (part << 4) | (rev << 0); + + do { + // try to read more values, may be useful in eg. a big.LITTLE setup + if (!getstr(&a, "CPU implementer", &impl)) break; + if (!getstr(&a, "CPU architecture", &arch)) break; + if (!getstr(&a, "CPU variant", &var)) break; + if (!getstr(&a, "CPU part", &part)) break; + if (!getstr(&a, "CPU revision", &rev)) break; + + uint32_t m2 = (impl << 24) | (arch << 20) | (var << 16) | (part << 4) | (rev << 0); + if (m2 != midr) { + printf("found different MIDR on other core: 0x%08lx\n", m2); + } + } while (a); + +EXIT: + free(dest); + return midr; #else // NOTE: requires kernel mode execution level to read uint32_t res; @@ -65,40 +155,77 @@ inline static uint32_t jazelle_get_id(void) { return res; } -// TODO: use different implementations when porting to other stuff -#if defined(__linux__) && !defined(_LINUX_KERNEL_H) -#error "TODO: implement cache flushes for Linux userspace" -#elif defined(ARM9) -__attribute__((__naked__)) -static void DC_FlushAll() { +#if defined(__linux__) && !defined(__KERNEL__) +// TODO: better implementation lmao +static void DC_FlushAll(void) { + const size_t size = 65536*4; + // shrug + void* v = (void*)malloc(size); + if (!v) return; // a + for (size_t i = 0; i < size; i += 4) { + volatile uint32_t* p = (volatile uint32_t*)v; + p[i >> 2] = 0; + asm volatile("":::"memory"); + } + free(v); +} +__attribute__((__unused__)) +static void DC_InvalidateAll(void) { + DC_FlushAll(); +} +__attribute__((__naked__, __no_inline__)) +static void IC_InvalidateAll(void) { asm volatile( -" mov r1, #0\n" + ".fill 65536, 4, 0\n" // 64k nop instructions because eh + "bx lr\n" + : + : + :"memory" + ); +} +#elif (__ARM_ARCH == 5 /* ARM9 */) || (__ARM_ARCH == 6 && defined(__ARM_ARCH_6KZ__)) +__attribute__((__naked__)) +static void DC_FlushAll(void) { + // lifted from libnds + asm volatile( +#if __ARM_ARCH == 5 + "mov r1, #0\n" "outer_loop:\n" -" mov r0, #0\n" + "mov r0, #0\n" "inner_loop:\n" -" orr r2, r1, r0 @ generate segment and line address\n" -" mcr p15, 0, r2, c7, c14, 2 @ clean and flush the line\n" -" add r0, r0, #32\n" -" cmp r0, #0x1000/4\n" -" bne inner_loop\n" -" add r1, r1, #0x40000000\n" -" cmp r1, #0\n" -" bne outer_loop\n" - -" mov r0, #0\n" -" mcr p15, 0, r0, c7, c10, 4 @ drain write buffer\n" -" bx lr\n" - :::"memory"); -} -static inline void DC_InvalidateAll() { - asm volatile("mcr p15, 0, %0, c7, c6, 0" : : "r"(0)); -} -static inline void IC_InvalidateAll() { - asm volatile("mcr p15, 0, %0, c7, c5, 0" : : "r"(0)); -} + "orr r2, r1, r0 @ generate segment and line address\n" + "mcr p15, 0, r2, c7, c14, 2 @ clean and flush the line\n" + "add r0, r0, #32\n" // FIXME hardcoded cache line size + "cmp r0, #0x1000/4\n" // FIXME hardcoded cache size + "bne inner_loop\n" + "add r1, r1, #0x40000000\n" + "cmp r1, #0\n" + "bne outer_loop\n" +#elif __ARM_ARCH == 6 && defined(__ARM_ARCH_6KZ__) + "mov r0, #0\n" + "mcr p15, 0, r0, c7, c10, 0\n" // clean & flush entire data cache +#else +#error "wut" +#endif + + "mov r0, #0\n" + "mcr p15, 0, r0, c7, c10, 4 @ drain write buffer\n" // ARM9, ARM11 ok + "bx lr\n" + : + : + :"memory" + ); +} +__attribute__((__unused__)) +static inline void DC_InvalidateAll(void) { + asm volatile("mcr p15, 0, %0, c7, c6, 0" : : "r"(0)); // ARM9, ARM11 ok +} +static inline void IC_InvalidateAll(void) { + asm volatile("mcr p15, 0, %0, c7, c5, 0" : : "r"(0)); // ARM9, ARM11 ok +} +#elif __ARM_ARCH == 7 && defined(__ARM_ARCH_7A__) +#include "cache_cortexa.c" /* go sue me */ #else -// TODO: ARM11 -// TODO: Cortex-A[89] #error "TODO: define data and instruction cache functions for your target!" #endif @@ -111,25 +238,25 @@ static struct { /* * c0: Jazelle Identity register (read-only) - Bits 0-11: Subarchitecture-defined bits (reads as 4, meaning unknown) - Bits 12-19: Subarchitecture (reads as 0, Jazelle V1 according to documentation) - Bits 20-27: Implementor (reads as 0x41, ARM Limited according to documentation) - Bits 28-31: Architecture (reads as 6, ARMv5TEJ according to documentation) - c1: Operating System Control register - Bit 0: Configuration Disabled (CD) (documented) - Bit 1: Configuration Valid (CV) (documented) - c2: Main Configuration register - Bit 0: Jazelle Enable (JE) (documented) - Bits 26-28: Unknown - Bit 29: If set, array object contains its elements directly, otherwise it contains a pointer to its elements - Bit 31: Disable array instructions if set? - c3: Array object layout register - Bits 0-7: Unknown - Bits 8-11: Offset (in words) within array object of first element or of pointer to first element - Bits 12-15: Offset (in words) within array object of length - Bit 16: If set, offset to length is subtracted, otherwise added - Bits 17-19: Array length shift value (number of elements = stored length >> this) - Bits 20-21: Disable array instructions if set? + * Bits 0-11: Subarchitecture-defined bits (reads as 4, meaning unknown) + * Bits 12-19: Subarchitecture (reads as 0, Jazelle V1 according to documentation) + * Bits 20-27: Implementor (reads as 0x41, ARM Limited according to documentation) + * Bits 28-31: Architecture (reads as 6, ARMv5TEJ according to documentation) + * c1: Operating System Control register + * Bit 0: Configuration Disabled (CD) (documented) + * Bit 1: Configuration Valid (CV) (documented) + * c2: Main Configuration register + * Bit 0: Jazelle Enable (JE) (documented) + * Bits 26-28: Unknown + * Bit 29: If set, array object contains its elements directly, otherwise it contains a pointer to its elements + * Bit 31: Disable array instructions if set? + * c3: Array object layout register + * Bits 0-7: Unknown + * Bits 8-11: Offset (in words) within array object of first element or of pointer to first element + * Bits 12-15: Offset (in words) within array object of length + * Bit 16: If set, offset to length is subtracted, otherwise added + * Bits 17-19: Array length shift value (number of elements = stored length >> this) + * Bits 20-21: Disable array instructions if set? */ static uint32_t jazelle_exit_save; @@ -148,6 +275,7 @@ static int jazelle_exec_native(const void* bytecode, const void* block) { "add r6, r5, #0x800\n" "add r7, r5, #0x900\n" // "r8: Pointer to constant pool? (haven't checked this yet)" -Hackspire + // libjz contradicts this... // set configuration valid & jazelle enable bits "mov r0, #2\n" @@ -185,6 +313,24 @@ static int jazelle_exec_native(const void* bytecode, const void* block) { } +extern uint32_t backup_r5; +uint32_t backup_r5 = 0; + +__attribute__((__naked__)) +static void handler_0xfe(void) { + (void)&backup_r5; + asm volatile( + "ldr r0, =backup_r5\n" + "str r5, [r0]\n" + + // return to jazelle (yes lr has to be incremented otherwise the + // current instruction keeps getting executed in a loop) + "add lr, #1\n" + "bxj r12\n" // FIXME: r12 can be modified by jazelle so it should be restored to something + //: + //:[br5]"m"(backup_r5) + ); +} __attribute__((__naked__)) static void handler_idiv(void) { @@ -243,7 +389,6 @@ static void handler_ireturn(void) { :[exsav]"m"(jazelle_exit_save) :"r12" ); - __builtin_unreachable(); } @@ -267,7 +412,6 @@ static void handler_wasexec() { ,[we]"m"(was_exec) :"r0","r12" ); - __builtin_unreachable(); } __attribute__((__naked__)) @@ -280,7 +424,6 @@ static void handler_noexec() { :[exsav]"m"(jazelle_exit_save) :"r0","r12" ); - __builtin_unreachable(); } static uint8_t bytecode_testh[] = { @@ -290,7 +433,7 @@ static uint8_t bytecode_testh[] = { 0x04, 0x04, 0x6C, // +8 0x00, 0x00, 0x00, 0x00, // up to 4 nops of argument bytes eg for invokeXYZ - 0x00, 0x00, 0x00, 0x00, // except 2nd byte is 3 for goto offset (BE) (3 aka iconst_0) + 0x00, 0x00, 0x00, 0x00, // also needs to be modified to make some insns work 0xBA, // +17 // invokedynamic, complex enough to never be implemented in hw 0x00, 0x00, 0x00, 0x00, // up to 4 nops of argument bytes eg for invokeXYZ }; @@ -321,19 +464,20 @@ static void jazelle_test_handlers(uint8_t hflags[256/8]) { } memset(&jazelle_block, 0, sizeof jazelle_block); // initialize local 1 for a return address for the 'ret' opcode - uint32_t retval = &bytecode_testh[17]; + uint32_t retval = (uint32_t)&bytecode_testh[17]; jazelle_block.locals[4] = retval >> 0; jazelle_block.locals[5] = retval >> 8; jazelle_block.locals[6] = retval >>16; jazelle_block.locals[7] = retval >>24; DC_FlushAll(); - DC_InvalidateAll(); + //DC_InvalidateAll(); IC_InvalidateAll(); jazelle_block.handlers[i] = handler_wasexec; jazelle_block.handlers[0xba] = handler_noexec; + //iprintf("bc 0x%02x\r\n", i); was_exec = 0; jazelle_exec_native(bytecode_testh, &jazelle_block); @@ -353,6 +497,7 @@ static uint8_t bytecode_test1[] = { 0x06, // iconst_3 0x07, // iconst_4 0x05, // iconst_2 + 0xFE, // ??? (r5 readout) 0x6C, // idiv 0x04, // iconst_1 0x60, // iadd @@ -364,17 +509,20 @@ static uint8_t bytecode_test1[] = { static int jazelle_exec(const uint8_t* bytecode) { jazelle_block.handlers[0x6C] = handler_idiv; jazelle_block.handlers[0xAC] = handler_ireturn; -/* - * +000-3FF: Unhandled bytecodes - The stack is flushed to memory before calling any of these handlers, so they may modify r0-r3 freely - +400: Null pointer exception - +404: Array index out of bounds exception - +40C: Jazelle mode entered with JE = 0 - +410: Configuration invalid (Jazelle mode entered with CV = 0) - CV is automatically set to 1 on entering this handler - +414: Prefetch abort occurred in middle of instruction - -Hackspire - */ + jazelle_block.handlers[0xFE] = handler_0xfe; + /* + * +000-3FF: Unhandled bytecodes + * The stack is flushed to memory before calling any of these handlers, so + * they may modify r0-r3 freely + * +400: Null pointer exception + * +404: Array index out of bounds exception + * +40C: Jazelle mode entered with JE = 0 + * +410: Configuration invalid (Jazelle mode entered with CV = 0) + * CV is automatically set to 1 on entering this handler + * +414: Prefetch abort occurred in middle of instruction + * + * -Hackspire + */ const void* block = &jazelle_block; return jazelle_exec_native(bytecode, block); @@ -387,7 +535,7 @@ void jazelle_main(void) { while (jid == 0) ; int r = jazelle_exec(bytecode_test1); - iprintf("retcode=%d\r\n", r); + iprintf("retcode=%d; r5 was 0x%08lx\r\n", r, backup_r5); if (r == 0) { static uint8_t hflags[256/8]; diff --git a/rpi/Makefile b/rpi/Makefile index e867269..078709a 100644 --- a/rpi/Makefile +++ b/rpi/Makefile @@ -5,7 +5,7 @@ OPENOCD_PORT_TELNET ?= 4444 PREFIX ?= arm-none-eabi- CC = $(PREFIX)gcc -CFLAGS = -mcpu=arm1176jzf-s +CFLAGS = -mcpu=arm1176jzf-s -Wall LDFLAGS = -T rpi.ld -nostartfiles -nostdlib GDB ?= $(PREFIX)gdb PYTHON3 ?= python3 @@ -21,6 +21,12 @@ rpi.o: rpi.c rpi.elf: rpi.o $(CC) $(CFLAGS) $(LDFLAGS) -o "$@" $^ +#jazelle.S: ../jazelle.c +# $(CC) $(CFLAGS) -S -o "$@" "$<" + +jazelle.o: ../jazelle.c + $(CC) $(CFLAGS) -c -o "$@" "$<" + openocd-launch: openocd -f interface/cmsis-dap.cfg -c "transport select jtag" \ -c "adapter speed 50" -f target/bcm2835.cfg \ diff --git a/zynq/.gitignore b/zynq/.gitignore index 4136b11..6f2a286 100644 --- a/zynq/.gitignore +++ b/zynq/.gitignore @@ -1,3 +1,4 @@ *.elf *.o __pycache__/ +xil_printf.h diff --git a/zynq/Makefile b/zynq/Makefile index 823143b..105728f 100644 --- a/zynq/Makefile +++ b/zynq/Makefile @@ -1,7 +1,7 @@ PREFIX ?= arm-none-eabi- CC = $(PREFIX)gcc -CFLAGS = -mcpu=cortex-a9 +CFLAGS = -mcpu=cortex-a9 -I. -DZYNQ -Wall LDFLAGS = -T zynq.ld -nostartfiles -nostdlib GDB ?= $(PREFIX)gdb PYTHON3 ?= python3 @@ -21,6 +21,9 @@ zynq.o: zynq.c zynq.elf: zynq.o $(CC) $(CFLAGS) $(LDFLAGS) -o "$@" $^ +jazelle.o: ../jazelle.c cache_cortexa.c zynq_printf.h + $(CC) $(CFLAGS) -c -o "$@" "$<" + openocd-load: zynq.elf { $(PYTHON3) ./elf2oocd.py "$<"; echo exit; } | $(NC) $(OPENOCD_HOST) $(OPENOCD_PORT_TELNET) # printf 'halt\nload_image zynq.elf\nexit\n' \ diff --git a/zynq/cache_cortexa.c b/zynq/cache_cortexa.c new file mode 100644 index 0000000..d1d1510 --- /dev/null +++ b/zynq/cache_cortexa.c @@ -0,0 +1,176 @@ + +// stolen from +// https://github.com/Xilinx/embeddedsw/blob/master/lib/bsp/standalone/src/arm/cortexa9/xil_cache.c + +#define IRQ_FIQ_MASK 0xC0 + +// CP15_CACHE_SIZE_SEL, CP15_CACHE_SIZE_ID, CP15_INVAL_IC_POU +// asm_cp15_inval_dc_line_sw +// asm_cp15_clean_inval_dc_line_sw + +#define XREG_CP15_CACHE_SIZE_SEL "p15, 2, %0, c0, c0, 0" +#define XREG_CP15_CACHE_SIZE_ID "p15, 1, %0, c0, c0, 0" +#define XREG_CP15_INVAL_IC_POU "p15, 0, %0, c7, c5, 0" +#define XREG_CP15_INVAL_DC_LINE_SW "p15, 0, %0, c7, c6, 2" +#define XREG_CP15_CLEAN_INVAL_DC_LINE_SW "p15, 0, %0, c7, c14, 2" + + +#define asm_cp15_inval_dc_line_sw(param) __asm__ __volatile__("mcr " \ + XREG_CP15_INVAL_DC_LINE_SW :: "r" (param)); \ + +#define asm_cp15_clean_inval_dc_line_sw(param) __asm__ __volatile__("mcr " \ + XREG_CP15_CLEAN_INVAL_DC_LINE_SW :: "r" (param)); \ + + +#define dsb() asm volatile("dsb":::"memory") +#define isb() asm volatile("isb":::"memory") + +#define mfcpsr() ({uint32_t rval = 0U; \ + __asm__ __volatile__(\ + "mrs %0, cpsr\n"\ + : "=r" (rval)\ + );\ + rval;\ + }) \ + +#define mtcpsr(v) __asm__ __volatile__(\ + "msr cpsr,%0\n"\ + : : "r" (v) : "cc" \ + ) \ + +/* CP15 operations */ +#define mtcp(rn, v) __asm__ __volatile__(\ + "mcr " rn "\n"\ + : : "r" (v)\ + ); \ + +#define mfcp(rn) ({uint32_t rval = 0U; \ + __asm__ __volatile__(\ + "mrc " rn "\n"\ + : "=r" (rval)\ + );\ + rval;\ + }) \ + + +__attribute__((__unused__)) +static void DC_InvalidateAll(void) { + register uint32_t CsidReg, C7Reg; + uint32_t CacheSize, LineSize, NumWays; + uint32_t Way, WayIndex, Set, SetIndex, NumSet; + uint32_t currmask; + + currmask = mfcpsr(); + mtcpsr(currmask | IRQ_FIQ_MASK); + + /* Select cache level 0 and D cache in CSSR */ + mtcp(XREG_CP15_CACHE_SIZE_SEL, 0U); + + CsidReg = mfcp(XREG_CP15_CACHE_SIZE_ID); + /* Determine Cache Size */ + CacheSize = (CsidReg >> 13U) & 0x1FFU; + CacheSize +=1U; + CacheSize *=128U; /* to get number of bytes */ + + /* Number of Ways */ + NumWays = (CsidReg & 0x3ffU) >> 3U; + NumWays += 1U; + + /* Get the cacheline size, way size, index size from csidr */ + LineSize = (CsidReg & 0x07U) + 4U; + + NumSet = CacheSize/NumWays; + NumSet /= (0x00000001U << LineSize); + + Way = 0U; + Set = 0U; + + /* Invalidate all the cachelines */ + for (WayIndex =0U; WayIndex < NumWays; WayIndex++) { + for (SetIndex =0U; SetIndex < NumSet; SetIndex++) { + C7Reg = Way | Set; + + /* Invalidate by Set/Way */ + asm_cp15_inval_dc_line_sw(C7Reg); + Set += (0x00000001U << LineSize); + } + Set=0U; + Way += 0x40000000U; + } + + /* Wait for L1 invalidate to complete */ + dsb(); + mtcpsr(currmask); +} + +static void DC_FlushAll(void) { + register uint32_t CsidReg, C7Reg; + uint32_t CacheSize, LineSize, NumWays; + uint32_t Way; + uint32_t WayIndex, Set, SetIndex, NumSet; + uint32_t currmask; + + currmask = mfcpsr(); + mtcpsr(currmask | IRQ_FIQ_MASK); + + /* Select cache level 0 and D cache in CSSR */ + mtcp(XREG_CP15_CACHE_SIZE_SEL, 0); + + CsidReg = mfcp(XREG_CP15_CACHE_SIZE_ID); + + /* Determine Cache Size */ + + CacheSize = (CsidReg >> 13U) & 0x1FFU; + CacheSize +=1U; + CacheSize *=128U; /* to get number of bytes */ + + /* Number of Ways */ + NumWays = (CsidReg & 0x3ffU) >> 3U; + NumWays += 1U; + + /* Get the cacheline size, way size, index size from csidr */ + LineSize = (CsidReg & 0x07U) + 4U; + + NumSet = CacheSize/NumWays; + NumSet /= (0x00000001U << LineSize); + + Way = 0U; + Set = 0U; + + /* Invalidate all the cachelines */ + for (WayIndex =0U; WayIndex < NumWays; WayIndex++) { + for (SetIndex =0U; SetIndex < NumSet; SetIndex++) { + C7Reg = Way | Set; + /* Flush by Set/Way */ + + asm_cp15_clean_inval_dc_line_sw(C7Reg); + Set += (0x00000001U << LineSize); + } + Set = 0U; + Way += 0x40000000U; + } + + /* Wait for L1 flush to complete */ + dsb(); + mtcpsr(currmask); +} + +static void IC_InvalidateAll(void) { + mtcp(XREG_CP15_CACHE_SIZE_SEL, 1U); + /* invalidate the instruction cache */ + mtcp(XREG_CP15_INVAL_IC_POU, 0U); + + /* Wait for L1 invalidate to complete */ + dsb(); + isb(); +} + +#undef dsb +#undef mfcpsr +#undef mtcpsr +#undef mtcp +#undef mfcp +#undef asm_cp15_inval_dc_line_sw +#undef asm_cp15_clean_inval_dc_line_sw +#undef IRQ_FIQ_MASK + diff --git a/zynq/zynq_printf.h b/zynq/zynq_printf.h new file mode 100644 index 0000000..b89df58 --- /dev/null +++ b/zynq/zynq_printf.h @@ -0,0 +1,81 @@ + +// because xil_printf's behavior isn't very portable, and the xilinx bare-metal +// libraries don't provide a printf() implementation, we will make one here +// using xil_printf under the hood + +#include +#include +#include + +#define XIL_NEWLINE "\n\r" /* yeah... wat */ + +static char* pbuf = NULL; +static int psize = 0; + +int vdprintf(int fd, const char* fmt, va_list args) { + (void)fd; + + if (pbuf == NULL) { + psize = 80; // initial value: "default" terminal width + pbuf = malloc(psize); + } + + int r; + do { + r = vsnprintf(pbuf, psize, fmt, args); + if (r < 0) return r; + + if (r < psize - 1) // it fits into the buffer, so we can stop + break; + + // make buffer larger, and try again + psize <<= 1; + pbuf = realloc(pbuf, psize); + } while (1); + + // print line by line + char* start = pbuf; + for (char* c = pbuf; *c && (ptrdiff_t)c < (ptrdiff_t)pbuf + r; ++c) { + if (*c == '\n') { + *c = '\0'; + xil_printf("%s" XIL_NEWLINE, start); + start = c + 1; + } + } + if (*start && (ptrdiff_t)start < (ptrdiff_t)pbuf + r) { + xil_printf("%s", start); + } + + return r; +} +int dprintf(int fd, const char* fmt, ...) { + va_list args; + va_start(args, fmt); + int r = vdprintf(fd, fmt, args); + va_end(args); + return r; +} + +int vprintf(const char* fmt, va_list args) { + return vdprintf(1 /*STDOUT_FILENO*/, fmt, args); +} +int printf(const char* fmt, ...) { + va_list args; + va_start(args, fmt); + int r = vprintf(fmt, args); + va_end(args); + return r; +} + +int vfprintf(FILE* f, const char* fmt, va_list args) { + (void)f; + return vdprintf(/*fileno(f)*/ 1, fmt, args); +} +int fprintf(FILE* f, const char* fmt, ...) { + va_list args; + va_start(args, fmt); + int r = vfprintf(f, fmt, args); + va_end(args); + return r; +} +