From 7ae4fd03c2dc6a3f39a36c90cb42f1e88d45c2ec Mon Sep 17 00:00:00 2001
From: CTCaer <ctcaer@gmail.com>
Date: Thu, 20 Jan 2022 12:43:24 +0200
Subject: [PATCH] bdk: minerva: prep for ATF direct boot support

---
 bdk/mem/minerva.c   | 49 ++++++++++++++++++++++++++++++++++++++++++---
 bdk/mem/minerva.h   |  5 ++++-
 bdk/utils/sprintf.h |  2 ++
 3 files changed, 52 insertions(+), 4 deletions(-)

diff --git a/bdk/mem/minerva.c b/bdk/mem/minerva.c
index 76935b6..643e2e2 100644
--- a/bdk/mem/minerva.c
+++ b/bdk/mem/minerva.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019 CTCaer
+ * Copyright (c) 2019-2022 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,
@@ -97,9 +97,10 @@ u32 minerva_init()
 		return 1;
 
 	// Get current frequency
+	u32 current_emc_clk_src = CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC);
 	for (curr_ram_idx = 0; curr_ram_idx < 10; curr_ram_idx++)
 	{
-		if (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) == mtc_cfg->mtc_table[curr_ram_idx].clk_src_emc)
+		if (current_emc_clk_src == mtc_cfg->mtc_table[curr_ram_idx].clk_src_emc)
 			break;
 	}
 
@@ -156,6 +157,39 @@ void minerva_prep_boot_freq()
 	minerva_change_freq(FREQ_800);
 }
 
+void minerva_prep_boot_l4t()
+{
+	if (!minerva_cfg)
+		return;
+
+	mtc_config_t *mtc_cfg = (mtc_config_t *)&nyx_str->mtc_cfg;
+
+	// Set init frequency.
+	minerva_change_freq(FREQ_204);
+
+	// Train the rest of the frequencies.
+	mtc_cfg->train_mode = OP_TRAIN;
+	for (u32 i = 0; i < mtc_cfg->table_entries; i++)
+	{
+		mtc_cfg->rate_to = mtc_cfg->mtc_table[i].rate_khz;
+		// Skip already trained frequencies.
+		if (mtc_cfg->rate_to == FREQ_204 || mtc_cfg->rate_to == FREQ_800 || mtc_cfg->rate_to == FREQ_1600)
+			continue;
+
+		// Train frequency.
+		minerva_cfg(mtc_cfg, NULL);
+	}
+
+	// Do FSP WAR and scale to 800 MHz as boot freq.
+	bool fsp_opwr_enabled = !!(EMC(EMC_MRW3) & 0xC0);
+	if (fsp_opwr_enabled)
+		minerva_change_freq(FREQ_666);
+	minerva_change_freq(FREQ_800);
+
+	// Do not let other mtc ops.
+	mtc_cfg->init_done = 0;
+}
+
 void minerva_periodic_training()
 {
 	if (!minerva_cfg)
@@ -167,4 +201,13 @@ void minerva_periodic_training()
 		mtc_cfg->train_mode = OP_PERIODIC_TRAIN;
 		minerva_cfg(mtc_cfg, NULL);
 	}
-}
\ No newline at end of file
+}
+
+emc_table_t *minerva_get_mtc_table()
+{
+	if (!minerva_cfg)
+		return NULL;
+
+	mtc_config_t *mtc_cfg = (mtc_config_t *)&nyx_str->mtc_cfg;
+	return mtc_cfg->mtc_table;
+}
diff --git a/bdk/mem/minerva.h b/bdk/mem/minerva.h
index a81cdc8..9e7f684 100644
--- a/bdk/mem/minerva.h
+++ b/bdk/mem/minerva.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019 CTCaer
+ * Copyright (c) 2019-2022 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,
@@ -53,6 +53,7 @@ enum train_mode_t
 typedef enum
 {
 	FREQ_204  = 204000,
+	FREQ_666  = 665600,
 	FREQ_800  = 800000,
 	FREQ_1600 = 1600000
 } minerva_freq_t;
@@ -61,6 +62,8 @@ 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_periodic_training();
+emc_table_t *minerva_get_mtc_table();
 
 #endif
diff --git a/bdk/utils/sprintf.h b/bdk/utils/sprintf.h
index 412fb41..2fb863b 100644
--- a/bdk/utils/sprintf.h
+++ b/bdk/utils/sprintf.h
@@ -17,6 +17,8 @@
 #ifndef _SPRINTF_H_
 #define _SPRINTF_H_
 
+#include <stdarg.h>
+
 #include <utils/types.h>
 
 void s_printf(char *out_buf, const char *fmt, ...);