port stuff to rpi and zynq
This commit is contained in:
parent
809ad7cf87
commit
6742aa38e3
254
jazelle.c
254
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 <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#if defined(__linux__) && !defined(__KERNEL__)
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#define iprintf(fmt, ...) printf((fmt), ##__VA_ARGS__)
|
||||
|
||||
#elif defined(__KERNEL__)
|
||||
|
||||
#include <linux/kernel.h>
|
||||
#include <linux/module.h>
|
||||
#define iprintf(fmt, ...) printk(KERN_ERR " " fmt, ##__VA_ARGS__)
|
||||
// TODO: is a memcpy impl needed?
|
||||
|
||||
#elif defined(ZYNQ) /* zynq baremetal */
|
||||
|
||||
#include <stdarg.h>
|
||||
#include <string.h>
|
||||
#include "xil_printf.h"
|
||||
#include "zynq_printf.h"
|
||||
#define iprintf(fmt, ...) printf((fmt), ##__VA_ARGS__)
|
||||
|
||||
#else
|
||||
|
||||
// newlib assumed
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#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(
|
||||
".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"
|
||||
"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"
|
||||
"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"
|
||||
"mcr p15, 0, r0, c7, c10, 4 @ drain write buffer\n" // ARM9, ARM11 ok
|
||||
"bx lr\n"
|
||||
:::"memory");
|
||||
:
|
||||
:
|
||||
:"memory"
|
||||
);
|
||||
}
|
||||
static inline void DC_InvalidateAll() {
|
||||
asm volatile("mcr p15, 0, %0, c7, c6, 0" : : "r"(0));
|
||||
__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() {
|
||||
asm volatile("mcr p15, 0, %0, c7, c5, 0" : : "r"(0));
|
||||
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,16 +509,19 @@ 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;
|
||||
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
|
||||
* 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;
|
||||
|
||||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
*.elf
|
||||
*.o
|
||||
__pycache__/
|
||||
xil_printf.h
|
||||
|
|
|
|||
|
|
@ -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' \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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 <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue