.global _start
.section .text.boot, "ax"

_start:
    mrs x0, mpidr_el1
    and x0, x0, #3

    mov x6, x0
    mov x1, #0x0
    add x2, x1, #0x400000
    lsl x6, x6, #18
    sub x3, x2, x6
    mov sp, x3

    bl uart_init

    mrs x0, mpidr_el1
    and x0, x0, #3
    cbz x0, core0
    cmp x0, #1
    b.eq core1
    cmp x0, #2
    b.eq core2
    cmp x0, #3
    b.eq core3

core0:
    bl print0
    b hang

core1:
    bl print1
    b hang

core2:
    bl print2
    b hang

core3:
    bl print3
    b hang

hang:
    wfi
    b hang

extern void delay(unsigned long);
extern void put32(unsigned long, unsigned int);
extern unsigned int get32(unsigned long);
extern int get_el(void);

#define PBASE 0x3F000000
#define GPFSEL1	  (PBASE + 0x00200004)
#define GPSET0	  (PBASE + 0x0020001C)
#define GPCLR0	  (PBASE + 0x00200028)
#define GPPUD	  (PBASE + 0x00200094)
#define GPPUDCLK0 (PBASE + 0x00200098)

#define AUX_ENABLES	(PBASE + 0x00215004)
#define AUX_MU_IO_REG	(PBASE + 0x00215040)
#define AUX_MU_IER_REG	(PBASE + 0x00215044)
#define AUX_MU_IIR_REG	(PBASE + 0x00215048)
#define AUX_MU_LCR_REG	(PBASE + 0x0021504C)
#define AUX_MU_MCR_REG	(PBASE + 0x00215050)
#define AUX_MU_LSR_REG	(PBASE + 0x00215054)
#define AUX_MU_MSR_REG	(PBASE + 0x00215058)
#define AUX_MU_SCRATCH	(PBASE + 0x0021505C)
#define AUX_MU_CNTL_REG (PBASE + 0x00215060)
#define AUX_MU_STAT_REG (PBASE + 0x00215064)
#define AUX_MU_BAUD_REG (PBASE + 0x00215068)

void uart_send_char(char c)
{
	while (1) {
		if (get32(AUX_MU_LSR_REG) & 0x20)
			break;
	}
	put32(AUX_MU_IO_REG, c);
}

void uart_init(void)
{
	unsigned int selector;

	selector = get32(GPFSEL1);
	selector &= ~(7 << 12);
	selector |= 2 << 12;
	selector &= ~(7 << 15);
	selector |= 2 << 15;
	put32(GPFSEL1, selector);

	put32(GPPUD, 0);
	delay(150);
	put32(GPPUDCLK0, (1 << 14) | (1 << 15));
	delay(150);
	put32(GPPUDCLK0, 0);

	put32(AUX_ENABLES, 1);
	put32(AUX_MU_CNTL_REG, 0);
	put32(AUX_MU_IER_REG, 0);
	put32(AUX_MU_LCR_REG, 3);
	put32(AUX_MU_MCR_REG, 0);
	put32(AUX_MU_BAUD_REG, 270);

	put32(AUX_MU_CNTL_REG, 3);
}

void print0() {
	for(int j = 0; j <= 3; j++) {
		for(int i = 0; i <= 200000000; i++) {
			if (i == 1)
				uart_send_char('0');
		}
	}
}

void print1() {
	for(int j = 0; j <= 3; j++) {
		for(int i = 0; i <= 200000000; i++) {
			if (i == 10000000)
				uart_send_char('1');
		}
	}		
}

void print2() {
	for(int j = 0; j <= 3; j++) {
		for(int i = 0; i <= 200000000; i++) {
			if (i == 90000000)
				uart_send_char('2');
		}
	}		
}

void print3() {
	for(int j = 0; j <= 3; j++) {
		for(int i = 0; i <= 200000000; i++) {
			if (i == 80000000)
				uart_send_char('3');
		}
	}		
}

.globl get_el
get_el:
	mrs x0, CurrentEL
	lsr x0, x0, #2
	ret

.globl put32
put32:
	str w1,[x0]
	ret

.globl get32
get32:
	ldr w0,[x0]
	ret

.globl delay
delay:
	subs x0, x0, #1
	bne delay
	ret

ENTRY(_start)

SECTIONS {
    . = 0x80000;

    .text : {
        *(.text.boot)
        *(.text)
    }

    .data : {
        *(.data)
    }
}