From ff214f25c1228e1ea3e2e0978f060f92ec11f2bc Mon Sep 17 00:00:00 2001
From: CTCaer <ctcaer@gmail.com>
Date: Wed, 23 Mar 2022 00:58:20 +0200
Subject: [PATCH] bdk: update l4t hekatf prep functions

---
 bdk/mem/minerva.c | 35 ++++++++++++++++++++++++++++++++++-
 bdk/mem/minerva.h |  3 ++-
 2 files changed, 36 insertions(+), 2 deletions(-)

diff --git a/bdk/mem/minerva.c b/bdk/mem/minerva.c
index a8dd4c5..e4f1130 100644
--- a/bdk/mem/minerva.c
+++ b/bdk/mem/minerva.c
@@ -157,13 +157,23 @@ void minerva_prep_boot_freq()
 	minerva_change_freq(FREQ_800);
 }
 
-void minerva_prep_boot_l4t()
+void minerva_prep_boot_l4t(int oc_freq)
 {
 	if (!minerva_cfg)
 		return;
 
 	mtc_config_t *mtc_cfg = (mtc_config_t *)&nyx_str->mtc_cfg;
 
+	// Add OC frequency.
+	if (oc_freq && mtc_cfg->mtc_table[mtc_cfg->table_entries - 1].rate_khz == FREQ_1600)
+	{
+		memcpy(&mtc_cfg->mtc_table[mtc_cfg->table_entries],
+			   &mtc_cfg->mtc_table[mtc_cfg->table_entries - 1],
+			   sizeof(emc_table_t));
+		mtc_cfg->mtc_table[mtc_cfg->table_entries].rate_khz = oc_freq;
+		mtc_cfg->table_entries++;
+	}
+
 	// Set init frequency.
 	minerva_change_freq(FREQ_204);
 
@@ -186,6 +196,20 @@ void minerva_prep_boot_l4t()
 		minerva_change_freq(FREQ_666);
 	minerva_change_freq(FREQ_800);
 
+	// Trim table.
+	int entries = 0;
+	for (u32 i = 0; i < mtc_cfg->table_entries; i++)
+	{
+		// Copy freqs from 204 MHz to 800 MHz and 1600 MHz and above.
+		int rate = mtc_cfg->mtc_table[i].rate_khz;
+		if ((rate >= FREQ_204 && rate <= FREQ_800) || rate >= FREQ_1600)
+		{
+			memcpy(&mtc_cfg->mtc_table[entries], &mtc_cfg->mtc_table[i], sizeof(emc_table_t));
+			entries++;
+		}
+	}
+	mtc_cfg->table_entries = entries;
+
 	// Do not let other mtc ops.
 	mtc_cfg->init_done = 0;
 }
@@ -211,3 +235,12 @@ emc_table_t *minerva_get_mtc_table()
 	mtc_config_t *mtc_cfg = (mtc_config_t *)&nyx_str->mtc_cfg;
 	return mtc_cfg->mtc_table;
 }
+
+int minerva_get_mtc_table_entries()
+{
+	if (!minerva_cfg)
+		return 0;
+
+	mtc_config_t *mtc_cfg = (mtc_config_t *)&nyx_str->mtc_cfg;
+	return mtc_cfg->table_entries;
+}
diff --git a/bdk/mem/minerva.h b/bdk/mem/minerva.h
index 9e7f684..9320ecb 100644
--- a/bdk/mem/minerva.h
+++ b/bdk/mem/minerva.h
@@ -62,8 +62,9 @@ extern void (*minerva_cfg)(mtc_config_t *mtc_cfg, void *);
 u32  minerva_init();
 void minerva_change_freq(minerva_freq_t freq);
 void minerva_prep_boot_freq();
-void minerva_prep_boot_l4t();
+void minerva_prep_boot_l4t(int oc_freq);
 void minerva_periodic_training();
 emc_table_t *minerva_get_mtc_table();
+int minerva_get_mtc_table_entries();
 
 #endif