diff --git a/Makefile b/Makefile
index d0c3e2e..7d54ad4 100755
--- a/Makefile
+++ b/Makefile
@@ -51,6 +51,7 @@ OBJS = $(addprefix $(BUILD)/$(TARGET)/, \
 	dirlist.o \
 	ini.o \
 	ianos.o \
+	smmu.o \
 )
 
 OBJS += $(addprefix $(BUILD)/$(TARGET)/, \
diff --git a/bootloader/hos/hos.c b/bootloader/hos/hos.c
index 03d0cb9..aedf79c 100644
--- a/bootloader/hos/hos.c
+++ b/bootloader/hos/hos.c
@@ -32,6 +32,7 @@
 #include "../soc/cluster.h"
 #include "../soc/fuse.h"
 #include "../soc/pmc.h"
+#include "../soc/smmu.h"
 #include "../soc/t210.h"
 #include "../storage/nx_emmc.h"
 #include "../storage/sdmmc.h"
@@ -40,6 +41,7 @@
 #include "../gfx/gfx.h"
 extern gfx_ctxt_t gfx_ctxt;
 extern gfx_con_t gfx_con;
+
 extern hekate_config h_cfg;
 
 extern void sd_unmount();
@@ -47,14 +49,6 @@ extern void sd_unmount();
 //#define DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)
 #define DPRINTF(...)
 
-#define KB_FIRMWARE_VERSION_100_200 0
-#define KB_FIRMWARE_VERSION_300 1
-#define KB_FIRMWARE_VERSION_301 2
-#define KB_FIRMWARE_VERSION_400 3
-#define KB_FIRMWARE_VERSION_500 4
-#define KB_FIRMWARE_VERSION_600 5
-#define KB_FIRMWARE_VERSION_MAX KB_FIRMWARE_VERSION_600
-
 // Exosphère magic "XBC0".
 #define MAGIC_EXOSPHERE 0x30434258
 
@@ -79,25 +73,31 @@ static const u8 console_keyseed[0x10] =
 static const u8 key8_keyseed[] =
 	{ 0xFB, 0x8B, 0x6A, 0x9C, 0x79, 0x00, 0xC8, 0x49, 0xEF, 0xD2, 0x4D, 0x85, 0x4D, 0x30, 0xA0, 0xC7 };
 
-static const u8 master_keyseed_4xx_5xx[0x10] = 
+static const u8 master_keyseed_4xx_5xx_610[0x10] = 
 	{ 0x2D, 0xC1, 0xF4, 0x8D, 0xF3, 0x5B, 0x69, 0x33, 0x42, 0x10, 0xAC, 0x65, 0xDA, 0x90, 0x46, 0x66 };
 
+static const u8 master_keyseed_620[0x10] =
+    { 0x37, 0x4B, 0x77, 0x29, 0x59, 0xB4, 0x04, 0x30, 0x81, 0xF6, 0xE5, 0x8C, 0x6D, 0x36, 0x17, 0x9A };
+
 static const u8 console_keyseed_4xx_5xx[0x10] = 
 	{ 0x0C, 0x91, 0x09, 0xDB, 0x93, 0x93, 0x07, 0x81, 0x07, 0x3C, 0xC4, 0x16, 0x22, 0x7C, 0x6C, 0x28 };
 
 
-static void _se_lock()
+static void _se_lock(bool lock_se)
 {
-	for (u32 i = 0; i < 16; i++)
+	if (lock_se)
+	{
+		for (u32 i = 0; i < 16; i++)
 		se_key_acc_ctrl(i, 0x15);
 
-	for (u32 i = 0; i < 2; i++)
-		se_rsa_acc_ctrl(i, 1);
+		for (u32 i = 0; i < 2; i++)
+			se_rsa_acc_ctrl(i, 1);
+		SE(0x4) = 0; // Make this reg secure only.
+		SE(SE_KEY_TABLE_ACCESS_LOCK_OFFSET) = 0; // Make all key access regs secure only.
+		SE(SE_RSA_KEYTABLE_ACCESS_LOCK_OFFSET) = 0; // Make all RSA access regs secure only.
+		SE(SE_SECURITY_0) &= 0xFFFFFFFB; // Make access lock regs secure only.
+	}
 
-	SE(0x4) = 0; // Make this reg secure only.
-	SE(SE_KEY_TABLE_ACCESS_LOCK_OFFSET) = 0; // Make all key access regs secure only.
-	SE(SE_RSA_KEYTABLE_ACCESS_LOCK_OFFSET) = 0; // Make all RSA access regs secure only.
-	SE(SE_SECURITY_0) &= 0xFFFFFFFB; // Make access lock regs secure only.
 	memset((void *)IPATCH_BASE, 0, 13);
 	SB(SB_CSR) = 0x10; // Protected IROM enable.
 
@@ -134,7 +134,6 @@ void _pmc_scratch_lock(u32 kb)
 	case KB_FIRMWARE_VERSION_400:
 	case KB_FIRMWARE_VERSION_500:
 	case KB_FIRMWARE_VERSION_600:
-	default:
 		PMC(APBDEV_PMC_SEC_DISABLE2) |= 0x3FCFFFF;
 	    PMC(APBDEV_PMC_SEC_DISABLE4) |= 0x3F3FFFFF;
 	    PMC(APBDEV_PMC_SEC_DISABLE5)  = 0xFFFFFFFF;
@@ -145,75 +144,126 @@ void _pmc_scratch_lock(u32 kb)
 	}
 }
 
-int keygen(u8 *keyblob, u32 kb, void *tsec_fw)
+void _sysctr0_reset()
 {
-	u8 tmp[0x10];
+	SYSCTR0(SYSCTR0_CNTFID0) = 19200000;
+	SYSCTR0(SYSCTR0_CNTCR) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID0) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID1) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID2) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID3) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID4) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID5) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID6) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID7) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID8) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID9) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID10) = 0;
+	SYSCTR0(SYSCTR0_COUNTERID11) = 0;
+}
+
+int keygen(u8 *keyblob, u32 kb, tsec_ctxt_t *tsec_ctxt)
+{
+	u8 tmp[0x20];
+	u32 retries = 0;
+
+	tsec_ctxt->size = 0xF00;
 
 	if (kb > KB_FIRMWARE_VERSION_MAX)
 		return 0;
 
-	se_key_acc_ctrl(13, 0x15);
-	se_key_acc_ctrl(14, 0x15);
-
 	// Get TSEC key.
-	if (tsec_query(tmp, 1, tsec_fw) < 0)
-		return 0;
-
-	se_aes_key_set(13, tmp, 0x10);
-
-	// Derive keyblob keys from TSEC+SBK.
-	se_aes_crypt_block_ecb(13, 0, tmp, keyblob_keyseeds[0]);
-	se_aes_unwrap_key(15, 14, tmp);
-	se_aes_crypt_block_ecb(13, 0, tmp, keyblob_keyseeds[kb]);
-	se_aes_unwrap_key(13, 14, tmp);
-
-	// Clear SBK.
-	se_aes_key_clear(14);
-
-	//TODO: verify keyblob CMAC.
-	//se_aes_unwrap_key(11, 13, cmac_keyseed);
-	//se_aes_cmac(tmp, 0x10, 11, keyblob + 0x10, 0xA0);
-	//if (!memcmp(keyblob, tmp, 0x10))
-	//	return 0;
-
-	se_aes_crypt_block_ecb(13, 0, tmp, cmac_keyseed);
-	se_aes_unwrap_key(11, 13, cmac_keyseed);
-
-	// Decrypt keyblob and set keyslots.
-	se_aes_crypt_ctr(13, keyblob + 0x20, 0x90, keyblob + 0x20, 0x90, keyblob + 0x10);
-	se_aes_key_set(11, keyblob + 0x20 + 0x80, 0x10); // Package1 key.
-	se_aes_key_set(12, keyblob + 0x20, 0x10);
-	se_aes_key_set(13, keyblob + 0x20, 0x10);
-
-	se_aes_crypt_block_ecb(12, 0, tmp, master_keyseed_retail);
-
-	switch (kb)
+	if (kb >= KB_FIRMWARE_VERSION_620)
 	{
-	case KB_FIRMWARE_VERSION_100_200:
-	case KB_FIRMWARE_VERSION_300:
-	case KB_FIRMWARE_VERSION_301:
-		se_aes_unwrap_key(13, 15, console_keyseed);
-		se_aes_unwrap_key(12, 12, master_keyseed_retail);
-		break;
-	case KB_FIRMWARE_VERSION_400:
-		se_aes_unwrap_key(13, 15, console_keyseed_4xx_5xx);
-		se_aes_unwrap_key(15, 15, console_keyseed);
-		se_aes_unwrap_key(14, 12, master_keyseed_4xx_5xx);
-		se_aes_unwrap_key(12, 12, master_keyseed_retail);
-		break;
-	case KB_FIRMWARE_VERSION_500:
-	case KB_FIRMWARE_VERSION_600:
-	default:
-		se_aes_unwrap_key(10, 15, console_keyseed_4xx_5xx);
-		se_aes_unwrap_key(15, 15, console_keyseed);
-		se_aes_unwrap_key(14, 12, master_keyseed_4xx_5xx);
-		se_aes_unwrap_key(12, 12, master_keyseed_retail);
-		break;
+		tsec_ctxt->size = 0x2900;
+		u8 *tsec_paged = (u8 *)page_alloc(3);
+		memcpy(tsec_paged, (void *)tsec_ctxt->fw, tsec_ctxt->size);
+		tsec_ctxt->fw = tsec_paged;
 	}
 
-	// Package2 key.
-	se_key_acc_ctrl(8, 0x15);
-	se_aes_unwrap_key(8, 12, key8_keyseed);
+	while (tsec_query(tmp, kb, tsec_ctxt) < 0)
+	{
+		memset(tmp, 0x00, 0x20);
+		retries++;
+
+		if (retries > 3)
+			return 0;
+	}
+
+	if (kb >= KB_FIRMWARE_VERSION_620)
+	{
+		// Set TSEC key.
+		se_aes_key_set(12, tmp, 0x10);
+		// Set TSEC root key.
+		se_aes_key_set(13, tmp + 0x10, 0x10);
+
+		// Package2 key.
+		se_aes_key_set(8, tmp + 0x10, 0x10);
+		se_aes_unwrap_key(8, 8, master_keyseed_620);
+		se_aes_unwrap_key(8, 8, master_keyseed_retail);
+		se_aes_unwrap_key(8, 8, key8_keyseed);
+	}
+	else
+	{
+		se_key_acc_ctrl(13, 0x15);
+		se_key_acc_ctrl(14, 0x15);
+
+		// Set TSEC key.
+		se_aes_key_set(13, tmp, 0x10);
+
+		// Derive keyblob keys from TSEC+SBK.
+		se_aes_crypt_block_ecb(13, 0, tmp, keyblob_keyseeds[0]);
+		se_aes_unwrap_key(15, 14, tmp);
+		se_aes_crypt_block_ecb(13, 0, tmp, keyblob_keyseeds[kb]);
+		se_aes_unwrap_key(13, 14, tmp);
+
+		// Clear SBK.
+		se_aes_key_clear(14);
+
+		//TODO: verify keyblob CMAC.
+		//se_aes_unwrap_key(11, 13, cmac_keyseed);
+		//se_aes_cmac(tmp, 0x10, 11, keyblob + 0x10, 0xA0);
+		//if (!memcmp(keyblob, tmp, 0x10))
+		//	return 0;
+
+		se_aes_crypt_block_ecb(13, 0, tmp, cmac_keyseed);
+		se_aes_unwrap_key(11, 13, cmac_keyseed);
+
+		// Decrypt keyblob and set keyslots.
+		se_aes_crypt_ctr(13, keyblob + 0x20, 0x90, keyblob + 0x20, 0x90, keyblob + 0x10);
+		se_aes_key_set(11, keyblob + 0x20 + 0x80, 0x10); // Package1 key.
+		se_aes_key_set(12, keyblob + 0x20, 0x10);
+		se_aes_key_set(13, keyblob + 0x20, 0x10);
+
+		se_aes_crypt_block_ecb(12, 0, tmp, master_keyseed_retail);
+
+		switch (kb)
+		{
+		case KB_FIRMWARE_VERSION_100_200:
+		case KB_FIRMWARE_VERSION_300:
+		case KB_FIRMWARE_VERSION_301:
+			se_aes_unwrap_key(13, 15, console_keyseed);
+			se_aes_unwrap_key(12, 12, master_keyseed_retail);
+			break;
+		case KB_FIRMWARE_VERSION_400:
+			se_aes_unwrap_key(13, 15, console_keyseed_4xx_5xx);
+			se_aes_unwrap_key(15, 15, console_keyseed);
+			se_aes_unwrap_key(14, 12, master_keyseed_4xx_5xx_610);
+			se_aes_unwrap_key(12, 12, master_keyseed_retail);
+			break;
+		case KB_FIRMWARE_VERSION_500:
+		case KB_FIRMWARE_VERSION_600:
+			se_aes_unwrap_key(10, 15, console_keyseed_4xx_5xx);
+			se_aes_unwrap_key(15, 15, console_keyseed);
+			se_aes_unwrap_key(14, 12, master_keyseed_4xx_5xx_610);
+			se_aes_unwrap_key(12, 12, master_keyseed_retail);
+			break;
+		}
+
+		// Package2 key.
+		se_key_acc_ctrl(8, 0x15);
+		se_aes_unwrap_key(8, 12, key8_keyseed);
+	}
 
 	return 1;
 }
@@ -312,6 +362,7 @@ int hos_launch(ini_sec_t *cfg)
 	int bootStatePkg2Continue = 0;
 	int exoFwNumber = 0;
 	launch_ctxt_t ctxt;
+	tsec_ctxt_t tsec_ctxt;
 
 	memset(&ctxt, 0, sizeof(launch_ctxt_t));
 	list_init(&ctxt.kip1_list);
@@ -333,18 +384,29 @@ int hos_launch(ini_sec_t *cfg)
 	gfx_printf(&gfx_con, "Loaded package1 and keyblob\n");
 
 	// Generate keys.
-	if (!h_cfg.se_keygen_done)
+	if (!h_cfg.se_keygen_done || ctxt.pkg1_id->kb >= KB_FIRMWARE_VERSION_620)
 	{
-		keygen(ctxt.keyblob, ctxt.pkg1_id->kb, (u8 *)ctxt.pkg1 + ctxt.pkg1_id->tsec_off);
-		h_cfg.se_keygen_done = 1;
+		tsec_ctxt.key_ver = 1;
+		tsec_ctxt.fw = (u8 *)ctxt.pkg1 + ctxt.pkg1_id->tsec_off;
+		tsec_ctxt.pkg1 = ctxt.pkg1;
+		tsec_ctxt.pkg11_off = ctxt.pkg1_id->pkg11_off;
+		tsec_ctxt.secmon_base = ctxt.pkg1_id->secmon_base;
+
+		if (!keygen(ctxt.keyblob, ctxt.pkg1_id->kb, &tsec_ctxt))
+			return 0;
 		DPRINTF("Generated keys\n");
+
+		h_cfg.se_keygen_done = 1;
 	}
 
 	// Decrypt and unpack package1 if we require parts of it.
 	if (!ctxt.warmboot || !ctxt.secmon)
 	{
-		pkg1_decrypt(ctxt.pkg1_id, ctxt.pkg1);
+		if (ctxt.pkg1_id->kb <= KB_FIRMWARE_VERSION_600)
+			pkg1_decrypt(ctxt.pkg1_id, ctxt.pkg1);
+
 		pkg1_unpack((void *)ctxt.pkg1_id->warmboot_base, (void *)ctxt.pkg1_id->secmon_base, NULL, ctxt.pkg1_id, ctxt.pkg1);
+
 		gfx_printf(&gfx_con, "Decrypted and unpacked package1\n");
 	}
 
@@ -434,6 +496,7 @@ int hos_launch(ini_sec_t *cfg)
 
 	// Rebuild and encrypt package2.
 	pkg2_build_encrypt((void *)0xA9800000, ctxt.kernel, ctxt.kernel_size, &kip1_info);
+
 	gfx_printf(&gfx_con, "Rebuilt and loaded package2\n");
 
 	// Unmount SD card.
@@ -441,6 +504,7 @@ int hos_launch(ini_sec_t *cfg)
 
 	gfx_printf(&gfx_con, "\n%kBooting...%k\n", 0xFF96FF00, 0xFFCCCCCC);
 
+	// Clear pkg1/pkg2 keys.
 	se_aes_key_clear(8);
 	se_aes_key_clear(11);
 
@@ -486,7 +550,7 @@ int hos_launch(ini_sec_t *cfg)
 	}
 
 	// Clear BCT area for retail units and copy it over if dev unit.
-	if (ctxt.pkg1_id->kb < KB_FIRMWARE_VERSION_600)
+	if (ctxt.pkg1_id->kb <= KB_FIRMWARE_VERSION_500)
 	{
 		memset((void *)0x4003D000, 0, 0x3000);
 		if ((fuse_read_odm(4) & 3) == 3)
@@ -514,8 +578,16 @@ int hos_launch(ini_sec_t *cfg)
 	if (ctxt.pkg1_id->kb <= KB_FIRMWARE_VERSION_301)
 		mc_config_carveout();
 
-	// Lock SE before starting 'SecureMonitor'.
-	_se_lock();
+	// Lock SE before starting 'SecureMonitor' if < 6.2.0, otherwise finalize 6.2.0 keygen and reset sysctr0 counters.
+	if (ctxt.pkg1_id->kb <= KB_FIRMWARE_VERSION_600)
+		_se_lock(true);
+	else
+	{
+		// Lock bootrom and ipatches only.
+		_se_lock(false);
+		// Reset sysctr0 counters.
+		_sysctr0_reset();
+	}
 
 	// Free allocated memory.
 	ini_free_section(cfg);
@@ -538,7 +610,10 @@ int hos_launch(ini_sec_t *cfg)
 	display_end();
 
 	// Wait for secmon to get ready.
-	cluster_boot_cpu0(ctxt.pkg1_id->secmon_base);
+	if (smmu_is_used())
+		smmu_exit();
+    else
+		cluster_boot_cpu0(ctxt.pkg1_id->secmon_base);
 	while (!*mb_out)
 		usleep(1); // This only works when in IRAM or with a trained DRAM.
 
diff --git a/bootloader/hos/hos.h b/bootloader/hos/hos.h
index 384e944..9f206ee 100644
--- a/bootloader/hos/hos.h
+++ b/bootloader/hos/hos.h
@@ -21,6 +21,16 @@
 #include "pkg2.h"
 #include "../utils/types.h"
 #include "../config/ini.h"
+#include "../sec/tsec.h"
+
+#define KB_FIRMWARE_VERSION_100_200 0
+#define KB_FIRMWARE_VERSION_300 1
+#define KB_FIRMWARE_VERSION_301 2
+#define KB_FIRMWARE_VERSION_400 3
+#define KB_FIRMWARE_VERSION_500 4
+#define KB_FIRMWARE_VERSION_600 5
+#define KB_FIRMWARE_VERSION_620 6
+#define KB_FIRMWARE_VERSION_MAX KB_FIRMWARE_VERSION_620
 
 typedef struct _launch_ctxt_t
 {
@@ -55,6 +65,6 @@ typedef struct _merge_kip_t
 } merge_kip_t;
 
 int hos_launch(ini_sec_t *cfg);
-int keygen(u8 *keyblob, u32 kb, void *tsec_fw);
+int keygen(u8 *keyblob, u32 kb, tsec_ctxt_t *tsec_ctxt);
 
 #endif
diff --git a/bootloader/hos/pkg1.c b/bootloader/hos/pkg1.c
index 40490b2..452d673 100644
--- a/bootloader/hos/pkg1.c
+++ b/bootloader/hos/pkg1.c
@@ -2,6 +2,7 @@
  * Copyright (c) 2018 naehrwert
  * Copyright (c) 2018 st4rk
  * 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,
@@ -71,6 +72,14 @@ PATCHSET_DEF(_secmon_6_patchset,
 	{ 0xDC8 + 0x112C, _NOP() } //Sections SHA2.
 );
 
+PATCHSET_DEF(_secmon_620_patchset,
+	// Patch package2 decryption and signature/hash checks.
+	{ 0xDC8 + 0x604, _NOP() }, //package2 structure.
+	{ 0xDC8 + 0x610, _NOP() }, //Version.
+	{ 0xDC8 + 0xC74, _NOP() }, //Header signature.
+	{ 0xDC8 + 0xF10, _NOP() }  //Sections SHA2.
+);
+
 /*
  * package1.1 header: <wb, ldr, sm>
  * package1.1 layout:
@@ -81,16 +90,18 @@ PATCHSET_DEF(_secmon_6_patchset,
  * 4.0: {ldr, sm, wb} { 1, 2, 0 }
  * 5.0: {ldr, sm, wb} { 1, 2, 0 }
  * 6.0: {ldr, sm, wb} { 1, 2, 0 }
+ * 6.2: {ldr, sm, wb} { 1, 2, 0 }
  */
 
 static const pkg1_id_t _pkg1_ids[] = {
-	{ "20161121183008", 0, 0x1900, 0x3FE0, { 2, 1, 0 }, SM_100_ADR, 0x8000D000, true, _secmon_1_patchset }, //1.0.0 (Patched relocator)
-	{ "20170210155124", 0, 0x1900, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, true, _secmon_2_patchset }, //2.0.0 - 2.3.0
-	{ "20170519101410", 1, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, true, _secmon_3_patchset }, //3.0.0
-	{ "20170710161758", 2, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, true, _secmon_3_patchset }, //3.0.1 - 3.0.2
-	{ "20170921172629", 3, 0x1800, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003B000, false, _secmon_4_patchset }, //4.0.0 - 4.1.0
-	{ "20180220163747", 4, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003B000, false, _secmon_5_patchset }, //5.0.0 - 5.1.0
-	{ "20180802162753", 5, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003D800, false, _secmon_6_patchset }, //6.0.0 - 6.0.0
+	{ "20161121183008", 0, 0x1900, 0x3FE0, { 2, 1, 0 }, SM_100_ADR, 0x8000D000, true,  _secmon_1_patchset },   //1.0.0 (Patched relocator)
+	{ "20170210155124", 0, 0x1900, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, true,  _secmon_2_patchset },   //2.0.0 - 2.3.0
+	{ "20170519101410", 1, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, true,  _secmon_3_patchset },   //3.0.0
+	{ "20170710161758", 2, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, true,  _secmon_3_patchset },   //3.0.1 - 3.0.2
+	{ "20170921172629", 3, 0x1800, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003B000, false, _secmon_4_patchset },   //4.0.0 - 4.1.0
+	{ "20180220163747", 4, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003B000, false, _secmon_5_patchset },   //5.0.0 - 5.1.0
+	{ "20180802162753", 5, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003D800, false, _secmon_6_patchset },   //6.0.0 - 6.1.0
+	{ "20181107105733", 6, 0x0E00, 0x6FE0, { 1, 2, 0 }, 0x4002B000, 0x4003D800, false, _secmon_620_patchset }, //6.2.0
 	{ NULL } //End.
 };
 
diff --git a/bootloader/hos/pkg2.c b/bootloader/hos/pkg2.c
index 6705c1e..fdf4647 100644
--- a/bootloader/hos/pkg2.c
+++ b/bootloader/hos/pkg2.c
@@ -254,7 +254,7 @@ static const pkg2_kernel_id_t _pkg2_kernel_ids[] =
 	{ 0xe0e8cdc4, _kernel_302_patchset }, //3.0.2
 	{ 0x485d0157, _kernel_4_patchset },   //4.0.0 - 4.1.0
 	{ 0xf3c363f2, _kernel_5_patchset },   //5.0.0 - 5.1.0
-	{ 0x64ce1a44, _kernel_6_patchset },   //6.0.0
+	{ 0x64ce1a44, _kernel_6_patchset },   //6.0.0 - 6.2.0
 	{ 0, 0 }                              //End.
 };
 
diff --git a/bootloader/sec/tsec.c b/bootloader/sec/tsec.c
index 33a5782..a9cddf0 100644
--- a/bootloader/sec/tsec.c
+++ b/bootloader/sec/tsec.c
@@ -1,5 +1,7 @@
 /*
  * 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,
@@ -17,10 +19,18 @@
 #include <string.h>
 
 #include "../sec/tsec.h"
+#include "../sec/tsec_t210.h"
+#include "../sec/se_t210.h"
 #include "../soc/clock.h"
+#include "../soc/smmu.h"
 #include "../soc/t210.h"
 #include "../mem/heap.h"
+#include "../mem/mc.h"
 #include "../utils/util.h"
+#include "../hos/hos.h"
+
+/* #include "../gfx/gfx.h"
+extern gfx_con_t gfx_con; */
 
 static int _tsec_dma_wait_idle()
 {
@@ -49,9 +59,11 @@ static int _tsec_dma_pa_to_internal_100(int not_imem, int i_offset, int pa_offse
 	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)
 {
 	int res = 0;
+	u8 *fwbuf = NULL;
+	u32 *pdir, *car, *fuse, *pmc, *flowctrl, *se, *mc, *iram, *evec;
 
 	//Enable clocks.
 	clock_enable_host1x();
@@ -72,17 +84,75 @@ int tsec_query(u8 *dst, u32 rev, void *fw)
 		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
+		TSEC(TSEC_DMATRFBASE) = (u32)tsec_ctxt->fw >> 8;
+
+	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.
+		pdir = smmu_init_for_tsec();
+		smmu_init(tsec_ctxt->secmon_base);
+		// 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);
+		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);
+	}
 
 	//Execute firmware.
 	HOST1X(0x3300) = 0x34C2E1DA;
@@ -90,36 +160,83 @@ int tsec_query(u8 *dst, u32 rev, void *fw)
 	TSEC(0x1040) = rev;
 	TSEC(0x1104) = 0; // tsec_bootvec_r
 	TSEC(0x1100) = 2; // tsec_cpuctl_r
-	if (!_tsec_dma_wait_idle())
+
+	if (kb <= KB_FIRMWARE_VERSION_600)
 	{
-		res = -3;
-		goto out_free;
-	}
-	u32 timeout = get_tmr_ms() + 2000;
-	while (!TSEC(0x1044))
-		if (get_tmr_ms() > timeout)
+		if (!_tsec_dma_wait_idle())
 		{
-			res = -4;
+			res = -3;
+			goto out_free;
+		}
+		u32 timeout = get_tmr_ms() + 2000;
+		while (!TSEC(0x1044))
+			if (get_tmr_ms() > timeout)
+			{
+				res = -4;
+				goto out_free;
+			}
+		if (TSEC(0x1044) != 0xB0B0B0B0)
+		{
+			res = -5;
 			goto out_free;
 		}
-	if (TSEC(0x1044) != 0xB0B0B0B0)
-	{
-		res = -5;
-		goto out_free;
-	}
 
-	//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);
+		//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
+	{
+		u32 start = get_tmr_us();
+		u32 k = se[SE_KEYTABLE_DATA0_REG_OFFSET / 4];
+		u32 key[16] = {0};
+		u32 kidx = 0;
+
+		while (memcmp((u8 *)(iram + ((tsec_ctxt->pkg11_off + 0x20) / 4)), "PK11", 4))
+		{
+			smmu_flush_all();
+			if (k == se[SE_KEYTABLE_DATA0_REG_OFFSET / 4])
+				continue;
+			k = se[SE_KEYTABLE_DATA0_REG_OFFSET / 4];
+			key[kidx++] = k;
+
+		}
+
+		if (kidx != 8)
+		{
+			res = -6;
+			smmu_deinit_for_tsec();
+
+			goto out;
+		}
+
+		memcpy(tsec_keys, &key, 0x20);
+		memcpy(tsec_ctxt->pkg1, iram, 0x30000);
+		
+		smmu_deinit_for_tsec();
+
+		// for (int i = 0; i < kidx; i++)
+		// 	gfx_printf(&gfx_con, "key %08X\n", key[i]);
+
+		// gfx_printf(&gfx_con, "cpuctl (%08X) mbox (%08X)\n", TSEC(TSEC_CPUCTL), TSEC(TSEC_STATUS));
+
+		// u32 errst = MC(MC_ERR_STATUS);
+		// gfx_printf(&gfx_con, " MC %08X %08X %08X\n", MC(MC_INTSTATUS), errst, MC(MC_ERR_ADR));
+		// gfx_printf(&gfx_con, " type: %02X\n", errst >> 28);
+		// gfx_printf(&gfx_con, " smmu: %02X\n", (errst >> 25) & 3);
+		// gfx_printf(&gfx_con, " dir:  %s\n", (errst >> 16) & 1 ? "W" : "R");
+		// gfx_printf(&gfx_con, " cid:  %02x\n", errst & 0xFF);
+	}
 
 out_free:;
 	free(fwbuf);
diff --git a/bootloader/sec/tsec.h b/bootloader/sec/tsec.h
index 255feb0..5a04741 100644
--- a/bootloader/sec/tsec.h
+++ b/bootloader/sec/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 "../utils/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/bootloader/sec/tsec_t210.h b/bootloader/sec/tsec_t210.h
new file mode 100644
index 0000000..befe269
--- /dev/null
+++ b/bootloader/sec/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/bootloader/soc/smmu.c b/bootloader/soc/smmu.c
new file mode 100644
index 0000000..85a1deb
--- /dev/null
+++ b/bootloader/soc/smmu.c
@@ -0,0 +1,171 @@
+/*
+ * 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 "../soc/cluster.h"
+#include "../soc/t210.h"
+#include "../mem/mc_t210.h"
+#include "../utils/util.h"
+#include "../utils/aarch64_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;
+	msleep(100);
+
+	smmu_flush_all();
+}
+
+bool smmu_is_used()
+{
+	return smmu_used;
+}
+
+void smmu_exit()
+{
+	*(uint32_t *)(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/bootloader/soc/smmu.h b/bootloader/soc/smmu.h
new file mode 100644
index 0000000..827d58b
--- /dev/null
+++ b/bootloader/soc/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 "../utils/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/bootloader/soc/t210.h b/bootloader/soc/t210.h
index 267f96b..eb41a30 100644
--- a/bootloader/soc/t210.h
+++ b/bootloader/soc/t210.h
@@ -119,6 +119,12 @@
 #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
@@ -126,6 +132,19 @@
 
 /*! SYSCTR0 registers. */
 #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)
diff --git a/tools/smmu_payload.py b/tools/smmu_payload.py
new file mode 100644
index 0000000..8961b25
--- /dev/null
+++ b/tools/smmu_payload.py
@@ -0,0 +1,38 @@
+'''
+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/>.
+'''
+
+from keystone import *
+
+CODE = b'''
+	LDR	X1, =0x70019010
+	MOV X0, #0x1
+	STR W0, [X1]
+
+loop:
+	IC	IALLUIS
+	DSB ISH
+	B loop
+	MOV X0, #0x0
+	STR W0, [X1]
+	LDR	X0, =0x4002B000
+	BR	X0
+'''
+try:
+	ks = Ks(KS_ARCH_ARM64, KS_MODE_LITTLE_ENDIAN)
+	encoding, count = ks.asm(CODE, 0x0)
+	print("%s = %s (number of statements: %u)" %(CODE, ', '.join([('0x%02x' % (x)) for x in encoding]), count))
+except KsError as e:
+	print("ERROR: %s" %e)
\ No newline at end of file