diff --git a/.gitmodules b/.gitmodules
index 3162054..7c1e3cd 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,3 @@
-[submodule "raspi_fw"]
-	path = raspi_fw
+[submodule "third-party/raspi-firmware"]
+	path = third-party/raspi-firmware
 	url = https://github.com/raspberrypi/firmware
diff --git a/Makefile b/Makefile
index 6d42cc3..94b0924 100644
--- a/Makefile
+++ b/Makefile
@@ -1,38 +1,83 @@
-CC		:= $(PREFIX)gcc
-AS		:= $(PREFIX)as
-OBJCPY		:= $(PREFIX)objcopy
-boot.o: src/boot.S
-	$(AS) -c src/boot.S -o boot.o
+CC		:= clang
+OBJCPY		:= llvm-objcopy
+LD		:= ld.lld
 
-kernel.o: src/kernel.c
-	$(CC) -ffreestanding -c src/kernel.c -o kernel.o -O2 -Wall -Wextra
+BUILD_DIR = build
+SRC_DIR = src
 
-os.elf: boot.o kernel.o src/linker.ld
-	$(CC) -T src/linker.ld -o os.elf -ffreestanding -O2 -nostdlib boot.o kernel.o -lgcc
+GENERAL_OPTIONS = 
 
-kernel8.img: os.elf
-	$(OBJCPY) os.elf -O binary kernel8.img
+CLANGOPS = -Wall -nostdlib -ffreestanding -mgeneral-regs-only -Iinclude -mcpu=cortex-a72+nosimd --target=aarch64-elf
+ASMOPS = $(CLANGOPS)
+COPS = $(CLANGOPS)
 
-.PHONY: image kernel
+C_FILES = $(wildcard $(SRC_DIR)/*.c)
+ASM_FILES = $(wildcard $(SRC_DIR)/*.S)
+OBJ_FILES = $(C_FILES:$(SRC_DIR)/%.c=$(BUILD_DIR)/%_c.o)
+OBJ_FILES += $(ASM_FILES:$(SRC_DIR)/%.S=$(BUILD_DIR)/%_s.o)
 
-image: kernel8.img
+ifndef VERBOSE
+    VERB := @
+endif
 
-kernel: os.elf
+$(BUILD_DIR)/%_c.o: $(SRC_DIR)/%.c
+	$(VERB) echo Compiling $<
+	$(VERB) mkdir -p $(@D)
+	$(VERB) $(CC) $(COPS) -MMD -c $< -o $@
 
-real_hardware: kernel8.img
-	mkdir -p staging
-	cp -v raspi_fw/boot/bcm2710-rpi-3-b.dtb staging
-	cp -v raspi_fw/boot/bcm2710-rpi-3-b-plus.dtb staging
-	cp -v raspi_fw/boot/bcm2710-rpi-cm3.dtb staging
-	cp -v raspi_fw/boot/bcm2711-rpi-4-b.dtb staging
-	cp -v raspi_fw/boot/bcm2711-rpi-400.dtb staging
-	cp -v raspi_fw/boot/bcm2711-rpi-cm4.dtb staging
-	cp -v raspi_fw/boot/bcm2711-rpi-cm4s.dtb staging
-	cp -v raspi_fw/boot/*.dat staging
-	cp -v raspi_fw/boot/*.elf staging
-	cp -v raspi_fw/boot/bootcode.bin staging
-	cp -v kernel8.img staging
+$(BUILD_DIR)/%_s.o: $(SRC_DIR)/%.S
+	$(VERB) echo Compiling $<
+	$(VERB) mkdir -p $(@D)
+	$(VERB) $(CC) $(ASMOPS) -MMD -c $< -o $@
 
+$(BUILD_DIR)/kernel8.elf: $(SRC_DIR)/linker.ld $(OBJ_FILES)
+	$(VERB) echo Linking kernel8.elf
+	$(VERB) $(LD) -T $(SRC_DIR)/linker.ld -o $(BUILD_DIR)/kernel8.elf  $(OBJ_FILES)
+	
+$(BUILD_DIR)/kernel8.img: $(BUILD_DIR)/kernel8.elf
+	$(VERB) echo Building kernel8.img
+	$(VERB) $(OBJCPY) $(BUILD_DIR)/kernel8.elf -O binary $(BUILD_DIR)/kernel8.img
+
+.PHONY: image kernel real_hardware
+
+flat: $(BUILD_DIR)/kernel8.img
+
+kernel: $(BUILD_DIR)/kernel8.elf
+
+$(BUILD_DIR)/r4.img: $(BUILD_DIR)/kernel8.img src/config.txt third-party/raspi-firmware/boot/*
+	$(VERB) echo Building the image
+	
+	$(VERB) echo -- Making the image file \($(BUILD_DIR)/tmp.img\)
+	$(VERB) dd if=/dev/zero of=$(BUILD_DIR)/tmp.img count=64 bs=1M
+	$(VERB) echo -e "unit: sectors\n/dev/hdc1 : Id=0c" | sfdisk $(BUILD_DIR)/tmp.img > /dev/null
+	$(VERB) mkfs.vfat -F 32 $(BUILD_DIR)/tmp.img > /dev/null
+	
+	$(VERB) echo -- Copying files to `$(BUILD_DIR)/staging`
+	$(VERB) mkdir -p $(BUILD_DIR)/staging
+	$(VERB) cp third-party/raspi-firmware/boot/bcm2710-rpi-3-b.dtb $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/bcm2710-rpi-3-b-plus.dtb $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/bcm2710-rpi-cm3.dtb $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/bcm2711-rpi-4-b.dtb $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/bcm2711-rpi-400.dtb $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/bcm2711-rpi-cm4.dtb $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/bcm2711-rpi-cm4s.dtb $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/*.dat $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/*.elf $(BUILD_DIR)/staging/
+	$(VERB) cp third-party/raspi-firmware/boot/bootcode.bin $(BUILD_DIR)/staging/
+	$(VERB) cp src/config.txt $(BUILD_DIR)/staging/
+	$(VERB) cp $(BUILD_DIR)/kernel8.img $(BUILD_DIR)/staging/kernel8.img
+	
+	$(VERB) echo -- Gzipping the kernel
+	$(VERB) gzip $(BUILD_DIR)/staging/kernel8.img
+	$(VERB) mv $(BUILD_DIR)/staging/kernel8.img.gz $(BUILD_DIR)/staging/kernel8.img
+	
+	$(VERB) echo -- Copying files into the image
+	$(VERB) mcopy -i $(BUILD_DIR)/tmp.img $(BUILD_DIR)/staging/* ::/	
+
+	$(VERB) mv $(BUILD_DIR)/tmp.img $(BUILD_DIR)/r4.img
+	$(VERB) echo Done!
+
+real_hardware: $(BUILD_DIR)/r4.img
+	
 clean:
-	rm boot.o kernel.o os.elf kernel8.img
-	rm -rf staging
+	rm -rf $(BUILD_DIR) *.img
diff --git a/include/mini_uart.h b/include/mini_uart.h
new file mode 100644
index 0000000..d3bd2dc
--- /dev/null
+++ b/include/mini_uart.h
@@ -0,0 +1,9 @@
+#ifndef	_MINI_UART_H
+#define	_MINI_UART_H
+
+void uart_init ( void );
+char uart_recv ( void );
+void uart_send ( char c );
+void uart_send_string(char* str);
+
+#endif  /*_MINI_UART_H */
diff --git a/include/mm.h b/include/mm.h
new file mode 100644
index 0000000..784ac4b
--- /dev/null
+++ b/include/mm.h
@@ -0,0 +1,19 @@
+#ifndef	_MM_H
+#define	_MM_H
+
+#define PAGE_SHIFT	 		12
+#define TABLE_SHIFT 			9
+#define SECTION_SHIFT			(PAGE_SHIFT + TABLE_SHIFT)
+
+#define PAGE_SIZE   			(1 << PAGE_SHIFT)	
+#define SECTION_SIZE			(1 << SECTION_SHIFT)	
+
+#define LOW_MEMORY              	(2 * SECTION_SIZE)
+
+#ifndef __ASSEMBLER__
+
+void memzero(unsigned long src, unsigned long n);
+
+#endif
+
+#endif  /*_MM_H */
diff --git a/src/mmio.h b/include/mmio.h
similarity index 100%
rename from src/mmio.h
rename to include/mmio.h
diff --git a/include/peripherals/base.h b/include/peripherals/base.h
new file mode 100644
index 0000000..a055708
--- /dev/null
+++ b/include/peripherals/base.h
@@ -0,0 +1,6 @@
+#ifndef	_P_BASE_H
+#define	_P_BASE_H
+
+#define PBASE 0xFE000000
+
+#endif  /*_P_BASE_H */
diff --git a/include/peripherals/gpio.h b/include/peripherals/gpio.h
new file mode 100644
index 0000000..5e9af26
--- /dev/null
+++ b/include/peripherals/gpio.h
@@ -0,0 +1,12 @@
+#ifndef	_P_GPIO_H
+#define	_P_GPIO_H
+
+#include "peripherals/base.h"
+
+#define GPFSEL1         (PBASE+0x00200004)
+#define GPSET0          (PBASE+0x0020001C)
+#define GPCLR0          (PBASE+0x00200028)
+#define GPPUD           (PBASE+0x00200094)
+#define GPPUDCLK0       (PBASE+0x00200098)
+
+#endif  /*_P_GPIO_H */
diff --git a/include/peripherals/mini_uart.h b/include/peripherals/mini_uart.h
new file mode 100644
index 0000000..af95a3b
--- /dev/null
+++ b/include/peripherals/mini_uart.h
@@ -0,0 +1,19 @@
+#ifndef	_P_MINI_UART_H
+#define	_P_MINI_UART_H
+
+#include "peripherals/base.h"
+
+#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)
+
+#endif  /*_P_MINI_UART_H */
diff --git a/src/uart.h b/include/uart.h
similarity index 100%
rename from src/uart.h
rename to include/uart.h
diff --git a/include/utils.h b/include/utils.h
new file mode 100644
index 0000000..a23ae37
--- /dev/null
+++ b/include/utils.h
@@ -0,0 +1,8 @@
+#ifndef	_BOOT_H
+#define	_BOOT_H
+
+extern void delay ( unsigned long);
+extern void put32 ( unsigned long, unsigned int );
+extern unsigned int get32 ( unsigned long );
+
+#endif  /*_BOOT_H */
diff --git a/src/boot.S b/src/boot.S
index b0a7729..4c61785 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -1,33 +1,24 @@
-// AArch64 mode
 
-// To keep this in the first portion of the binary.
+#include "mm.h"
+
 .section ".text.boot"
 
-// Make _start global.
-.globl _start
-
-// Entry point for the kernel. Registers:
-// x0 -> 32 bit pointer to DTB in memory (primary core only) / 0 (secondary cores)
-// x1 -> 0
-// x2 -> 0
-// x3 -> 0
-// x4 -> 32 bit kernel entry point, _start location
+.global _start
 _start:
-    // set stack before our code
-    ldr     x5, =_start
-    mov     sp, x5
+	mrs	x0, mpidr_el1		
+	and	x0, x0,#0xFF		// Check processor id
+	cbz	x0, master		// Hang for all non-primary CPU
+	b	proc_hang
 
-    // clear bss
-    ldr     x5, =__bss_start
-    ldr     w6, =__bss_size
-1:  cbz     w6, 2f
-    str     xzr, [x5], #8
-    sub     w6, w6, #1
-    cbnz    w6, 1b
+proc_hang: 
+	b 	proc_hang
 
-    // jump to C code, should not return
-2:  bl      kernel_main
-    // for failsafe, halt this core
-halt:
-    wfe
-    b halt
+master:
+	adr	x0, bss_begin
+	adr	x1, bss_end
+	sub	x1, x1, x0
+	bl 	memzero
+
+	mov	sp, #LOW_MEMORY 
+	bl	kernel_main
+	b 	proc_hang		// should never come here
diff --git a/src/config.txt b/src/config.txt
new file mode 100644
index 0000000..df34f9b
--- /dev/null
+++ b/src/config.txt
@@ -0,0 +1,3 @@
+disable_commandline_tags=1
+core_freq_min=500
+
diff --git a/src/kernel.c b/src/kernel.c
index a570b75..259f5d4 100644
--- a/src/kernel.c
+++ b/src/kernel.c
@@ -1,45 +1,11 @@
-#include <stddef.h>
-#include <stdint.h>
-#include "mmio.h"
-#include "uart.h"
+#include "mini_uart.h"
 
-volatile uint64_t** core_1 = (void*)0xE0;
-volatile uint64_t** core_2 = (void*)0xE8;
-volatile uint64_t** core_3 = (void*)0xF0;
-
-
-void _start_core_1(void* addr) {
-    *core_1 = (uint64_t*)addr;
-}
-
-
-void _start_core_2(void* addr) {
-    *core_2 = (uint64_t*)addr;
-}
-
-
-void _start_core_3(void* addr) {
-    *core_3 = (uint64_t*)addr;
-}
-
-void kernel_main(uint64_t dtb_ptr32, uint64_t x1, uint64_t x2, uint64_t x3) {
-    uint32_t reg;
-    char *board;
-
-    asm volatile ("mrs %x0, midr_el1" : "=r" (reg));
-
-    switch ((reg >> 4) & 0xFFF) {
-        case 0xD03:
-            board = "Raspberry Pi 3\0";
-            MMIO_BASE = 0x3F000000;
-            break;
-        case 0xD08:
-            board = "Raspberry Pi 4\0";
-            MMIO_BASE = 0xFE000000;
-            break;
-    }
+void kernel_main(void)
+{
     uart_init();
-    uart_puts(board);
+    uart_send_string("Hello, world!\r\n");
     
-    return;
+    while (1) {
+        uart_send(uart_recv() + 1);
+    }
 }
diff --git a/src/linker.ld b/src/linker.ld
index 3776a92..b66baac 100644
--- a/src/linker.ld
+++ b/src/linker.ld
@@ -1,44 +1,12 @@
-ENTRY(_start)
-
 SECTIONS
 {
-    /* Starts at LOADER_ADDR. */
     . = 0x80000;
-    /* For AArch64, use . = 0x80000; */
-    __start = .;
-    __text_start = .;
-    .text :
-    {
-        KEEP(*(.text.boot))
-        *(.text)
-    }
-    . = ALIGN(4096); /* align to page size */
-    __text_end = .;
-
-    __rodata_start = .;
-    .rodata :
-    {
-        *(.rodata)
-    }
-    . = ALIGN(4096); /* align to page size */
-    __rodata_end = .;
-
-    __data_start = .;
-    .data :
-    {
-        *(.data)
-    }
-    . = ALIGN(4096); /* align to page size */
-    __data_end = .;
-
-    __bss_start = .;
-    .bss :
-    {
-        bss = .;
-        *(.bss)
-    }
-    . = ALIGN(4096); /* align to page size */
-    __bss_end = .;
-    __bss_size = __bss_end - __bss_start;
-    __end = .;
+    .text.boot : { *(.text.boot) }
+    .text :  { *(.text) }
+    .rodata : { *(.rodata*) }
+    .data : { *(.data) }
+    . = ALIGN(0x8);
+    bss_begin = .;
+    .bss : { *(.bss*) } 
+    bss_end = .;
 }
diff --git a/src/mini_uart.c b/src/mini_uart.c
new file mode 100644
index 0000000..ae4c32a
--- /dev/null
+++ b/src/mini_uart.c
@@ -0,0 +1,56 @@
+#include "utils.h"
+#include "peripherals/mini_uart.h"
+#include "peripherals/gpio.h"
+
+void uart_send ( char c )
+{
+	while(1) {
+		if(get32(AUX_MU_LSR_REG)&0x20) 
+			break;
+	}
+	put32(AUX_MU_IO_REG,c);
+}
+
+char uart_recv ( void )
+{
+	while(1) {
+		if(get32(AUX_MU_LSR_REG)&0x01) 
+			break;
+	}
+	return(get32(AUX_MU_IO_REG)&0xFF);
+}
+
+void uart_send_string(char* str)
+{
+	for (int i = 0; str[i] != '\0'; i ++) {
+		uart_send((char)str[i]);
+	}
+}
+
+void uart_init ( void )
+{
+	unsigned int selector;
+
+	selector = get32(GPFSEL1);
+	selector &= ~(7<<12);                   // clean gpio14
+	selector |= 2<<12;                      // set alt5 for gpio14
+	selector &= ~(7<<15);                   // clean gpio15
+	selector |= 2<<15;                      // set alt5 for gpio15
+	put32(GPFSEL1,selector);
+
+	put32(GPPUD,0);
+	delay(150);
+	put32(GPPUDCLK0,(1<<14)|(1<<15));
+	delay(150);
+	put32(GPPUDCLK0,0);
+
+	put32(AUX_ENABLES,1);                   //Enable mini uart (this also enables access to its registers)
+	put32(AUX_MU_CNTL_REG,0);               //Disable auto flow control and disable receiver and transmitter (for now)
+	put32(AUX_MU_IER_REG,0);                //Disable receive and transmit interrupts
+	put32(AUX_MU_LCR_REG,3);                //Enable 8 bit mode
+	put32(AUX_MU_MCR_REG,0);                //Set RTS line to be always high
+	put32(AUX_MU_BAUD_REG,((500000000/(115200*8))-1));             //Set baud rate to 115200
+    put32(AUX_MU_IIR_REG,0);
+
+	put32(AUX_MU_CNTL_REG,3);               //Finally, enable transmitter and receiver
+}
diff --git a/src/mm.S b/src/mm.S
new file mode 100644
index 0000000..1bd32ff
--- /dev/null
+++ b/src/mm.S
@@ -0,0 +1,6 @@
+.globl memzero
+memzero:
+	str xzr, [x0], #8
+	subs x1, x1, #8
+	b.gt memzero
+	ret
diff --git a/src/utils.S b/src/utils.S
new file mode 100644
index 0000000..c35527a
--- /dev/null
+++ b/src/utils.S
@@ -0,0 +1,15 @@
+.globl put32
+put32:
+	str w1,[x0]
+	ret
+
+.globl get32
+get32:
+	ldr w0,[x0]
+	ret
+
+.globl delay
+delay:
+	subs x0, x0, #1
+	bne delay
+	ret
diff --git a/raspi_fw b/third-party/raspi-firmware
similarity index 100%
rename from raspi_fw
rename to third-party/raspi-firmware