From 4a320447b308f0eb6e35478338a4ab7b4db1691c Mon Sep 17 00:00:00 2001
From: shchmue <shchmue@gmail.com>
Date: Fri, 19 Apr 2019 13:00:10 -0400
Subject: [PATCH] Fix tsec timeout during smmu emulation on 6.2.0

---
 source/keys/keys.c | 7 ++++++-
 source/sec/tsec.c  | 8 ++++----
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/source/keys/keys.c b/source/keys/keys.c
index 0392034..fa93e85 100644
--- a/source/keys/keys.c
+++ b/source/keys/keys.c
@@ -22,6 +22,7 @@
 #include "../hos/sept.h"
 #include "../libs/fatfs/ff.h"
 #include "../mem/heap.h"
+#include "../mem/mc.h"
 #include "../mem/sdram.h"
 #include "../sec/se.h"
 #include "../sec/se_t210.h"
@@ -314,16 +315,20 @@ get_tsec: ;
 
     int res = 0;
 
+    mc_disable_ahb_redirect();
+
     while (tsec_query(tsec_keys, pkg1_id->kb, &tsec_ctxt) < 0) {
         memset(tsec_keys, 0x00, 0x20);
         retries++;
-        if (retries > 3) {
+        if (retries > 15) {
             res = -1;
             break;
         }
     }
     free(pkg1);
 
+    mc_enable_ahb_redirect();
+
     if (res < 0) {
         EPRINTFARGS("ERROR %x dumping TSEC.\n", res);
         goto out_wait;
diff --git a/source/sec/tsec.c b/source/sec/tsec.c
index 698588f..c99d7a0 100644
--- a/source/sec/tsec.c
+++ b/source/sec/tsec.c
@@ -187,10 +187,10 @@ int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt)
 		{
 			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 (k != se[SE_KEYTABLE_DATA0_REG_OFFSET / 4]) {
+				k = se[SE_KEYTABLE_DATA0_REG_OFFSET / 4];
+				key[kidx++] = k;
+			}
 
 			// Failsafe.
 			if ((u32)get_tmr_us() - start > 125000)