diff --git a/src/firmware.c b/src/firmware.c
diff --git a/src/firmware.h b/src/firmware.h
@@ -29,17 +29,4 @@
 #define PAYLOAD_ADDR 0xCFF00000
-  
-// TODO: Maybe find these with memsearch
-  
-static const pk11_offs _pk11_offs[] = {
-  
-    { KB_FIRMWARE_VERSION_100, 0x1900, 0x3FE0, { 2, 1, 0 }, 0x40014020, 0x8000D000, 1 }, //1.0.0
-  
-    { KB_FIRMWARE_VERSION_200, 0x1900, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, 1 }, //2.0.0 - 2.3.0
-  
-    { KB_FIRMWARE_VERSION_300, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, 1 }, //3.0.0
-  
-    { KB_FIRMWARE_VERSION_301, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, 1 }, //3.0.1 - 3.0.2
-  
-    { KB_FIRMWARE_VERSION_400, 0x1800, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003B000, 0 }, //4.0.0 - 4.1.0
-  
-    { KB_FIRMWARE_VERSION_500, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003B000, 0 }, //5.0.0 - 5.0.2
-  
-    { KB_FIRMWARE_VERSION_600, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003D800, 0 }, //6.0.0
-  
-    { KB_FIRMWARE_VERSION_620, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003D800, 0 }, //6.2.0
-  
-    { 0, 0, 0, 0, 0, 0, 0 } // End.
-  
-};
-  
-
 void firmware();
diff --git a/src/hwinit.h b/src/hwinit.h
@@ -43,5 +43,6 @@
 #include "hwinit/se_t210.h"
 #include "hwinit/mmc.h"
 #include "hwinit/gfx.h"
+  
+#include "hwinit/smmu.h"
-  
-#define DEFAULT_TEXT_COL ORANGE
+  
+#define DEFAULT_TEXT_COL ORANGE
diff --git a/src/hwinit/arm64.h b/src/hwinit/arm64.h
diff --git a/src/hwinit/emc.h b/src/hwinit/emc.h
diff --git a/src/hwinit/heap.c b/src/hwinit/heap.c
@@ -1,48 +1,36 @@
 /*
-  
-* Copyright (c) 2018 naehrwert
-  
-*
-  
-* This program is free software; you can redistribute it and/or modify it
-  
-* under the terms and conditions of the GNU General Public License,
-  
-* version 2, as published by the Free Software Foundation.
-  
-*
-  
-* This program is distributed in the hope it will be useful, but WITHOUT
-  
-* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-  
-* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-  
-* more details.
-  
-*
-  
-* You should have received a copy of the GNU General Public License
-  
-* along with this program.  If not, see <http://www.gnu.org/licenses/>.
-  
-*/
+  
+ * Copyright (c) 2018 naehrwert
+  
+ * Copyright (c) 2018 M4xw
+  
+  * Copyright (c) 2018 Reisyukaku
+  
+ *
+  
+ * This program is free software; you can redistribute it and/or modify it
+  
+ * under the terms and conditions of the GNU General Public License,
+  
+ * version 2, as published by the Free Software Foundation.
+  
+ *
+  
+ * This program is distributed in the hope it will be useful, but WITHOUT
+  
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  
+ * more details.
+  
+ *
+  
+ * You should have received a copy of the GNU General Public License
+  
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+  
+ */
 #include <string.h>
 #include "heap.h"
-  
-typedef struct _hnode
-  
-{
-  
-	int used;
-  
-	u32 size;
-  
-	struct _hnode *prev;
-  
-	struct _hnode *next;
-  
-} hnode_t;
-  
-
-  
-typedef struct _heap
-  
-{
-  
-	u32 start;
-  
-	hnode_t *first;
-  
-} heap_t;
-  
-
 static void _heap_create(heap_t *heap, u32 start)
 {
 	heap->start = start;
 	heap->first = NULL;
 }
-  
-static u32 _heap_alloc(heap_t *heap, u32 size)
+  
+static u32 _heap_alloc(heap_t *heap, u32 size, u32 alignment)
 {
 	hnode_t *node, *new;
 	int search = 1;
-  
-	size = ALIGN(size, 0x10);
+  
+	size = ALIGN(size, alignment);
 	if (!heap->first)
 	{
@@ -110,33 +98,41 @@ static void _heap_free(heap_t *heap, u32 addr)
 	}
 }
-  
-static heap_t _heap;
+  
+heap_t _heap;
-  
-void heap_init(u32 base) {
+  
+void heap_init(u32 base)
+  
+{
 	_heap_create(&_heap, base);
 }
-  
-void *malloc(u32 size) {
-  
-	return (void *)_heap_alloc(&_heap, size);
+  
+void *malloc(u32 size)
+  
+{
+  
+	return (void *)_heap_alloc(&_heap, size, 0x10);
 }
-  
-void *realloc (void * ptr, size_t size) {
-  
-	hnode_t *node = (hnode_t *) ptr - 1;
-  
-	void *new = malloc(size);
-  
-	if (new && ptr) {
-  
-		memcpy(new, ptr, node -> size);
-  
-		free(ptr);
-  
-	}
-  
-	return new;
+  
+void *memalign(u32 align, u32 size)
+  
+{
+  
+	return (void *)_heap_alloc(&_heap, size, align);
 }
-  
-void *calloc(u32 num, u32 size) {
-  
-	void *res = (void *)_heap_alloc(&_heap, num * size);
+  
+void *calloc(u32 num, u32 size)
+  
+{
+  
+	void *res = (void *)_heap_alloc(&_heap, num * size, 0x10);
 	memset(res, 0, num * size);
 	return res;
 }
-  
-void free(void *buf) {
-  
-	if (buf != NULL)
+  
+void free(void *buf)
+  
+{
+  
+	if ((buf != NULL) || ((u32)buf > (_heap.start - 1)))
 		_heap_free(&_heap, (u32)buf);
 }
+  
+void *realloc (void * ptr, size_t size) {
+  
+	hnode_t *node = (hnode_t *) ptr - 1;
+  
+	void *new = malloc(size);
+  
+	if (new && ptr) {
+  
+		memcpy(new, ptr, node -> size);
+  
+		free(ptr);
+  
+	}
+  
+	return new;
+  
+}
diff --git a/src/hwinit/heap.h b/src/hwinit/heap.h
@@ -19,10 +19,25 @@
 #include "types.h"
+  
+typedef struct _hnode
+  
+{
+  
+	int used;
+  
+	u32 size;
+  
+	struct _hnode *prev;
+  
+	struct _hnode *next;
+  
+} hnode_t;
+  
+
+  
+typedef struct _heap
+  
+{
+  
+	u32 start;
+  
+	hnode_t *first;
+  
+} heap_t;
+  
+
 void heap_init(u32 base);
 void *malloc(u32 size);
-  
-void *realloc(void * ptr, size_t size);
 void *calloc(u32 num, u32 size);
 void free(void *buf);
+  
+void *memalign(u32 align, u32 size);
+  
+void *realloc(void * ptr, size_t size);
 #endif
diff --git a/src/hwinit/ini.c b/src/hwinit/ini.c
diff --git a/src/hwinit/ini.h b/src/hwinit/ini.h
diff --git a/src/hwinit/smmu.c b/src/hwinit/smmu.c
@@ -0,0 +1,170 @@
+  
+/*
+  
+ * Copyright (c) 2018 naehrwert
+  
+ * Copyright (c) 2018 balika011
+  
+ *
+  
+ * This program is free software; you can redistribute it and/or modify it
+  
+ * under the terms and conditions of the GNU General Public License,
+  
+ * version 2, as published by the Free Software Foundation.
+  
+ *
+  
+ * This program is distributed in the hope it will be useful, but WITHOUT
+  
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  
+ * more details.
+  
+ *
+  
+ * You should have received a copy of the GNU General Public License
+  
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+  
+ */
+  
+
+  
+#include <string.h>
+  
+
+  
+#include "smmu.h"
+  
+#include "cluster.h"
+  
+#include "t210.h"
+  
+#include "mc_t210.h"
+  
+#include "util.h"
+  
+
+  
+bool smmu_used = false;
+  
+u8 *_pageheap = (u8 *)SMMU_HEAP_ADDR;
+  
+
+  
+//Enabling SMMU requires a TZ secure write: MC(MC_SMMU_CONFIG) = 1;
+  
+u8 smmu_payload[] __attribute__((aligned(16))) = {
+  
+	0x41, 0x01, 0x00, 0x58, // 0x00: LDR  X1, =0x70019010
+  
+	0x20, 0x00, 0x80, 0xD2, // 0x04: MOV  X0, #0x1
+  
+	0x20, 0x00, 0x00, 0xB9, // 0x08: STR  W0, [X1]
+  
+	0x1F, 0x71, 0x08, 0xD5, // 0x0C: IC   IALLUIS
+  
+	0x9F, 0x3B, 0x03, 0xD5, // 0x10: DSB  ISH
+  
+	0xFE, 0xFF, 0xFF, 0x17, // 0x14: B    loop
+  
+	0x00, 0x00, 0x80, 0xD2, // 0x18: MOV  X0, #0x0
+  
+	0x20, 0x00, 0x00, 0xB9, // 0x1C: STR  W0, [X1]
+  
+	0x80, 0x00, 0x00, 0x58, // 0x20: LDR  X0, =0x4002B000
+  
+	0x00, 0x00, 0x1F, 0xD6, // 0x28: BR   X0
+  
+	0x10, 0x90, 0x01, 0x70, // 0x28: MC_SMMU_CONFIG
+  
+	0x00, 0x00, 0x00, 0x00, // 0x2C:
+  
+	0x00, 0x00, 0x00, 0x00, // 0x30: secmon address
+  
+	0x00, 0x00, 0x00, 0x00  // 0x34:
+  
+};
+  
+
+  
+void *page_alloc(u32 num)
+  
+{
+  
+	u8 *res = _pageheap;
+  
+	_pageheap += 0x1000 * num;
+  
+	memset(res, 0, 0x1000 * num);
+  
+	return res;
+  
+}
+  
+
+  
+u32 *smmu_alloc_pdir()
+  
+{
+  
+	u32 *pdir = (u32 *)page_alloc(1);
+  
+	for (int pdn = 0; pdn < SMMU_PDIR_COUNT; pdn++)
+  
+		pdir[pdn] = _PDE_VACANT(pdn);
+  
+	return pdir;
+  
+}
+  
+
+  
+void smmu_flush_regs()
+  
+{
+  
+	(void)MC(MC_SMMU_PTB_DATA);
+  
+}
+  
+
+  
+void smmu_flush_all()
+  
+{
+  
+	MC(MC_SMMU_PTC_FLUSH) = 0;
+  
+	smmu_flush_regs();
+  
+	MC(MC_SMMU_TLB_FLUSH) = 0;
+  
+	smmu_flush_regs();
+  
+}
+  
+
+  
+void smmu_init(u32 secmon_base)
+  
+{
+  
+	MC(MC_SMMU_PTB_ASID) = 0;
+  
+	MC(MC_SMMU_PTB_DATA) = 0;
+  
+	MC(MC_SMMU_TLB_CONFIG) = 0x30000030;
+  
+	MC(MC_SMMU_PTC_CONFIG) = 0x28000F3F;
+  
+	MC(MC_SMMU_PTC_FLUSH) = 0;
+  
+	MC(MC_SMMU_TLB_FLUSH) = 0;
+  
+
+  
+	// Set the secmon address
+  
+	*(u32 *)(smmu_payload + 0x30) = secmon_base;
+  
+}
+  
+
+  
+void smmu_enable()
+  
+{
+  
+	if (smmu_used)
+  
+		return;
+  
+
+  
+	cluster_boot_cpu0((u32)smmu_payload);
+  
+	smmu_used = true;
+  
+	musleep(100);
+  
+
+  
+	smmu_flush_all();
+  
+}
+  
+
+  
+bool smmu_is_used()
+  
+{
+  
+	return smmu_used;
+  
+}
+  
+
+  
+void smmu_exit()
+  
+{
+  
+	*(u32 *)(smmu_payload + 0x14) = _NOP();
+  
+}
+  
+
+  
+u32 *smmu_init_domain4(u32 dev_base, u32 asid)
+  
+{
+  
+	u32 *pdir = smmu_alloc_pdir();
+  
+
+  
+	MC(MC_SMMU_PTB_ASID) = asid;
+  
+	MC(MC_SMMU_PTB_DATA) = SMMU_MK_PDIR((u32)pdir, _PDIR_ATTR);
+  
+	smmu_flush_regs();
+  
+
+  
+	MC(dev_base) = 0x80000000 | (asid << 24) | (asid << 16) | (asid << 8) | (asid);
+  
+	smmu_flush_regs();
+  
+
+  
+	return pdir;
+  
+}
+  
+
+  
+u32 *smmu_get_pte(u32 *pdir, u32 iova)
+  
+{
+  
+	u32 ptn = SMMU_ADDR_TO_PFN(iova);
+  
+	u32 pdn = SMMU_ADDR_TO_PDN(iova);
+  
+	u32 *ptbl;
+  
+
+  
+	if (pdir[pdn] != _PDE_VACANT(pdn))
+  
+		ptbl = (u32 *)((pdir[pdn] & SMMU_PFN_MASK) << SMMU_PDIR_SHIFT);
+  
+	else
+  
+	{
+  
+		ptbl = (u32 *)page_alloc(1);
+  
+		u32 addr = SMMU_PDN_TO_ADDR(pdn);
+  
+		for (int pn = 0; pn < SMMU_PTBL_COUNT; pn++, addr += SMMU_PAGE_SIZE)
+  
+			ptbl[pn] = _PTE_VACANT(addr);
+  
+		pdir[pdn] = SMMU_MK_PDE((u32)ptbl, _PDE_ATTR | _PDE_NEXT);
+  
+		smmu_flush_all();
+  
+	}
+  
+
+  
+	return &ptbl[ptn % SMMU_PTBL_COUNT];
+  
+}
+  
+
+  
+void smmu_map(u32 *pdir, u32 addr, u32 page, int cnt, u32 attr)
+  
+{
+  
+	for (int i = 0; i < cnt; i++)
+  
+	{
+  
+		u32 *pte = smmu_get_pte(pdir, addr);
+  
+		*pte = SMMU_ADDR_TO_PFN(page) | attr;
+  
+		addr += 0x1000;
+  
+		page += 0x1000;
+  
+	}
+  
+	smmu_flush_all();
+  
+}
+  
+
+  
+u32 *smmu_init_for_tsec()
+  
+{
+  
+	return smmu_init_domain4(MC_SMMU_TSEC_ASID, 1);
+  
+}
+  
+
+  
+void smmu_deinit_for_tsec()
+  
+{
+  
+	MC(MC_SMMU_PTB_ASID) = 1;
+  
+	MC(MC_SMMU_PTB_DATA) = 0;
+  
+	MC(MC_SMMU_TSEC_ASID) = 0;
+  
+	smmu_flush_regs();
+  
+}
+  
+
diff --git a/src/hwinit/smmu.h b/src/hwinit/smmu.h
@@ -0,0 +1,82 @@
+  
+/*
+  
+ * Copyright (c) 2018 naehrwert
+  
+ *
+  
+ * This program is free software; you can redistribute it and/or modify it
+  
+ * under the terms and conditions of the GNU General Public License,
+  
+ * version 2, as published by the Free Software Foundation.
+  
+ *
+  
+ * This program is distributed in the hope it will be useful, but WITHOUT
+  
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  
+ * more details.
+  
+ *
+  
+ * You should have received a copy of the GNU General Public License
+  
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+  
+ */
+  
+
+  
+#include "types.h"
+  
+
+  
+#define SMMU_HEAP_ADDR 0xA0000000
+  
+
+  
+#define MC_INTSTATUS                 0x0
+  
+#define MC_INTMASK                   0x4
+  
+#define MC_ERR_STATUS                0x8
+  
+#define MC_ERR_ADR                   0xc
+  
+#define MC_SMMU_CONFIG               0x10
+  
+#define MC_SMMU_TLB_CONFIG           0x14
+  
+#define MC_SMMU_PTC_CONFIG           0x18
+  
+#define MC_SMMU_PTB_ASID             0x1c
+  
+#define MC_SMMU_PTB_DATA             0x20
+  
+#define MC_SMMU_TLB_FLUSH            0x30
+  
+#define MC_SMMU_PTC_FLUSH            0x34
+  
+#define MC_SMMU_ASID_SECURITY        0x38
+  
+#define MC_SMMU_TSEC_ASID            0x294
+  
+#define MC_SMMU_TRANSLATION_ENABLE_0 0x228
+  
+#define MC_SMMU_TRANSLATION_ENABLE_1 0x22c
+  
+#define MC_SMMU_TRANSLATION_ENABLE_2 0x230
+  
+#define MC_SMMU_TRANSLATION_ENABLE_3 0x234
+  
+#define MC_SMMU_TRANSLATION_ENABLE_4 0xb98
+  
+
+  
+#define SMMU_PDE_NEXT_SHIFT 28
+  
+#define MC_SMMU_PTB_DATA_0_ASID_NONSECURE_SHIFT 29
+  
+#define MC_SMMU_PTB_DATA_0_ASID_WRITABLE_SHIFT  30
+  
+#define MC_SMMU_PTB_DATA_0_ASID_READABLE_SHIFT  31
+  
+#define SMMU_PAGE_SHIFT 12
+  
+#define SMMU_PAGE_SIZE  (1 << SMMU_PAGE_SHIFT)
+  
+#define SMMU_PDIR_COUNT 1024
+  
+#define SMMU_PDIR_SIZE  (sizeof(u32) * SMMU_PDIR_COUNT)
+  
+#define SMMU_PTBL_COUNT 1024
+  
+#define SMMU_PTBL_SIZE  (sizeof(u32) * SMMU_PTBL_COUNT)
+  
+#define SMMU_PDIR_SHIFT 12
+  
+#define SMMU_PDE_SHIFT  12
+  
+#define SMMU_PTE_SHIFT  12
+  
+#define SMMU_PFN_MASK   0x000FFFFF
+  
+#define SMMU_ADDR_TO_PFN(addr) ((addr) >> 12)
+  
+#define SMMU_ADDR_TO_PDN(addr) ((addr) >> 22)
+  
+#define SMMU_PDN_TO_ADDR(addr) ((pdn) << 22)
+  
+#define _READABLE  (1 << MC_SMMU_PTB_DATA_0_ASID_READABLE_SHIFT)
+  
+#define _WRITABLE  (1 << MC_SMMU_PTB_DATA_0_ASID_WRITABLE_SHIFT)
+  
+#define _NONSECURE (1 << MC_SMMU_PTB_DATA_0_ASID_NONSECURE_SHIFT)
+  
+#define _PDE_NEXT  (1 << SMMU_PDE_NEXT_SHIFT)
+  
+#define _MASK_ATTR (_READABLE | _WRITABLE | _NONSECURE)
+  
+#define _PDIR_ATTR (_READABLE | _WRITABLE | _NONSECURE)
+  
+#define _PDE_ATTR  (_READABLE | _WRITABLE | _NONSECURE)
+  
+#define _PDE_VACANT(pdn) (((pdn) << 10) | _PDE_ATTR)
+  
+#define _PTE_ATTR  (_READABLE | _WRITABLE | _NONSECURE)
+  
+#define _PTE_VACANT(addr) (((addr) >> SMMU_PAGE_SHIFT) | _PTE_ATTR)
+  
+#define SMMU_MK_PDIR(page, attr) (((page) >> SMMU_PDIR_SHIFT) | (attr))
+  
+#define SMMU_MK_PDE(page, attr)  (((page) >> SMMU_PDE_SHIFT) | (attr))
+  
+
+  
+void *page_alloc(u32 num);
+  
+u32 *smmu_alloc_pdir();
+  
+void smmu_flush_regs();
+  
+void smmu_flush_all();
+  
+void smmu_init(u32 secmon_base);
+  
+void smmu_enable();
+  
+bool smmu_is_used();
+  
+void smmu_exit();
+  
+u32 *smmu_init_domain4(u32 dev_base, u32 asid);
+  
+u32 *smmu_get_pte(u32 *pdir, u32 iova);
+  
+void smmu_map(u32 *pdir, u32 addr, u32 page, int cnt, u32 attr);
+  
+u32 *smmu_init_for_tsec();
+  
+void smmu_deinit_for_tsec();
diff --git a/src/hwinit/t210.h b/src/hwinit/t210.h
@@ -19,7 +19,9 @@
 #include "types.h"
+  
+#define BOOTROM_BASE 0x100000
 #define HOST1X_BASE 0x50000000
+  
+#define BPMP_CACHE_BASE 0x50040000
 #define DISPLAY_A_BASE 0x54200000
 #define DSI_BASE 0x54300000
 #define VIC_BASE 0x54340000
@@ -40,23 +42,28 @@
 #define GPIO_7_BASE (GPIO_BASE + 0x600)
 #define GPIO_8_BASE (GPIO_BASE + 0x700)
 #define EXCP_VEC_BASE 0x6000F000
+  
+#define IPATCH_BASE 0x6001DC00
 #define APB_MISC_BASE 0x70000000
 #define PINMUX_AUX_BASE 0x70003000
 #define UART_BASE 0x70006000
+  
+#define PWM_BASE 0x7000A000
 #define RTC_BASE 0x7000E000
 #define PMC_BASE 0x7000E400
-  
-#define SYSCTR0_BASE 0x7000F000
+  
+#define SYSCTR0_BASE 0x700F0000
 #define FUSE_BASE 0x7000F800
 #define KFUSE_BASE 0x7000FC00
 #define SE_BASE 0x70012000
 #define MC_BASE 0x70019000
 #define EMC_BASE 0x7001B000
 #define MIPI_CAL_BASE 0x700E3000
+  
+#define CL_DVFS_BASE 0x70110000
 #define I2S_BASE 0x702D1000
+  
+#define TZRAM_BASE 0x7C010000
 #define _REG(base, off) *(vu32 *)((base) + (off))
 #define HOST1X(off) _REG(HOST1X_BASE, off)
+  
+#define BPMP_CACHE_CTRL(off) _REG(BPMP_CACHE_BASE, off)
 #define DISPLAY_A(off) _REG(DISPLAY_A_BASE, off)
 #define DSI(off) _REG(DSI_BASE, off)
 #define VIC(off) _REG(VIC_BASE, off)
@@ -79,6 +86,7 @@
 #define EXCP_VEC(off) _REG(EXCP_VEC_BASE, off)
 #define APB_MISC(off) _REG(APB_MISC_BASE, off)
 #define PINMUX_AUX(off) _REG(PINMUX_AUX_BASE, off)
+  
+#define PWM(off) _REG(PWM_BASE, off)
 #define RTC(off) _REG(RTC_BASE, off)
 #define PMC(off) _REG(PMC_BASE, off)
 #define SYSCTR0(off) _REG(SYSCTR0_BASE, off)
@@ -89,21 +97,86 @@
 #define EMC(off) _REG(EMC_BASE, off)
 #define MIPI_CAL(off) _REG(MIPI_CAL_BASE, off)
 #define I2S(off) _REG(I2S_BASE, off)
+  
+#define CL_DVFS(off) _REG(CL_DVFS_BASE, off)
+  
+#define TEST_REG(off) _REG(0x0, off)
+  
+#define FUSE_RESERVED_ODMX(x) (0x1C8 + 4 * (x))
+  
+
+  
+/*! EVP registers. */
+  
+#define EVP_CPU_RESET_VECTOR 0x100
 /*! Misc registers. */
+  
+#define APB_MISC_PP_STRAPPING_OPT_A 0x08
 #define APB_MISC_PP_PINMUX_GLOBAL 0x40
+  
+#define APB_MISC_GP_LCD_BL_PWM_CFGPADCTRL 0xA34
+  
+#define APB_MISC_GP_WIFI_EN_CFGPADCTRL 0xB64
+  
+#define APB_MISC_GP_WIFI_RST_CFGPADCTRL 0xB68
 /*! System registers. */
 #define AHB_ARBITRATION_XBAR_CTRL 0xE0
 #define AHB_AHB_SPARE_REG 0x110
 /*! Secure boot registers. */
 #define SB_CSR 0x0
-  
-#define SB_AA64_RESET_LOW 0x30
+  
+#define SB_AA64_RESET_LOW  0x30
 #define SB_AA64_RESET_HIGH 0x34
+  
+/*! SOR registers. */
+  
+#define SOR_NV_PDISP_SOR_DP_HDCP_BKSV_LSB   0x1E8
+  
+#define SOR_NV_PDISP_SOR_TMDS_HDCP_BKSV_LSB 0x21C
+  
+#define SOR_NV_PDISP_SOR_TMDS_HDCP_CN_MSB   0x208
+  
+#define SOR_NV_PDISP_SOR_TMDS_HDCP_CN_LSB   0x20C
+  
+
+  
+/*! RTC registers. */
+  
+#define APBDEV_RTC_SECONDS        0x8
+  
+#define APBDEV_RTC_SHADOW_SECONDS 0xC
+  
+#define APBDEV_RTC_MILLI_SECONDS  0x10
+  
+
 /*! SYSCTR0 registers. */
-  
-#define SYSCTR0_CNTFID0 0x20
+  
+#define SYSCTR0_CNTFID0     0x20
+  
+#define SYSCTR0_CNTCR       0x00
+  
+#define SYSCTR0_COUNTERID0  0xFE0
+  
+#define SYSCTR0_COUNTERID1  0xFE4
+  
+#define SYSCTR0_COUNTERID2  0xFE8
+  
+#define SYSCTR0_COUNTERID3  0xFEC
+  
+#define SYSCTR0_COUNTERID4  0xFD0
+  
+#define SYSCTR0_COUNTERID5  0xFD4
+  
+#define SYSCTR0_COUNTERID6  0xFD8
+  
+#define SYSCTR0_COUNTERID7  0xFDC
+  
+#define SYSCTR0_COUNTERID8  0xFF0
+  
+#define SYSCTR0_COUNTERID9  0xFF4
+  
+#define SYSCTR0_COUNTERID10 0xFF8
+  
+#define SYSCTR0_COUNTERID11 0xFFC
+  
+
+  
+/*! TMR registers. */
+  
+#define TIMERUS_CNTR_1US          (0x10 + 0x0)
+  
+#define TIMERUS_USEC_CFG          (0x10 + 0x4)
+  
+#define TIMER_TMR9_TMR_PTV        0x80
+  
+#define  TIMER_EN     (1 << 31)
+  
+#define  TIMER_PER_EN (1 << 30)
+  
+#define TIMER_WDT4_CONFIG         (0x100 + 0x80)
+  
+#define  TIMER_SRC(TMR) (TMR & 0xF)
+  
+#define  TIMER_PER(PER) ((PER & 0xFF) << 4)
+  
+#define  TIMER_SYSRESET_EN (1 << 14)
+  
+#define  TIMER_PMCRESET_EN (1 << 15)
+  
+#define TIMER_WDT4_COMMAND        (0x108 + 0x80)
+  
+#define  TIMER_START_CNT   (1 << 0)
+  
+#define  TIMER_CNT_DISABLE (1 << 1)
+  
+#define TIMER_WDT4_UNLOCK_PATTERN (0x10C + 0x80)
+  
+#define  TIMER_MAGIC_PTRN 0xC45A
+  
+
+  
+/*! I2S registers. */
+  
+#define I2S1_CG   0x88
+  
+#define I2S1_CTRL 0xA0
+  
+#define I2S2_CG   0x188
+  
+#define I2S2_CTRL 0x1A0
+  
+#define I2S3_CG   0x288
+  
+#define I2S3_CTRL 0x2A0
+  
+#define I2S4_CG   0x388
+  
+#define I2S4_CTRL 0x3A0
+  
+#define I2S5_CG   0x488
+  
+#define I2S5_CTRL 0x4A0
+  
+#define  I2S_CG_SLCG_ENABLE (1 << 0)
+  
+#define  I2S_CTRL_MASTER_EN (1 << 10)
 /*! Fuse registers. */
 #define FUSE_CTRL 0x0
@@ -124,8 +197,4 @@
 #define FUSE_SOC_SPEEDO_1 0x138
 #define FUSE_SPARE_BIT_5 0x394
-  
-/*! Fuse cache registers. */
-  
-#define FUSE_RESERVED_ODMX(x) (0x1C8 + 4 * (x))
-  
-
-  
-
 #endif
diff --git a/src/hwinit/tsec.c b/src/hwinit/tsec.c
@@ -1,31 +1,43 @@
 /*
-  
-* Copyright (c) 2018 naehrwert
-  
-*
-  
-* This program is free software; you can redistribute it and/or modify it
-  
-* under the terms and conditions of the GNU General Public License,
-  
-* version 2, as published by the Free Software Foundation.
-  
-*
-  
-* This program is distributed in the hope it will be useful, but WITHOUT
-  
-* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-  
-* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-  
-* more details.
-  
-*
-  
-* You should have received a copy of the GNU General Public License
-  
-* along with this program.  If not, see <http://www.gnu.org/licenses/>.
-  
-*/
+  
+ * Copyright (c) 2018 naehrwert
+  
+ * Copyright (c) 2018 CTCaer
+  
+ * Copyright (c) 2018 balika011
+  
+ *
+  
+ * This program is free software; you can redistribute it and/or modify it
+  
+ * under the terms and conditions of the GNU General Public License,
+  
+ * version 2, as published by the Free Software Foundation.
+  
+ *
+  
+ * This program is distributed in the hope it will be useful, but WITHOUT
+  
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  
+ * more details.
+  
+ *
+  
+ * You should have received a copy of the GNU General Public License
+  
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+  
+ */
 #include <string.h>
+  
+
 #include "tsec.h"
+  
+#include "tsec_t210.h"
+  
+#include "se_t210.h"
 #include "clock.h"
+  
+#include "smmu.h"
 #include "t210.h"
 #include "heap.h"
+  
+#include "mc.h"
+  
+#include "util.h"
+  
+#include "gfx.h"
+  
+
+  
+/* #include "../gfx/gfx.h"
+  
+extern gfx_con_t gfx_con; */
 static int _tsec_dma_wait_idle()
 {
-  
-	u32 timeout = TMR(0x10) + 10000000;
+  
+	u32 timeout = get_tmr_ms() + 10000;
-  
-	while (!(TSEC(0x1118) & 2))
-  
-		if (TMR(0x10) > timeout)
+  
+	while (!(TSEC(TSEC_DMATRFCMD) & TSEC_DMATRFCMD_IDLE))
+  
+		if (get_tmr_ms() > timeout)
 			return 0;
 	return 1;
@@ -36,20 +48,23 @@ static int _tsec_dma_pa_to_internal_100(int not_imem, int i_offset, int pa_offse
 	u32 cmd;
 	if (not_imem)
-  
-		cmd = 0x600; // DMA 0x100 bytes
+  
+		cmd = TSEC_DMATRFCMD_SIZE_256B; // DMA 256 bytes
 	else
-  
-		cmd = 0x10; // dma imem
+  
+		cmd = TSEC_DMATRFCMD_IMEM;      // DMA IMEM (Instruction memmory)
-  
-	TSEC(0x1114) = i_offset; // tsec_dmatrfmoffs_r
-  
-	TSEC(0x111C) = pa_offset; // tsec_dmatrffboffs_r
-  
-	TSEC(0x1118) = cmd; // tsec_dmatrfcmd_r
+  
+	TSEC(TSEC_DMATRFMOFFS) = i_offset;
+  
+	TSEC(TSEC_DMATRFFBOFFS) = pa_offset;
+  
+	TSEC(TSEC_DMATRFCMD) = cmd;
 	return _tsec_dma_wait_idle();
 }
-  
-int tsec_query(u8 *dst, u32 rev, void *fw)
+  
+int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt)
 {
+  
+    print("TSEC KB : %d\n", kb);
 	int res = 0;
+  
+	u8 *fwbuf = NULL;
+  
+	u32 *pdir, *car, *fuse, *pmc, *flowctrl, *se, *mc, *iram, *evec;
 	//Enable clocks.
 	clock_enable_host1x();
@@ -60,65 +75,165 @@ int tsec_query(u8 *dst, u32 rev, void *fw)
 	clock_enable_kfuse();
 	//Configure Falcon.
-  
-	TSEC(0x110C) = 0; // tsec_dmactl_r
-  
-	TSEC(0x1010) = 0xFFF2; // tsec_irqmset_r
-  
-	TSEC(0x101C) = 0xFFF0; // tsec_irqdest_r
-  
-	TSEC(0x1048) = 3; // tsec_itfen_r
+  
+	TSEC(TSEC_DMACTL) = 0;
+  
+	TSEC(TSEC_IRQMSET) = 0xFFF2;
+  
+	TSEC(TSEC_IRQDEST) = 0xFFF0;
+  
+	TSEC(TSEC_ITFEN) = 0x03;
 	if (!_tsec_dma_wait_idle())
 	{
 		res = -1;
 		goto out;
 	}
-  
-	//Load firmware.
-  
-	u8 *fwbuf = (u8 *)malloc(0x2000);
-  
-	u8 *fwbuf_aligned = (u8 *)ALIGN((u32)fwbuf + 0x1000, 0x100);
-  
-	memcpy(fwbuf_aligned, fw, 0xF00);
-  
-	TSEC(0x1110) = (u32)fwbuf_aligned >> 8;// tsec_dmatrfbase_r
-  
-	for (u32 addr = 0; addr < 0xF00; addr += 0x100)
-  
-		if (!_tsec_dma_pa_to_internal_100(0, addr, addr))
+  
+	//Load firmware or emulate memio environment for newer TSEC fw.
+  
+	if (kb <= KB_FIRMWARE_VERSION_600)
+  
+	{
+  
+		fwbuf = (u8 *)malloc(0x2000);
+  
+		u8 *fwbuf_aligned = (u8 *)ALIGN((u32)fwbuf + 0x1000, 0x100);
+  
+		memcpy(fwbuf_aligned, tsec_ctxt->fw, tsec_ctxt->size);
+  
+		TSEC(TSEC_DMATRFBASE) = (u32)fwbuf_aligned >> 8;
+  
+	}
+  
+	else{
+  
+		void * temp = malloc(0x3000);
+  
+		temp = (void *)ALIGN((u32)temp, 0x100);
+  
+		memcpy(temp, tsec_ctxt->fw, 0x2900);
+  
+		tsec_ctxt->fw = temp;
+  
+		TSEC(TSEC_DMATRFBASE) = (u32)tsec_ctxt->fw >> 8;
+  
+        free(temp);
+  
+	}
+  
+
+  
+
+  
+	for (u32 addr = 0; addr < tsec_ctxt->size; addr += 0x100)
+  
+	{
+  
+		if (!_tsec_dma_pa_to_internal_100(false, addr, addr))
 		{
 			res = -2;
 			goto out_free;
 		}
-  
-
+  
+	}
+  
+
+  
+	if (kb >= KB_FIRMWARE_VERSION_620)
+  
+	{
+  
+		// Init SMMU translation for TSEC.
+  
+		print("initing smmu\n");
+  
+		pdir = smmu_init_for_tsec();
+  
+		print("initing secmon base\n");
+  
+		smmu_init(tsec_ctxt->secmon_base);
+  
+		print("enalbling smmu\n");
+  
+		// Enable SMMU
+  
+		if (!smmu_is_used())
+  
+			smmu_enable();
+  
+
+  
+		// Clock reset controller.
+  
+		car = page_alloc(1);
+  
+		memcpy(car, (void *)CLOCK_BASE, 0x1000);
+  
+		car[CLK_RST_CONTROLLER_CLK_SOURCE_TSEC / 4] = 2;
+  
+		smmu_map(pdir, CLOCK_BASE, (u32)car, 1, _WRITABLE | _READABLE | _NONSECURE);
+  
+
+  
+		// Fuse driver.
+  
+		fuse = page_alloc(1);
+  
+		memcpy((void *)&fuse[0x800/4], (void *)FUSE_BASE, 0x400);
+  
+		fuse[0x82C / 4] = 0;
+  
+		fuse[0x9E0 / 4] = (1 << (kb + 2)) - 1;
+  
+		fuse[0x9E4 / 4] = (1 << (kb + 2)) - 1;
+  
+		smmu_map(pdir, (FUSE_BASE - 0x800), (u32)fuse, 1, _READABLE | _NONSECURE);
+  
+
+  
+		// Power management controller.
+  
+		pmc = page_alloc(1);
+  
+		smmu_map(pdir, RTC_BASE, (u32)pmc, 1, _READABLE | _NONSECURE);
+  
+
+  
+		// Flow control.
+  
+		flowctrl = page_alloc(1);
+  
+		smmu_map(pdir, FLOW_CTLR_BASE, (u32)flowctrl, 1, _WRITABLE | _NONSECURE);
+  
+
+  
+		// Security engine.
+  
+		se = page_alloc(1);
+  
+		memcpy(se, (void *)SE_BASE, 0x1000);
+  
+		smmu_map(pdir, SE_BASE, (u32)se, 1, _READABLE | _WRITABLE | _NONSECURE);
+  
+
+  
+		// Memory controller.
+  
+		mc = page_alloc(1);
+  
+		memcpy(mc, (void *)MC_BASE, 0x1000);
+  
+		mc[MC_IRAM_BOM / 4] = 0;
+  
+		mc[MC_IRAM_TOM / 4] = 0x80000000;
+  
+		smmu_map(pdir, MC_BASE, (u32)mc, 1, _READABLE | _NONSECURE);
+  
+
+  
+		// IRAM
+  
+		iram = page_alloc(0x30);
+  
+		memcpy(iram, tsec_ctxt->pkg1, 0x30000);
+  
+		smmu_map(pdir, 0x40010000, (u32)iram, 0x30, _READABLE | _WRITABLE | _NONSECURE);
+  
+
+  
+		// Exception vectors
+  
+		evec = page_alloc(1);
+  
+		smmu_map(pdir, EXCP_VEC_BASE, (u32)evec, 1, _READABLE | _WRITABLE | _NONSECURE);
+  
+	}
+  
+	print("Executing TSEC\n");
 	//Execute firmware.
 	HOST1X(0x3300) = 0x34C2E1DA;
-  
-	TSEC(0x1044) = 0;
-  
-	TSEC(0x1040) = rev;
-  
-	TSEC(0x1104) = 0; // tsec_bootvec_r
-  
-	TSEC(0x1100) = 2; // tsec_cpuctl_r
-  
-	if (!_tsec_dma_wait_idle())
+  
+	TSEC(TSEC_STATUS) = 0;
+  
+	TSEC(TSEC_BOOTKEYVER) = 1;
+  
+	TSEC(TSEC_BOOTVEC) = 0;
+  
+	TSEC(TSEC_CPUCTL) = TSEC_CPUCTL_STARTCPU;
+  
+
+  
+	if (kb <= KB_FIRMWARE_VERSION_600)
 	{
-  
-		res = -3;
-  
-		goto out_free;
-  
-	}
-  
-	u32 timeout = TMR(0x10) + 2000000;
-  
-	while (!TSEC(0x1044))
-  
-		if (TMR(0x10) > timeout)
+  
+		if (!_tsec_dma_wait_idle())
+  
+		{
+  
+			res = -3;
+  
+			goto out_free;
+  
+		}
+  
+		u32 timeout = get_tmr_ms() + 2000;
+  
+		while (!TSEC(TSEC_STATUS))
+  
+			if (get_tmr_ms() > timeout)
+  
+			{
+  
+				res = -4;
+  
+				goto out_free;
+  
+			}
+  
+		if (TSEC(TSEC_STATUS) != 0xB0B0B0B0)
 		{
-  
-			res = -4;
+  
+			res = -5;
 			goto out_free;
 		}
-  
-	if (TSEC(0x1044) != 0xB0B0B0B0)
+  
+
+  
+		//Fetch result.
+  
+		HOST1X(0x3300) = 0;
+  
+		u32 buf[4];
+  
+		buf[0] = SOR1(SOR_NV_PDISP_SOR_DP_HDCP_BKSV_LSB);
+  
+		buf[1] = SOR1(SOR_NV_PDISP_SOR_TMDS_HDCP_BKSV_LSB);
+  
+		buf[2] = SOR1(SOR_NV_PDISP_SOR_TMDS_HDCP_CN_MSB);
+  
+		buf[3] = SOR1(SOR_NV_PDISP_SOR_TMDS_HDCP_CN_LSB);
+  
+		SOR1(SOR_NV_PDISP_SOR_DP_HDCP_BKSV_LSB) = 0;
+  
+		SOR1(SOR_NV_PDISP_SOR_TMDS_HDCP_BKSV_LSB) = 0;
+  
+		SOR1(SOR_NV_PDISP_SOR_TMDS_HDCP_CN_MSB) = 0;
+  
+		SOR1(SOR_NV_PDISP_SOR_TMDS_HDCP_CN_LSB) = 0;
+  
+
+  
+		memcpy(tsec_keys, &buf, 0x10);
+  
+	}
+  
+	else
 	{
-  
-		res = -5;
-  
-		goto out_free;
+  
+		u32 start = get_tmr_us();
+  
+		u32 k = se[SE_KEYTABLE_DATA0_REG_OFFSET / 4];
+  
+		u32 key[16] = {0};
+  
+		u32 kidx = 0;
+  
+		int xx = 0;
+  
+
+  
+        u32 key_buf[0x20/4] = {0};
+  
+		volatile u32 *key_data = (vu32 *)((void *)se + 0x320);
+  
+		u32 old_key_data = *key_data;
+  
+		u32 buf_counter = 0;
+  
+		while (!(TSEC(TSEC_CPUCTL) & 0x10)) {
+  
+            const u32 new_key_data = *key_data;
+  
+            if (new_key_data != old_key_data) {
+  
+                    old_key_data = new_key_data;
+  
+                    key_buf[buf_counter] = new_key_data;
+  
+                    buf_counter++;
+  
+            }
+  
+		}
+  
+		memcpy(tsec_keys, key_buf, 0x20);
+  
+		memcpy(tsec_ctxt->pkg1, iram, 0x30000);
+  
+		smmu_deinit_for_tsec();
 	}
-  
-	//Fetch result.
-  
-	HOST1X(0x3300) = 0;
-  
-	u32 buf[4];
-  
-	buf[0] = SOR1(0x1E8);
-  
-	buf[1] = SOR1(0x21C);
-  
-	buf[2] = SOR1(0x208);
-  
-	buf[3] = SOR1(0x20C);
-  
-	SOR1(0x1E8) = 0;
-  
-	SOR1(0x21C) = 0;
-  
-	SOR1(0x208) = 0;
-  
-	SOR1(0x20C) = 0;
-  
-	memcpy(dst, &buf, 0x10);
-  
-
 out_free:;
 	free(fwbuf);

diff --git a/src/hwinit/tsec.h b/src/hwinit/tsec.h
@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+  
+* Copyright (c) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -19,6 +20,16 @@
 #include "types.h"
-  
-int tsec_query(u8 *dst, u32 rev, void *fw);
+  
+typedef struct _tsec_ctxt_t
+  
+{
+  
+	u32 key_ver;
+  
+	void *fw;
+  
+	u32 size;
+  
+	void *pkg1;
+  
+	u32 pkg11_off;
+  
+	u32 secmon_base;
+  
+} tsec_ctxt_t;
+  
+
+  
+int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt);
 #endif
diff --git a/src/hwinit/tsec_t210.h b/src/hwinit/tsec_t210.h
@@ -0,0 +1,50 @@
+  
+/*
+  
+* Copyright (c) 2018 CTCaer
+  
+*
+  
+* This program is free software; you can redistribute it and/or modify it
+  
+* under the terms and conditions of the GNU General Public License,
+  
+* version 2, as published by the Free Software Foundation.
+  
+*
+  
+* This program is distributed in the hope it will be useful, but WITHOUT
+  
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  
+* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  
+* more details.
+  
+*
+  
+* You should have received a copy of the GNU General Public License
+  
+* along with this program.  If not, see <http://www.gnu.org/licenses/>.
+  
+*/
+  
+
+  
+#ifndef _TSEC_T210_H_
+  
+#define _TSEC_T210_H_
+  
+
+  
+#define TSEC_BOOTKEYVER    0x1040
+  
+#define TSEC_STATUS        0x1044
+  
+#define TSEC_ITFEN         0x1048
+  
+#define  TSEC_ITFEN_CTXEN         (1 << 0)
+  
+#define  TSEC_ITFEN_MTHDEN        (1 << 1)
+  
+#define TSEC_IRQMSET       0x1010
+  
+#define  TSEC_IRQMSET_WDTMR       (1 << 1)
+  
+#define  TSEC_IRQMSET_HALT        (1 << 4)
+  
+#define  TSEC_IRQMSET_EXTERR      (1 << 5)
+  
+#define  TSEC_IRQMSET_SWGEN0      (1 << 6)
+  
+#define  TSEC_IRQMSET_SWGEN1      (1 << 7)
+  
+#define  TSEC_IRQMSET_EXT(val)    (((val) & 0xFF) << 8)
+  
+#define TSEC_IRQDEST       0x101C
+  
+#define  TSEC_IRQDEST_HALT        (1 << 4)
+  
+#define  TSEC_IRQDEST_EXTERR      (1 << 5)
+  
+#define  TSEC_IRQDEST_SWGEN0      (1 << 6)
+  
+#define  TSEC_IRQDEST_SWGEN1      (1 << 7)
+  
+#define  TSEC_IRQDEST_EXT(val)    (((val) & 0xFF) << 8)
+  
+#define TSEC_CPUCTL       0x1100
+  
+#define  TSEC_CPUCTL_STARTCPU     (1 << 1)
+  
+#define TSEC_BOOTVEC      0x1104
+  
+#define TSEC_DMACTL       0x110C
+  
+#define TSEC_DMATRFBASE   0x1110
+  
+#define TSEC_DMATRFMOFFS  0x1114
+  
+#define TSEC_DMATRFCMD    0x1118
+  
+#define  TSEC_DMATRFCMD_IDLE      (1 << 1)
+  
+#define  TSEC_DMATRFCMD_IMEM      (1 << 4)
+  
+#define  TSEC_DMATRFCMD_SIZE_256B (6 << 8)
+  
+#define TSEC_DMATRFFBOFFS 0x111C
+  
+
+  
+#endif
diff --git a/src/hwinit/types.h b/src/hwinit/types.h
@@ -19,6 +19,7 @@
 #include <stddef.h>
 #include <stdint.h>
+  
+#include <stdbool.h>
 #define ALIGN(x, a) (((x) + (a) - 1) & ~((a) - 1))
 #define MAX(a, b) ((a) > (b) ? (a) : (b))
@@ -47,14 +48,14 @@ typedef volatile uint64_t vu64;
 typedef uintptr_t uPtr;
 enum KB_FIRMWARE_VERSION {
-  
-	KB_FIRMWARE_VERSION_100 = 1,
-  
-    KB_FIRMWARE_VERSION_200 = 1,
-  
-	KB_FIRMWARE_VERSION_300 = 2,
-  
-	KB_FIRMWARE_VERSION_301 = 3,
-  
-	KB_FIRMWARE_VERSION_400 = 4,
-  
-	KB_FIRMWARE_VERSION_500 = 5,
-  
-    KB_FIRMWARE_VERSION_600 = 6,
-  
-    KB_FIRMWARE_VERSION_620 = 7
+  
+	KB_FIRMWARE_VERSION_100 = 0,
+  
+    KB_FIRMWARE_VERSION_200 = 0,
+  
+	KB_FIRMWARE_VERSION_300 = 1,
+  
+	KB_FIRMWARE_VERSION_301 = 2,
+  
+	KB_FIRMWARE_VERSION_400 = 3,
+  
+	KB_FIRMWARE_VERSION_500 = 4,
+  
+    KB_FIRMWARE_VERSION_600 = 5,
+  
+    KB_FIRMWARE_VERSION_620 = 6
 };
 #endif
diff --git a/src/hwinit/util.h b/src/hwinit/util.h
@@ -21,6 +21,19 @@
 #define byte_swap_32(num) ((num>>24)&0xff) | ((num<<8)&0xff0000) | \
 						((num>>8)&0xff00) | ((num<<24)&0xff000000) \
+  
+
+  
+#define LSL0 0
+  
+#define LSL16 16
+  
+#define LSL32 32
+  
+
+  
+#define _PAGEOFF(x) ((x) & 0xFFFFF000)
+  
+
+  
+#define _ADRP(r, o) 0x90000000 | ((((o) >> 12) & 0x3) << 29) | ((((o) >> 12) & 0x1FFFFC) << 3) | ((r) & 0x1F)
+  
+#define _BL(a, o) 0x94000000 | ((((o) - (a)) >> 2) & 0x3FFFFFF)
+  
+#define _B(a, o) 0x14000000 | ((((o) - (a)) >> 2) & 0x3FFFFFF)
+  
+#define _MOVKX(r, i, s) 0xF2800000 | (((s) & 0x30) << 17) | (((i) & 0xFFFF) << 5) | ((r) & 0x1F)
+  
+#define _MOVZX(r, i, s) 0xD2800000 | (((s) & 0x30) << 17) | (((i) & 0xFFFF) << 5) | ((r) & 0x1F)
+  
+#define _NOP() 0xD503201F
 typedef struct _cfg_op_t
 {

diff --git a/src/package.c b/src/package.c
@@ -20,6 +20,12 @@
 #include "package.h"
 #include "kippatches/fs.inc"
+  
+u8 *ReadBoot0(sdmmc_storage_t *storage){
+  
+    u8 *bctBuf = (u8 *)malloc(0x4000);
+  
+    sdmmc_storage_read(storage, 0 , 0x4000 / NX_EMMC_BLOCKSIZE, bctBuf);
+  
+    return bctBuf;
+  
+}
+  
+
 u8 *ReadPackage1Ldr(sdmmc_storage_t *storage) {
    u8 *pk11 = malloc(0x40000);
    sdmmc_storage_read(storage, 0x100000 / NX_EMMC_BLOCKSIZE, 0x40000 / NX_EMMC_BLOCKSIZE, pk11);
@@ -60,7 +66,7 @@ u8 *ReadPackage2(sdmmc_storage_t *storage) {
 pkg2_hdr_t *unpackFirmwarePackage(u8 *data) {
    print("Unpacking firmware...\n");
    pkg2_hdr_t *hdr = (pkg2_hdr_t *)(data + 0x100);
-  
-    
+  
+
    //Decrypt header.
    se_aes_crypt_ctr(8, hdr, sizeof(pkg2_hdr_t), hdr, sizeof(pkg2_hdr_t), hdr);
@@ -71,10 +77,9 @@ pkg2_hdr_t *unpackFirmwarePackage(u8 *data) {
    //Decrypt body
    data += (0x100 + sizeof(pkg2_hdr_t));
-  
-    
+  
+
    for (u32 i = 0; i < 4; i++) {
-  
-        if (!hdr->sec_size[i])
-  
-            continue;
+  
+        if (!hdr->sec_size[i]) continue;
        se_aes_crypt_ctr(8, data, hdr->sec_size[i], data, hdr->sec_size[i], &hdr->sec_ctr[i * 0x10]);
@@ -88,12 +93,12 @@ void pkg1_unpack(pk11_offs *offs, u32 pkg1Off) {
    u8 ret = 0;
    u8 *extWb;
    u8 *extSec;
-  
-    
+  
+
    pk11_header *hdr = (pk11_header *)(pkg1Off + 0x20);
    u32 sec_size[3] = { hdr->wb_size, hdr->ldr_size, hdr->sm_size };
    u8 *pdata = (u8 *)hdr + sizeof(pk11_header);
-  
-    
+  
+
    for (u32 i = 0; i < 3; i++) {
        if (offs->sec_map[i] == 0 && offs->warmboot_base) {
            u8 *extWb = NULL;
@@ -115,18 +120,18 @@ void pkg1_unpack(pk11_offs *offs, u32 pkg1Off) {
        pdata += sec_size[offs->sec_map[i]];
    }
    if(extWb != NULL) {
-  
-        free(extWb); 
+  
+        free(extWb);
        customWarmboot = 1;
    }
    if(extSec != NULL) {
-  
-        free(extSec); 
+  
+        free(extSec);
        customSecmon = 1;
    }
 }
 void buildFirmwarePackage(u8 *kernel, u32 kernel_size, link_t *kips_info) {
    u8 *pdst = (u8 *)0xA9800000;
-  
-
+  
+    
    // Signature.
    memset(pdst, 0, 0x100);
    pdst += 0x100;
@@ -152,7 +157,6 @@ void buildFirmwarePackage(u8 *kernel, u32 kernel_size, link_t *kips_info) {
    hdr->sec_off[PKG2_SEC_KERNEL] = 0x10000000;
    se_aes_crypt_ctr(8, pdst, kernel_size, pdst, kernel_size, &hdr->sec_ctr[PKG2_SEC_KERNEL * 0x10]);
    pdst += kernel_size;
-  
-    print("kernel encrypted\n");
    // INI1.
    u32 ini1_size = sizeof(pkg2_ini1_t);
@@ -171,7 +175,6 @@ void buildFirmwarePackage(u8 *kernel, u32 kernel_size, link_t *kips_info) {
    hdr->sec_size[PKG2_SEC_INI1] = ini1_size;
    hdr->sec_off[PKG2_SEC_INI1] = 0x14080000;
    se_aes_crypt_ctr(8, ini1, ini1_size, ini1, ini1_size, &hdr->sec_ctr[PKG2_SEC_INI1 * 0x10]);
-  
-    print("INI1 encrypted\n");
    // Encrypt header.
    *(u32 *)hdr->ctr = 0x100 + sizeof(pkg2_hdr_t) + kernel_size + ini1_size;
@@ -338,7 +341,7 @@ int nca_patch(u8 * kipdata, u64 kipdata_len) {
    char pattern[8] = {0xE5, 0x07, 0x00, 0x32, 0xE0, 0x03, 0x16, 0xAA};
    char buf[0x10];
    memcpy(buf, kipdata+0x1C450, 0x10);
-  
-    u32 * addr = memsearch(kipdata, kipdata_len, pattern, sizeof(pattern));
+  
+    u32 * addr = (u32*)memsearch(kipdata, kipdata_len, pattern, sizeof(pattern));
    int ret=0;
    int max_dist = 0x10;
    for(int i=0; i<max_dist; i++) {
@@ -354,12 +357,12 @@ int nca_patch(u8 * kipdata, u64 kipdata_len) {
 int kippatch_apply_set(u8 *kipdata, u64 kipdata_len, kippatchset_t *patchset) {
    char *patchFilter[] = { "nosigchk", "nocmac", "nogc", NULL };
-  
-    
+  
+
    if (!fopen("/ReiNX/nogc", "rb")) {
        patchFilter[2] = NULL;
        fclose();
    }
-  
-    
+  
+
    for (kippatch_t *p = patchset->patches; p && p->name; ++p) {
        int found = 0;
        for (char **filtname = patchFilter; filtname && *filtname; ++filtname) {
@@ -384,4 +387,4 @@ kippatchset_t *kippatch_find_set(u8 *kiphash, kippatchset_t *patchsets) {
        if (!memcmp(kiphash, ps->kip_hash, 0x10)) return ps;
    }
    return NULL;
-  
-}
+  
+}
diff --git a/src/package.h b/src/package.h
@@ -16,7 +16,6 @@
 #pragma once
 #include "hwinit.h"
-  
-#include "hwinit/arm64.h"
 #define PKG2_MAGIC 0x31324B50
 #define PKG2_SEC_BASE 0x80000000
@@ -222,6 +221,8 @@ static u32 PRC_ID_RCV_600[] =
 };
 extern kippatchset_t kip_patches[];
+  
+
+  
+u8 *ReadBoot0(sdmmc_storage_t *storage);
 u8 *ReadPackage1Ldr(sdmmc_storage_t *storage);
 u8 *ReadPackage2(sdmmc_storage_t *storage);
 int kippatch_apply(u8 *kipdata, u64 kipdata_len, kippatch_t *patch);

diff --git a/src/start.s b/src/start.s
@@ -36,7 +36,7 @@
 .type _start, %function
 _start:
 	ADR R0, _start
-  
-	LDR R1, =payload_start
+  
+	LDR R1, =__payload_start
 	CMP R0, R1
 	BEQ _real_start
@@ -51,7 +51,7 @@ _copy_loop:
 	BNE _copy_loop
 	/* Use the relocator to copy ourselves into the right place. */
-  
-	LDR R2, =payload_end
+  
+	LDR R2, =__payload_end
 	SUB R2, R2, R1
 	LDR R3, =_real_start
 	LDR R4, =0x4003FF00
@@ -73,7 +73,7 @@ _real_start:
 	LDR R2, =__bss_end
 	SUB R2, R2, R0
 	BL memset
-  
-	LDR R0, =0x90020000
+  
+	LDR R0, =__heap_start /* initting heap to this address */
 	BL heap_init
 	BL bootrom
 	BL bootloader