rpi start. doesn't work

6 files changed, 405 insertions, 0 deletions

diff --git a/hw/mcu/broadcom/bcm2711/boot.s b/hw/mcu/broadcom/bcm2711/boot.s
new file mode 100644
index 000000000..076b3a76a
--- /dev/null
+++ b/hw/mcu/broadcom/bcm2711/boot.s
@@ -0,0 +1,170 @@
+.section ".text.boot"  // Make sure the linker puts this at the start of the kernel image
+
+.global _start  // Execution starts here
+
+_start:
+    // Check processor ID is zero (executing on main core), else hang
+    mrs     x1, mpidr_el1
+    and     x1, x1, #3
+    cbz     x1, 2f
+    // We're not on the main core, so hang in an infinite wait loop
+1:  wfe
+    b       1b
+2:  // We're on the main core!
+
+    // Set stack to start below our code
+    ldr     x1, =_start
+    mov     sp, x1
+
+    adr    x0, vectors        // load VBAR_EL1 with virtual
+    // msr    vbar_el3, x0        // vector table address
+    msr    vbar_el1, x0        // vector table address
+    // msr    vbar_el2, x0        // vector table address
+    isb
+
+    // Clean the BSS section
+    ldr     x1, =__bss_start     // Start address
+    ldr     w2, =__bss_size      // Size of the section
+3:  cbz     w2, 4f               // Quit loop if zero
+    str     xzr, [x1], #8
+    sub     w2, w2, #1
+    cbnz    w2, 3b               // Loop if non-zero
+
+    // Jump to our main() routine in C (make sure it doesn't return)
+4:  bl      main
+    // In case it does return, halt the master core too
+    b       1b
+
+.macro  ventry  label
+.align  7
+b   \label
+.endm
+
+.macro handle_invalid_entry type
+irq_entry
+mov x0, #\type
+mrs x1, esr_el1
+mrs x2, elr_el1
+b   err_hang
+.endm
+
+.macro  irq_entry
+sub sp, sp, #272
+stp x0, x1, [sp, #16 * 0]
+stp x2, x3, [sp, #16 * 1]
+stp x4, x5, [sp, #16 * 2]
+stp x6, x7, [sp, #16 * 3]
+stp x8, x9, [sp, #16 * 4]
+stp x10, x11, [sp, #16 * 5]
+stp x12, x13, [sp, #16 * 6]
+stp x14, x15, [sp, #16 * 7]
+stp x16, x17, [sp, #16 * 8]
+stp x18, x19, [sp, #16 * 9]
+stp x20, x21, [sp, #16 * 10]
+stp x22, x23, [sp, #16 * 11]
+stp x24, x25, [sp, #16 * 12]
+stp x26, x27, [sp, #16 * 13]
+stp x28, x29, [sp, #16 * 14]
+str x30, [sp, #16 * 15] 
+.endm
+
+.macro  irq_exit
+ldp x0, x1, [sp, #16 * 0]
+ldp x2, x3, [sp, #16 * 1]
+ldp x4, x5, [sp, #16 * 2]
+ldp x6, x7, [sp, #16 * 3]
+ldp x8, x9, [sp, #16 * 4]
+ldp x10, x11, [sp, #16 * 5]
+ldp x12, x13, [sp, #16 * 6]
+ldp x14, x15, [sp, #16 * 7]
+ldp x16, x17, [sp, #16 * 8]
+ldp x18, x19, [sp, #16 * 9]
+ldp x20, x21, [sp, #16 * 10]
+ldp x22, x23, [sp, #16 * 11]
+ldp x24, x25, [sp, #16 * 12]
+ldp x26, x27, [sp, #16 * 13]
+ldp x28, x29, [sp, #16 * 14]
+ldr x30, [sp, #16 * 15] 
+add sp, sp, #272       
+eret
+.endm
+
+
+/*
+ * Exception vectors.
+ */
+.align  11
+.globl vectors 
+vectors:
+    ventry  sync_invalid_el1t           // Synchronous EL1t
+    ventry  irq_invalid_el1t            // IRQ EL1t
+    ventry  fiq_invalid_el1t            // FIQ EL1t
+    ventry  error_invalid_el1t          // Error EL1t
+
+    ventry  sync_invalid_el1h           // Synchronous EL1h
+    ventry  el1_irq                 // IRQ EL1h
+    ventry  fiq_invalid_el1h            // FIQ EL1h
+    ventry  error_invalid_el1h          // Error EL1h
+
+    ventry  sync_invalid_el0_64         // Synchronous 64-bit EL0
+    ventry  irq_invalid_el0_64          // IRQ 64-bit EL0
+    ventry  fiq_invalid_el0_64          // FIQ 64-bit EL0
+    ventry  error_invalid_el0_64            // Error 64-bit EL0
+
+    ventry  sync_invalid_el0_32         // Synchronous 32-bit EL0
+    ventry  irq_invalid_el0_32          // IRQ 32-bit EL0
+    ventry  fiq_invalid_el0_32          // FIQ 32-bit EL0
+    ventry  error_invalid_el0_32            // Error 32-bit EL0
+
+sync_invalid_el1t:
+    handle_invalid_entry  0
+
+irq_invalid_el1t:
+    handle_invalid_entry  1
+
+fiq_invalid_el1t:
+    handle_invalid_entry  2
+
+error_invalid_el1t:
+    handle_invalid_entry  3
+
+sync_invalid_el1h:
+    handle_invalid_entry  4
+
+fiq_invalid_el1h:
+    handle_invalid_entry  5
+
+error_invalid_el1h:
+    handle_invalid_entry  6
+
+sync_invalid_el0_64:
+    handle_invalid_entry  7
+
+irq_invalid_el0_64:
+    handle_invalid_entry  8
+
+fiq_invalid_el0_64:
+    handle_invalid_entry  9
+
+error_invalid_el0_64:
+    handle_invalid_entry  10
+
+sync_invalid_el0_32:
+    handle_invalid_entry  11
+
+irq_invalid_el0_32:
+    handle_invalid_entry  12
+
+fiq_invalid_el0_32:
+    handle_invalid_entry  13
+
+error_invalid_el0_32:
+    handle_invalid_entry  14
+
+el1_irq:
+    irq_entry 
+    bl  handle_irq
+    irq_exit 
+
+.globl err_hang
+err_hang: b err_hang
diff --git a/hw/mcu/broadcom/bcm2711/interrupts.c b/hw/mcu/broadcom/bcm2711/interrupts.c
new file mode 100644
index 000000000..cb5607526
--- /dev/null
+++ b/hw/mcu/broadcom/bcm2711/interrupts.c
@@ -0,0 +1,26 @@
+#include <stdbool.h>
+#include <stdint.h>
+
+void disable_async_interrupts(void) {
+    
+}
+
+void enable_async_interrupts(void) {
+    
+}
+
+void Default_Handler(void) {
+    while (true) {}
+}
+
+__attribute__((weak)) void handle_irq(void) {
+    unsigned int irq = *((volatile uint32_t*) 0x3F00B204);
+    switch (irq) {
+        // case (SYSTEM_TIMER_IRQ_1):
+        //     handle_timer_irq();
+        //     break;
+        default:
+            // printf("Unknown pending irq: %x\r\n", irq);
+            Default_Handler();
+    }
+}
\ No newline at end of file
diff --git a/hw/mcu/broadcom/bcm2711/interrupts.h b/hw/mcu/broadcom/bcm2711/interrupts.h
new file mode 100644
index 000000000..5f36d6daa
--- /dev/null
+++ b/hw/mcu/broadcom/bcm2711/interrupts.h
@@ -0,0 +1,8 @@
+#pragma once
+
+void disable_async_interrupts(void);
+void enable_async_interrupts(void);
+
+#define GIC_BASE 0x4c0040000
+#define NUM_CPUS 4
+#define NUM_SPIS 192
diff --git a/hw/mcu/broadcom/bcm2711/io.c b/hw/mcu/broadcom/bcm2711/io.c
new file mode 100644
index 000000000..213d6f2fb
--- /dev/null
+++ b/hw/mcu/broadcom/bcm2711/io.c
@@ -0,0 +1,168 @@
+// From: https://github.com/isometimes/rpi4-osdev/blob/master/part4-miniuart/io.c
+// CC-0 License
+
+// GPIO
+
+enum {
+    PERIPHERAL_BASE = 0xFE000000,
+    GPFSEL0         = PERIPHERAL_BASE + 0x200000,
+    GPSET0          = PERIPHERAL_BASE + 0x20001C,
+    GPCLR0          = PERIPHERAL_BASE + 0x200028,
+    GPPUPPDN0       = PERIPHERAL_BASE + 0x2000E4
+};
+
+enum {
+    GPIO_MAX_PIN       = 53,
+    GPIO_FUNCTION_OUT  = 1,
+    GPIO_FUNCTION_ALT5 = 2,
+    GPIO_FUNCTION_ALT3 = 7
+};
+
+enum {
+    Pull_None = 0,
+    Pull_Down = 1, // Are down and up the right way around?
+    Pull_Up = 2
+};
+
+void mmio_write(long reg, unsigned int val) { *(volatile unsigned int *)reg = val; }
+unsigned int mmio_read(long reg) { return *(volatile unsigned int *)reg; }
+
+unsigned int gpio_call(unsigned int pin_number, unsigned int value, unsigned int base, unsigned int field_size, unsigned int field_max) {
+    unsigned int field_mask = (1 << field_size) - 1;
+  
+    if (pin_number > field_max) return 0;
+    if (value > field_mask) return 0; 
+
+    unsigned int num_fields = 32 / field_size;
+    unsigned int reg = base + ((pin_number / num_fields) * 4);
+    unsigned int shift = (pin_number % num_fields) * field_size;
+
+    unsigned int curval = mmio_read(reg);
+    curval &= ~(field_mask << shift);
+    curval |= value << shift;
+    mmio_write(reg, curval);
+
+    return 1;
+}
+
+unsigned int gpio_set     (unsigned int pin_number, unsigned int value) { return gpio_call(pin_number, value, GPSET0, 1, GPIO_MAX_PIN); }
+unsigned int gpio_clear   (unsigned int pin_number, unsigned int value) { return gpio_call(pin_number, value, GPCLR0, 1, GPIO_MAX_PIN); }
+unsigned int gpio_pull    (unsigned int pin_number, unsigned int value) { return gpio_call(pin_number, value, GPPUPPDN0, 2, GPIO_MAX_PIN); }
+unsigned int gpio_function(unsigned int pin_number, unsigned int value) { return gpio_call(pin_number, value, GPFSEL0, 3, GPIO_MAX_PIN); }
+
+void gpio_useAsAlt3(unsigned int pin_number) {
+    gpio_pull(pin_number, Pull_None);
+    gpio_function(pin_number, GPIO_FUNCTION_ALT3);
+}
+
+void gpio_useAsAlt5(unsigned int pin_number) {
+    gpio_pull(pin_number, Pull_None);
+    gpio_function(pin_number, GPIO_FUNCTION_ALT5);
+}
+
+void gpio_initOutputPinWithPullNone(unsigned int pin_number) {
+    gpio_pull(pin_number, Pull_None);
+    gpio_function(pin_number, GPIO_FUNCTION_OUT);
+}
+
+void gpio_setPinOutputBool(unsigned int pin_number, unsigned int onOrOff) {
+    if (onOrOff) {
+        gpio_set(pin_number, 1);
+    } else {
+        gpio_clear(pin_number, 1);
+    }
+}
+
+// UART
+
+enum {
+    AUX_BASE        = PERIPHERAL_BASE + 0x215000,
+    AUX_IRQ         = AUX_BASE,
+    AUX_ENABLES     = AUX_BASE + 4,
+    AUX_MU_IO_REG   = AUX_BASE + 64,
+    AUX_MU_IER_REG  = AUX_BASE + 68,
+    AUX_MU_IIR_REG  = AUX_BASE + 72,
+    AUX_MU_LCR_REG  = AUX_BASE + 76,
+    AUX_MU_MCR_REG  = AUX_BASE + 80,
+    AUX_MU_LSR_REG  = AUX_BASE + 84,
+    AUX_MU_MSR_REG  = AUX_BASE + 88,
+    AUX_MU_SCRATCH  = AUX_BASE + 92,
+    AUX_MU_CNTL_REG = AUX_BASE + 96,
+    AUX_MU_STAT_REG = AUX_BASE + 100,
+    AUX_MU_BAUD_REG = AUX_BASE + 104,
+    AUX_UART_CLOCK  = 500000000,
+    UART_MAX_QUEUE  = 16 * 1024
+};
+
+#define AUX_MU_BAUD(baud) ((AUX_UART_CLOCK/(baud*8))-1)
+
+unsigned char uart_output_queue[UART_MAX_QUEUE];
+unsigned int uart_output_queue_write = 0;
+unsigned int uart_output_queue_read = 0;
+
+void uart_init(void) {
+    mmio_write(AUX_ENABLES, 1); //enable UART1
+    mmio_write(AUX_MU_IER_REG, 0);
+    mmio_write(AUX_MU_CNTL_REG, 0);
+    mmio_write(AUX_MU_LCR_REG, 3); //8 bits
+    mmio_write(AUX_MU_MCR_REG, 0);
+    mmio_write(AUX_MU_IER_REG, 0);
+    mmio_write(AUX_MU_IIR_REG, 0xC6); //disable interrupts
+    mmio_write(AUX_MU_BAUD_REG, AUX_MU_BAUD(115200));
+    gpio_useAsAlt5(14);
+    gpio_useAsAlt5(15);
+    mmio_write(AUX_MU_CNTL_REG, 3); //enable RX/TX
+}
+
+unsigned int uart_isOutputQueueEmpty(void) {
+    return uart_output_queue_read == uart_output_queue_write;
+}
+
+unsigned int uart_isReadByteReady(void)  { return mmio_read(AUX_MU_LSR_REG) & 0x01; }
+unsigned int uart_isWriteByteReady(void) { return mmio_read(AUX_MU_LSR_REG) & 0x20; }
+
+unsigned char uart_readByte(void) {
+    while (!uart_isReadByteReady());
+    return (unsigned char)mmio_read(AUX_MU_IO_REG);
+}
+
+void uart_writeByteBlockingActual(unsigned char ch) {
+    while (!uart_isWriteByteReady()); 
+    mmio_write(AUX_MU_IO_REG, (unsigned int)ch);
+}
+
+void uart_loadOutputFifo(void) {
+    while (!uart_isOutputQueueEmpty() && uart_isWriteByteReady()) {
+        uart_writeByteBlockingActual(uart_output_queue[uart_output_queue_read]);
+        uart_output_queue_read = (uart_output_queue_read + 1) & (UART_MAX_QUEUE - 1); // Don't overrun
+    }
+}
+
+void uart_writeByteBlocking(unsigned char ch) {
+    unsigned int next = (uart_output_queue_write + 1) & (UART_MAX_QUEUE - 1); // Don't overrun
+
+    while (next == uart_output_queue_read) uart_loadOutputFifo();
+
+    uart_output_queue[uart_output_queue_write] = ch;
+    uart_output_queue_write = next;
+}
+
+void uart_writeText(const char *buffer) {
+    while (*buffer) {
+       if (*buffer == '\n') uart_writeByteBlocking('\r');
+       uart_writeByteBlocking(*buffer++);
+    }
+}
+
+void uart_drainOutputQueue(void) {
+    while (!uart_isOutputQueueEmpty()) uart_loadOutputFifo();
+}
+
+void uart_update(void) {
+    uart_loadOutputFifo();
+
+    if (uart_isReadByteReady()) {
+       unsigned char ch = uart_readByte();
+       if (ch == '\r') uart_writeText("\n"); else uart_writeByteBlocking(ch);
+    }
+}
diff --git a/hw/mcu/broadcom/bcm2711/io.h b/hw/mcu/broadcom/bcm2711/io.h
new file mode 100644
index 000000000..9f70ce504
--- /dev/null
+++ b/hw/mcu/broadcom/bcm2711/io.h
@@ -0,0 +1,13 @@
+// From: https://github.com/isometimes/rpi4-osdev/blob/master/part4-miniuart/io.h
+// CC-0 License
+
+void uart_init(void);
+void uart_writeText(const char *buffer);
+void uart_loadOutputFifo(void);
+unsigned char uart_readByte(void);
+unsigned int uart_isReadByteReady(void);
+void uart_writeByteBlocking(unsigned char ch);
+void uart_update(void);
+void gpio_setPinOutputBool(unsigned int pin_number, unsigned int onOrOff);
+void uart_writeByteBlockingActual(unsigned char ch);
+void gpio_initOutputPinWithPullNone(unsigned int pin_number);
diff --git a/hw/mcu/broadcom/bcm2711/link.ld b/hw/mcu/broadcom/bcm2711/link.ld
new file mode 100644
index 000000000..0d5ebe2ab
--- /dev/null
+++ b/hw/mcu/broadcom/bcm2711/link.ld
@@ -0,0 +1,20 @@
+SECTIONS
+{
+    . = 0x80000;     /* Kernel load address for AArch64 */
+    .text : { KEEP(*(.text.boot)) *(.text .text.* .gnu.linkonce.t*) }
+    .rodata : { *(.rodata .rodata.* .gnu.linkonce.r*) }
+    PROVIDE(_data = .);
+    .data : { *(.data .data.* .gnu.linkonce.d*) }
+    .bss (NOLOAD) : {
+        . = ALIGN(16);
+        __bss_start = .;
+        *(.bss .bss.*)
+        *(COMMON)
+        __bss_end = .;
+    }
+    _end = .;
+    end = .;
+
+   /DISCARD/ : { *(.comment) *(.gnu*) *(.note*) *(.eh_frame*) }
+}
+__bss_size = (__bss_end - __bss_start)>>3;