Bump version to v1.8.3

Minor heap fragmentation was not worth preventing

Makefile

@@ -10,8 +10,8 @@ include $(DEVKITARM)/base_rules
 
 IPL_LOAD_ADDR := 0x40003000
 LPVERSION_MAJOR := 1
-LPVERSION_MINOR := 3
-LPVERSION_BUGFX := 0
+LPVERSION_MINOR := 8
+LPVERSION_BUGFX := 3
 
 ################################################################################
 
@@ -29,8 +29,8 @@ OBJS =	$(patsubst $(SOURCEDIR)/%.S, $(BUILDDIR)/$(TARGET)/%.o, \
 CUSTOMDEFINES := -DIPL_LOAD_ADDR=$(IPL_LOAD_ADDR)
 CUSTOMDEFINES += -DLP_VER_MJ=$(LPVERSION_MAJOR) -DLP_VER_MN=$(LPVERSION_MINOR) -DLP_VER_BF=$(LPVERSION_BUGFX)
 
-ARCH := -march=armv4t -mtune=arm7tdmi -mthumb-interwork
-CFLAGS = $(ARCH) -O2 -nostdlib -ffunction-sections -fdata-sections -fomit-frame-pointer -std=gnu11 -Wall $(CUSTOMDEFINES)
+ARCH := -march=armv4t -mtune=arm7tdmi -mthumb -mthumb-interwork
+CFLAGS = $(ARCH) -O2 -nostdlib -ffunction-sections -fno-inline -fdata-sections -fomit-frame-pointer -std=gnu11 -Wall $(CUSTOMDEFINES)
 LDFLAGS = $(ARCH) -nostartfiles -lgcc -Wl,--nmagic,--gc-sections -Xlinker --defsym=IPL_LOAD_ADDR=$(IPL_LOAD_ADDR)
 
 ################################################################################

README.md

@@ -2,13 +2,14 @@ Lockpick_RCM
 =
 Lockpick_RCM is a bare metal Nintendo Switch payload that derives encryption keys for use in Switch file handling software like hactool, hactoolnet/LibHac, ChoiDujour, etc. without booting Horizon OS.
 
-Due to changes imposed by firmware 7.0.0, Lockpick homebrew can no longer derive the latest keys. In the boot-time environment however, there are fewer limitations.
+Due to changes imposed by firmware 7.0.0, Lockpick homebrew can no longer derive the latest keys. In the boot-time environment however, there is no such limitation.
 
 Usage
 =
-* Launch Lockpick_RCM.bin using your favorite payload injector
-* Upon completion, keys will be saved to `/switch/prod.keys` on SD
-* If the console has Firmware 7.x, the `/sept/` folder from [Atmosphère](https://github.com/Atmosphere-NX/Atmosphere/releases) or [Kosmos](https://github.com/AtlasNX/Kosmos/releases) release zip containing both `sept-primary.bin` and `sept-secondary.enc` must be present on SD or else only keyblob master key derivation is possible (ie. up to `master_key_05` only)
+* It is highly recommended, but not required, to place Minerva on SD from the latest [Hekate](https://github.com/CTCaer/hekate/releases) for best performance, especially while dumping titlekeys - the file and path is `/bootloader/sys/libsys_minerva.bso`
+* Launch Lockpick_RCM.bin using your favorite payload injector or chainloader
+* Upon completion, keys will be saved to `/switch/prod.keys` and titlekeys to `/switch/title.keys` on SD
+* If the console has Firmware 7.x or higher, the `/sept/` folder from [Atmosphère](https://github.com/Atmosphere-NX/Atmosphere/releases) or [Kosmos](https://github.com/AtlasNX/Kosmos/releases) release zip must be present on SD or else only keyblob master key derivation is possible (ie. up to `master_key_05` only)
 
 Building
 =
@@ -18,6 +19,6 @@ Massive Thanks to CTCaer!
 =
 This software is heavily based on [Hekate](https://github.com/CTCaer/hekate). Beyond that, CTCaer was exceptionally helpful in the development of this project, lending loads of advice, expertise, and humor.
 
-Known Issues
+License
 =
-* Chainloading from SX will hang immediately due to quirks in their hwinit code, please launch payload directly
+This project is under the GPLv2 license. The Save processing module is adapted from [hactool](https://github.com/SciresM/hactool) code under ISC.

common/common_heap.h

@@ -25,6 +25,7 @@ typedef struct _hnode
 	u32 size;
 	struct _hnode *prev;
 	struct _hnode *next;
+	u32 align[4]; // Align to arch cache line size.
 } hnode_t;
 
 typedef struct _heap
@@ -32,3 +33,9 @@ typedef struct _heap
 	u32 start;
 	hnode_t *first;
 } heap_t;
+
+typedef struct
+{
+    u32 total;
+    u32 used;
+} heap_monitor_t;

common/common_module.h

@@ -0,0 +1,39 @@
+/*
+ * Common Module Header
+ * Copyright (C) 2018 M4xw
+ *
+ * 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/>.
+*/
+
+#pragma once
+#include <stddef.h>
+//TODO: Move it to BDK
+#include "common_gfx.h"
+#include "common_heap.h"
+
+// Module Callback
+typedef void (*cbMainModule_t)(const char *s);
+typedef void (*memcpy_t)(void *, void *, size_t);
+typedef void (*memset_t)(void *, int, size_t);
+
+typedef struct _bdkParams_t
+{
+	gfx_con_t *gfxCon;
+	gfx_ctxt_t *gfxCtx;
+	heap_t *sharedHeap;
+	memcpy_t memcpy;
+	memset_t memset;
+} *bdkParams_t;
+
+// Module Entrypoint
+typedef void (*moduleEntrypoint_t)(void *, bdkParams_t);

common/memory_map.h

@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2019 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 _MEMORY_MAP_H_
+#define _MEMORY_MAP_H_
+
+//#define IPL_STACK_TOP  0x4003FF00
+/* --- BIT/BCT: 0x40000000 - 0x40003000 --- */
+/* ---     IPL: 0x40003000 - 0x40028000 --- */
+#define IPL_LOAD_ADDR     0x40003000
+#define  IPL_SZ_MAX          0x20000 // 128KB.
+//#define IRAM_LIB_ADDR     0x4002B000
+#define SDRAM_PARAMS_ADDR 0x40030000 // SDRAM extraction buffer during sdram init.
+#define CBFS_DRAM_EN_ADDR 0x4003e000 // u32.
+
+/* --- DRAM START --- */
+#define DRAM_START     0x80000000
+#define  HOS_RSVD       0x1000000 // Do not write anything in this area.
+
+#define NYX_LOAD_ADDR  0x81000000
+#define  NYX_SZ_MAX     0x1000000 // 16MB
+/* --- Gap: 0x82000000 - 0x82FFFFFF --- */
+
+/* Stack theoretical max: 33MB */
+#define IPL_STACK_TOP  0x83100000
+#define IPL_HEAP_START 0x84000000
+#define  IPL_HEAP_SZ   0x20000000 // 512MB.
+/* --- Gap: 1040MB 0xA4000000 - 0xE4FFFFFF --- */
+
+// Virtual disk / Chainloader buffers.
+#define RAM_DISK_ADDR     0xA4000000
+#define  RAM_DISK_SZ      0x41000000 // 1040MB.
+
+//#define DRAM_LIB_ADDR    0xE0000000
+/* --- Chnldr: 252MB 0xC03C0000 - 0xCFFFFFFF --- */ //! Only used when chainloading.
+
+// SDMMC DMA buffers 1
+#define SDMMC_UPPER_BUFFER 0xE5000000
+#define  SDMMC_UP_BUF_SZ    0x8000000 // 128MB.
+
+// Nyx buffers.
+#define NYX_STORAGE_ADDR 0xED000000
+#define NYX_RES_ADDR     0xEE000000
+#define  NYX_RES_SZ       0x1000000 // 16MB.
+
+// SDMMC DMA buffers 2
+#define SDXC_BUF_ALIGNED   0xEF000000
+#define MIXD_BUF_ALIGNED   0xF0000000
+#define EMMC_BUF_ALIGNED   MIXD_BUF_ALIGNED
+#define  SDMMC_DMA_BUF_SZ   0x1000000 // 16MB (4MB currently used).
+
+// Nyx LvGL buffers.
+#define NYX_LV_VDB_ADR   0xF1000000
+#define  NYX_FB_SZ         0x384000 // 1280 x 720 x 4.
+#define NYX_LV_MEM_ADR   0xF1400000
+#define  NYX_LV_MEM_SZ    0x6600000 // 70MB.
+
+// Framebuffer addresses.
+#define IPL_FB_ADDRESS   0xF5A00000
+#define  IPL_FB_SZ         0x384000 // 720 x 1280 x 4.
+#define LOG_FB_ADDRESS   0xF5E00000
+#define  LOG_FB_SZ         0x334000 // 1280 x 656 x 4.
+#define NYX_FB_ADDRESS   0xF6200000
+#define NYX_FB2_ADDRESS  0xF6600000
+#define  NYX_FB_SZ         0x384000 // 1280 x 720 x 4.
+
+#define DRAM_MEM_HOLE_ADR 0xF6A00000
+#define DRAM_MEM_HOLE_SZ   0x8140000
+/* ---   Hole: 129MB 0xF6A00000 - 0xFEB3FFFF --- */
+#define DRAM_START2       0xFEB40000
+
+// NX BIS driver sector cache.
+#define NX_BIS_CACHE_ADDR 0xFEE00000
+#define  NX_BIS_CACHE_SZ      0x8800
+
+// USB buffers.
+#define USBD_ADDR                 0xFEF00000
+#define USB_DESCRIPTOR_ADDR       0xFEF40000
+#define USB_EP_CONTROL_BUF_ADDR   0xFEF80000
+#define USB_EP_BULK_IN_BUF_ADDR   0xFF000000
+#define USB_EP_BULK_OUT_BUF_ADDR  0xFF800000
+#define  USB_EP_BULK_OUT_MAX_XFER   0x800000
+
+// #define EXT_PAYLOAD_ADDR    0xC0000000
+// #define RCM_PAYLOAD_ADDR    (EXT_PAYLOAD_ADDR + ALIGN(PATCHED_RELOC_SZ, 0x10))
+// #define COREBOOT_ADDR       (0xD0000000 - rom_size)
+
+#endif

source/config/config.c

@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2018-2020 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/>.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+
+#include "config.h"
+#include "ini.h"
+#include "../gfx/gfx.h"
+#include "../gfx/tui.h"
+#include "../libs/fatfs/ff.h"
+#include "../soc/t210.h"
+#include "../storage/nx_sd.h"
+#include "../storage/sdmmc.h"
+#include "../utils/btn.h"
+#include "../utils/list.h"
+#include "../utils/util.h"
+
+extern hekate_config h_cfg;
+
+void set_default_configuration()
+{
+	h_cfg.autoboot = 0;
+	h_cfg.autoboot_list = 0;
+	h_cfg.bootwait = 3;
+	h_cfg.se_keygen_done = 0;
+	h_cfg.sbar_time_keeping = 0;
+	h_cfg.backlight = 100;
+	h_cfg.autohosoff = 0;
+	h_cfg.autonogc = 1;
+	h_cfg.updater2p = 0;
+	h_cfg.brand = NULL;
+	h_cfg.tagline = NULL;
+	h_cfg.errors = 0;
+	h_cfg.sept_run = EMC(EMC_SCRATCH0) & EMC_SEPT_RUN;
+	h_cfg.rcm_patched = true;
+	h_cfg.emummc_force_disable = false;
+
+	sd_power_cycle_time_start = 0;
+}

source/config/config.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2018-2019 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 _CONFIG_H_
+#define _CONFIG_H_
+
+#include "../utils/types.h"
+
+typedef struct _hekate_config
+{
+	// Non-volatile config.
+	u32 autoboot;
+	u32 autoboot_list;
+	u32 bootwait;
+	u32 backlight;
+	u32 autohosoff;
+	u32 autonogc;
+	u32 updater2p;
+	char *brand;
+	char *tagline;
+	// Global temporary config.
+	bool se_keygen_done;
+	bool sept_run;
+	bool emummc_force_disable;
+	bool rcm_patched;
+	u32  sbar_time_keeping;
+	u32  errors;
+} hekate_config;
+
+void set_default_configuration();
+int create_config_entry();
+void config_autoboot();
+void config_bootdelay();
+void config_backlight();
+void config_auto_hos_poweroff();
+void config_nogc();
+
+#endif /* _CONFIG_H_ */

source/config/ini.c

@@ -0,0 +1,189 @@
+/*
+ * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2020 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/>.
+ */
+
+#include <string.h>
+
+#include "ini.h"
+#include "../libs/fatfs/ff.h"
+#include "../mem/heap.h"
+#include "../utils/dirlist.h"
+
+static char *_strdup(char *str)
+{
+	if (!str)
+		return NULL;
+
+	// Remove starting space.
+	if (str[0] == ' ' && strlen(str))
+		str++;
+
+	char *res = (char *)malloc(strlen(str) + 1);
+	strcpy(res, str);
+
+	// Remove trailing space.
+	if (strlen(res) && res[strlen(res) - 1] == ' ')
+		res[strlen(res) - 1] = 0;
+
+	return res;
+}
+
+u32 _find_section_name(char *lbuf, u32 lblen, char schar)
+{
+	u32 i;
+	// Depends on 'FF_USE_STRFUNC 2' that removes \r.
+	for (i = 0; i < lblen  && lbuf[i] != schar && lbuf[i] != '\n'; i++)
+		;
+	lbuf[i] = 0;
+
+	return i;
+}
+
+ini_sec_t *_ini_create_section(link_t *dst, ini_sec_t *csec, char *name, u8 type)
+{
+	if (csec)
+		list_append(dst, &csec->link);
+
+	csec = (ini_sec_t *)calloc(sizeof(ini_sec_t), 1);
+	csec->name = _strdup(name);
+	csec->type = type;
+
+	return csec;
+}
+
+int ini_parse(link_t *dst, char *ini_path, bool is_dir)
+{
+	u32 lblen;
+	u32 pathlen = strlen(ini_path);
+	u32 k = 0;
+	char lbuf[512];
+	char *filelist = NULL;
+	FIL fp;
+	ini_sec_t *csec = NULL;
+
+	char *filename = (char *)malloc(256);
+
+	strcpy(filename, ini_path);
+
+	// Get all ini filenames.
+	if (is_dir)
+	{
+		filelist = dirlist(filename, "*.ini", false);
+		if (!filelist)
+		{
+			free(filename);
+			return 0;
+		}
+		strcpy(filename + pathlen, "/");
+		pathlen++;
+	}
+
+	do
+	{
+		// Copy ini filename in path string.
+		if (is_dir)
+		{
+			if (filelist[k * 256])
+			{
+				strcpy(filename + pathlen, &filelist[k * 256]);
+				k++;
+			}
+			else
+				break;
+		}
+
+		// Open ini.
+		if (f_open(&fp, filename, FA_READ) != FR_OK)
+		{
+			free(filelist);
+			free(filename);
+
+			return 0;
+		}
+
+		do
+		{
+			// Fetch one line.
+			lbuf[0] = 0;
+			f_gets(lbuf, 512, &fp);
+			lblen = strlen(lbuf);
+
+			// Remove trailing newline. Depends on 'FF_USE_STRFUNC 2' that removes \r.
+			if (lblen && lbuf[lblen - 1] == '\n')
+				lbuf[lblen - 1] = 0;
+
+			if (lblen > 2 && lbuf[0] == '[') // Create new section.
+			{
+				_find_section_name(lbuf, lblen, ']');
+
+				csec = _ini_create_section(dst, csec, &lbuf[1], INI_CHOICE);
+				list_init(&csec->kvs);
+			}
+			else if (lblen > 1 && lbuf[0] == '{') // Create new caption. Support empty caption '{}'.
+			{
+				_find_section_name(lbuf, lblen, '}');
+
+				csec = _ini_create_section(dst, csec, &lbuf[1], INI_CAPTION);
+				csec->color = 0xFF0AB9E6;
+			}
+			else if (lblen > 2 && lbuf[0] == '#') // Create comment.
+			{
+				csec = _ini_create_section(dst, csec, &lbuf[1], INI_COMMENT);
+			}
+			else if (lblen < 2) // Create empty line.
+			{
+				csec = _ini_create_section(dst, csec, NULL, INI_NEWLINE);
+			}
+			else if (csec && csec->type == INI_CHOICE) // Extract key/value.
+			{
+				u32 i = _find_section_name(lbuf, lblen, '=');
+
+				ini_kv_t *kv = (ini_kv_t *)calloc(sizeof(ini_kv_t), 1);
+				kv->key = _strdup(&lbuf[0]);
+				kv->val = _strdup(&lbuf[i + 1]);
+				list_append(&csec->kvs, &kv->link);
+			}
+		} while (!f_eof(&fp));
+
+		f_close(&fp);
+
+		if (csec)
+		{
+			list_append(dst, &csec->link);
+			if (is_dir)
+				csec = NULL;
+		}
+	} while (is_dir);
+
+	free(filename);
+	free(filelist);
+
+	return 1;
+}
+
+char *ini_check_payload_section(ini_sec_t *cfg)
+{
+	if (cfg == NULL)
+		return NULL;
+
+	LIST_FOREACH_ENTRY(ini_kv_t, kv, &cfg->kvs, link)
+	{
+		if (!strcmp("payload", kv->key))
+			return kv->val;
+	}
+
+	return NULL;
+}

source/config/ini.h

@@ -0,0 +1,50 @@
+/*
+ * 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,
+ * 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 _INI_H_
+#define _INI_H_
+
+#include "../utils/types.h"
+#include "../utils/list.h"
+
+#define INI_CHOICE  3
+#define INI_CAPTION 5
+#define INI_CHGLINE 6
+#define INI_NEWLINE 0xFE
+#define INI_COMMENT 0xFF
+
+typedef struct _ini_kv_t
+{
+	char *key;
+	char *val;
+	link_t link;
+} ini_kv_t;
+
+typedef struct _ini_sec_t
+{
+	char *name;
+	link_t kvs;
+	link_t link;
+	u32 type;
+	u32 color;
+} ini_sec_t;
+
+int ini_parse(link_t *dst, char *ini_path, bool is_dir);
+char *ini_check_payload_section(ini_sec_t *cfg);
+
+#endif
+

source/gfx/di.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (c) 2018 CTCaer
+ * Copyright (c) 2018-2019 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,
@@ -18,19 +18,24 @@
 #include <string.h>
 
 #include "di.h"
-#include "../soc/t210.h"
-#include "../utils/util.h"
-#include "../soc/i2c.h"
-#include "../soc/pmc.h"
+#include "../gfx/gfx.h"
 #include "../power/max77620.h"
 #include "../power/max7762x.h"
-#include "../soc/gpio.h"
-#include "../soc/pinmux.h"
 #include "../soc/clock.h"
+#include "../soc/gpio.h"
+#include "../soc/i2c.h"
+#include "../soc/pinmux.h"
+#include "../soc/pmc.h"
+#include "../soc/t210.h"
+#include "../utils/util.h"
 
 #include "di.inl"
 
-static u32 _display_ver = 0;
+extern volatile nyx_storage_t *nyx_str;
+
+static u32 _display_id = 0;
+
+void display_end();
 
 static void _display_dsi_wait(u32 timeout, u32 off, u32 mask)
 {
@@ -40,67 +45,89 @@ static void _display_dsi_wait(u32 timeout, u32 off, u32 mask)
 	usleep(5);
 }
 
+static void _display_dsi_send_cmd(u8 cmd, u32 param, u32 wait)
+{
+	DSI(_DSIREG(DSI_WR_DATA)) = (param << 8) | cmd;
+	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+
+	if (wait)
+		usleep(wait);
+}
+
 void display_init()
 {
+	// Check if display is already initialized.
+	if (CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_SET) & 0x18000000)
+		display_end();
+
 	// Power on.
 	max77620_regulator_set_volt_and_flags(REGULATOR_LDO0, 1200000, MAX77620_POWER_MODE_NORMAL); // Configure to 1.2V.
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_GPIO7, MAX77620_CNFG_GPIO_OUTPUT_VAL_HIGH | MAX77620_CNFG_GPIO_DRV_PUSHPULL);
 
-	// Enable MIPI CAL, DSI, DISP1, HOST1X, UART_FST_MIPI_CAL, DSIA LP clocks.
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_CLR) = 0x1010000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = 0x1010000;
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_CLR) = 0x18000000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_SET) = 0x18000000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) = 0x20000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_UART_FST_MIP_CAL) = 0xA;
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_W_SET) = 0x80000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_DSIA_LP) = 0xA;
+	// Enable Display Interface specific clocks.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_CLR) = 0x1010000;  // Clear reset DSI, MIPI_CAL.
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = 0x1010000;  // Set enable clock DSI, MIPI_CAL.
 
-	// DPD idle.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_CLR) = 0x18000000; // Clear reset DISP1, HOST1X.
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_SET) = 0x18000000; // Set enable clock DISP1, HOST1X.
+
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) = 0x20000;    // Set enable clock UART_FST_MIPI_CAL.
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_UART_FST_MIPI_CAL) = 10; // Set PLLP_OUT3 and div 6 (17MHz).
+
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_W_SET) = 0x80000;    // Set enable clock DSIA_LP.
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_DSIA_LP) = 10;    // Set PLLP_OUT and div 6 (68MHz).
+
+	// Disable deep power down.
 	PMC(APBDEV_PMC_IO_DPD_REQ) = 0x40000000;
 	PMC(APBDEV_PMC_IO_DPD2_REQ) = 0x40000000;
 
-	// Config pins.
-	PINMUX_AUX(PINMUX_AUX_NFC_EN) &= ~PINMUX_TRISTATE;
-	PINMUX_AUX(PINMUX_AUX_NFC_INT) &= ~PINMUX_TRISTATE;
-	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) &= ~PINMUX_TRISTATE;
-	PINMUX_AUX(PINMUX_AUX_LCD_BL_EN) &= ~PINMUX_TRISTATE;
-	PINMUX_AUX(PINMUX_AUX_LCD_RST) &= ~PINMUX_TRISTATE;
+	// Config LCD and Backlight pins.
+	PINMUX_AUX(PINMUX_AUX_NFC_EN) &= ~PINMUX_TRISTATE;     // PULL_DOWN
+	PINMUX_AUX(PINMUX_AUX_NFC_INT) &= ~PINMUX_TRISTATE;    // PULL_DOWN
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) &= ~PINMUX_TRISTATE; // PULL_DOWN | 1
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_EN) &= ~PINMUX_TRISTATE;  // PULL_DOWN
+	PINMUX_AUX(PINMUX_AUX_LCD_RST) &= ~PINMUX_TRISTATE;    // PULL_DOWN
 
-	gpio_config(GPIO_PORT_I, GPIO_PIN_0 | GPIO_PIN_1, GPIO_MODE_GPIO); // Backlight +-5V.
-	gpio_output_enable(GPIO_PORT_I, GPIO_PIN_0 | GPIO_PIN_1, GPIO_OUTPUT_ENABLE); // Backlight +-5V.
+	// Set Backlight +-5V pins mode and direction
+	gpio_config(GPIO_PORT_I, GPIO_PIN_0 | GPIO_PIN_1, GPIO_MODE_GPIO);
+	gpio_output_enable(GPIO_PORT_I, GPIO_PIN_0 | GPIO_PIN_1, GPIO_OUTPUT_ENABLE);
+
+	// Enable Backlight power.
 	gpio_write(GPIO_PORT_I, GPIO_PIN_0, GPIO_HIGH); // Backlight +5V enable.
-
 	usleep(10000);
-
 	gpio_write(GPIO_PORT_I, GPIO_PIN_1, GPIO_HIGH); // Backlight -5V enable.
-
 	usleep(10000);
 
-	gpio_config(GPIO_PORT_V, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2, GPIO_MODE_GPIO); // Backlight PWM, Enable, Reset.
+	// Configure Backlight pins (PWM, EN, RST).
+	gpio_config(GPIO_PORT_V, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2, GPIO_MODE_GPIO);
 	gpio_output_enable(GPIO_PORT_V, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2, GPIO_OUTPUT_ENABLE);
-	gpio_write(GPIO_PORT_V, GPIO_PIN_1, GPIO_HIGH); // Backlight Enable enable.
+	gpio_write(GPIO_PORT_V, GPIO_PIN_1, GPIO_HIGH); // Enable Backlight EN.
 
-	// Config display interface and display.
-	MIPI_CAL(MIPI_CAL_MIPI_BIAS_PAD_CFG2) = 0;
+	// Power up supply regulator for display interface.
+	MIPI_CAL(_DSIREG(MIPI_CAL_MIPI_BIAS_PAD_CFG2)) = 0;
 
-	exec_cfg((u32 *)CLOCK_BASE, _display_config_1, 4);
-	exec_cfg((u32 *)DISPLAY_A_BASE, _display_config_2, 94);
-	exec_cfg((u32 *)DSI_BASE, _display_config_3, 61);
+	// Set DISP1 clock source and parent clock.
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_DISP1) = 0x40000000; // PLLD_OUT.
+	u32 plld_div = (3 << 20) | (20 << 11) | 1; // DIVM: 1, DIVN: 20, DIVP: 3. PLLD_OUT: 768 MHz, PLLD_OUT0 (DSI): 96 MHz.
+	CLOCK(CLK_RST_CONTROLLER_PLLD_BASE) = PLLCX_BASE_ENABLE | PLLCX_BASE_LOCK | plld_div;
+	CLOCK(CLK_RST_CONTROLLER_PLLD_MISC1) = 0x20;    // PLLD_SETUP.
+	CLOCK(CLK_RST_CONTROLLER_PLLD_MISC) = 0x2D0AAA; // PLLD_ENABLE_CLK.
 
+	// Setup display communication interfaces.
+	exec_cfg((u32 *)DISPLAY_A_BASE, _display_dc_setup_win_config, 94);
+	exec_cfg((u32 *)DSI_BASE, _display_dsi_init_config, 61);
 	usleep(10000);
 
-	gpio_write(GPIO_PORT_V, GPIO_PIN_2, GPIO_HIGH); // Backlight Reset enable.
-
+	// Enable Backlight Reset.
+	gpio_write(GPIO_PORT_V, GPIO_PIN_2, GPIO_HIGH);
 	usleep(60000);
 
+	// Setups DSI packet configuration and request display id.
 	DSI(_DSIREG(DSI_BTA_TIMING)) = 0x50204;
-	DSI(_DSIREG(DSI_WR_DATA)) = 0x337; // MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE
-	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+	_display_dsi_send_cmd(MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE, 3, 0);
 	_display_dsi_wait(250000, _DSIREG(DSI_TRIGGER), DSI_TRIGGER_HOST | DSI_TRIGGER_VIDEO);
 
-	DSI(_DSIREG(DSI_WR_DATA)) = 0x406; // MIPI_DCS_GET_DISPLAY_ID
-	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+	_display_dsi_send_cmd(MIPI_DSI_DCS_READ, MIPI_DCS_GET_DISPLAY_ID, 0);
 	_display_dsi_wait(250000, _DSIREG(DSI_TRIGGER), DSI_TRIGGER_HOST | DSI_TRIGGER_VIDEO);
 
 	DSI(_DSIREG(DSI_HOST_CONTROL)) = DSI_HOST_CONTROL_TX_TRIG_HOST | DSI_HOST_CONTROL_IMM_BTA | DSI_HOST_CONTROL_CS | DSI_HOST_CONTROL_ECC;
@@ -108,45 +135,81 @@ void display_init()
 
 	usleep(5000);
 
-	_display_ver = DSI(_DSIREG(DSI_RD_DATA));
-	if (_display_ver == 0x10)
-		exec_cfg((u32 *)DSI_BASE, _display_config_4, 43);
+	// MIPI_DCS_GET_DISPLAY_ID reply is a long read, size 3 u32.
+	for (u32 i = 0; i < 3; i++)
+		_display_id = DSI(_DSIREG(DSI_RD_DATA)); // Skip ack and msg type info and get the payload (display id).
 
-	DSI(_DSIREG(DSI_WR_DATA)) = 0x1105; // MIPI_DCS_EXIT_SLEEP_MODE
-	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+	// Save raw Display ID to Nyx storage.
+	nyx_str->info.disp_id = _display_id;
 
-	usleep(180000);
+	// Decode Display ID.
+	_display_id = ((_display_id >> 8) & 0xFF00) | (_display_id & 0xFF);
 
-	DSI(_DSIREG(DSI_WR_DATA)) = 0x2905; // MIPI_DCS_SET_DISPLAY_ON
-	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+	if ((_display_id & 0xFF) == PANEL_JDI_LPM062M)
+		_display_id = PANEL_JDI_LPM062M;
 
-	usleep(20000);
+	// Initialize display panel.
+	switch (_display_id)
+	{
+	case PANEL_JDI_LPM062M:
+		exec_cfg((u32 *)DSI_BASE, _display_init_config_jdi, 43);
+		_display_dsi_send_cmd(MIPI_DSI_DCS_SHORT_WRITE, MIPI_DCS_EXIT_SLEEP_MODE, 180000);
+		break;
+	case PANEL_INL_P062CCA_AZ1:
+	case PANEL_AUO_A062TAN01:
+		_display_dsi_send_cmd(MIPI_DSI_DCS_SHORT_WRITE, MIPI_DCS_EXIT_SLEEP_MODE, 180000);
+		DSI(_DSIREG(DSI_WR_DATA)) = 0x439;          // MIPI_DSI_DCS_LONG_WRITE: 4 bytes.
+		DSI(_DSIREG(DSI_WR_DATA)) = 0x9483FFB9;     // Enable extension cmd. (Pass: FF 83 94).
+		DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+		usleep(5000);
+		DSI(_DSIREG(DSI_WR_DATA)) = 0x739;          // MIPI_DSI_DCS_LONG_WRITE: 7 bytes.
+		if (_display_id == PANEL_INL_P062CCA_AZ1)
+			DSI(_DSIREG(DSI_WR_DATA)) = 0x751548B1; // Set Power control. (Not deep standby, BT5 / XDK, VRH gamma volt adj 53 / x40).
+		else
+			DSI(_DSIREG(DSI_WR_DATA)) = 0x711148B1; // Set Power control. (Not deep standby, BT1 / XDK, VRH gamma volt adj 49 / x40).
+		DSI(_DSIREG(DSI_WR_DATA)) = 0x143209;       // (NVRH gamma volt adj 9, Amplifier current small / x30, FS0 freq Fosc/80 / FS1 freq Fosc/32).
+		DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+		usleep(5000);
+		break;
+	case PANEL_INL_P062CCA_AZ2:
+	case PANEL_AUO_A062TAN02:
+	default: // Allow spare part displays to work.
+		_display_dsi_send_cmd(MIPI_DSI_DCS_SHORT_WRITE, MIPI_DCS_EXIT_SLEEP_MODE, 120000);
+		break;
+	}
 
-	exec_cfg((u32 *)CLOCK_BASE, _display_config_6, 3);
-	exec_cfg((u32 *)DSI_BASE, _display_config_5, 21);
-	DISPLAY_A(_DIREG(DC_DISP_DISP_CLOCK_CONTROL)) = 4;
-	exec_cfg((u32 *)DSI_BASE, _display_config_7, 10);
+	_display_dsi_send_cmd(MIPI_DSI_DCS_SHORT_WRITE, MIPI_DCS_SET_DISPLAY_ON, 20000);
 
+	// Configure PLLD for DISP1.
+	plld_div = (1 << 20) | (24 << 11) | 1; // DIVM: 1, DIVN: 24, DIVP: 1. PLLD_OUT: 768 MHz, PLLD_OUT0 (DSI): 460.8 MHz.
+	CLOCK(CLK_RST_CONTROLLER_PLLD_BASE) = PLLCX_BASE_ENABLE | PLLCX_BASE_LOCK | plld_div;
+	CLOCK(CLK_RST_CONTROLLER_PLLD_MISC1) = 0x20;
+	CLOCK(CLK_RST_CONTROLLER_PLLD_MISC) = 0x2DFC00; // Use new PLLD_SDM_DIN.
+
+	// Finalize DSI configuration.
+	exec_cfg((u32 *)DSI_BASE, _display_dsi_packet_config, 21);
+	DISPLAY_A(_DIREG(DC_DISP_DISP_CLOCK_CONTROL)) = 4; // PCD1 | div3.
+	exec_cfg((u32 *)DSI_BASE, _display_dsi_mode_config, 10);
 	usleep(10000);
 
-	exec_cfg((u32 *)MIPI_CAL_BASE, _display_config_8, 6);
-	exec_cfg((u32 *)DSI_BASE, _display_config_9, 4);
-	exec_cfg((u32 *)MIPI_CAL_BASE, _display_config_10, 16);
-
+	// Calibrate display communication pads.
+	exec_cfg((u32 *)MIPI_CAL_BASE, _display_mipi_pad_cal_config, 6);
+	exec_cfg((u32 *)DSI_BASE, _display_dsi_pad_cal_config, 4);
+	exec_cfg((u32 *)MIPI_CAL_BASE, _display_mipi_apply_dsi_cal_config, 16);
 	usleep(10000);
 
-	exec_cfg((u32 *)DISPLAY_A_BASE, _display_config_11, 113);
+	// Enable video display controller.
+	exec_cfg((u32 *)DISPLAY_A_BASE, _display_video_disp_controller_enable_config, 113);
 }
 
 void display_backlight_pwm_init()
 {
 	clock_enable_pwm();
 
-	PWM(PWM_CONTROLLER_PWM_CSR_0) = (1 << 31); // Enable PWM
+	PWM(PWM_CONTROLLER_PWM_CSR_0) = PWM_CSR_EN; // Enable PWM and set it to 25KHz PFM.
 
-	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) = (PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) >> 2) << 2 | 1; // PWM clock source.
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) = (PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) & 0xFFFFFFFC) | 1; // PWM clock source.
 	gpio_config(GPIO_PORT_V, GPIO_PIN_0, GPIO_MODE_SPIO); // Backlight power mode.
-	
 }
 
 void display_backlight(bool enable)
@@ -167,7 +230,7 @@ void display_backlight_brightness(u32 brightness, u32 step_delay)
 	{
 		for (u32 i = old_value; i < brightness + 1; i++)
 		{
-			PWM(PWM_CONTROLLER_PWM_CSR_0) = (1 << 31) | (i << 16); // Enable PWM
+			PWM(PWM_CONTROLLER_PWM_CSR_0) = PWM_CSR_EN | (i << 16); // Enable PWM and set it to 25KHz PFM.
 			usleep(step_delay);
 		}
 	}
@@ -175,7 +238,7 @@ void display_backlight_brightness(u32 brightness, u32 step_delay)
 	{
 		for (u32 i = old_value; i > brightness; i--)
 		{
-			PWM(PWM_CONTROLLER_PWM_CSR_0) = (1 << 31) | (i << 16); // Enable PWM
+			PWM(PWM_CONTROLLER_PWM_CSR_0) = PWM_CSR_EN | (i << 16); // Enable PWM and set it to 25KHz PFM.
 			usleep(step_delay);
 		}
 	}
@@ -187,50 +250,76 @@ void display_end()
 {
 	display_backlight_brightness(0, 1000);
 
-	DSI(_DSIREG(DSI_VIDEO_MODE_CONTROL)) = 1;
+	DSI(_DSIREG(DSI_VIDEO_MODE_CONTROL)) = DSI_CMD_PKT_VID_ENABLE;
 	DSI(_DSIREG(DSI_WR_DATA)) = 0x2805; // MIPI_DCS_SET_DISPLAY_OFF
 
 	DISPLAY_A(_DIREG(DC_CMD_STATE_ACCESS)) = READ_MUX | WRITE_MUX;
-	DSI(_DSIREG(DSI_VIDEO_MODE_CONTROL)) = 0;
-
-	exec_cfg((u32 *)DISPLAY_A_BASE, _display_config_12, 17);
-	exec_cfg((u32 *)DSI_BASE, _display_config_13, 16);
+	DSI(_DSIREG(DSI_VIDEO_MODE_CONTROL)) = 0; // Disable host cmd packet.
 
+	// De-initialize video controller.
+	exec_cfg((u32 *)DISPLAY_A_BASE, _display_video_disp_controller_disable_config, 17);
+	exec_cfg((u32 *)DSI_BASE, _display_dsi_timing_deinit_config, 16);
 	usleep(10000);
 
-	if (_display_ver == 0x10)
-		exec_cfg((u32 *)DSI_BASE, _display_config_14, 22);
+	// De-initialize display panel.
+	switch (_display_id)
+	{
+	case PANEL_JDI_LPM062M:
+		exec_cfg((u32 *)DSI_BASE, _display_deinit_config_jdi, 22);
+		break;
+	case PANEL_AUO_A062TAN01:
+		exec_cfg((u32 *)DSI_BASE, _display_deinit_config_auo, 37);
+		break;
+	case PANEL_INL_P062CCA_AZ2:
+	case PANEL_AUO_A062TAN02:
+		DSI(_DSIREG(DSI_WR_DATA)) = 0x439; // MIPI_DSI_DCS_LONG_WRITE: 4 bytes.
+		DSI(_DSIREG(DSI_WR_DATA)) = 0x9483FFB9; // Enable extension cmd. (Pass: FF 83 94).
+		DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+		usleep(5000);
+		// Set Power.
+		DSI(_DSIREG(DSI_WR_DATA)) = 0xB39; // MIPI_DSI_DCS_LONG_WRITE: 11 bytes.
+		if (_display_id == PANEL_INL_P062CCA_AZ2)
+			DSI(_DSIREG(DSI_WR_DATA)) = 0x751548B1; // Set Power control. (Not deep standby, BT5 / XDK, VRH gamma volt adj 53 / x40).
+		else
+			DSI(_DSIREG(DSI_WR_DATA)) = 0x711148B1; // Set Power control. (Not deep standby, BT1 / XDK, VRH gamma volt adj 49 / x40).
+		// Set Power control. (NVRH gamma volt adj 9, Amplifier current small / x30, FS0 freq Fosc/80 / FS1 freq Fosc/32, Enter standby / PON / VCOMG).
+		DSI(_DSIREG(DSI_WR_DATA)) = 0x71143209;
+		DSI(_DSIREG(DSI_WR_DATA)) = 0x114D31; // Set Power control. (Unknown).
+		DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+		usleep(5000);
+		break;
+	case PANEL_INL_P062CCA_AZ1:
+	default:
+		break;
+	}
 
-	DSI(_DSIREG(DSI_WR_DATA)) = 0x1005; // MIPI_DCS_ENTER_SLEEP_MODE
-	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
-
-	usleep(50000);
+	_display_dsi_send_cmd(MIPI_DSI_DCS_SHORT_WRITE, MIPI_DCS_ENTER_SLEEP_MODE, 50000);
 
+	// Disable display and backlight pins.
 	gpio_write(GPIO_PORT_V, GPIO_PIN_2, GPIO_LOW); //Backlight Reset disable.
-
 	usleep(10000);
 
 	gpio_write(GPIO_PORT_I, GPIO_PIN_1, GPIO_LOW); //Backlight -5V disable.
-
 	usleep(10000);
 
 	gpio_write(GPIO_PORT_I, GPIO_PIN_0, GPIO_LOW); //Backlight +5V disable.
-
 	usleep(10000);
 
-	// Disable clocks.
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_SET) = 0x1010000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_CLR) = 0x1010000;
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_SET) = 0x18000000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_CLR) = 0x18000000;
+	// Disable Display Interface specific clocks.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_SET) = 0x1010000;  // Set reset clock DSI, MIPI_CAL.
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_CLR) = 0x1010000;  // Clear enable clock DSI, MIPI_CAL.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_SET) = 0x18000000; // Set reset DISP1, HOST1X.
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_CLR) = 0x18000000; // Clear enable DISP1, HOST1X.
 
+	// Power down pads.
 	DSI(_DSIREG(DSI_PAD_CONTROL_0)) = DSI_PAD_CONTROL_VS1_PULLDN_CLK | DSI_PAD_CONTROL_VS1_PULLDN(0xF) | DSI_PAD_CONTROL_VS1_PDIO_CLK | DSI_PAD_CONTROL_VS1_PDIO(0xF);
 	DSI(_DSIREG(DSI_POWER_CONTROL)) = 0;
 
+	// Switch to automatic function mode.
 	gpio_config(GPIO_PORT_V, GPIO_PIN_0, GPIO_MODE_SPIO); // Backlight PWM.
 
 	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) = (PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) & ~PINMUX_TRISTATE) | PINMUX_TRISTATE;
-	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) = (PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) >> 2) << 2 | 1;
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) = (PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) & 0xFFFFFFFC)| 1;
 }
 
 void display_color_screen(u32 color)
@@ -243,7 +332,6 @@ void display_color_screen(u32 color)
 	DISPLAY_A(_DIREG(DC_WIN_CD_WIN_OPTIONS)) = 0;
 	DISPLAY_A(_DIREG(DC_DISP_BLEND_BACKGROUND_COLOR)) = color;
 	DISPLAY_A(_DIREG(DC_CMD_STATE_CONTROL)) = (DISPLAY_A(_DIREG(DC_CMD_STATE_CONTROL)) & 0xFFFFFFFE) | GENERAL_ACT_REQ;
-
 	usleep(35000);
 
 	display_backlight(true);
@@ -252,11 +340,12 @@ void display_color_screen(u32 color)
 u32 *display_init_framebuffer()
 {
 	// Sanitize framebuffer area.
-	memset((u32 *)0xC0000000, 0, 0x3C0000);
-	// This configures the framebuffer @ 0xC0000000 with a resolution of 1280x720 (line stride 768).
-	exec_cfg((u32 *)DISPLAY_A_BASE, cfg_display_framebuffer, 32);
+	memset((u32 *)IPL_FB_ADDRESS, 0, 0x3C0000);
 
+	// This configures the framebuffer @ IPL_FB_ADDRESS with a resolution of 1280x720 (line stride 720).
+	exec_cfg((u32 *)DISPLAY_A_BASE, cfg_display_framebuffer, 32);
 	usleep(35000);
 
-	return (u32 *)0xC0000000;
+	return (u32 *)IPL_FB_ADDRESS;
 }
+

source/gfx/di.h

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (C) 2018 CTCaer
+ * Copyright (c) 2018-2019 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,
@@ -18,11 +18,27 @@
 #ifndef _DI_H_
 #define _DI_H_
 
+#include "../../common/memory_map.h"
 #include "../utils/types.h"
 
 /*! Display registers. */
 #define _DIREG(reg) ((reg) * 4)
 
+// Display controller scratch registers.
+#define DC_D_WINBUF_DD_SCRATCH_REGISTER_0 0xED
+#define DC_D_WINBUF_DD_SCRATCH_REGISTER_1 0xEE
+#define DC_T_WINBUF_TD_SCRATCH_REGISTER_0 0x16D
+#define DC_T_WINBUF_TD_SCRATCH_REGISTER_1 0x16E
+#define DC_COM_SCRATCH_REGISTER_A 0x325
+#define DC_COM_SCRATCH_REGISTER_B 0x326
+#define DC_A_WINBUF_AD_SCRATCH_REGISTER_0 0xBED
+#define DC_A_WINBUF_AD_SCRATCH_REGISTER_1 0xBEE
+#define DC_B_WINBUF_BD_SCRATCH_REGISTER_0 0xDED
+#define DC_B_WINBUF_BD_SCRATCH_REGISTER_1 0xDEE
+#define DC_C_WINBUF_CD_SCRATCH_REGISTER_0 0xFED
+#define DC_C_WINBUF_CD_SCRATCH_REGISTER_1 0xFEE
+
+// DC_CMD non-shadowed command/sync registers.
 #define DC_CMD_GENERAL_INCR_SYNCPT 0x00
 
 #define DC_CMD_GENERAL_INCR_SYNCPT_CNTRL 0x01
@@ -49,6 +65,7 @@
 #define  PM0_ENABLE (1 << 16)
 #define  PM1_ENABLE (1 << 18)
 
+#define DC_CMD_INT_STATUS 0x37
 #define DC_CMD_INT_MASK 0x38
 #define DC_CMD_INT_ENABLE 0x39
 
@@ -61,11 +78,13 @@
 #define  WIN_A_ACT_REQ   (1 <<  1)
 #define  WIN_B_ACT_REQ   (1 <<  2)
 #define  WIN_C_ACT_REQ   (1 <<  3)
+#define  WIN_D_ACT_REQ   (1 <<  4)
 #define  CURSOR_ACT_REQ  (1 <<  7)
 #define  GENERAL_UPDATE  (1 <<  8)
 #define  WIN_A_UPDATE    (1 <<  9)
 #define  WIN_B_UPDATE    (1 << 10)
 #define  WIN_C_UPDATE    (1 << 11)
+#define  WIN_D_UPDATE    (1 << 12)
 #define  CURSOR_UPDATE   (1 << 15)
 #define  NC_HOST_TRIG    (1 << 24)
 
@@ -73,15 +92,38 @@
 #define  WINDOW_A_SELECT (1 << 4)
 #define  WINDOW_B_SELECT (1 << 5)
 #define  WINDOW_C_SELECT (1 << 6)
+#define  WINDOW_D_SELECT (1 << 7)
 
 #define DC_CMD_REG_ACT_CONTROL 0x043
 
+// DC_D_WIN_DD window D instance of DC_WIN
+#define DC_D_WIN_DD_WIN_OPTIONS 0x80
+#define DC_D_WIN_DD_COLOR_DEPTH 0x83
+#define DC_D_WIN_DD_POSITION    0x84
+#define DC_D_WIN_DD_SIZE        0x85
+#define DC_D_WIN_DD_LINE_STRIDE 0x8A
+#define DC_D_WIN_DD_BLEND_LAYER_CONTROL 0x96
+#define DC_D_WIN_DD_BLEND_MATCH_SELECT  0x97
+#define DC_D_WIN_DD_BLEND_ALPHA_1BIT    0x99
+
+// DC_D_WINBUF_DD window D instance of DC_WINBUF
+#define DC_D_WINBUF_DD_START_ADDR    0xC0
+#define DC_D_WINBUF_DD_ADDR_H_OFFSET 0xC6
+#define DC_D_WINBUF_DD_ADDR_V_OFFSET 0xC8
+#define DC_D_WINBUF_DD_START_ADDR_HI 0xCD
+#define DC_D_WINBUF_DD_MEMFETCH_CONTROL 0xEB
+
+// DC_T_WIN_TD macro for using DD defines.
+#define _DC_T(reg) ((reg) + 0x80)
+
+// DC_COM non-shadowed registers.
 #define DC_COM_CRC_CONTROL 0x300
 #define DC_COM_PIN_OUTPUT_ENABLE(x) (0x302 + (x))
 #define DC_COM_PIN_OUTPUT_POLARITY(x) (0x306 + (x))
 
 #define DC_COM_DSC_TOP_CTL 0x33E
 
+// DC_DISP shadowed registers.
 #define DC_DISP_DISP_WIN_OPTIONS 0x402
 #define  HDMI_ENABLE     (1 << 30)
 #define  DSI_ENABLE      (1 << 29)
@@ -160,6 +202,32 @@
 #define  DE_CONTROL_EARLY        (3 << 2)
 #define  DE_CONTROL_ACTIVE_BLANK (4 << 2)
 
+// Cursor configuration registers.
+#define DC_DISP_CURSOR_FOREGROUND    0x43C
+#define DC_DISP_CURSOR_BACKGROUND    0x43D
+#define  CURSOR_COLOR(r,g,b) (((r) & 0xFF) | (((g) & 0xFF) << 8) | (((b) & 0xFF) << 16))
+
+#define DC_DISP_CURSOR_START_ADDR    0x43E
+#define  CURSOR_CLIPPING(w) ((w) << 28)
+#define   CURSOR_CLIP_WIN_A 1
+#define   CURSOR_CLIP_WIN_B 2
+#define   CURSOR_CLIP_WIN_C 3
+#define  CURSOR_SIZE_32  (0 << 24)
+#define  CURSOR_SIZE_64  (1 << 24)
+#define  CURSOR_SIZE_128 (2 << 24)
+#define  CURSOR_SIZE_256 (3 << 24)
+#define DC_DISP_CURSOR_POSITION      0x440
+#define DC_DISP_CURSOR_START_ADDR_HI 0x4EC
+#define DC_DISP_BLEND_CURSOR_CONTROL 0x4F1
+#define  CURSOR_BLEND_2BIT     (0 << 24)
+#define  CURSOR_BLEND_R8G8B8A8 (1 << 24)
+#define  CURSOR_BLEND_SRC_FACTOR(n) ((n) << 8)
+#define  CURSOR_BLEND_DST_FACTOR(n) ((n) << 16)
+#define   CURSOR_BLEND_ZRO 0
+#define   CURSOR_BLEND_K1  1
+#define   CURSOR_BLEND_NK1 2
+// End of cursor cfg regs.
+
 #define DC_DISP_DC_MCCIF_FIFOCTRL 0x480
 #define DC_DISP_SD_BL_PARAMETERS 0x4D7
 #define DC_DISP_SD_BL_CONTROL 0x4DC
@@ -186,6 +254,13 @@
 #define  CSC_ENABLE   (1 << 18)
 #define  WIN_ENABLE   (1 << 30)
 
+#define DC_WIN_BUFFER_CONTROL 0x702
+#define  BUFFER_CONTROL_HOST  0
+#define  BUFFER_CONTROL_VI    1
+#define  BUFFER_CONTROL_EPP   2
+#define  BUFFER_CONTROL_MPEGE 3
+#define  BUFFER_CONTROL_SB2D  4
+
 #define DC_WIN_COLOR_DEPTH 0x703
 #define  WIN_COLOR_DEPTH_P1             0x0
 #define  WIN_COLOR_DEPTH_P2             0x1
@@ -210,8 +285,9 @@
 #define  WIN_COLOR_DEPTH_YCbCr422RA     0x18
 #define  WIN_COLOR_DEPTH_YUV422RA       0x19
 
-#define DC_WIN_BUFFER_CONTROL 0x702
 #define DC_WIN_POSITION 0x704
+#define  H_POSITION(x) (((x) & 0xFfff) <<  0)
+#define  V_POSITION(x) (((x) & 0x1fff) << 16)
 
 #define DC_WIN_SIZE 0x705
 #define  H_SIZE(x) (((x) & 0x1fff) <<  0)
@@ -233,7 +309,43 @@
 #define  UV_LINE_STRIDE(x) (((x) & 0xffff) << 16)
 #define DC_WIN_DV_CONTROL 0x70E
 
-// The following registers are A/B/C shadows of the 0xBC0/0xDC0/0xFC0 registers (see DISPLAY_WINDOW_HEADER).
+#define DC_WINBUF_BLEND_LAYER_CONTROL 0x716
+#define  WIN_K1(x) (((x) & 0xff) << 8)
+#define  WIN_K2(x) (((x) & 0xff) << 16)
+#define  WIN_BLEND_ENABLE (0 << 24)
+#define  WIN_BLEND_BYPASS (1 << 24)
+
+#define DC_WINBUF_BLEND_MATCH_SELECT 0x717
+#define  WIN_BLEND_FACT_SRC_COLOR_MATCH_SEL_ZERO             (0 << 0)
+#define  WIN_BLEND_FACT_SRC_COLOR_MATCH_SEL_ONE              (1 << 0)
+#define  WIN_BLEND_FACT_SRC_COLOR_MATCH_SEL_K1               (2 << 0)
+#define  WIN_BLEND_FACT_SRC_COLOR_MATCH_SEL_K1_TIMES_DST     (3 << 0)
+#define  WIN_BLEND_FACT_SRC_COLOR_MATCH_SEL_NEG_K1_TIMES_DST (4 << 0)
+#define  WIN_BLEND_FACT_SRC_COLOR_MATCH_SEL_K1_TIMES_SRC     (5 << 0)
+
+#define  WIN_BLEND_FACT_DST_COLOR_MATCH_SEL_ZERO             (0 << 4)
+#define  WIN_BLEND_FACT_DST_COLOR_MATCH_SEL_ONE              (1 << 4)
+#define  WIN_BLEND_FACT_DST_COLOR_MATCH_SEL_K1               (2 << 4)
+#define  WIN_BLEND_FACT_DST_COLOR_MATCH_SEL_K2               (3 << 4)
+#define  WIN_BLEND_FACT_DST_COLOR_MATCH_SEL_K1_TIMES_DST     (4 << 4)
+#define  WIN_BLEND_FACT_DST_COLOR_MATCH_SEL_NEG_K1_TIMES_DST (5 << 4)
+#define  WIN_BLEND_FACT_DST_COLOR_MATCH_SEL_NEG_K1_TIMES_SRC (6 << 4)
+#define  WIN_BLEND_FACT_DST_COLOR_MATCH_SEL_NEG_K1           (7 << 4)
+
+#define  WIN_BLEND_FACT_SRC_ALPHA_MATCH_SEL_ZERO             (0 << 8)
+#define  WIN_BLEND_FACT_SRC_ALPHA_MATCH_SEL_K1               (1 << 8)
+#define  WIN_BLEND_FACT_SRC_ALPHA_MATCH_SEL_K2               (2 << 8)
+
+#define  WIN_BLEND_FACT_DST_ALPHA_MATCH_SEL_ZERO             (0 << 12)
+#define  WIN_BLEND_FACT_DST_ALPHA_MATCH_SEL_ONE              (1 << 12)
+#define  WIN_BLEND_FACT_DST_ALPHA_MATCH_SEL_NEG_K1_TIMES_SRC (2 << 12)
+#define  WIN_BLEND_FACT_DST_ALPHA_MATCH_SEL_K2               (3 << 12)
+
+#define DC_WINBUF_BLEND_ALPHA_1BIT 0x719
+#define  WIN_ALPHA_1BIT_WEIGHT0(x) (((x) & 0xff) << 0)
+#define  WIN_ALPHA_1BIT_WEIGHT1(x) (((x) & 0xff) << 8)
+
+/*! The following registers are A/B/C shadows of the 0xBC0/0xDC0/0xFC0 registers (see DISPLAY_WINDOW_HEADER). */
 #define DC_WINBUF_START_ADDR 0x800
 #define DC_WINBUF_ADDR_H_OFFSET 0x806
 #define DC_WINBUF_ADDR_V_OFFSET 0x808
@@ -333,8 +445,9 @@
 #define  DSI_PAD_CONTROL_VS1_PDIO_CLK   (1 <<  8)
 #define  DSI_PAD_CONTROL_VS1_PDIO(x)    (((x) & 0xf) <<  0)
 
-#define DSI_PAD_CONTROL_CD 0x4c
+#define DSI_PAD_CONTROL_CD 0x4C
 #define DSI_VIDEO_MODE_CONTROL 0x4E
+#define  DSI_CMD_PKT_VID_ENABLE 1
 
 #define DSI_PAD_CONTROL_1 0x4F
 #define DSI_PAD_CONTROL_2 0x50
@@ -348,7 +461,57 @@
 #define DSI_PAD_CONTROL_4 0x52
 #define DSI_INIT_SEQ_DATA_15 0x5F
 
-#define MIPI_CAL_MIPI_BIAS_PAD_CFG2 0x60
+/*! MIPI registers. */
+#define MIPI_CAL_MIPI_CAL_CTRL          (0x00 / 0x4)
+#define MIPI_CAL_CIL_MIPI_CAL_STATUS    (0x08 / 0x4)
+#define MIPI_CAL_CILA_MIPI_CAL_CONFIG   (0x14 / 0x4)
+#define MIPI_CAL_CILB_MIPI_CAL_CONFIG   (0x18 / 0x4)
+#define MIPI_CAL_CILC_MIPI_CAL_CONFIG   (0x1C / 0x4)
+#define MIPI_CAL_CILD_MIPI_CAL_CONFIG   (0x20 / 0x4)
+#define MIPI_CAL_CILE_MIPI_CAL_CONFIG   (0x24 / 0x4)
+#define MIPI_CAL_CILF_MIPI_CAL_CONFIG   (0x28 / 0x4)
+#define MIPI_CAL_DSIA_MIPI_CAL_CONFIG   (0x38 / 0x4)
+#define MIPI_CAL_DSIB_MIPI_CAL_CONFIG   (0x3C / 0x4)
+#define MIPI_CAL_DSIC_MIPI_CAL_CONFIG   (0x40 / 0x4)
+#define MIPI_CAL_DSID_MIPI_CAL_CONFIG   (0x44 / 0x4)
+#define MIPI_CAL_MIPI_BIAS_PAD_CFG0     (0x58 / 0x4)
+#define MIPI_CAL_MIPI_BIAS_PAD_CFG1     (0x5C / 0x4)
+#define MIPI_CAL_MIPI_BIAS_PAD_CFG2     (0x60 / 0x4)
+#define MIPI_CAL_DSIA_MIPI_CAL_CONFIG_2 (0x64 / 0x4)
+#define MIPI_CAL_DSIB_MIPI_CAL_CONFIG_2 (0x68 / 0x4)
+#define MIPI_CAL_DSIC_MIPI_CAL_CONFIG_2 (0x70 / 0x4)
+#define MIPI_CAL_DSID_MIPI_CAL_CONFIG_2 (0x74 / 0x4)
+
+/*! MIPI CMDs. */
+#define MIPI_DSI_DCS_SHORT_WRITE 0x05
+#define MIPI_DSI_DCS_READ 0x06
+#define MIPI_DSI_DCS_SHORT_WRITE_PARAM 0x15
+#define MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE 0x37
+#define MIPI_DSI_DCS_LONG_WRITE 0x39
+
+/*! MIPI DCS CMDs. */
+#define MIPI_DCS_GET_DISPLAY_ID   0x04
+#define MIPI_DCS_ENTER_SLEEP_MODE 0x10
+#define MIPI_DCS_EXIT_SLEEP_MODE  0x11
+#define MIPI_DCS_SET_DISPLAY_ON   0x29
+
+/* Switch Panels:
+ * [10] 81 [26]: JDI LPM062M326A
+ * [10] 96 [09]: JDI LAM062M109A
+ * [20] 93 [0F]: InnoLux P062CCA-AZ1 (Rev A1)
+ * [20] XX [10]: InnoLux P062CCA-AZ2 [UNCONFIRMED ID]
+ * [30] 94 [0F]: AUO A062TAN01 (59.06A33.001)
+ * [30] XX [10]: AUO A062TAN02 (59.06A33.002) [UNCONFIRMED ID]
+ */
+
+enum
+{
+	PANEL_JDI_LPM062M     = 0x10,
+	PANEL_INL_P062CCA_AZ1 = 0x0F20,
+	PANEL_AUO_A062TAN01   = 0x0F30,
+	PANEL_INL_P062CCA_AZ2 = 0x1020,
+	PANEL_AUO_A062TAN02   = 0x1030
+};
 
 void display_init();
 void display_backlight_pwm_init();
@@ -363,5 +526,8 @@ void display_backlight_brightness(u32 brightness, u32 step_delay);
 
 /*! Init display in full 1280x720 resolution (B8G8R8A8, line stride 768, framebuffer size = 1280*768*4 bytes). */
 u32 *display_init_framebuffer();
+void display_init_cursor(void *crs_fb, u32 size);
+void display_set_pos_cursor(u32 x, u32 y);
+void display_deinit_cursor();
 
 #endif

source/gfx/di.inl

@@ -1,6 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
-* Copyright (C) 2018 CTCaer
+* Copyright (c) 2018-2019 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,
@@ -15,16 +15,8 @@
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-//Clock config.
-static const cfg_op_t _display_config_1[4] = {
-	{0x4E, 0x40000000}, //CLK_RST_CONTROLLER_CLK_SOURCE_DISP1
-	{0x34, 0x4830A001}, //CLK_RST_CONTROLLER_PLLD_BASE
-	{0x36, 0x20},       //CLK_RST_CONTROLLER_PLLD_MISC1
-	{0x37, 0x2D0AAA}    //CLK_RST_CONTROLLER_PLLD_MISC
-};
-
 //Display A config.
-static const cfg_op_t _display_config_2[94] = {
+static const cfg_op_t _display_dc_setup_win_config[94] = {
 	{DC_CMD_STATE_ACCESS, 0},
 	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
 	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
@@ -122,13 +114,13 @@ static const cfg_op_t _display_config_2[94] = {
 	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
 	{DC_DISP_DISP_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_COMMAND, 0},
+	{DC_CMD_DISPLAY_COMMAND, DISP_CTRL_MODE_STOP},
 	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE | WIN_A_UPDATE | WIN_B_UPDATE | WIN_C_UPDATE},
 	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ | WIN_A_ACT_REQ | WIN_B_ACT_REQ | WIN_C_ACT_REQ}
 };
 
 //DSI Init config.
-static const cfg_op_t _display_config_3[61] = { 
+static const cfg_op_t _display_dsi_init_config[61] = {
 	{DSI_WR_DATA, 0},
 	{DSI_INT_ENABLE, 0},
 	{DSI_INT_STATUS, 0},
@@ -192,14 +184,14 @@ static const cfg_op_t _display_config_3[61] = {
 	{DSI_INIT_SEQ_CONTROL, 0}
 };
 
-//DSI config (if ver == 0x10).
-static const cfg_op_t _display_config_4[43] = {
-	{DSI_WR_DATA, 0x439},
-	{DSI_WR_DATA, 0x9483FFB9},
+//DSI panel config.
+static const cfg_op_t _display_init_config_jdi[43] = {
+	{DSI_WR_DATA, 0x439},      // MIPI_DSI_DCS_LONG_WRITE: 4 bytes.
+	{DSI_WR_DATA, 0x9483FFB9}, // Enable extension cmd. (Pass: FF 83 94).
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0xBD15},
+	{DSI_WR_DATA, 0x00BD15},   // MIPI_DSI_DCS_SHORT_WRITE_PARAM: 0x0BD.
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0x1939},
+	{DSI_WR_DATA, 0x1939},     // MIPI_DSI_DCS_LONG_WRITE: 25 bytes.
 	{DSI_WR_DATA, 0xAAAAAAD8},
 	{DSI_WR_DATA, 0xAAAAAAEB},
 	{DSI_WR_DATA, 0xAAEBAAAA},
@@ -208,9 +200,9 @@ static const cfg_op_t _display_config_4[43] = {
 	{DSI_WR_DATA, 0xAAEBAAAA},
 	{DSI_WR_DATA, 0xAA},
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0x1BD15},
+	{DSI_WR_DATA, 0x01BD15},   // MIPI_DSI_DCS_SHORT_WRITE_PARAM: 0x1BD.
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0x2739},
+	{DSI_WR_DATA, 0x2739},     // MIPI_DSI_DCS_LONG_WRITE: 39 bytes.
 	{DSI_WR_DATA, 0xFFFFFFD8},
 	{DSI_WR_DATA, 0xFFFFFFFF},
 	{DSI_WR_DATA, 0xFFFFFFFF},
@@ -222,25 +214,25 @@ static const cfg_op_t _display_config_4[43] = {
 	{DSI_WR_DATA, 0xFFFFFFFF},
 	{DSI_WR_DATA, 0xFFFFFF},
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0x2BD15},
+	{DSI_WR_DATA, 0x02BD15},   // MIPI_DSI_DCS_SHORT_WRITE_PARAM: 0x2BD.
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0xF39},
+	{DSI_WR_DATA, 0xF39},      // MIPI_DSI_DCS_LONG_WRITE: 15 bytes.
 	{DSI_WR_DATA, 0xFFFFFFD8},
 	{DSI_WR_DATA, 0xFFFFFFFF},
 	{DSI_WR_DATA, 0xFFFFFFFF},
 	{DSI_WR_DATA, 0xFFFFFF},
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0xBD15},
+	{DSI_WR_DATA, 0x00BD15},   // MIPI_DSI_DCS_SHORT_WRITE_PARAM: 0x0BD.
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0x6D915},
+	{DSI_WR_DATA, 0x06D915},   // MIPI_DSI_DCS_SHORT_WRITE_PARAM: 0x6D9.
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0x439},
-	{DSI_WR_DATA, 0xB9},
+	{DSI_WR_DATA, 0x439},      // MIPI_DSI_DCS_LONG_WRITE: 4 bytes.
+	{DSI_WR_DATA, 0x000000B9}, // Disable extension cmd.
 	{DSI_TRIGGER, DSI_TRIGGER_HOST}
 };
 
-//DSI config.
-static const cfg_op_t _display_config_5[21] = {
+//DSI packet config.
+static const cfg_op_t _display_dsi_packet_config[21] = {
 	{DSI_PAD_CONTROL_1, 0},
 	{DSI_PHY_TIMING_0, 0x6070601},
 	{DSI_PHY_TIMING_1, 0x40A0E05},
@@ -264,15 +256,8 @@ static const cfg_op_t _display_config_5[21] = {
 	{DSI_HOST_CONTROL, 0},
 };
 
-//Clock config.
-static const cfg_op_t _display_config_6[3] = {
-	{0x34, 0x4810C001}, //CLK_RST_CONTROLLER_PLLD_BASE
-	{0x36, 0x20},       //CLK_RST_CONTROLLER_PLLD_MISC1
-	{0x37, 0x2DFC00}    //CLK_RST_CONTROLLER_PLLD_MISC
-};
-
-//DSI config.
-static const cfg_op_t _display_config_7[10] = {
+//DSI mode config.
+static const cfg_op_t _display_dsi_mode_config[10] = {
 	{DSI_TRIGGER, 0},
 	{DSI_CONTROL, 0},
 	{DSI_SOL_DELAY, 6},
@@ -286,17 +271,17 @@ static const cfg_op_t _display_config_7[10] = {
 };
 
 //MIPI CAL config.
-static const cfg_op_t _display_config_8[6] = {
-	{0x18, 0},          // MIPI_CAL_MIPI_BIAS_PAD_CFG2
-	{0x02, 0xF3F10000}, // MIPI_CAL_CIL_MIPI_CAL_STATUS
-	{0x16, 0},          // MIPI_CAL_MIPI_BIAS_PAD_CFG0
-	{0x18, 0},          // MIPI_CAL_MIPI_BIAS_PAD_CFG2
-	{0x18, 0x10010},    // MIPI_CAL_MIPI_BIAS_PAD_CFG2
-	{0x17, 0x300}       // MIPI_CAL_MIPI_BIAS_PAD_CFG1
+static const cfg_op_t _display_mipi_pad_cal_config[6] = {
+	{MIPI_CAL_MIPI_BIAS_PAD_CFG2,  0},
+	{MIPI_CAL_CIL_MIPI_CAL_STATUS, 0xF3F10000},
+	{MIPI_CAL_MIPI_BIAS_PAD_CFG0,  0},
+	{MIPI_CAL_MIPI_BIAS_PAD_CFG2,  0},
+	{MIPI_CAL_MIPI_BIAS_PAD_CFG2,  0x10010},
+	{MIPI_CAL_MIPI_BIAS_PAD_CFG1,  0x300}
 };
 
 //DSI config.
-static const cfg_op_t _display_config_9[4] = {
+static const cfg_op_t _display_dsi_pad_cal_config[4] = {
 	{DSI_PAD_CONTROL_1, 0},
 	{DSI_PAD_CONTROL_2, 0},
 	{DSI_PAD_CONTROL_3, DSI_PAD_PREEMP_PD_CLK(0x3) | DSI_PAD_PREEMP_PU_CLK(0x3) | DSI_PAD_PREEMP_PD(0x03) | DSI_PAD_PREEMP_PU(0x3)},
@@ -304,27 +289,27 @@ static const cfg_op_t _display_config_9[4] = {
 };
 
 //MIPI CAL config.
-static const cfg_op_t _display_config_10[16] = {
-	{0x0E, 0x200200}, // MIPI_CAL_DSIA_MIPI_CAL_CONFIG
-	{0x0F, 0x200200}, // MIPI_CAL_DSIB_MIPI_CAL_CONFIG
-	{0x19, 0x200002}, // MIPI_CAL_DSIA_MIPI_CAL_CONFIG_2
-	{0x1A, 0x200002}, // MIPI_CAL_DSIB_MIPI_CAL_CONFIG_2
-	{0x05, 0},        // MIPI_CAL_CILA_MIPI_CAL_CONFIG
-	{0x06, 0},        // MIPI_CAL_CILB_MIPI_CAL_CONFIG
-	{0x07, 0},        // MIPI_CAL_CILC_MIPI_CAL_CONFIG
-	{0x08, 0},        // MIPI_CAL_CILD_MIPI_CAL_CONFIG
-	{0x09, 0},        // MIPI_CAL_CILE_MIPI_CAL_CONFIG
-	{0x0A, 0},        // MIPI_CAL_CILF_MIPI_CAL_CONFIG
-	{0x10, 0},        // MIPI_CAL_DSIC_MIPI_CAL_CONFIG
-	{0x11, 0},        // MIPI_CAL_DSID_MIPI_CAL_CONFIG
-	{0x1A, 0},        // MIPI_CAL_DSIB_MIPI_CAL_CONFIG_2
-	{0x1C, 0},        // MIPI_CAL_DSIC_MIPI_CAL_CONFIG_2
-	{0x1D, 0},        // MIPI_CAL_DSID_MIPI_CAL_CONFIG_2
-	{0, 0x2A000001}   // MIPI_CAL_DSIA_MIPI_CAL_CONFIG
+static const cfg_op_t _display_mipi_apply_dsi_cal_config[16] = {
+	{MIPI_CAL_DSIA_MIPI_CAL_CONFIG,   0x200200},
+	{MIPI_CAL_DSIB_MIPI_CAL_CONFIG,   0x200200},
+	{MIPI_CAL_DSIA_MIPI_CAL_CONFIG_2, 0x200002},
+	{MIPI_CAL_DSIB_MIPI_CAL_CONFIG_2, 0x200002},
+	{MIPI_CAL_CILA_MIPI_CAL_CONFIG,   0},
+	{MIPI_CAL_CILB_MIPI_CAL_CONFIG,   0},
+	{MIPI_CAL_CILC_MIPI_CAL_CONFIG,   0},
+	{MIPI_CAL_CILD_MIPI_CAL_CONFIG,   0},
+	{MIPI_CAL_CILE_MIPI_CAL_CONFIG,   0},
+	{MIPI_CAL_CILF_MIPI_CAL_CONFIG,   0},
+	{MIPI_CAL_DSIC_MIPI_CAL_CONFIG,   0},
+	{MIPI_CAL_DSID_MIPI_CAL_CONFIG,   0},
+	{MIPI_CAL_DSIB_MIPI_CAL_CONFIG_2, 0},
+	{MIPI_CAL_DSIC_MIPI_CAL_CONFIG_2, 0},
+	{MIPI_CAL_DSID_MIPI_CAL_CONFIG_2, 0},
+	{MIPI_CAL_MIPI_CAL_CTRL,          0x2A000001}
 };
 
 //Display A config.
-static const cfg_op_t _display_config_11[113] = {
+static const cfg_op_t _display_video_disp_controller_enable_config[113] = {
 	{DC_CMD_STATE_ACCESS, 0},
 	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
@@ -405,7 +390,7 @@ static const cfg_op_t _display_config_11[113] = {
 	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
 	{DC_DISP_DISP_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_COMMAND, 0},
+	{DC_CMD_DISPLAY_COMMAND, DISP_CTRL_MODE_STOP},
 	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE | WIN_A_UPDATE | WIN_B_UPDATE | WIN_C_UPDATE},
 	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ | WIN_A_ACT_REQ | WIN_B_ACT_REQ | WIN_C_ACT_REQ},
 	{DC_CMD_STATE_ACCESS, 0},
@@ -415,7 +400,7 @@ static const cfg_op_t _display_config_11[113] = {
 	{DC_DISP_SYNC_WIDTH,  0x10048},
 	{DC_DISP_BACK_PORCH,  0x90048},
 	{DC_DISP_ACTIVE,      0x50002D0},
-	{DC_DISP_FRONT_PORCH, 0xA0088}, // Sources say that this should be above the DC_DISP_ACTIVE cmd.
+	{DC_DISP_FRONT_PORCH, 0xA0088},   // Sources say that this should be above the DC_DISP_ACTIVE cmd.
 	/* End of Display timings */
 	{DC_DISP_SHIFT_CLOCK_OPTIONS, SC1_H_QUALIFIER_NONE | SC0_H_QUALIFIER_NONE},
 	{DC_COM_PIN_OUTPUT_ENABLE(1), 0},
@@ -449,13 +434,13 @@ static const cfg_op_t _display_config_11[113] = {
 };
 
 ////Display A config.
-static const cfg_op_t _display_config_12[17] = {
+static const cfg_op_t _display_video_disp_controller_disable_config[17] = {
 	{DC_DISP_FRONT_PORCH, 0xA0088},
 	{DC_CMD_INT_MASK, 0},
 	{DC_CMD_STATE_ACCESS, 0},
 	{DC_CMD_INT_ENABLE, 0},
 	{DC_CMD_CONT_SYNCPT_VSYNC, 0},
-	{DC_CMD_DISPLAY_COMMAND, 0},
+	{DC_CMD_DISPLAY_COMMAND, DISP_CTRL_MODE_STOP},
 	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
 	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
 	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
@@ -470,7 +455,7 @@ static const cfg_op_t _display_config_12[17] = {
 };
 
 //DSI config.
-static const cfg_op_t _display_config_13[16] = {
+static const cfg_op_t _display_dsi_timing_deinit_config[16] = {
 	{DSI_POWER_CONTROL, 0},
 	{DSI_PAD_CONTROL_1, 0},
 	{DSI_PHY_TIMING_0, 0x6070601},
@@ -490,11 +475,11 @@ static const cfg_op_t _display_config_13[16] = {
 };
 
 //DSI config (if ver == 0x10).
-static const cfg_op_t _display_config_14[22] = {
-	{DSI_WR_DATA, 0x439},
-	{DSI_WR_DATA, 0x9483FFB9},
+static const cfg_op_t _display_deinit_config_jdi[22] = {
+	{DSI_WR_DATA, 0x439},      // MIPI_DSI_DCS_LONG_WRITE: 4 bytes.
+	{DSI_WR_DATA, 0x9483FFB9}, // Enable extension cmd. (Pass: FF 83 94).
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0x2139},
+	{DSI_WR_DATA, 0x2139},     // MIPI_DSI_DCS_LONG_WRITE: 33 bytes.
 	{DSI_WR_DATA, 0x191919D5},
 	{DSI_WR_DATA, 0x19191919},
 	{DSI_WR_DATA, 0x19191919},
@@ -505,60 +490,107 @@ static const cfg_op_t _display_config_14[22] = {
 	{DSI_WR_DATA, 0x19191919},
 	{DSI_WR_DATA, 0x19},
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0xB39},
-	{DSI_WR_DATA, 0x4F0F41B1},
+	{DSI_WR_DATA, 0xB39},      // MIPI_DSI_DCS_LONG_WRITE: 11 bytes.
+	{DSI_WR_DATA, 0x4F0F41B1}, // Set Power control.
 	{DSI_WR_DATA, 0xF179A433},
-	{DSI_WR_DATA, 0x2D81},
+	{DSI_WR_DATA, 0x002D81},
 	{DSI_TRIGGER, DSI_TRIGGER_HOST},
-	{DSI_WR_DATA, 0x439},
-	{DSI_WR_DATA, 0xB9},
+	{DSI_WR_DATA, 0x439},      // MIPI_DSI_DCS_LONG_WRITE: 4 bytes.
+	{DSI_WR_DATA, 0x000000B9}, // Disable extension cmd.
 	{DSI_TRIGGER, DSI_TRIGGER_HOST}
 };
 
+static const cfg_op_t _display_deinit_config_auo[37] = {
+	{DSI_WR_DATA, 0x439},      // MIPI_DSI_DCS_LONG_WRITE: 4 bytes.
+	{DSI_WR_DATA, 0x9483FFB9}, // Enable extension cmd. (Pass: FF 83 94).
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x2C39},     // MIPI_DSI_DCS_LONG_WRITE: 44 bytes.
+	{DSI_WR_DATA, 0x191919D5},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x2C39},     // MIPI_DSI_DCS_LONG_WRITE: 44 bytes.
+	{DSI_WR_DATA, 0x191919D6},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_WR_DATA, 0x19191919},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0xB39},      // MIPI_DSI_DCS_LONG_WRITE: 11 bytes.
+	{DSI_WR_DATA, 0x711148B1}, // Set Power control. (Not deep standby, BT1 / XDK, VRH gamma volt adj 49 / x40).
+	// Set Power control. (NVRH gamma volt adj 9, Amplifier current small / x30, FS0 freq Fosc/80 / FS1 freq Fosc/32, Enter standby / PON / VCOMG).
+	{DSI_WR_DATA, 0x71143209},
+	{DSI_WR_DATA, 0x114D31},   // Set Power control. (Unknown).
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x439},      // MIPI_DSI_DCS_LONG_WRITE: 4 bytes.
+	{DSI_WR_DATA, 0x000000B9}, // Disable extension cmd.
+	{DSI_TRIGGER, DSI_TRIGGER_HOST}
+};
+
+static const cfg_op_t _display_init_config_invert[3] = {
+	{DSI_WR_DATA, 0x239},
+	{DSI_WR_DATA, 0x02C1}, // INV_EN.
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+};
+
 //Display A config.
 static const cfg_op_t cfg_display_one_color[8] = {
-	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT}, //Enable window A.
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT}, //Enable window B.
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT}, //Enable window C.
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE}, //DSI_ENABLE
-	{DC_CMD_DISPLAY_COMMAND, DISP_CTRL_MODE_C_DISPLAY} //DISPLAY_CTRL_MODE: continuous display.
+	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE},
+	{DC_CMD_DISPLAY_COMMAND, DISP_CTRL_MODE_C_DISPLAY} // Continuous display.
 };
 
 //Display A config.
 static const cfg_op_t cfg_display_framebuffer[32] = {
-	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT}, //Enable window C.
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT}, //Enable window B.
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT}, //Enable window A.
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE}, //DSI_ENABLE
-	{DC_WIN_COLOR_DEPTH, WIN_COLOR_DEPTH_B8G8R8A8}, //T_A8R8G8B8 //NX Default: T_A8B8G8R8, WIN_COLOR_DEPTH_R8G8B8A8
+	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE},
+	{DC_WIN_COLOR_DEPTH, WIN_COLOR_DEPTH_B8G8R8A8}, //NX Default: T_A8B8G8R8, WIN_COLOR_DEPTH_R8G8B8A8
 	{DC_WIN_WIN_OPTIONS, 0},
 	{DC_WIN_WIN_OPTIONS, 0},
 	{DC_WIN_POSITION, 0}, //(0,0)
 	{DC_WIN_H_INITIAL_DDA, 0},
 	{DC_WIN_V_INITIAL_DDA, 0},
-	{DC_WIN_PRESCALED_SIZE, V_PRESCALED_SIZE(1280) | H_PRESCALED_SIZE(2880)}, //Pre-scaled size: 1280x2880 bytes.
-	{DC_WIN_DDA_INC, V_DDA_INC(0x1000) | H_DDA_INC(0x1000)},
-	{DC_WIN_SIZE, V_SIZE(1280) | H_SIZE(720)}, //Window size: 1280 vertical lines x 720 horizontal pixels.
-	{DC_WIN_LINE_STRIDE, UV_LINE_STRIDE(720 * 2) | LINE_STRIDE(720 * 4)}, //768*2x768*4 (= 0x600 x 0xC00) bytes, see TRM for alignment requirements.
-	{DC_WIN_BUFFER_CONTROL, 0},
-	{DC_WINBUF_SURFACE_KIND, 0}, //Regular surface.
-	{DC_WINBUF_START_ADDR, 0xC0000000}, //Framebuffer address.
+	{DC_WIN_PRESCALED_SIZE, V_PRESCALED_SIZE(1280) | H_PRESCALED_SIZE(2880)},
+	{DC_WIN_DDA_INC, V_DDA_INC(0x1000) | H_DDA_INC(0x1000)}, // 1.0x
+	{DC_WIN_SIZE, V_SIZE(1280) | H_SIZE(720)},
+	{DC_WIN_LINE_STRIDE, UV_LINE_STRIDE(720 * 2) | LINE_STRIDE(720 * 4)}, // 720*2x720*4 (= 0x600 x 0xC00) bytes, see TRM for alignment requirements.
+	{DC_WIN_BUFFER_CONTROL, BUFFER_CONTROL_HOST},
+	{DC_WINBUF_SURFACE_KIND, PITCH},
+	{DC_WINBUF_START_ADDR, IPL_FB_ADDRESS}, // Framebuffer address.
 	{DC_WINBUF_ADDR_H_OFFSET, 0},
 	{DC_WINBUF_ADDR_V_OFFSET, 0},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE}, //DSI_ENABLE
+	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE}, //DSI_ENABLE
+	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE},
 	{DC_WIN_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE}, //DSI_ENABLE
-	{DC_WIN_WIN_OPTIONS, WIN_ENABLE}, //Enable window AD.
-	{DC_CMD_DISPLAY_COMMAND, DISP_CTRL_MODE_C_DISPLAY}, //DISPLAY_CTRL_MODE: continuous display.
-	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE | WIN_A_UPDATE}, //General update; window A update.
-	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ | WIN_A_ACT_REQ} //General activation request; window A activation request.
+	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE},
+	{DC_WIN_WIN_OPTIONS, WIN_ENABLE}, // Enable window AD.
+	{DC_CMD_DISPLAY_COMMAND, DISP_CTRL_MODE_C_DISPLAY}, // Continuous display.
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE | WIN_A_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ | WIN_A_ACT_REQ}
 };

source/gfx/gfx.c

@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (C) 2018-2019 CTCaer
- * Copyright (c) 2019 shchmue
+ * Copyright (c) 2018-2020 CTCaer
+ * Copyright (c) 2019-2020 shchmue
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -20,6 +20,10 @@
 #include <string.h>
 #include "gfx.h"
 
+// Global gfx console and context.
+gfx_ctxt_t gfx_ctxt;
+gfx_con_t gfx_con;
+
 static const u8 _gfx_font[] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Char 032 ( )
 	0x00, 0x30, 0x30, 0x18, 0x18, 0x00, 0x0C, 0x00, // Char 033 (!)
@@ -128,12 +132,12 @@ void gfx_init_ctxt(u32 *fb, u32 width, u32 height, u32 stride)
 
 void gfx_clear_grey(u8 color)
 {
-	memset(gfx_ctxt.fb, color, 0x3C0000);
+	memset(gfx_ctxt.fb, color, gfx_ctxt.width * gfx_ctxt.height * 4);
 }
 
 void gfx_clear_color(u32 color)
 {
-	for (u32 i = 0; i < gfx_ctxt.height * gfx_ctxt.stride; i++)
+	for (u32 i = 0; i < gfx_ctxt.width * gfx_ctxt.height; i++)
 		gfx_ctxt.fb[i] = color;
 }
 
@@ -188,7 +192,7 @@ void gfx_putc(char c)
 
 			for (u32 i = 0; i < 16; i+=2)
 			{
-				u8 v = *cbuf++;
+				u8 v = *cbuf;
 				for (u32 k = 0; k < 2; k++)
 				{
 					for (u32 j = 0; j < 8; j++)
@@ -213,9 +217,11 @@ void gfx_putc(char c)
 					fb += gfx_ctxt.stride - 16;
 					v = *cbuf;
 				}
+				cbuf++;
 			}
 			gfx_con.x += 16;
-			if (gfx_con.x >= gfx_ctxt.width - 16) {
+			if (gfx_con.x >= gfx_ctxt.width - 16)
+			{
 				gfx_con.x = 0;
 				gfx_con.y += 16;
 			}
@@ -249,7 +255,8 @@ void gfx_putc(char c)
 				fb += gfx_ctxt.stride - 8;
 			}
 			gfx_con.x += 8;
-			if (gfx_con.x >= gfx_ctxt.width - 8) {
+			if (gfx_con.x >= gfx_ctxt.width - 8)
+			{
 				gfx_con.x = 0;
 				gfx_con.y += 8;
 			}
@@ -263,7 +270,6 @@ void gfx_putc(char c)
 		}
 		break;
 	}
-	
 }
 
 void gfx_puts(const char *s)

source/gfx/gfx.h

@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (C) 2018-2019 CTCaer
- * Copyright (C) 2018 M4xw
+ * Copyright (c) 2018-2020 CTCaer
+ * Copyright (c) 2018 M4xw
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -49,7 +49,7 @@ void gfx_set_rect_argb(const u32 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 po
 void gfx_render_bmp_argb(const u32 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y);
 
 // Global gfx console and context.
-gfx_ctxt_t gfx_ctxt;
-gfx_con_t gfx_con;
+extern gfx_ctxt_t gfx_ctxt;
+extern gfx_con_t gfx_con;
 
 #endif

source/gfx/tui.c

@@ -0,0 +1,226 @@
+/*
+ * 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,
+ * 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 "di.h"
+#include "tui.h"
+#include "../utils/btn.h"
+#include "../config/config.h"
+#include "../power/max17050.h"
+#include "../utils/util.h"
+
+#ifdef MENU_LOGO_ENABLE
+extern u8 *Kc_MENU_LOGO;
+#define X_MENU_LOGO       119
+#define Y_MENU_LOGO        57
+#define X_POS_MENU_LOGO   577
+#define Y_POS_MENU_LOGO  1179
+#endif //MENU_LOGO_ENABLE
+
+extern hekate_config h_cfg;
+
+void tui_sbar(bool force_update)
+{
+	u32 cx, cy;
+
+	u32 timePassed = get_tmr_s() - h_cfg.sbar_time_keeping;
+	if (!force_update)
+		if (timePassed < 5)
+			return;
+
+	u8 prevFontSize = gfx_con.fntsz;
+	gfx_con.fntsz = 16;
+	h_cfg.sbar_time_keeping = get_tmr_s();
+
+	u32 battPercent = 0;
+	int battVoltCurr = 0;
+
+	gfx_con_getpos(&cx, &cy);
+	gfx_con_setpos(0,  1260);
+
+	max17050_get_property(MAX17050_RepSOC, (int *)&battPercent);
+	max17050_get_property(MAX17050_VCELL, &battVoltCurr);
+
+	gfx_clear_partial_grey(0x30, 1256, 24);
+	gfx_printf("%K%k Battery: %d.%d%% (%d mV) - Charge:", 0xFF303030, 0xFF888888,
+		(battPercent >> 8) & 0xFF, (battPercent & 0xFF) / 26, battVoltCurr);
+
+	max17050_get_property(MAX17050_Current, &battVoltCurr);
+
+	if (battVoltCurr >= 0)
+		gfx_printf(" %k+%d mA%k%K\n",
+			0xFF008800, battVoltCurr / 1000, 0xFFCCCCCC, 0xFF1B1B1B);
+	else
+		gfx_printf(" %k-%d mA%k%K\n",
+			0xFF880000, (~battVoltCurr) / 1000, 0xFFCCCCCC, 0xFF1B1B1B);
+	gfx_con.fntsz = prevFontSize;
+	gfx_con_setpos(cx, cy);
+}
+
+void tui_pbar(int x, int y, u32 val, u32 fgcol, u32 bgcol)
+{
+	u32 cx, cy;
+	if (val > 200)
+		val = 200;
+
+	gfx_con_getpos(&cx, &cy);
+
+	gfx_con_setpos(x, y);
+
+	gfx_printf("%k[%3d%%]%k", fgcol, val, 0xFFCCCCCC);
+
+	x += 7 * gfx_con.fntsz;
+
+	for (int i = 0; i < (gfx_con.fntsz >> 3) * 6; i++)
+	{
+		gfx_line(x, y + i + 1, x + 3 * val, y + i + 1, fgcol);
+		gfx_line(x + 3 * val, y + i + 1, x + 3 * 100, y + i + 1, bgcol);
+	}
+
+	gfx_con_setpos(cx, cy);
+
+	// Update status bar.
+	tui_sbar(false);
+}
+
+void *tui_do_menu(menu_t *menu)
+{
+	int idx = 0, prev_idx = 0, cnt = 0x7FFFFFFF;
+
+	gfx_clear_partial_grey(0x1B, 0, 1256);
+	tui_sbar(true);
+
+#ifdef MENU_LOGO_ENABLE
+	gfx_set_rect_rgb(Kc_MENU_LOGO,
+		X_MENU_LOGO, Y_MENU_LOGO, X_POS_MENU_LOGO, Y_POS_MENU_LOGO);
+#endif //MENU_LOGO_ENABLE
+
+	while (true)
+	{
+		gfx_con_setcol(0xFFCCCCCC, 1, 0xFF1B1B1B);
+		gfx_con_setpos(menu->x, menu->y);
+		gfx_printf("[%kLo%kck%kpi%kck%k_R%kCM%k v%d.%d.%d%k]\n\n",
+			colors[0], colors[1], colors[2], colors[3], colors[4], colors[5], 0xFFFF00FF, LP_VER_MJ, LP_VER_MN, LP_VER_BF, 0xFFCCCCCC);
+
+		// Skip caption or seperator lines selection.
+		while (menu->ents[idx].type == MENT_CAPTION ||
+			menu->ents[idx].type == MENT_CHGLINE)
+		{
+			if (prev_idx <= idx || (!idx && prev_idx == cnt - 1))
+			{
+				idx++;
+				if (idx > (cnt - 1))
+				{
+					idx = 0;
+					prev_idx = 0;
+				}
+			}
+			else
+			{
+				idx--;
+				if (idx < 0)
+				{
+					idx = cnt - 1;
+					prev_idx = cnt;
+				}
+			}
+		}
+		prev_idx = idx;
+
+		// Draw the menu.
+		for (cnt = 0; menu->ents[cnt].type != MENT_END; cnt++)
+		{
+			if (cnt == idx)
+				gfx_con_setcol(0xFF1B1B1B, 1, 0xFFCCCCCC);
+			else
+				gfx_con_setcol(0xFFCCCCCC, 1, 0xFF1B1B1B);
+			if (menu->ents[cnt].type != MENT_CHGLINE && menu->ents[cnt].type != MENT_MENU)
+			{
+				if (cnt == idx)
+					gfx_printf(" %s", menu->ents[cnt].caption);
+				else
+					gfx_printf("%k %s", menu->ents[cnt].color, menu->ents[cnt].caption);
+			}
+			if(menu->ents[cnt].type == MENT_MENU)
+				gfx_printf("%k...", 0xFF0099EE);
+			gfx_printf(" \n");
+		}
+		gfx_con_setcol(0xFFCCCCCC, 1, 0xFF1B1B1B);
+		gfx_putc('\n');
+
+		// Print help and battery status.
+		gfx_con_setpos(0,  1127);
+		if (h_cfg.emummc_force_disable)
+			gfx_printf("%kNo emuMMC config found.\n", 0xFF800000);
+		gfx_con_setpos(0,  1191);
+		gfx_printf("%k VOL: Move up/down\n PWR: Select option%k", 0xFF555555, 0xFFCCCCCC);
+
+		display_backlight_brightness(h_cfg.backlight, 1000);
+
+		// Wait for user command.
+		u32 btn = btn_wait();
+
+		if (btn & BTN_VOL_DOWN && idx < (cnt - 1))
+			idx++;
+		else if (btn & BTN_VOL_DOWN && idx == (cnt - 1))
+		{
+			idx = 0;
+			prev_idx = -1;
+		}
+		if (btn & BTN_VOL_UP && idx > 0)
+			idx--;
+		else if (btn & BTN_VOL_UP && idx == 0)
+		{
+			idx = cnt - 1;
+			prev_idx = cnt;
+		}
+		if (btn & BTN_POWER)
+		{
+			ment_t *ent = &menu->ents[idx];
+			switch (ent->type)
+			{
+			case MENT_HANDLER:
+				ent->handler(ent->data);
+				break;
+			case MENT_MENU:
+				return tui_do_menu(ent->menu);
+				break;
+			case MENT_DATA:
+				return ent->data;
+				break;
+			case MENT_BACK:
+				return NULL;
+				break;
+			case MENT_HDLR_RE:
+				ent->handler(ent);
+				if (!ent->data)
+					return NULL;
+				break;
+			default:
+				break;
+			}
+			gfx_con.fntsz = 16;
+			gfx_clear_partial_grey(0x1B, 0, 1256);
+#ifdef MENU_LOGO_ENABLE
+			gfx_set_rect_rgb(Kc_MENU_LOGO,
+				X_MENU_LOGO, Y_MENU_LOGO, X_POS_MENU_LOGO, Y_POS_MENU_LOGO);
+#endif //MENU_LOGO_ENABLE
+		}
+		tui_sbar(false);
+	}
+
+	return NULL;
+}

source/gfx/tui.h

@@ -0,0 +1,66 @@
+/*
+* 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,
+* 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 _TUI_H_
+#define _TUI_H_
+
+#include "../utils/types.h"
+#include "gfx.h"
+
+#define MENT_END     0
+#define MENT_HANDLER 1
+#define MENT_MENU    2
+#define MENT_DATA    3
+#define MENT_BACK    4
+#define MENT_CAPTION 5
+#define MENT_CHGLINE 6
+#define MENT_HDLR_RE 7
+
+typedef struct _ment_t
+{
+	u32 type;
+	const char *caption;
+	u32 color;
+	void *data;
+	union
+	{
+		void(*handler)(void *);
+		struct _menu_t *menu;
+	};
+} ment_t;
+
+typedef struct _menu_t
+{
+	ment_t *ents;
+	const char *caption;
+	u32 x;
+	u32 y;
+} menu_t;
+
+#define MDEF_END() {MENT_END}
+#define MDEF_HANDLER(caption, _handler, color) { MENT_HANDLER, caption, color, NULL, { .handler = _handler } }
+#define MDEF_HANDLER_EX(caption, data, _handler, color) { MENT_HANDLER, caption, color, data, { .handler = _handler } }
+#define MDEF_MENU(caption, _menu) { MENT_MENU, caption, 0, NULL, { .menu = _menu } }
+#define MDEF_BACK() { MENT_BACK, "Back" }
+#define MDEF_CAPTION(caption, color) { MENT_CAPTION, caption, color }
+#define MDEF_CHGLINE() {MENT_CHGLINE}
+
+void tui_sbar(bool force_update);
+void tui_pbar(int x, int y, u32 val, u32 fgcol, u32 bgcol);
+void *tui_do_menu(menu_t *menu);
+
+#endif

source/hos/fss.c

@@ -0,0 +1,220 @@
+/*
+ * Atmosphère Fusée Secondary Storage parser.
+ *
+ * Copyright (c) 2019-2020 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/>.
+ */
+
+#include <string.h>
+
+#include "fss.h"
+// #include "hos.h"
+#include "../config/config.h"
+#include "../libs/fatfs/ff.h"
+#include "../mem/heap.h"
+#include "../storage/emummc.h"
+#include "../storage/nx_sd.h"
+
+#include "../gfx/gfx.h"
+#define DPRINTF(...)
+
+extern hekate_config h_cfg;
+
+extern bool is_ipl_updated(void *buf, char *path, bool force);
+
+// FSS0 Magic and Meta header offset.
+#define FSS0_MAGIC 0x30535346
+#define FSS0_META_OFFSET 0x4
+
+// FSS0 Content Types.
+#define CNT_TYPE_FSP 0
+#define CNT_TYPE_EXO 1  // Exosphere (Secure Monitor).
+#define CNT_TYPE_WBT 2  // Warmboot (SC7Exit fw).
+#define CNT_TYPE_RBT 3  // Rebootstub (Warmboot based reboot fw).
+#define CNT_TYPE_SP1 4  // Sept Primary (TSEC and Sept Secondary loader).
+#define CNT_TYPE_SP2 5  // Sept Secondary (Acts as pkg11 and derives keys).
+#define CNT_TYPE_KIP 6  // KIP1 (Used for replacement or addition).
+#define CNT_TYPE_BMP 7
+#define CNT_TYPE_EMC 8
+#define CNT_TYPE_KLD 9  // Kernel Loader.
+#define CNT_TYPE_KRN 10 // Kernel.
+
+// FSS0 Content Flags.
+#define CNT_FLAG0_EXPERIMENTAL (1 << 0)
+
+// FSS0 Meta Header.
+typedef struct _fss_meta_t
+{
+	u32 magic;
+	u32 size;
+	u32 crt0_off;
+	u32 cnt_off;
+	u32 cnt_count;
+	u32 hos_ver;
+	u32 version;
+	u32 git_rev;
+} fss_meta_t;
+
+// FSS0 Content Header.
+typedef struct _fss_content_t
+{
+	u32 offset;
+	u32 size;
+	u8 type;
+	u8 flags0;
+	u8 flags1;
+	u8 flags2;
+	u32 rsvd1;
+	char name[0x10];
+} fss_content_t;
+
+int parse_fss(launch_ctxt_t *ctxt, const char *path, fss0_sept_t *sept_ctxt)
+{
+	FIL fp;
+
+	bool stock = false;
+	int sept_used = 0;
+
+	if (!sept_ctxt)
+	{
+		LIST_FOREACH_ENTRY(ini_kv_t, kv, &ctxt->cfg->kvs, link)
+		{
+			if (!strcmp("stock", kv->key))
+				if (kv->val[0] == '1')
+					stock = true;
+		}
+
+		if (stock && ctxt->pkg1_id->kb <= KB_FIRMWARE_VERSION_620 && (!emu_cfg.enabled || h_cfg.emummc_force_disable))
+			return 1;
+	}
+
+	if (f_open(&fp, path, FA_READ) != FR_OK)
+		return 0;
+
+	void *fss = malloc(f_size(&fp));
+
+	// Read first 1024 bytes of the fss file.
+	f_read(&fp, fss, 1024, NULL);
+
+	// Get FSS0 Meta header offset.
+	u32 fss_meta_addr = *(u32 *)(fss + FSS0_META_OFFSET);
+	fss_meta_t *fss_meta = (fss_meta_t *)(fss + fss_meta_addr);
+
+	// Check if valid FSS0 and parse it.
+	if (fss_meta->magic == FSS0_MAGIC)
+	{
+		gfx_printf("Found FSS0, Atmosphere %d.%d.%d-%08x\n"
+			"Max HOS supported: %d.%d.%d\n"
+			"Unpacking and loading components..  ",
+			fss_meta->version >> 24, (fss_meta->version >> 16) & 0xFF, (fss_meta->version >> 8) & 0xFF, fss_meta->git_rev,
+			fss_meta->hos_ver >> 24, (fss_meta->hos_ver >> 16) & 0xFF, (fss_meta->hos_ver >> 8) & 0xFF);
+
+		if (!sept_ctxt)
+		{
+			ctxt->atmosphere = true;
+			ctxt->fss0_hosver = fss_meta->hos_ver;
+		}
+
+		// Parse FSS0 contents.
+		fss_content_t *curr_fss_cnt = (fss_content_t *)(fss + fss_meta->cnt_off);
+		void *content;
+		for (u32 i = 0; i < fss_meta->cnt_count; i++)
+		{
+			content = (void *)(fss + curr_fss_cnt[i].offset);
+
+			// Check if offset is inside limits.
+			if ((curr_fss_cnt[i].offset + curr_fss_cnt[i].size) > fss_meta->size)
+				continue;
+
+			// If content is experimental and experimental flag is not enabled, skip it.
+			if ((curr_fss_cnt[i].flags0 & CNT_FLAG0_EXPERIMENTAL) && !ctxt->fss0_enable_experimental)
+				continue;
+
+			// Parse content.
+			if (!sept_ctxt)
+			{
+				// Prepare content context.
+				switch (curr_fss_cnt[i].type)
+				{
+				case CNT_TYPE_KIP:
+					if (stock)
+						continue;
+					merge_kip_t *mkip1 = (merge_kip_t *)malloc(sizeof(merge_kip_t));
+					mkip1->kip1 = content;
+					list_append(&ctxt->kip1_list, &mkip1->link);
+					DPRINTF("Loaded %s.kip1 from FSS0 (size %08X)\n", curr_fss_cnt[i].name, curr_fss_cnt[i].size);
+					break;
+				case CNT_TYPE_EXO:
+					ctxt->secmon_size = curr_fss_cnt[i].size;
+					ctxt->secmon = content;
+					break;
+				case CNT_TYPE_WBT:
+					ctxt->warmboot_size = curr_fss_cnt[i].size;
+					ctxt->warmboot = content;
+					break;
+				default:
+					continue;
+				}
+
+				// Load content to launch context.
+				f_lseek(&fp, curr_fss_cnt[i].offset);
+				f_read(&fp, content, curr_fss_cnt[i].size, NULL);
+			}
+			else
+			{
+				// Load sept content directly to launch context.
+				switch (curr_fss_cnt[i].type)
+				{
+				case CNT_TYPE_SP1:
+					f_lseek(&fp, curr_fss_cnt[i].offset);
+					f_read(&fp, sept_ctxt->sept_primary, curr_fss_cnt[i].size, NULL);
+					break;
+				case CNT_TYPE_SP2:
+					if (!memcmp(curr_fss_cnt[i].name, (sept_ctxt->kb < KB_FIRMWARE_VERSION_810) ? "septsecondary00" : "septsecondary01", 15))
+					{
+						f_lseek(&fp, curr_fss_cnt[i].offset);
+						f_read(&fp, sept_ctxt->sept_secondary, curr_fss_cnt[i].size, NULL);
+						sept_used = 1;
+						goto out;
+					}
+					break;
+				default:
+					break;
+				}
+			}
+		}
+
+out:
+		gfx_printf("Done!\n");
+		f_close(&fp);
+
+		return (!sept_ctxt ? 1 : sept_used);
+	}
+
+	f_close(&fp);
+	free(fss);
+
+	return 0;
+}
+
+int load_sept_from_ffs0(fss0_sept_t *sept_ctxt)
+{
+	LIST_FOREACH_ENTRY(ini_kv_t, kv, &sept_ctxt->cfg_sec->kvs, link)
+	{
+		if (!strcmp("fss0", kv->key))
+			return parse_fss(NULL, kv->val, sept_ctxt);
+	}
+
+	return 0;
+}

source/hos/fss.h

@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2019 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 _FSS_H_
+#define _FSS_H_
+
+#include "hos.h"
+
+typedef struct _fss0_sept_t
+{
+	u32 kb;
+	ini_sec_t *cfg_sec;
+	void *sept_primary;
+	void *sept_secondary;
+
+} fss0_sept_t;
+
+int parse_fss(launch_ctxt_t *ctxt, const char *path, fss0_sept_t *sept_ctxt);
+int load_sept_from_ffs0(fss0_sept_t *sept_ctxt);
+
+#endif

source/hos/hos.h

@@ -0,0 +1,120 @@
+/*
+ * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2020 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 _HOS_H_
+#define _HOS_H_
+
+#include "pkg1.h"
+#include "pkg2.h"
+#include "../utils/types.h"
+#include "../config/ini.h"
+#include "../sec/tsec.h"
+
+#include <assert.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_700 7
+#define KB_FIRMWARE_VERSION_810 8
+#define KB_FIRMWARE_VERSION_900 9
+#define KB_FIRMWARE_VERSION_910 10
+#define KB_FIRMWARE_VERSION_MAX KB_FIRMWARE_VERSION_910
+
+#define HOS_PKG11_MAGIC 0x31314B50
+#define HOS_EKS_MAGIC   0x30534B45
+
+typedef struct _exo_ctxt_t
+{
+	bool no_user_exceptions;
+	bool user_pmu;
+	bool *cal0_blank;
+	bool *cal0_allow_writes_sys;
+} exo_ctxt_t;
+
+typedef struct _hos_eks_keys_t
+{
+	u8 dkg[0x10];
+	u8 mkk[0x10];
+	u8 fdk[0x10];
+	u8 dkk[0x10];
+} hos_eks_keys_t;
+
+typedef struct _hos_eks_mbr_t
+{
+	u32 magic;
+	u32 enabled;
+	u32 sbk_low[2];
+	hos_eks_keys_t keys[6];
+	u32 magic2;
+	u32 rsvd2[3];
+} hos_eks_mbr_t;
+
+static_assert(sizeof(hos_eks_mbr_t) == 416, "HOS EKS storage bigger than MBR!");
+
+typedef struct _launch_ctxt_t
+{
+	void *keyblob;
+
+	void *pkg1;
+	const pkg1_id_t *pkg1_id;
+	const pkg2_kernel_id_t *pkg2_kernel_id;
+
+	void *warmboot;
+	u32   warmboot_size;
+	void *secmon;
+	u32   secmon_size;
+
+	void *pkg2;
+	u32   pkg2_size;
+	bool  new_pkg2;
+
+	void  *kernel;
+	u32    kernel_size;
+	link_t kip1_list;
+	char*  kip1_patches;
+
+	u32  fss0_hosver;
+	bool svcperm;
+	bool debugmode;
+	bool stock;
+	bool atmosphere;
+	bool fss0_enable_experimental;
+	bool emummc_forced;
+
+	exo_ctxt_t exo_cfg;
+
+	ini_sec_t *cfg;
+} launch_ctxt_t;
+
+typedef struct _merge_kip_t
+{
+	void *kip1;
+	link_t link;
+} merge_kip_t;
+
+void hos_eks_get();
+void hos_eks_save(u32 kb);
+void hos_eks_clear(u32 kb);
+int  hos_launch(ini_sec_t *cfg);
+int  hos_keygen(u8 *keyblob, u32 kb, tsec_ctxt_t *tsec_ctxt, launch_ctxt_t *hos_ctxt);
+
+#endif

source/hos/pkg1.c

@@ -22,19 +22,27 @@
 #include "pkg1.h"
 #include "../sec/se.h"
 
+#define HASH_ORDER_100_100 {2, 3, 4, 0, 5, 6, 1}
+#define HASH_ORDER_200_510 {2, 3, 4, 0, 5, 7, 10, 12, 11, 6, 8, 1}
+#define HASH_ORDER_600_620 {6, 5, 10, 7, 8, 2, 3, 4, 0, 12, 11, 1}
+#define HASH_ORDER_700_10x {6, 5, 10, 7, 8, 2, 3, 4, 0, 12, 11, 9, 1}
+
 static const pkg1_id_t _pkg1_ids[] = {
-	{ "20161121183008", 0 }, //1.0.0
-	{ "20170210155124", 0 }, //2.0.0 - 2.3.0
-	{ "20170519101410", 1 }, //3.0.0
-	{ "20170710161758", 2 }, //3.0.1 - 3.0.2
-	{ "20170921172629", 3 }, //4.0.0 - 4.1.0
-	{ "20180220163747", 4 }, //5.0.0 - 5.1.0
-	{ "20180802162753", 5 }, //6.0.0 - 6.1.0
-	{ "20181107105733", 6 }, //6.2.0
-	{ "20181218175730", 7 }, //7.0.0
-	{ "20190208150037", 7 }, //7.0.1
-	{ "20190314172056", 7 }, //8.0.0
-	{ "20190531152432", 8 }, //8.1.0
+	{ "20161121183008", 0, {0x1b517, 0x125bc2, 1, 16,  6, HASH_ORDER_100_100,      0, 0x449dc} }, //1.0.0
+	{ "20170210155124", 0, {0x1d226,   0x26fe, 0, 16, 11, HASH_ORDER_200_510, 0x557b, 0x3d41a} }, //2.0.0 - 2.3.0
+	{ "20170519101410", 1, {0x1ffa6,   0x298b, 0, 16, 11, HASH_ORDER_200_510, 0x552d, 0x3cb81} }, //3.0.0
+	{ "20170710161758", 2, {0x20026,   0x29ab, 0, 16, 11, HASH_ORDER_200_510, 0x552d, 0x3cb81} }, //3.0.1 - 3.0.2
+	{ "20170921172629", 3, {0x1c64c,   0x37eb, 0, 16, 11, HASH_ORDER_200_510, 0x5382, 0x3711c} }, //4.0.0 - 4.1.0
+	{ "20180220163747", 4, {0x1f3b4,   0x465b, 0, 16, 11, HASH_ORDER_200_510, 0x5a63, 0x37901} }, //5.0.0 - 5.1.0
+	{ "20180802162753", 5, {0x27350,  0x17ff5, 1,  8, 11, HASH_ORDER_600_620, 0x5674, 0x1d5be} }, //6.0.0 - 6.1.0
+	{ "20181107105733", 6, {0x27350,  0x17ff5, 1,  8, 11, HASH_ORDER_600_620, 0x5674, 0x1d5be} }, //6.2.0
+	{ "20181218175730", 7, {0x29c50,   0x6a73, 0,  8, 12, HASH_ORDER_700_10x, 0x5563, 0x1d437} }, //7.0.0
+	{ "20190208150037", 7, {0x29c50,   0x6a73, 0,  8, 12, HASH_ORDER_700_10x, 0x5563, 0x1d437} }, //7.0.1
+	{ "20190314172056", 7, {0x29c50,   0x6a73, 0,  8, 12, HASH_ORDER_700_10x, 0x5563, 0x1d437} }, //8.0.0 - 8.0.1
+	{ "20190531152432", 8, {0x29c50,   0x6a73, 0,  8, 12, HASH_ORDER_700_10x, 0x5563, 0x1d437} }, //8.1.0
+	{ "20190809135709", 9, {0x2ec10,   0x5573, 0,  1, 12, HASH_ORDER_700_10x, 0x6495, 0x1d807} }, //9.0.0 - 9.0.1
+	{ "20191021113848", 10,{0x2ec10,   0x5573, 0,  1, 12, HASH_ORDER_700_10x, 0x6495, 0x1d807} }, //9.1.0
+	{ "20200303104606", 10,{0x30ea0,   0x5e4b, 0,  1, 12, HASH_ORDER_700_10x, 0x663c, 0x1d9a4} }, //10.0.0
 	{ NULL } //End.
 };
 

source/hos/pkg1.h

@@ -19,10 +19,23 @@
 
 #include "../utils/types.h"
 
+typedef struct _key_info_t
+{
+	u32 start_offset;
+	u32 hks_offset;
+	bool hks_offset_is_from_end;
+	u32 alignment;
+	u32 hash_max;
+	u8 hash_order[13];
+	u32 es_offset;
+	u32 ssl_offset;
+} key_info_t;
+
 typedef struct _pkg1_id_t
 {
 	const char *id;
 	u32 kb;
+	key_info_t key_info;
 } pkg1_id_t;
 
 const pkg1_id_t *pkg1_identify(u8 *pkg1);

source/hos/pkg2.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (c) 2018-2019 CTCaer
+ * Copyright (c) 2018-2020 CTCaer
  * Copyright (c) 2018 Atmosphère-NX
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -26,6 +26,10 @@
 
 #include "../gfx/gfx.h"
 
+u32 pkg2_newkern_ini1_val;
+u32 pkg2_newkern_ini1_start;
+u32 pkg2_newkern_ini1_end;
+
 /*#include "util.h"
 #define DPRINTF(...) gfx_printf(__VA_ARGS__)
 #define DEBUG_PRINTING*/
@@ -39,14 +43,48 @@ static u32 _pkg2_calc_kip1_size(pkg2_kip1_t *kip1)
 	return size;
 }
 
-void pkg2_parse_kips(link_t *info, pkg2_hdr_t *pkg2)
+void pkg2_get_newkern_info(u8 *kern_data)
+{
+	u32 pkg2_newkern_ini1_off = 0;
+	pkg2_newkern_ini1_start = 0;
+
+	// Find static OP offset that is close to INI1 offset.
+	u32 counter_ops = 0x100;
+	while (counter_ops)
+	{
+		if (*(u32 *)(kern_data + 0x100 - counter_ops) == PKG2_NEWKERN_GET_INI1_HEURISTIC)
+		{
+			pkg2_newkern_ini1_off = 0x100 - counter_ops + 12; // OP found. Add 12 for the INI1 offset.
+			break;
+		}
+
+		counter_ops -= 4;
+	}
+
+	// Offset not found?
+	if (!counter_ops)
+		return;
+
+	u32 info_op = *(u32 *)(kern_data + pkg2_newkern_ini1_off);
+	pkg2_newkern_ini1_val = ((info_op & 0xFFFF) >> 3) + pkg2_newkern_ini1_off; // Parse ADR and PC.
+
+	pkg2_newkern_ini1_start = *(u32 *)(kern_data + pkg2_newkern_ini1_val);
+	pkg2_newkern_ini1_end   = *(u32 *)(kern_data + pkg2_newkern_ini1_val + 0x8);
+	}
+
+bool pkg2_parse_kips(link_t *info, pkg2_hdr_t *pkg2, bool *new_pkg2)
 {
 	u8 *ptr;
 	// Check for new pkg2 type.
 	if (!pkg2->sec_size[PKG2_SEC_INI1])
 	{
-		u32 kernel_ini1_off = *(u32 *)(pkg2->data + PKG2_NEWKERN_INI1_START);
-		ptr = pkg2->data + kernel_ini1_off;
+		pkg2_get_newkern_info(pkg2->data);
+
+		if (!pkg2_newkern_ini1_start)
+			return false;
+
+		ptr = pkg2->data + pkg2_newkern_ini1_start;
+		*new_pkg2 = true;
 	}
 	else
 		ptr = pkg2->data + pkg2->sec_size[PKG2_SEC_KERNEL];
@@ -64,6 +102,8 @@ void pkg2_parse_kips(link_t *info, pkg2_hdr_t *pkg2)
 		ptr += ki->size;
 DPRINTF(" kip1 %d:%s @ %08X (%08X)\n", i, kip1->name, (u32)kip1, ki->size);
 	}
+
+	return true;
 }
 
 int pkg2_decompress_kip(pkg2_kip1_info_t* ki, u32 sectsToDecomp)
@@ -74,19 +114,19 @@ int pkg2_decompress_kip(pkg2_kip1_info_t* ki, u32 sectsToDecomp)
 
 	pkg2_kip1_t hdr;
 	memcpy(&hdr, ki->kip1, sizeof(hdr));
-	
+
 	unsigned int newKipSize = sizeof(hdr);
 	for (u32 sectIdx = 0; sectIdx < KIP1_NUM_SECTIONS; sectIdx++)
 	{
 		u32 sectCompBit = 1u << sectIdx;
 		// For compressed, cant get actual decompressed size without doing it, so use safe "output size".
-		if (sectIdx < 3 && (sectsToDecomp & sectCompBit) && (hdr.flags & sectCompBit)) 
+		if (sectIdx < 3 && (sectsToDecomp & sectCompBit) && (hdr.flags & sectCompBit))
 			newKipSize += hdr.sections[sectIdx].size_decomp;
 		else
 			newKipSize += hdr.sections[sectIdx].size_comp;
 	}
 
-	pkg2_kip1_t* newKip = malloc(newKipSize);	
+	pkg2_kip1_t* newKip = malloc(newKipSize);
 	unsigned char* dstDataPtr = newKip->data;
 	const unsigned char* srcDataPtr = ki->kip1->data;
 	for (u32 sectIdx = 0; sectIdx < KIP1_NUM_SECTIONS; sectIdx++)
@@ -110,7 +150,7 @@ int pkg2_decompress_kip(pkg2_kip1_info_t* ki, u32 sectsToDecomp)
 		//gfx_printf("Decomping %s KIP1 sect %d of size %d...\n", (const char*)hdr.name, sectIdx, compSize);
 		if (blz_uncompress_srcdest(srcDataPtr, compSize, dstDataPtr, outputSize) == 0)
 		{
-			gfx_printf("%kERROR decomping sect %d of %s KIP!%k\n", 0xFFFF0000, sectIdx, (char*)hdr.name, 0xFFCCCCCC);			
+			gfx_printf("%kERROR decomping sect %d of %s KIP!%k\n", 0xFFFF0000, sectIdx, (char*)hdr.name, 0xFFCCCCCC);
 			free(newKip);
 
 			return 1;
@@ -138,7 +178,7 @@ int pkg2_decompress_kip(pkg2_kip1_info_t* ki, u32 sectsToDecomp)
 pkg2_hdr_t *pkg2_decrypt(void *data)
 {
 	u8 *pdata = (u8 *)data;
-	
+
 	// Skip signature.
 	pdata += 0x100;
 

source/hos/pkg2.h

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (C) 2018-2019 CTCaer
+ * Copyright (c) 2018-2020 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,
@@ -26,7 +26,19 @@
 #define PKG2_SEC_KERNEL 0
 #define PKG2_SEC_INI1 1
 
-#define PKG2_NEWKERN_INI1_START 0x168
+#define PKG2_NEWKERN_GET_INI1_HEURISTIC 0xD2800015 // Offset of OP + 12 is the INI1 offset.
+
+extern u32 pkg2_newkern_ini1_val;
+extern u32 pkg2_newkern_ini1_start;
+extern u32 pkg2_newkern_ini1_end;
+
+typedef struct _kernel_patch_t
+{
+	u32 id;
+	u32 off;
+	u32 val;
+	u32 *ptr;
+} kernel_patch_t;
 
 typedef struct _pkg2_hdr_t
 {
@@ -83,7 +95,13 @@ typedef struct _pkg2_kip1_info_t
 	link_t link;
 } pkg2_kip1_info_t;
 
-void pkg2_parse_kips(link_t *info, pkg2_hdr_t *pkg2);
+typedef struct _pkg2_kernel_id_t
+{
+	u8 hash[8];
+	kernel_patch_t *kernel_patchset;
+} pkg2_kernel_id_t;
+
+bool pkg2_parse_kips(link_t *info, pkg2_hdr_t *pkg2, bool *new_pkg2);
 int pkg2_decompress_kip(pkg2_kip1_info_t* ki, u32 sectsToDecomp);
 pkg2_hdr_t *pkg2_decrypt(void *data);
 

source/hos/sept.c

@@ -17,14 +17,20 @@
 #include <string.h>
 
 #include "sept.h"
+#include "../config/ini.h"
 #include "../gfx/di.h"
+#include "../hos/fss.h"
+#include "../hos/hos.h"
 #include "../libs/fatfs/ff.h"
 #include "../mem/heap.h"
+#include "../soc/hw_init.h"
 #include "../soc/pmc.h"
 #include "../soc/t210.h"
 #include "../storage/nx_emmc.h"
+#include "../storage/nx_sd.h"
 #include "../storage/sdmmc.h"
 #include "../utils/btn.h"
+#include "../utils/list.h"
 #include "../utils/types.h"
 
 #include "../gfx/gfx.h"
@@ -38,7 +44,7 @@ u8 warmboot_reboot[] = {
 	0x14, 0x00, 0x9F, 0xE5, // LDR R0, =0x7000E450
 	0x01, 0x10, 0xB0, 0xE3, // MOVS R1, #1
 	0x00, 0x10, 0x80, 0xE5, // STR R1, [R0]
-	0x0C, 0x00, 0x9F, 0xE5, // LDR R0, =0x7000E400 
+	0x0C, 0x00, 0x9F, 0xE5, // LDR R0, =0x7000E400
 	0x10, 0x10, 0xB0, 0xE3, // MOVS R1, #0x10
 	0x00, 0x10, 0x80, 0xE5, // STR R1, [R0]
 	0xFE, 0xFF, 0xFF, 0xEA, // LOOP
@@ -55,42 +61,83 @@ u8 warmboot_reboot[] = {
 #define SEPT_STG2_ADDR  (SEPT_PK1T_ADDR + 0x60E0)
 #define SEPT_PKG_SZ     (0x2F100 + WB_RST_SIZE)
 
+extern u32 color_idx;
 extern boot_cfg_t b_cfg;
-extern void sd_unmount();
 extern void reloc_patcher(u32 payload_dst, u32 payload_src, u32 payload_size);
 
 int reboot_to_sept(const u8 *tsec_fw, const u32 tsec_size, const u32 kb)
 {
 	FIL fp;
+	bool fss0_sept_used = false;
 
 	// Copy warmboot reboot code and TSEC fw.
 	memcpy((u8 *)(SEPT_PK1T_ADDR - WB_RST_SIZE), (u8 *)warmboot_reboot, sizeof(warmboot_reboot));
 	memcpy((void *)SEPT_PK1T_ADDR, tsec_fw, tsec_size);
 	*(vu32 *)SEPT_TCSZ_ADDR = tsec_size;
 
-	// Copy sept-primary.
-	if (f_open(&fp, "sd:/sept/sept-primary.bin", FA_READ))
-		goto error;
-
-	if (f_read(&fp, (u8 *)SEPT_STG1_ADDR, f_size(&fp), NULL))
+	LIST_INIT(ini_sections);
+	if (ini_parse(&ini_sections, "bootloader/hekate_ipl.ini", false))
 	{
-		f_close(&fp);
-		goto error;
+		bool found = false;
+		LIST_FOREACH_ENTRY(ini_sec_t, ini_sec, &ini_sections, link)
+		{
+			// Only parse non config sections.
+			if (ini_sec->type == INI_CHOICE && strcmp(ini_sec->name, "config"))
+			{
+				LIST_FOREACH_ENTRY(ini_kv_t, kv, &ini_sec->kvs, link)
+				{
+					if (!strcmp("fss0", kv->key))
+					{
+						fss0_sept_t sept_ctxt;
+						sept_ctxt.kb = kb;
+						sept_ctxt.sept_primary = (void *)SEPT_STG1_ADDR;
+						sept_ctxt.sept_secondary = (void *)SEPT_STG2_ADDR;
+						fss0_sept_used = parse_fss(NULL, kv->val, &sept_ctxt);
+
+						found = true;
+						break;
+					}
+				}
+			}
+			if (found)
+				break;
+		}
 	}
-	f_close(&fp);
 
-	// Copy sept-secondary.
-	if ((kb == 7) && f_open(&fp, "sd:/sept/sept-secondary.enc", FA_READ) && f_open(&fp, "sd:/sept/sept-secondary_00.enc", FA_READ))
-		goto error;
-	else if ((kb == 8) && f_open(&fp, "sd:/sept/sept-secondary_01.enc", FA_READ))
-		goto error;
 
-	if (f_read(&fp, (u8 *)SEPT_STG2_ADDR, f_size(&fp), NULL))
+	if (!fss0_sept_used)
 	{
+		// Copy sept-primary.
+		if (f_open(&fp, "sd:/sept/sept-primary.bin", FA_READ))
+			goto error;
+
+		if (f_read(&fp, (u8 *)SEPT_STG1_ADDR, f_size(&fp), NULL))
+		{
+			f_close(&fp);
+			goto error;
+		}
+		f_close(&fp);
+
+		// Copy sept-secondary.
+		if (kb < KB_FIRMWARE_VERSION_810)
+		{
+			if (f_open(&fp, "sd:/sept/sept-secondary_00.enc", FA_READ))
+				if (f_open(&fp, "sd:/sept/sept-secondary.enc", FA_READ)) // Try the deprecated version.
+					goto error;
+		}
+		else
+		{
+			if (f_open(&fp, "sd:/sept/sept-secondary_01.enc", FA_READ))
+				goto error;
+		}
+
+		if (f_read(&fp, (u8 *)SEPT_STG2_ADDR, f_size(&fp), NULL))
+		{
+			f_close(&fp);
+			goto error;
+		}
 		f_close(&fp);
-		goto error;
 	}
-	f_close(&fp);
 
 	// Save auto boot config to sept payload, if any.
 	boot_cfg_t *tmp_cfg = malloc(sizeof(boot_cfg_t));
@@ -99,7 +146,10 @@ int reboot_to_sept(const u8 *tsec_fw, const u32 tsec_size, const u32 kb)
 	tmp_cfg->boot_cfg |= BOOT_CFG_SEPT_RUN;
 
 	if (f_open(&fp, "sd:/sept/payload.bin", FA_READ | FA_WRITE))
+	{
+		free(tmp_cfg);
 		goto error;
+	}
 
 	f_lseek(&fp, PATCHED_RELOC_SZ);
 	f_write(&fp, tmp_cfg, sizeof(boot_cfg_t), NULL);
@@ -107,8 +157,7 @@ int reboot_to_sept(const u8 *tsec_fw, const u32 tsec_size, const u32 kb)
 	f_close(&fp);
 
 	sd_unmount();
-	gfx_printf("\n%kPress Power or Vol +/-\n%k   to Reboot to Sept...", COLOR_BLUE, COLOR_VIOLET);
-	btn_wait();
+	gfx_printf("\n%kPress Power or Vol +/-\n   to Reboot to Sept...", colors[(color_idx++) % 6]);
 
 	u32 pk1t_sept = SEPT_PK1T_ADDR - (ALIGN(PATCHED_RELOC_SZ, 0x10) + WB_RST_SIZE);
 
@@ -123,12 +172,12 @@ int reboot_to_sept(const u8 *tsec_fw, const u32 tsec_size, const u32 kb)
 	PMC(APBDEV_PMC_SCRATCH33) = SEPT_PRI_ADDR;
 	PMC(APBDEV_PMC_SCRATCH40) = 0x6000F208;
 
-	display_end();
+	reconfig_hw_workaround(false, 0);
 
 	(*sept)();
 
 error:
-	EPRINTF("Sept files not found in sd:/sept!\nPlace appropriate files and try again.");
+	EPRINTF("\nSept files not found in sd:/sept!\nPlace appropriate files and try again.");
 	display_backlight_brightness(100, 1000);
 
 	btn_wait();

source/ianos/ianos.c

@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2018 M4xw
+ * Copyright (c) 2018-2019 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/>.
+ */
+
+#include <string.h>
+
+#include "ianos.h"
+#include "../../common/common_module.h"
+#include "../gfx/gfx.h"
+#include "../libs/elfload/elfload.h"
+#include "../mem/heap.h"
+#include "../storage/nx_sd.h"
+#include "../utils/types.h"
+
+#define IRAM_LIB_ADDR 0x4002B000
+#define DRAM_LIB_ADDR 0xE0000000
+
+extern heap_t _heap;
+
+void *elfBuf = NULL;
+void *fileBuf = NULL;
+
+static void _ianos_call_ep(moduleEntrypoint_t entrypoint, void *moduleConfig)
+{
+	bdkParams_t bdkParameters = (bdkParams_t)malloc(sizeof(struct _bdkParams_t));
+	bdkParameters->gfxCon = &gfx_con;
+	bdkParameters->gfxCtx = &gfx_ctxt;
+	bdkParameters->memcpy = (memcpy_t)&memcpy;
+	bdkParameters->memset = (memset_t)&memset;
+	bdkParameters->sharedHeap = &_heap;
+
+	entrypoint(moduleConfig, bdkParameters);
+}
+
+static void *_ianos_alloc_cb(el_ctx *ctx, Elf_Addr phys, Elf_Addr virt, Elf_Addr size)
+{
+	(void)ctx;
+	(void)phys;
+	(void)size;
+	return (void *)virt;
+}
+
+static bool _ianos_read_cb(el_ctx *ctx, void *dest, size_t numberBytes, size_t offset)
+{
+	(void)ctx;
+
+	memcpy(dest, fileBuf + offset, numberBytes);
+
+	return true;
+}
+
+//TODO: Support shared libraries.
+uintptr_t ianos_loader(bool sdmount, char *path, elfType_t type, void *moduleConfig)
+{
+	uintptr_t epaddr = 0;
+
+	if (sdmount)
+	{
+		if (!sd_mount())
+			goto elfLoadFinalOut;
+	}
+
+	fileBuf = sd_file_read(path, NULL);
+
+	if (sdmount)
+		sd_unmount();
+
+	if (!fileBuf)
+		goto elfLoadFinalOut;
+
+
+	el_ctx ctx;
+	ctx.pread = _ianos_read_cb;
+
+	if (el_init(&ctx))
+		goto elfLoadFinalOut;
+
+	// Set our relocated library's buffer.
+	switch (type & 0xFFFF)
+	{
+	case EXEC_ELF:
+	case AR64_ELF:
+		elfBuf = (void *)DRAM_LIB_ADDR;
+		sd_unmount();
+		break;
+	default:
+		elfBuf = malloc(ctx.memsz); // Aligned to 0x10 by default.
+	}
+
+	if (!elfBuf)
+		goto elfLoadFinalOut;
+
+	// Load and relocate library.
+	ctx.base_load_vaddr = ctx.base_load_paddr = (uintptr_t)elfBuf;
+	if (el_load(&ctx, _ianos_alloc_cb))
+		goto elfFreeOut;
+
+	if (el_relocate(&ctx))
+		goto elfFreeOut;
+
+	// Launch.
+	epaddr = ctx.ehdr.e_entry + (uintptr_t)elfBuf;
+	moduleEntrypoint_t ep = (moduleEntrypoint_t)epaddr;
+
+	_ianos_call_ep(ep, moduleConfig);
+
+elfFreeOut:
+	free(fileBuf);
+	elfBuf = NULL;
+	fileBuf = NULL;
+
+elfLoadFinalOut:
+
+	return epaddr;
+}

source/ianos/ianos.h

@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2018 M4xw
+ * 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 IANOS_H
+#define IANOS_H
+
+#include "../utils/types.h"
+
+typedef enum
+{
+	DRAM_LIB = 0, // DRAM library.
+	EXEC_ELF = 1, // Executable elf that does not return.
+	DR64_LIB = 2, // AARCH64 DRAM library.
+	AR64_ELF = 3, // Executable elf that does not return.
+	KEEP_IN_RAM = (1 << 31)  // Shared library mask.
+} elfType_t;
+
+uintptr_t ianos_loader(bool sdmount, char *path, elfType_t type, void* config);
+
+#endif

source/keys/key_sources.inl

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019 shchmue
+ * Copyright (c) 2019-2020 shchmue
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -14,7 +14,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-static const u8 zeros[0x10] = {0};
+static u8 zeros[0x10] = {0};
 
 static const u8 keyblob_key_source[][0x10] = {
     {0xDF, 0x20, 0x6F, 0x59, 0x44, 0x54, 0xEF, 0xDC, 0x70, 0x74, 0x48, 0x3B, 0x0D, 0xED, 0x9F, 0xD3}, //1.0.0
@@ -29,6 +29,8 @@ static const u8 master_kek_sources[KB_FIRMWARE_VERSION_MAX - KB_FIRMWARE_VERSION
     {0x37, 0x4B, 0x77, 0x29, 0x59, 0xB4, 0x04, 0x30, 0x81, 0xF6, 0xE5, 0x8C, 0x6D, 0x36, 0x17, 0x9A}, //6.2.0
     {0x9A, 0x3E, 0xA9, 0xAB, 0xFD, 0x56, 0x46, 0x1C, 0x9B, 0xF6, 0x48, 0x7F, 0x5C, 0xFA, 0x09, 0x5C}, //7.0.0
     {0xDE, 0xDC, 0xE3, 0x39, 0x30, 0x88, 0x16, 0xF8, 0xAE, 0x97, 0xAD, 0xEC, 0x64, 0x2D, 0x41, 0x41}, //8.1.0
+    {0x1A, 0xEC, 0x11, 0x82, 0x2B, 0x32, 0x38, 0x7A, 0x2B, 0xED, 0xBA, 0x01, 0x47, 0x7E, 0x3B, 0x67}, //9.0.0
+    {0x30, 0x3F, 0x02, 0x7E, 0xD8, 0x38, 0xEC, 0xD7, 0x93, 0x25, 0x34, 0xB5, 0x30, 0xEB, 0xCA, 0x7A}, //9.1.0
 };
 
 static const u8 mkey_vectors[KB_FIRMWARE_VERSION_MAX+1][0x10] =
@@ -42,6 +44,8 @@ static const u8 mkey_vectors[KB_FIRMWARE_VERSION_MAX+1][0x10] =
     {0x1E, 0x1E, 0x22, 0xC0, 0x5A, 0x33, 0x3C, 0xB9, 0x0B, 0xA9, 0x03, 0x04, 0xBA, 0xDB, 0x07, 0x57}, /* Master key 05 encrypted with Master key 06. */
     {0xA4, 0xD4, 0x52, 0x6F, 0xD1, 0xE4, 0x36, 0xAA, 0x9F, 0xCB, 0x61, 0x27, 0x1C, 0x67, 0x65, 0x1F}, /* Master key 06 encrypted with Master key 07. */
     {0xEA, 0x60, 0xB3, 0xEA, 0xCE, 0x8F, 0x24, 0x46, 0x7D, 0x33, 0x9C, 0xD1, 0xBC, 0x24, 0x98, 0x29}, /* Master key 07 encrypted with Master key 08. */
+    {0x4D, 0xD9, 0x98, 0x42, 0x45, 0x0D, 0xB1, 0x3C, 0x52, 0x0C, 0x9A, 0x44, 0xBB, 0xAD, 0xAF, 0x80}, /* Master key 08 encrypted with Master key 09. */
+    {0xB8, 0x96, 0x9E, 0x4A, 0x00, 0x0D, 0xD6, 0x28, 0xB3, 0xD1, 0xDB, 0x68, 0x5F, 0xFB, 0xE1, 0x2A}, /* Master key 09 encrypted with Master key 0A. */
 };
 
 //======================================Keys======================================//
@@ -66,6 +70,30 @@ static const u8 master_key_source[0x10] = {
     0xD8, 0xA2, 0x41, 0x0A, 0xC6, 0xC5, 0x90, 0x01, 0xC6, 0x1D, 0x6A, 0x26, 0x7C, 0x51, 0x3F, 0x3C};
 static const u8 per_console_key_source[0x10] = {
     0x4F, 0x02, 0x5F, 0x0E, 0xB6, 0x6D, 0x11, 0x0E, 0xDC, 0x32, 0x7D, 0x41, 0x86, 0xC2, 0xF4, 0x78};
+static const u8 per_console_key_source_4x[0x10] = {
+    0x0C, 0x91, 0x09, 0xDB, 0x93, 0x93, 0x07, 0x81, 0x07, 0x3C, 0xC4, 0x16, 0x22, 0x7C, 0x6C, 0x28};
+
+static const u8 new_device_key_sources[KB_FIRMWARE_VERSION_MAX - KB_FIRMWARE_VERSION_400 + 1][0x10] = {
+    {0x8B, 0x4E, 0x1C, 0x22, 0x42, 0x07, 0xC8, 0x73, 0x56, 0x94, 0x08, 0x8B, 0xCC, 0x47, 0x0F, 0x5D}, /* 4.x   New Device Key Source. */
+    {0x6C, 0xEF, 0xC6, 0x27, 0x8B, 0xEC, 0x8A, 0x91, 0x99, 0xAB, 0x24, 0xAC, 0x4F, 0x1C, 0x8F, 0x1C}, /* 5.x   New Device Key Source. */
+    {0x70, 0x08, 0x1B, 0x97, 0x44, 0x64, 0xF8, 0x91, 0x54, 0x9D, 0xC6, 0x84, 0x8F, 0x1A, 0xB2, 0xE4}, /* 6.x   New Device Key Source. */
+    {0x8E, 0x09, 0x1F, 0x7A, 0xBB, 0xCA, 0x6A, 0xFB, 0xB8, 0x9B, 0xD5, 0xC1, 0x25, 0x9C, 0xA9, 0x17}, /* 6.2.0 New Device Key Source. */
+    {0x8F, 0x77, 0x5A, 0x96, 0xB0, 0x94, 0xFD, 0x8D, 0x28, 0xE4, 0x19, 0xC8, 0x16, 0x1C, 0xDB, 0x3D}, /* 7.0.0 New Device Key Source. */
+    {0x67, 0x62, 0xD4, 0x8E, 0x55, 0xCF, 0xFF, 0x41, 0x31, 0x15, 0x3B, 0x24, 0x0C, 0x7C, 0x07, 0xAE}, /* 8.1.0 New Device Key Source. */
+    {0x4A, 0xC3, 0x4E, 0x14, 0x8B, 0x96, 0x4A, 0xD5, 0xD4, 0x99, 0x73, 0xC4, 0x45, 0xAB, 0x8B, 0x49}, /* 9.0.0 New Device Key Source. */
+    {0x14, 0xB8, 0x74, 0x12, 0xCB, 0xBD, 0x0B, 0x8F, 0x20, 0xFB, 0x30, 0xDA, 0x27, 0xE4, 0x58, 0x94}, /* 9.1.0 New Device Key Source. */
+};
+
+static const u8 new_device_keygen_sources[KB_FIRMWARE_VERSION_MAX - KB_FIRMWARE_VERSION_400 + 1][0x10] = {
+    {0x88, 0x62, 0x34, 0x6E, 0xFA, 0xF7, 0xD8, 0x3F, 0xE1, 0x30, 0x39, 0x50, 0xF0, 0xB7, 0x5D, 0x5D}, /* 4.x   New Device Keygen Source. */
+    {0x06, 0x1E, 0x7B, 0xE9, 0x6D, 0x47, 0x8C, 0x77, 0xC5, 0xC8, 0xE7, 0x94, 0x9A, 0xA8, 0x5F, 0x2E}, /* 5.x   New Device Keygen Source. */
+    {0x99, 0xFA, 0x98, 0xBD, 0x15, 0x1C, 0x72, 0xFD, 0x7D, 0x9A, 0xD5, 0x41, 0x00, 0xFD, 0xB2, 0xEF}, /* 6.x   New Device Keygen Source. */
+    {0x81, 0x3C, 0x6C, 0xBF, 0x5D, 0x21, 0xDE, 0x77, 0x20, 0xD9, 0x6C, 0xE3, 0x22, 0x06, 0xAE, 0xBB}, /* 6.2.0 New Device Keygen Source. */
+    {0x86, 0x61, 0xB0, 0x16, 0xFA, 0x7A, 0x9A, 0xEA, 0xF6, 0xF5, 0xBE, 0x1A, 0x13, 0x5B, 0x6D, 0x9E}, /* 7.0.0 New Device Keygen Source. */
+    {0xA6, 0x81, 0x71, 0xE7, 0xB5, 0x23, 0x74, 0xB0, 0x39, 0x8C, 0xB7, 0xFF, 0xA0, 0x62, 0x9F, 0x8D}, /* 8.1.0 New Device Keygen Source. */
+    {0x03, 0xE7, 0xEB, 0x43, 0x1B, 0xCF, 0x5F, 0xB5, 0xED, 0xDC, 0x97, 0xAE, 0x21, 0x8D, 0x19, 0xED}, /* 9.0.0 New Device Keygen Source. */
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, /* TODO: 9.1.0 New Device Keygen Source to be added on next change-of-keys. */
+};
 
 // from SPL
 static const u8 aes_key_generation_source[0x10] = {
@@ -86,7 +114,7 @@ static const u8 bis_key_source[3][0x20] = {
         0x4D, 0x12, 0xE1, 0x4B, 0x2A, 0x47, 0x4C, 0x1C, 0x09, 0xCB, 0x03, 0x59, 0xF0, 0x15, 0xF4, 0xE4}
 };
 
-static const u8 fs_hashes_sha256[10][0x20] = {
+static const u8 fs_hashes_sha256[13][0x20] = {
     { // header_kek_source
         0x18, 0x88, 0xca, 0xed, 0x55, 0x51, 0xb3, 0xed, 0xe0, 0x14, 0x99, 0xe8, 0x7c, 0xe0, 0xd8, 0x68,
         0x27, 0xf8, 0x08, 0x20, 0xef, 0xb2, 0x75, 0x92, 0x10, 0x55, 0xaa, 0x4e, 0x2a, 0xbd, 0xff, 0xc2},
@@ -108,6 +136,15 @@ static const u8 fs_hashes_sha256[10][0x20] = {
     { // save_mac_key_source
         0xB4, 0x7B, 0x60, 0x0B, 0x1A, 0xD3, 0x14, 0xF9, 0x41, 0x14, 0x7D, 0x8B, 0x39, 0x1D, 0x4B, 0x19,
         0x87, 0xCC, 0x8C, 0x88, 0x4A, 0xC8, 0x9F, 0xFC, 0x91, 0xCA, 0xE2, 0x21, 0xC5, 0x24, 0x51, 0xF7},
+    { // save_mac_sd_card_kek_source
+        0x60, 0x1a, 0x60, 0xbe, 0x13, 0xf6, 0x3e, 0xda, 0xec, 0xcc, 0x96, 0x7f, 0x27, 0xa3, 0xa3, 0x64,
+        0x65, 0xcb, 0xe8, 0xf0, 0x29, 0xf0, 0xc4, 0x14, 0xb2, 0x36, 0x6a, 0x8b, 0x8a, 0x0f, 0x13, 0x00},
+    { // save_mac_sd_card_key_source        
+        0xc2, 0x22, 0x0a, 0x38, 0xb6, 0x87, 0x2b, 0x63, 0xee, 0x77, 0xac, 0x8c, 0x28, 0x24, 0x7a, 0x44,
+        0x02, 0xe6, 0xdd, 0x85, 0x24, 0x8b, 0x41, 0x9a, 0x6f, 0x9b, 0x17, 0x93, 0xc0, 0x50, 0x3f, 0x21},
+    { // sd_card_custom_storage_key_source
+        0x6b, 0x8f, 0xd2, 0x6c, 0x76, 0x5b, 0x7c, 0x67, 0x70, 0x0c, 0x68, 0x54, 0x90, 0x8e, 0xbe, 0x88,
+        0x45, 0xb0, 0x55, 0xa6, 0xbb, 0xbb, 0xea, 0x0c, 0x06, 0x3a, 0x85, 0x04, 0x12, 0xd4, 0xca, 0x53},
     { // sd_card_kek_source
         0x6B, 0x2E, 0xD8, 0x77, 0xC2, 0xC5, 0x23, 0x34, 0xAC, 0x51, 0xE5, 0x9A, 0xBF, 0xA7, 0xEC, 0x45,
         0x7F, 0x4A, 0x7D, 0x01, 0xE4, 0x62, 0x91, 0xE9, 0xF2, 0xEA, 0xA4, 0x5F, 0x01, 0x1D, 0x24, 0xB7},

source/keys/keys.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019 shchmue
+ * Copyright (c) 2019-2020 shchmue
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,

source/keys/save.c

@@ -0,0 +1,828 @@
+#include <stdint.h>
+#include <string.h>
+#include "save.h"
+
+#include "../gfx/gfx.h"
+#include "../mem/heap.h"
+#include "../sec/se.h"
+#include "../utils/types.h"
+#include "../utils/util.h"
+
+#define REMAP_ENTRY_LENGTH 0x20
+
+static inline void save_bitmap_set_bit(void *buffer, size_t bit_offset) {
+    *((uint8_t *)buffer + (bit_offset >> 3)) |= 1 << (bit_offset & 7);
+}
+
+static inline void save_bitmap_clear_bit(void *buffer, size_t bit_offset) {
+    *((uint8_t *)buffer + (bit_offset >> 3)) &= ~(uint8_t)(1 << (bit_offset & 7));
+}
+
+static inline uint8_t save_bitmap_check_bit(const void *buffer, size_t bit_offset) {
+    return *((uint8_t *)buffer + (bit_offset >> 3)) & (1 << (bit_offset & 7));
+}
+
+void save_duplex_storage_init(duplex_storage_ctx_t *ctx, duplex_fs_layer_info_t *layer, void *bitmap, uint64_t bitmap_size) {
+    ctx->data_a = layer->data_a;
+    ctx->data_b = layer->data_b;
+    ctx->bitmap_storage = (uint8_t *)bitmap;
+    ctx->block_size = 1 << layer->info.block_size_power;
+
+    ctx->bitmap.data = ctx->bitmap_storage;
+    ctx->bitmap.bitmap = malloc(bitmap_size >> 3);
+
+    uint32_t bits_remaining = bitmap_size;
+    uint32_t bitmap_pos = 0;
+    uint32_t *buffer_pos = (uint32_t *)bitmap;
+    while (bits_remaining) {
+        uint32_t bits_to_read = bits_remaining < 32 ? bits_remaining : 32;
+        uint32_t val = *buffer_pos;
+        for (uint32_t i = 0; i < bits_to_read; i++) {
+            if (val & 0x80000000)
+                save_bitmap_set_bit(ctx->bitmap.bitmap, bitmap_pos);
+            else
+                save_bitmap_clear_bit(ctx->bitmap.bitmap, bitmap_pos);
+            bitmap_pos++;
+            bits_remaining--;
+            val <<= 1;
+        }
+        buffer_pos++;
+    }
+}
+
+uint32_t save_duplex_storage_read(duplex_storage_ctx_t *ctx, void *buffer, uint64_t offset, size_t count) {
+    uint64_t in_pos = offset;
+    uint32_t out_pos = 0;
+    uint32_t remaining = count;
+
+    while (remaining) {
+        uint32_t block_num = (uint32_t)(in_pos / ctx->block_size);
+        uint32_t block_pos = (uint32_t)(in_pos % ctx->block_size);
+        uint32_t bytes_to_read = ctx->block_size - block_pos < remaining ? ctx->block_size - block_pos : remaining;
+
+        uint8_t *data = save_bitmap_check_bit(ctx->bitmap.bitmap, block_num) ? ctx->data_b : ctx->data_a;
+        memcpy((uint8_t *)buffer + out_pos, data + in_pos, bytes_to_read);
+
+        out_pos += bytes_to_read;
+        in_pos += bytes_to_read;
+        remaining -= bytes_to_read;
+    }
+    return out_pos;
+}
+
+remap_segment_ctx_t *save_remap_init_segments(remap_header_t *header, remap_entry_ctx_t *map_entries, uint32_t num_map_entries) {
+    remap_segment_ctx_t *segments =  calloc(1, sizeof(remap_segment_ctx_t) * header->map_segment_count);
+    unsigned int entry_idx = 0;
+
+    for (unsigned int i = 0; i < header->map_segment_count; i++) {
+        remap_segment_ctx_t *seg = &segments[i];
+        seg->entry_count = 0;
+        seg->entries = malloc(sizeof(remap_entry_ctx_t *));
+        seg->entries[seg->entry_count++] = &map_entries[entry_idx];
+        seg->offset = map_entries[entry_idx].virtual_offset;
+        map_entries[entry_idx++].segment = seg;
+
+        while (entry_idx < num_map_entries && map_entries[entry_idx - 1].virtual_offset_end == map_entries[entry_idx].virtual_offset) {
+            map_entries[entry_idx].segment = seg;
+            map_entries[entry_idx - 1].next = &map_entries[entry_idx];
+            remap_entry_ctx_t **ptr = calloc(1, sizeof(remap_entry_ctx_t *) * (seg->entry_count + 1));
+            memcpy(ptr, seg->entries, sizeof(remap_entry_ctx_t *) * (seg->entry_count));
+            free(seg->entries);
+            seg->entries = ptr;
+            seg->entries[seg->entry_count++] = &map_entries[entry_idx++];
+        }
+        seg->length = seg->entries[seg->entry_count - 1]->virtual_offset_end - seg->entries[0]->virtual_offset;
+    }
+    return segments;
+}
+
+remap_entry_ctx_t *save_remap_get_map_entry(remap_storage_ctx_t *ctx, uint64_t offset) {
+    uint32_t segment_idx = (uint32_t)(offset >> (64 - ctx->header->segment_bits));
+    if (segment_idx < ctx->header->map_segment_count) {
+        for (unsigned int i = 0; i < ctx->segments[segment_idx].entry_count; i++)
+            if (ctx->segments[segment_idx].entries[i]->virtual_offset_end > offset)
+                return ctx->segments[segment_idx].entries[i];
+    }
+    return NULL;
+}
+
+uint32_t save_remap_read(remap_storage_ctx_t *ctx, void *buffer, uint64_t offset, size_t count) {
+    remap_entry_ctx_t *entry = save_remap_get_map_entry(ctx, offset);
+    uint64_t in_pos = offset;
+    uint32_t out_pos = 0;
+    uint32_t remaining = count;
+
+    while (remaining) {
+        uint64_t entry_pos = in_pos - entry->virtual_offset;
+        uint32_t bytes_to_read = entry->virtual_offset_end - in_pos < remaining ? (uint32_t)(entry->virtual_offset_end - in_pos) : remaining;
+
+        switch (ctx->type) {
+            case STORAGE_BYTES:
+                f_lseek(ctx->file, ctx->base_storage_offset + entry->physical_offset + entry_pos);
+                f_read(ctx->file, (uint8_t *)buffer + out_pos, bytes_to_read, NULL);
+                break;
+            case STORAGE_DUPLEX:
+                save_duplex_storage_read(ctx->duplex, (uint8_t *)buffer + out_pos, ctx->base_storage_offset + entry->physical_offset + entry_pos, bytes_to_read);
+                break;
+            default:
+                break;
+        }
+
+        out_pos += bytes_to_read;
+        in_pos += bytes_to_read;
+        remaining -= bytes_to_read;
+
+        if (in_pos >= entry->virtual_offset_end)
+            entry = entry->next;
+    }
+    return out_pos;
+}
+
+uint32_t save_journal_storage_read(journal_storage_ctx_t *ctx, remap_storage_ctx_t *remap, void *buffer, uint64_t offset, size_t count) {
+    uint64_t in_pos = offset;
+    uint32_t out_pos = 0;
+    uint32_t remaining = count;
+
+    while (remaining) {
+        uint32_t block_num = (uint32_t)(in_pos / ctx->block_size);
+        uint32_t block_pos = (uint32_t)(in_pos % ctx->block_size);
+        uint64_t physical_offset = ctx->map.entries[block_num].physical_index * ctx->block_size + block_pos;
+        uint32_t bytes_to_read = ctx->block_size - block_pos < remaining ? ctx->block_size - block_pos : remaining;
+
+        save_remap_read(remap, (uint8_t *)buffer + out_pos, ctx->journal_data_offset + physical_offset, bytes_to_read);
+
+        out_pos += bytes_to_read;
+        in_pos += bytes_to_read;
+        remaining -= bytes_to_read;
+    }
+    return out_pos;
+}
+
+void save_ivfc_storage_init(hierarchical_integrity_verification_storage_ctx_t *ctx, uint64_t master_hash_offset, ivfc_save_hdr_t *ivfc) {
+    ivfc_level_save_ctx_t *levels = ctx->levels;
+    levels[0].type = STORAGE_BYTES;
+    levels[0].hash_offset = master_hash_offset;
+    for (unsigned int i = 1; i < 4; i++) {
+        ivfc_level_hdr_t *level = &ivfc->level_headers[i - 1];
+        levels[i].type = STORAGE_REMAP;
+        levels[i].data_offset = level->logical_offset;
+        levels[i].data_size = level->hash_data_size;
+    }
+    if (ivfc->num_levels == 5) {
+        ivfc_level_hdr_t *data_level = &ivfc->level_headers[ivfc->num_levels - 2];
+        levels[ivfc->num_levels - 1].type = STORAGE_JOURNAL;
+        levels[ivfc->num_levels - 1].data_offset = data_level->logical_offset;
+        levels[ivfc->num_levels - 1].data_size = data_level->hash_data_size;
+    }
+
+    struct salt_source_t {
+        char string[50];
+        uint32_t length;
+    };
+
+    static const struct salt_source_t salt_sources[6] = {
+        {"HierarchicalIntegrityVerificationStorage::Master", 48},
+        {"HierarchicalIntegrityVerificationStorage::L1", 44},
+        {"HierarchicalIntegrityVerificationStorage::L2", 44},
+        {"HierarchicalIntegrityVerificationStorage::L3", 44},
+        {"HierarchicalIntegrityVerificationStorage::L4", 44},
+        {"HierarchicalIntegrityVerificationStorage::L5", 44}
+    };
+    integrity_verification_info_ctx_t init_info[ivfc->num_levels];
+
+    init_info[0].data = &levels[0];
+    init_info[0].block_size = 0;
+    for (unsigned int i = 1; i < ivfc->num_levels; i++) {
+        init_info[i].data = &levels[i];
+        init_info[i].block_size = 1 << ivfc->level_headers[i - 1].block_size;
+        se_calc_hmac_sha256(init_info[i].salt, ivfc->salt_source, 0x20, salt_sources[i - 1].string, salt_sources[i - 1].length);
+    }
+
+    ctx->integrity_storages[0].next_level = NULL;
+    ctx->level_validities = malloc(sizeof(validity_t *) * (ivfc->num_levels - 1));
+    for (unsigned int i = 1; i < ivfc->num_levels; i++) {
+        integrity_verification_storage_ctx_t *level_data = &ctx->integrity_storages[i - 1];
+        level_data->hash_storage = &levels[i - 1];
+        level_data->base_storage = &levels[i];
+        level_data->sector_size = init_info[i].block_size;
+        level_data->_length = init_info[i].data->data_size;
+        level_data->sector_count = (level_data->_length + level_data->sector_size - 1) / level_data->sector_size;
+        memcpy(level_data->salt, init_info[i].salt, 0x20);
+        level_data->block_validities = calloc(1, sizeof(validity_t) * level_data->sector_count);
+        ctx->level_validities[i - 1] = level_data->block_validities;
+        if (i > 1) {
+            level_data->next_level = &ctx->integrity_storages[i - 2];
+        }
+    }
+    ctx->data_level = &levels[ivfc->num_levels - 1];
+    ctx->_length = ctx->integrity_storages[ivfc->num_levels - 2]._length;
+}
+
+size_t save_ivfc_level_fread(ivfc_level_save_ctx_t *ctx, void *buffer, uint64_t offset, size_t count) {
+    switch (ctx->type) {
+        case STORAGE_BYTES:
+            f_lseek(ctx->save_ctx->file, ctx->hash_offset + offset);
+            UINT br = 0;
+            f_read(ctx->save_ctx->file, buffer, count, &br);
+            return br;
+        case STORAGE_REMAP:
+            save_remap_read(&ctx->save_ctx->meta_remap_storage, buffer, ctx->data_offset + offset, count);
+            return count;
+        case STORAGE_JOURNAL:
+            save_journal_storage_read(&ctx->save_ctx->journal_storage, &ctx->save_ctx->data_remap_storage, buffer, ctx->data_offset + offset, count);
+            return count;
+        default:
+            return 0;
+    }
+}
+
+void save_ivfc_storage_read(integrity_verification_storage_ctx_t *ctx, void *buffer, uint64_t offset, size_t count, uint32_t verify) {
+    if (count > ctx->sector_size) {
+        EPRINTF("IVFC read exceeds sector size!");
+        return;
+    }
+
+    uint64_t block_index = offset / ctx->sector_size;
+
+    if (ctx->block_validities[block_index] == VALIDITY_INVALID && verify) {
+        EPRINTFARGS("Hash error from previous check\n found at offset %x count %x!", (u32)offset, count);
+        return;
+    }
+
+    uint8_t hash_buffer[0x20] = {0};
+    uint8_t zeroes[0x20] = {0};
+    uint64_t hash_pos = block_index * 0x20;
+    if (ctx->next_level) {
+        save_ivfc_storage_read(ctx->next_level, hash_buffer, hash_pos, 0x20, verify);
+    } else {
+        save_ivfc_level_fread(ctx->hash_storage, hash_buffer, hash_pos, 0x20);
+    }
+
+    if (!memcmp(hash_buffer, zeroes, 0x20)) {
+        memset(buffer, 0, count);
+        ctx->block_validities[block_index] = VALIDITY_VALID;
+        return;
+    }
+
+    save_ivfc_level_fread(ctx->base_storage, buffer, offset, count);
+
+    if (!(verify && ctx->block_validities[block_index] == VALIDITY_UNCHECKED)) {
+        return;
+    }
+
+    uint8_t hash[0x20] = {0};
+    uint8_t *data_buffer = calloc(1, ctx->sector_size + 0x20);
+    memcpy(data_buffer, ctx->salt, 0x20);
+    memcpy(data_buffer + 0x20, buffer, ctx->sector_size);
+
+    se_calc_sha256(hash, data_buffer, ctx->sector_size + 0x20);
+    hash[0x1F] |= 0x80;
+
+    free(data_buffer);
+    if (memcmp(hash_buffer, hash, 0x20) != 0) {
+        ctx->block_validities[block_index] = VALIDITY_INVALID;
+    } else {
+        ctx->block_validities[block_index] = VALIDITY_VALID;
+    }
+
+    if (ctx->block_validities[block_index] == VALIDITY_INVALID && verify) {
+        EPRINTFARGS("Hash error from current check\n found at offset %x count %x!", (u32)offset, count);
+        return;
+    }
+}
+
+uint32_t save_allocation_table_read_entry_with_length(allocation_table_ctx_t *ctx, allocation_table_entry_t *entry) {
+    uint32_t length = 1;
+    uint32_t entry_index = allocation_table_block_to_entry_index(entry->next);
+
+    allocation_table_entry_t *entries = (allocation_table_entry_t *)((uint8_t *)(ctx->base_storage) + entry_index * SAVE_FAT_ENTRY_SIZE);
+    if ((entries[0].next & 0x80000000) == 0) {
+        if (entries[0].prev & 0x80000000 && entries[0].prev != 0x80000000) {
+            EPRINTF("Invalid range entry in allocation table!");
+            return 0;
+        }
+    } else {
+        length = entries[1].next - entry_index + 1;
+    }
+
+    if (allocation_table_is_list_end(&entries[0])) {
+        entry->next = 0xFFFFFFFF;
+    } else {
+        entry->next = allocation_table_entry_index_to_block(allocation_table_get_next(&entries[0]));
+    }
+
+    if (allocation_table_is_list_start(&entries[0])) {
+        entry->prev = 0xFFFFFFFF;
+    } else {
+        entry->prev = allocation_table_entry_index_to_block(allocation_table_get_prev(&entries[0]));
+    }
+
+    return length;
+}
+
+uint32_t save_allocation_table_get_list_length(allocation_table_ctx_t *ctx, uint32_t block_index) {
+    allocation_table_entry_t entry;
+    entry.next = block_index;
+    uint32_t total_length = 0;
+    uint32_t table_size = ctx->header->allocation_table_block_count;
+    uint32_t nodes_iterated = 0;
+
+    while (entry.next != 0xFFFFFFFF) {
+        total_length += save_allocation_table_read_entry_with_length(ctx, &entry);
+        nodes_iterated++;
+        if (nodes_iterated > table_size) {
+            EPRINTF("Cycle detected in allocation table!");
+            return 0;
+        }
+    }
+    return total_length;
+}
+
+uint64_t save_allocation_table_get_free_space_size(save_filesystem_ctx_t *ctx) {
+    uint32_t free_list_start = save_allocation_table_get_free_list_block_index(&ctx->allocation_table);
+
+    if (free_list_start == 0xFFFFFFFF) return 0;
+
+    return ctx->header->block_size * save_allocation_table_get_list_length(&ctx->allocation_table, free_list_start);
+}
+
+void save_allocation_table_iterator_begin(allocation_table_iterator_ctx_t *ctx, allocation_table_ctx_t *table, uint32_t initial_block) {
+    ctx->fat = table;
+    ctx->physical_block = initial_block;
+    ctx->virtual_block = 0;
+
+    allocation_table_entry_t entry = {0, 0};
+    entry.next = initial_block;
+    ctx->current_segment_size = save_allocation_table_read_entry_with_length(ctx->fat, &entry);
+    ctx->next_block = entry.next;
+    ctx->prev_block = entry.prev;
+
+    if (ctx->prev_block != 0xFFFFFFFF) {
+        EPRINTFARGS("Attempted to start FAT iteration from\n invalid block %x!", initial_block);
+        return;
+    }
+}
+
+int save_allocation_table_iterator_move_next(allocation_table_iterator_ctx_t *ctx) {
+    if (ctx->next_block == 0xFFFFFFFF) return 0;
+
+    ctx->virtual_block += ctx->current_segment_size;
+    ctx->physical_block = ctx->next_block;
+
+    allocation_table_entry_t entry = {0, 0};
+    entry.next = ctx->next_block;
+    ctx->current_segment_size = save_allocation_table_read_entry_with_length(ctx->fat, &entry);
+    ctx->next_block = entry.next;
+    ctx->prev_block = entry.prev;
+
+    return 1;
+}
+
+int save_allocation_table_iterator_move_prev(allocation_table_iterator_ctx_t *ctx) {
+    if (ctx->prev_block == 0xFFFFFFFF) return 0;
+
+    ctx->physical_block = ctx->prev_block;
+
+    allocation_table_entry_t entry = {0, 0};
+    entry.next = ctx->prev_block;
+    ctx->current_segment_size = save_allocation_table_read_entry_with_length(ctx->fat, &entry);
+    ctx->next_block = entry.next;
+    ctx->prev_block = entry.prev;
+
+    ctx->virtual_block -= ctx->current_segment_size;
+
+    return 1;
+}
+
+int save_allocation_table_iterator_seek(allocation_table_iterator_ctx_t *ctx, uint32_t block) {
+    while (1) {
+        if (block < ctx->virtual_block) {
+            if (!save_allocation_table_iterator_move_prev(ctx)) return 0;
+        } else if (block >= ctx->virtual_block + ctx->current_segment_size) {
+            if (!save_allocation_table_iterator_move_next(ctx)) return 0;
+        } else {
+            return 1;
+        }
+
+    }
+}
+
+uint32_t save_allocation_table_storage_read(allocation_table_storage_ctx_t *ctx, void *buffer, uint64_t offset, size_t count) {
+    allocation_table_iterator_ctx_t iterator;
+    save_allocation_table_iterator_begin(&iterator, ctx->fat, ctx->initial_block);
+    uint64_t in_pos = offset;
+    uint32_t out_pos = 0;
+    uint32_t remaining = count;
+
+    while (remaining) {
+        uint32_t block_num = (uint32_t)(in_pos / ctx->block_size);
+        save_allocation_table_iterator_seek(&iterator, block_num);
+
+        uint32_t segment_pos = (uint32_t)(in_pos - (uint64_t)iterator.virtual_block * ctx->block_size);
+        uint64_t physical_offset = iterator.physical_block * ctx->block_size + segment_pos;
+
+        uint32_t remaining_in_segment = iterator.current_segment_size * ctx->block_size - segment_pos;
+        uint32_t bytes_to_read = remaining < remaining_in_segment ? remaining : remaining_in_segment;
+
+        uint32_t sector_size = ctx->base_storage->integrity_storages[3].sector_size;
+        uint32_t chunk_remaining = bytes_to_read;
+        for (unsigned int i = 0; i < bytes_to_read; i += sector_size) {
+            uint32_t bytes_to_request = chunk_remaining < sector_size ? chunk_remaining : sector_size;
+            save_ivfc_storage_read(&ctx->base_storage->integrity_storages[3], (uint8_t *)buffer + out_pos + i, physical_offset + i, bytes_to_request, ctx->base_storage->data_level->save_ctx->tool_ctx.action & ACTION_VERIFY);
+            chunk_remaining -= bytes_to_request;
+        }
+
+        out_pos += bytes_to_read;
+        in_pos += bytes_to_read;
+        remaining -= bytes_to_read;
+    }
+    return out_pos;
+}
+
+uint32_t save_fs_list_get_capacity(save_filesystem_list_ctx_t *ctx) {
+    if (!ctx->capacity)
+        save_allocation_table_storage_read(&ctx->storage, &ctx->capacity, 4, 4);
+    return ctx->capacity;
+}
+
+uint32_t save_fs_list_read_entry(save_filesystem_list_ctx_t *ctx, uint32_t index, save_fs_list_entry_t *entry) {
+    return save_allocation_table_storage_read(&ctx->storage, entry, index * SAVE_FS_LIST_ENTRY_SIZE, SAVE_FS_LIST_ENTRY_SIZE);
+}
+
+int save_fs_list_get_value(save_filesystem_list_ctx_t *ctx, uint32_t index, save_fs_list_entry_t *value) {
+    if (index >= save_fs_list_get_capacity(ctx)) {
+        return 0;
+    }
+    save_fs_list_read_entry(ctx, index, value);
+    return 1;
+}
+
+uint32_t save_fs_list_get_index_from_key(save_filesystem_list_ctx_t *ctx, save_entry_key_t *key, uint32_t *prev_index) {
+    save_fs_list_entry_t entry;
+    uint32_t capacity = save_fs_list_get_capacity(ctx);
+    save_fs_list_read_entry(ctx, ctx->used_list_head_index, &entry);
+    uint32_t prev;
+    if (!prev_index) {
+        prev_index = &prev;
+    }
+    *prev_index = ctx->used_list_head_index;
+    uint32_t index = entry.next;
+    while (index) {
+        if (index > capacity) {
+            EPRINTFARGS("Save entry index %d out of range!", index);
+            *prev_index = 0xFFFFFFFF;
+            return 0xFFFFFFFF;
+        }
+        save_fs_list_read_entry(ctx, index, &entry);
+        if (entry.parent == key->parent && !strcmp(entry.name, key->name)) {
+            return index;
+        }
+        *prev_index = index;
+        index = entry.next;
+    }
+    *prev_index = 0xFFFFFFFF;
+    return 0xFFFFFFFF;
+}
+
+int save_hierarchical_file_table_find_path_recursive(hierarchical_save_file_table_ctx_t *ctx, save_entry_key_t *key, const char *path) {
+    key->parent = 0;
+    const char *pos = strchr(path, '/');
+    while (pos) {
+        memset(key->name, 0, SAVE_FS_LIST_MAX_NAME_LENGTH);
+        const char *tmp = strchr(pos, '/');
+        if (!tmp) {
+            memcpy(key->name, pos, strlen(pos));
+            break;
+        }
+        memcpy(key->name, pos, tmp - pos);
+        key->parent = save_fs_list_get_index_from_key(&ctx->directory_table, key, NULL);
+        if (key->parent == 0xFFFFFFFF)
+            return 0;
+        pos = tmp + 1;
+    }
+    return 1;
+}
+
+int save_hierarchical_file_table_find_next_file(hierarchical_save_file_table_ctx_t *ctx, save_find_position_t *position, save_file_info_t *info, char *name) {
+    if (position->next_file == 0) {
+        return 0;
+    }
+    save_fs_list_entry_t entry;
+    if(!save_fs_list_get_value(&ctx->file_table, position->next_file, &entry)) {
+        return 0;
+    }
+    position->next_file = entry.value.next_sibling;
+    memcpy(name, &entry.name, SAVE_FS_LIST_MAX_NAME_LENGTH);
+    memcpy(info, &entry.value.save_file_info, sizeof(save_file_info_t));
+    return 1;
+}
+
+int save_hierarchical_file_table_find_next_directory(hierarchical_save_file_table_ctx_t *ctx, save_find_position_t *position, char *name) {
+    if (position->next_directory == 0) {
+        return 0;
+    }
+    save_fs_list_entry_t entry;
+    if(!save_fs_list_get_value(&ctx->directory_table, position->next_directory, &entry)) {
+        return 0;
+    }
+    position->next_directory = entry.value.next_sibling;
+    memcpy(name, &entry.name, SAVE_FS_LIST_MAX_NAME_LENGTH);
+    return 1;
+}
+
+int save_hierarchical_file_table_get_file_entry_by_path(hierarchical_save_file_table_ctx_t *ctx, const char *path, save_fs_list_entry_t *entry) {
+    save_entry_key_t key;
+    if (!save_hierarchical_file_table_find_path_recursive(ctx, &key, path)) {
+        EPRINTF("Unable to locate file.");
+        return 0;
+    }
+    u32 index = save_fs_list_get_index_from_key(&ctx->file_table, &key, NULL);
+    if (index == 0xFFFFFFFF) {
+        EPRINTF("Unable to get table index for file.");
+        return 0;
+    }
+    if (!save_fs_list_get_value(&ctx->file_table, index, entry)) {
+        EPRINTF("Unable to get file entry from index.");
+        return 0;
+    }
+    return 1;
+}
+
+void save_open_fat_storage(save_filesystem_ctx_t *ctx, allocation_table_storage_ctx_t *storage_ctx, uint32_t block_index) {
+    storage_ctx->base_storage = ctx->base_storage;
+    storage_ctx->fat = &ctx->allocation_table;
+    storage_ctx->block_size = (uint32_t)ctx->header->block_size;
+    storage_ctx->initial_block = block_index;
+    storage_ctx->_length = block_index == 0xFFFFFFFF ? 0 : save_allocation_table_get_list_length(storage_ctx->fat, block_index) * storage_ctx->block_size;
+}
+
+void save_filesystem_init(save_filesystem_ctx_t *ctx, void *fat, save_fs_header_t *save_fs_header, fat_header_t *fat_header) {
+    ctx->allocation_table.base_storage = fat;
+    ctx->allocation_table.header = fat_header;
+    ctx->allocation_table.free_list_entry_index = 0;
+    ctx->header = save_fs_header;
+
+    save_open_fat_storage(ctx, &ctx->file_table.directory_table.storage, fat_header->directory_table_block);
+    save_open_fat_storage(ctx, &ctx->file_table.file_table.storage, fat_header->file_table_block);
+    ctx->file_table.file_table.free_list_head_index = 0;
+    ctx->file_table.file_table.used_list_head_index = 1;
+    ctx->file_table.directory_table.free_list_head_index = 0;
+    ctx->file_table.directory_table.used_list_head_index = 1;
+}
+
+validity_t save_ivfc_validate(hierarchical_integrity_verification_storage_ctx_t *ctx, ivfc_save_hdr_t *ivfc) {
+    validity_t result = VALIDITY_VALID;
+    for (unsigned int i = 0; i < ivfc->num_levels - 1 && result != VALIDITY_INVALID; i++) {
+        integrity_verification_storage_ctx_t *storage = &ctx->integrity_storages[i];
+
+        uint64_t block_size = storage->sector_size;
+        uint32_t block_count = (uint32_t)((storage->_length + block_size - 1) / block_size);
+
+        uint8_t *buffer = malloc(block_size);
+
+        for (unsigned int j = 0; j < block_count; j++) {
+            if (ctx->level_validities[ivfc->num_levels - 2][j] == VALIDITY_UNCHECKED) {
+                uint32_t to_read = storage->_length - block_size * j < block_size ? storage->_length - block_size * j : block_size;
+                save_ivfc_storage_read(storage, buffer, block_size * j, to_read, 1);
+            }
+            if (ctx->level_validities[ivfc->num_levels - 2][j] == VALIDITY_INVALID) {
+                result = VALIDITY_INVALID;
+                break;
+            }
+        }
+        free(buffer);
+    }
+
+    return result;
+}
+
+void save_ivfc_set_level_validities(hierarchical_integrity_verification_storage_ctx_t *ctx, ivfc_save_hdr_t *ivfc) {
+    for (unsigned int i = 0; i < ivfc->num_levels - 1; i++) {
+        validity_t level_validity = VALIDITY_VALID;
+        for (unsigned int j = 0; j < ctx->integrity_storages[i].sector_count; j++) {
+            if (ctx->level_validities[i][j] == VALIDITY_INVALID) {
+                level_validity = VALIDITY_INVALID;
+                break;
+            }
+            if (ctx->level_validities[i][j] == VALIDITY_UNCHECKED && level_validity != VALIDITY_INVALID) {
+                level_validity = VALIDITY_UNCHECKED;
+            }
+        }
+        ctx->levels[i].hash_validity = level_validity;
+    }
+}
+
+validity_t save_filesystem_verify(save_ctx_t *ctx) {
+    validity_t journal_validity = save_ivfc_validate(&ctx->core_data_ivfc_storage, &ctx->header.data_ivfc_header);
+    save_ivfc_set_level_validities(&ctx->core_data_ivfc_storage, &ctx->header.data_ivfc_header);
+
+    if (!ctx->fat_ivfc_storage.levels[0].save_ctx) return journal_validity;
+
+    validity_t fat_validity = save_ivfc_validate(&ctx->fat_ivfc_storage, &ctx->header.fat_ivfc_header);
+    save_ivfc_set_level_validities(&ctx->fat_ivfc_storage, &ctx->header.fat_ivfc_header);
+
+    if (journal_validity != VALIDITY_VALID) return journal_validity;
+    if (fat_validity != VALIDITY_VALID) return fat_validity;
+
+    return journal_validity;
+}
+
+bool save_process(save_ctx_t *ctx) {
+    /* Try to parse Header A. */
+    f_lseek(ctx->file, 0);
+    if (f_read(ctx->file, &ctx->header, sizeof(ctx->header), NULL)) {
+        EPRINTF("Failed to read save header!\n");
+        return false;
+    }
+
+    if (!save_process_header(ctx) || (ctx->header_hash_validity == VALIDITY_INVALID)) {
+        /* Try to parse Header B. */
+        f_lseek(ctx->file, 0x4000);
+        if (f_read(ctx->file, &ctx->header, sizeof(ctx->header), NULL)) {
+            EPRINTF("Failed to read save header!\n");
+            return false;
+        }
+
+        if (!save_process_header(ctx) || (ctx->header_hash_validity == VALIDITY_INVALID)) {
+            EPRINTF("Error: Save header is invalid!");
+            return false;
+        }
+    }
+
+    unsigned char cmac[0x10] = {};
+    se_aes_key_set(3, ctx->save_mac_key, 0x10);
+    se_aes_cmac(3, cmac, 0x10, &ctx->header.layout, sizeof(ctx->header.layout));
+    if (memcmp(cmac, &ctx->header.cmac, 0x10) == 0) {
+        ctx->header_cmac_validity = VALIDITY_VALID;
+    } else {
+        ctx->header_cmac_validity = VALIDITY_INVALID;
+    }
+
+    /* Initialize remap storages. */
+    ctx->data_remap_storage.type = STORAGE_BYTES;
+    ctx->data_remap_storage.base_storage_offset = ctx->header.layout.file_map_data_offset;
+    ctx->data_remap_storage.header = &ctx->header.main_remap_header;
+    ctx->data_remap_storage.map_entries = malloc(sizeof(remap_entry_ctx_t) * ctx->data_remap_storage.header->map_entry_count);
+    ctx->data_remap_storage.file = ctx->file;
+    f_lseek(ctx->file, ctx->header.layout.file_map_entry_offset);
+    for (unsigned int i = 0; i < ctx->data_remap_storage.header->map_entry_count; i++) {
+        f_read(ctx->file, &ctx->data_remap_storage.map_entries[i], 0x20, NULL);
+        ctx->data_remap_storage.map_entries[i].physical_offset_end = ctx->data_remap_storage.map_entries[i].physical_offset + ctx->data_remap_storage.map_entries[i].size;
+        ctx->data_remap_storage.map_entries[i].virtual_offset_end = ctx->data_remap_storage.map_entries[i].virtual_offset + ctx->data_remap_storage.map_entries[i].size;
+    }
+
+    /* Initialize data remap storage. */
+    ctx->data_remap_storage.segments = save_remap_init_segments(ctx->data_remap_storage.header, ctx->data_remap_storage.map_entries, ctx->data_remap_storage.header->map_entry_count);
+
+    /* Initialize duplex storage. */
+    ctx->duplex_layers[0].data_a = (uint8_t *)&ctx->header + ctx->header.layout.duplex_master_offset_a;
+    ctx->duplex_layers[0].data_b = (uint8_t *)&ctx->header + ctx->header.layout.duplex_master_offset_b;
+    memcpy(&ctx->duplex_layers[0].info, &ctx->header.duplex_header.layers[0], sizeof(duplex_info_t));
+
+    ctx->duplex_layers[1].data_a = malloc(ctx->header.layout.duplex_l1_size);
+    save_remap_read(&ctx->data_remap_storage, ctx->duplex_layers[1].data_a, ctx->header.layout.duplex_l1_offset_a, ctx->header.layout.duplex_l1_size);
+    ctx->duplex_layers[1].data_b = malloc(ctx->header.layout.duplex_l1_size);
+    save_remap_read(&ctx->data_remap_storage, ctx->duplex_layers[1].data_b, ctx->header.layout.duplex_l1_offset_b, ctx->header.layout.duplex_l1_size);
+    memcpy(&ctx->duplex_layers[1].info, &ctx->header.duplex_header.layers[1], sizeof(duplex_info_t));
+
+    ctx->duplex_layers[2].data_a = malloc(ctx->header.layout.duplex_data_size);
+    save_remap_read(&ctx->data_remap_storage, ctx->duplex_layers[2].data_a, ctx->header.layout.duplex_data_offset_a, ctx->header.layout.duplex_data_size);
+    ctx->duplex_layers[2].data_b = malloc(ctx->header.layout.duplex_data_size);
+    save_remap_read(&ctx->data_remap_storage, ctx->duplex_layers[2].data_b, ctx->header.layout.duplex_data_offset_b, ctx->header.layout.duplex_data_size);
+    memcpy(&ctx->duplex_layers[2].info, &ctx->header.duplex_header.layers[2], sizeof(duplex_info_t));
+
+    /* Initialize hierarchical duplex storage. */
+    uint8_t *bitmap = ctx->header.layout.duplex_index == 1 ? ctx->duplex_layers[0].data_b : ctx->duplex_layers[0].data_a;
+    save_duplex_storage_init(&ctx->duplex_storage.layers[0], &ctx->duplex_layers[1], bitmap, ctx->header.layout.duplex_master_size);
+    ctx->duplex_storage.layers[0]._length = ctx->header.layout.duplex_l1_size;
+
+    bitmap = malloc(ctx->duplex_storage.layers[0]._length);
+    save_duplex_storage_read(&ctx->duplex_storage.layers[0], bitmap, 0, ctx->duplex_storage.layers[0]._length);
+    save_duplex_storage_init(&ctx->duplex_storage.layers[1], &ctx->duplex_layers[2], bitmap, ctx->duplex_storage.layers[0]._length);
+    ctx->duplex_storage.layers[1]._length = ctx->header.layout.duplex_data_size;
+
+    ctx->duplex_storage.data_layer = ctx->duplex_storage.layers[1];
+
+    /* Initialize meta remap storage. */
+    ctx->meta_remap_storage.type = STORAGE_DUPLEX;
+    ctx->meta_remap_storage.duplex = &ctx->duplex_storage.data_layer;
+    ctx->meta_remap_storage.header = &ctx->header.meta_remap_header;
+    ctx->meta_remap_storage.map_entries = malloc(sizeof(remap_entry_ctx_t) * ctx->meta_remap_storage.header->map_entry_count);
+    ctx->meta_remap_storage.file = ctx->file;
+    f_lseek(ctx->file, ctx->header.layout.meta_map_entry_offset);
+    for (unsigned int i = 0; i < ctx->meta_remap_storage.header->map_entry_count; i++) {
+        f_read(ctx->file, &ctx->meta_remap_storage.map_entries[i], 0x20, NULL);
+        ctx->meta_remap_storage.map_entries[i].physical_offset_end = ctx->meta_remap_storage.map_entries[i].physical_offset + ctx->meta_remap_storage.map_entries[i].size;
+        ctx->meta_remap_storage.map_entries[i].virtual_offset_end = ctx->meta_remap_storage.map_entries[i].virtual_offset + ctx->meta_remap_storage.map_entries[i].size;
+    }
+
+    ctx->meta_remap_storage.segments = save_remap_init_segments(ctx->meta_remap_storage.header, ctx->meta_remap_storage.map_entries, ctx->meta_remap_storage.header->map_entry_count);
+
+   /* Initialize journal map. */
+    ctx->journal_map_info.map_storage = malloc(ctx->header.layout.journal_map_table_size);
+    save_remap_read(&ctx->meta_remap_storage, ctx->journal_map_info.map_storage, ctx->header.layout.journal_map_table_offset, ctx->header.layout.journal_map_table_size);
+
+    /* Initialize journal storage. */
+    ctx->journal_storage.header = &ctx->header.journal_header;
+    ctx->journal_storage.journal_data_offset = ctx->header.layout.journal_data_offset;
+    ctx->journal_storage._length = ctx->journal_storage.header->total_size - ctx->journal_storage.header->journal_size;
+    ctx->journal_storage.file = ctx->file;
+    ctx->journal_storage.map.header = &ctx->header.map_header;
+    ctx->journal_storage.map.map_storage = ctx->journal_map_info.map_storage;
+    ctx->journal_storage.map.entries = malloc(sizeof(journal_map_entry_t) * ctx->journal_storage.map.header->main_data_block_count);
+    uint32_t *pos = (uint32_t *)ctx->journal_storage.map.map_storage;
+    for (unsigned int i = 0; i < ctx->journal_storage.map.header->main_data_block_count; i++) {
+        ctx->journal_storage.map.entries[i].virtual_index = i;
+        ctx->journal_storage.map.entries[i].physical_index = *pos & 0x7FFFFFFF;
+        pos += 2;
+    }
+    ctx->journal_storage.block_size = ctx->journal_storage.header->block_size;
+    ctx->journal_storage._length = ctx->journal_storage.header->total_size - ctx->journal_storage.header->journal_size;
+
+    /* Initialize core IVFC storage. */
+    for (unsigned int i = 0; i < 5; i++) {
+        ctx->core_data_ivfc_storage.levels[i].save_ctx = ctx;
+    }
+    save_ivfc_storage_init(&ctx->core_data_ivfc_storage, ctx->header.layout.ivfc_master_hash_offset_a, &ctx->header.data_ivfc_header);
+
+    /* Initialize FAT storage. */
+    if (ctx->header.layout.version < 0x50000) {
+        ctx->fat_storage = malloc(ctx->header.layout.fat_size);
+        save_remap_read(&ctx->meta_remap_storage, ctx->fat_storage, ctx->header.layout.fat_offset, ctx->header.layout.fat_size);
+    } else {
+        for (unsigned int i = 0; i < 5; i++) {
+            ctx->fat_ivfc_storage.levels[i].save_ctx = ctx;
+        }
+        save_ivfc_storage_init(&ctx->fat_ivfc_storage, ctx->header.layout.fat_ivfc_master_hash_a, &ctx->header.fat_ivfc_header);
+        ctx->fat_storage = malloc(ctx->fat_ivfc_storage._length);
+        save_remap_read(&ctx->meta_remap_storage, ctx->fat_storage, ctx->header.fat_ivfc_header.level_headers[ctx->header.fat_ivfc_header.num_levels - 2].logical_offset, ctx->fat_ivfc_storage._length);
+    }
+
+    if (ctx->tool_ctx.action & ACTION_VERIFY) {
+        save_filesystem_verify(ctx);
+    }
+
+    /* Initialize core save filesystem. */
+    ctx->save_filesystem_core.base_storage = &ctx->core_data_ivfc_storage;
+    save_filesystem_init(&ctx->save_filesystem_core, ctx->fat_storage, &ctx->header.save_header, &ctx->header.fat_header);
+
+    return true;
+}
+
+bool save_process_header(save_ctx_t *ctx) {
+    if (ctx->header.layout.magic != MAGIC_DISF || ctx->header.duplex_header.magic != MAGIC_DPFS ||
+        ctx->header.data_ivfc_header.magic != MAGIC_IVFC || ctx->header.journal_header.magic != MAGIC_JNGL ||
+        ctx->header.save_header.magic != MAGIC_SAVE || ctx->header.main_remap_header.magic != MAGIC_RMAP ||
+        ctx->header.meta_remap_header.magic != MAGIC_RMAP)
+    {
+        EPRINTF("Error: Save header is corrupt!");
+        return false;
+    }
+
+    ctx->data_ivfc_master = (uint8_t *)&ctx->header + ctx->header.layout.ivfc_master_hash_offset_a;
+    ctx->fat_ivfc_master = (uint8_t *)&ctx->header + ctx->header.layout.fat_ivfc_master_hash_a;
+
+    uint8_t hash[0x20];
+    se_calc_sha256(hash, &ctx->header.duplex_header, 0x3D00);
+    ctx->header_hash_validity = memcmp(hash, ctx->header.layout.hash, 0x20) == 0 ? VALIDITY_VALID : VALIDITY_INVALID;
+
+    ctx->header.data_ivfc_header.num_levels = 5;
+
+    if (ctx->header.layout.version >= 0x50000) {
+        ctx->header.fat_ivfc_header.num_levels = 4;
+    }
+    return true;
+}
+
+void save_free_contexts(save_ctx_t *ctx) {
+    for (unsigned int i = 0; i < ctx->data_remap_storage.header->map_segment_count; i++) {
+        free(ctx->data_remap_storage.segments[i].entries);
+    }
+    free(ctx->data_remap_storage.segments);
+    for (unsigned int i = 0; i < ctx->meta_remap_storage.header->map_segment_count; i++) {
+        free(ctx->meta_remap_storage.segments[i].entries);
+    }
+    free(ctx->meta_remap_storage.segments);
+    free(ctx->data_remap_storage.map_entries);
+    free(ctx->meta_remap_storage.map_entries);
+    free(ctx->duplex_storage.layers[0].bitmap.bitmap);
+    free(ctx->duplex_storage.layers[1].bitmap.bitmap);
+    free(ctx->duplex_storage.layers[1].bitmap_storage);
+    for (unsigned int i = 1; i < 3; i++) {
+        free(ctx->duplex_layers[i].data_a);
+        free(ctx->duplex_layers[i].data_b);
+    }
+    free(ctx->journal_map_info.map_storage);
+    free(ctx->journal_storage.map.entries);
+    for (unsigned int i = 0; i < ctx->header.data_ivfc_header.num_levels - 1; i++) {
+        free(ctx->core_data_ivfc_storage.integrity_storages[i].block_validities);
+    }
+    free(ctx->core_data_ivfc_storage.level_validities);
+    if (ctx->header.layout.version >= 0x50000) {
+        for (unsigned int i = 0; i < ctx->header.fat_ivfc_header.num_levels - 1; i++) {
+            free(ctx->fat_ivfc_storage.integrity_storages[i].block_validities);
+        }
+    }
+    free(ctx->fat_ivfc_storage.level_validities);
+    free(ctx->fat_storage);
+}

source/keys/save.h

@@ -0,0 +1,485 @@
+#ifndef _SAVE_H
+#define _SAVE_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include "../libs/fatfs/ff.h"
+
+#define SAVE_HEADER_SIZE 0x4000
+#define SAVE_FAT_ENTRY_SIZE 8
+#define SAVE_FS_LIST_MAX_NAME_LENGTH 0x40
+#define SAVE_FS_LIST_ENTRY_SIZE 0x60
+
+#define IVFC_MAX_LEVEL 6
+
+#define MAGIC_DISF 0x46534944
+#define MAGIC_DPFS 0x53465044
+#define MAGIC_JNGL 0x4C474E4A
+#define MAGIC_SAVE 0x45564153
+#define MAGIC_RMAP 0x50414D52
+#define MAGIC_IVFC 0x43465649
+
+typedef enum {
+    VALIDITY_UNCHECKED = 0,
+    VALIDITY_INVALID,
+    VALIDITY_VALID
+} validity_t;
+
+typedef struct save_ctx_t save_ctx_t;
+
+typedef struct {
+    uint32_t magic; /* DISF */
+    uint32_t version;
+    uint8_t hash[0x20];
+    uint64_t file_map_entry_offset;
+    uint64_t file_map_entry_size;
+    uint64_t meta_map_entry_offset;
+    uint64_t meta_map_entry_size;
+    uint64_t file_map_data_offset;
+    uint64_t file_map_data_size;
+    uint64_t duplex_l1_offset_a;
+    uint64_t duplex_l1_offset_b;
+    uint64_t duplex_l1_size;
+    uint64_t duplex_data_offset_a;
+    uint64_t duplex_data_offset_b;
+    uint64_t duplex_data_size;
+    uint64_t journal_data_offset;
+    uint64_t journal_data_size_a;
+    uint64_t journal_data_size_b;
+    uint64_t journal_size;
+    uint64_t duplex_master_offset_a;
+    uint64_t duplex_master_offset_b;
+    uint64_t duplex_master_size;
+    uint64_t ivfc_master_hash_offset_a;
+    uint64_t ivfc_master_hash_offset_b;
+    uint64_t ivfc_master_hash_size;
+    uint64_t journal_map_table_offset;
+    uint64_t journal_map_table_size;
+    uint64_t journal_physical_bitmap_offset;
+    uint64_t journal_physical_bitmap_size;
+    uint64_t journal_virtual_bitmap_offset;
+    uint64_t journal_virtual_bitmap_size;
+    uint64_t journal_free_bitmap_offset;
+    uint64_t journal_free_bitmap_size;
+    uint64_t ivfc_l1_offset;
+    uint64_t ivfc_l1_size;
+    uint64_t ivfc_l2_offset;
+    uint64_t ivfc_l2_size;
+    uint64_t ivfc_l3_offset;
+    uint64_t ivfc_l3_size;
+    uint64_t fat_offset;
+    uint64_t fat_size;
+    uint64_t duplex_index;
+    uint64_t fat_ivfc_master_hash_a;
+    uint64_t fat_ivfc_master_hash_b;
+    uint64_t fat_ivfc_l1_offset;
+    uint64_t fat_ivfc_l1_size;
+    uint64_t fat_ivfc_l2_offset;
+    uint64_t fat_ivfc_l2_size;
+    uint8_t _0x190[0x70];
+} fs_layout_t;
+
+#pragma pack(push, 1)
+typedef struct {
+    uint64_t offset;
+    uint64_t length;
+    uint32_t block_size_power;
+} duplex_info_t;
+#pragma pack(pop)
+
+typedef struct {
+    uint32_t magic; /* DPFS */
+    uint32_t version;
+    duplex_info_t layers[3];
+} duplex_header_t;
+
+typedef struct {
+    uint32_t version;
+    uint32_t main_data_block_count;
+    uint32_t journal_block_count;
+    uint32_t _0x0C;
+} journal_map_header_t;
+
+typedef struct {
+    uint32_t magic; /* JNGL */
+    uint32_t version;
+    uint64_t total_size;
+    uint64_t journal_size;
+    uint64_t block_size;
+} journal_header_t;
+
+typedef struct {
+    uint32_t magic; /* SAVE */
+    uint32_t version;
+    uint64_t block_count;
+    uint64_t block_size;
+} save_fs_header_t;
+
+typedef struct {
+    uint64_t block_size;
+    uint64_t allocation_table_offset;
+    uint32_t allocation_table_block_count;
+    uint32_t _0x14;
+    uint64_t data_offset;
+    uint32_t data_block_count;
+    uint32_t _0x24;
+    uint32_t directory_table_block;
+    uint32_t file_table_block;
+} fat_header_t;
+
+typedef struct {
+    uint32_t magic; /* RMAP */
+    uint32_t version;
+    uint32_t map_entry_count;
+    uint32_t map_segment_count;
+    uint32_t segment_bits;
+    uint8_t _0x14[0x2C];
+} remap_header_t;
+
+typedef struct remap_segment_ctx_t remap_segment_ctx_t;
+typedef struct remap_entry_ctx_t remap_entry_ctx_t;
+
+#pragma pack(push, 1)
+struct remap_entry_ctx_t {
+    uint64_t virtual_offset;
+    uint64_t physical_offset;
+    uint64_t size;
+    uint32_t alignment;
+    uint32_t _0x1C;
+    uint64_t virtual_offset_end;
+    uint64_t physical_offset_end;
+    remap_segment_ctx_t *segment;
+    remap_entry_ctx_t *next;
+};
+#pragma pack(pop)
+
+struct remap_segment_ctx_t{
+    uint64_t offset;
+    uint64_t length;
+    remap_entry_ctx_t **entries;
+    uint64_t entry_count;
+};
+
+typedef struct {
+    uint8_t *data;
+    uint8_t *bitmap;
+} duplex_bitmap_t;
+
+typedef struct {
+    uint32_t block_size;
+    uint8_t *bitmap_storage;
+    uint8_t *data_a;
+    uint8_t *data_b;
+    duplex_bitmap_t bitmap;
+    uint64_t _length;
+} duplex_storage_ctx_t;
+
+enum base_storage_type {
+    STORAGE_BYTES = 0,
+    STORAGE_DUPLEX = 1,
+    STORAGE_REMAP = 2,
+    STORAGE_JOURNAL = 3
+};
+
+typedef struct {
+    remap_header_t *header;
+    remap_entry_ctx_t *map_entries;
+    remap_segment_ctx_t *segments;
+    enum base_storage_type type;
+    uint64_t base_storage_offset;
+    duplex_storage_ctx_t *duplex;
+    FIL *file;
+} remap_storage_ctx_t;
+
+typedef struct {
+    uint64_t title_id;
+    uint8_t user_id[0x10];
+    uint64_t save_id;
+    uint8_t save_data_type;
+    uint8_t _0x21[0x1F];
+    uint64_t save_owner_id;
+    uint64_t timestamp;
+    uint64_t _0x50;
+    uint64_t data_size;
+    uint64_t journal_size;
+    uint64_t commit_id;
+} extra_data_t;
+
+typedef struct {
+    uint64_t logical_offset;
+    uint64_t hash_data_size;
+    uint32_t block_size;
+    uint32_t reserved;
+} ivfc_level_hdr_t;
+
+typedef struct {
+    uint32_t magic;
+    uint32_t id;
+    uint32_t master_hash_size;
+    uint32_t num_levels;
+    ivfc_level_hdr_t level_headers[IVFC_MAX_LEVEL];
+    uint8_t salt_source[0x20];
+} ivfc_save_hdr_t;
+
+#pragma pack(push, 1)
+typedef struct {
+    uint8_t cmac[0x10];
+    uint8_t _0x10[0xF0];
+    fs_layout_t layout;
+    duplex_header_t duplex_header;
+    ivfc_save_hdr_t data_ivfc_header;
+    uint32_t _0x404;
+    journal_header_t journal_header;
+    journal_map_header_t map_header;
+    uint8_t _0x438[0x1D0];
+    save_fs_header_t save_header;
+    fat_header_t fat_header;
+    remap_header_t main_remap_header, meta_remap_header;
+    uint64_t _0x6D0;
+    extra_data_t extra_data;
+    uint8_t _0x748[0x390];
+    ivfc_save_hdr_t fat_ivfc_header;
+    uint8_t _0xB98[0x3468];
+} save_header_t;
+#pragma pack(pop)
+
+typedef struct {
+    duplex_storage_ctx_t layers[2];
+    duplex_storage_ctx_t data_layer;
+    uint64_t _length;
+} hierarchical_duplex_storage_ctx_t;
+
+typedef struct {
+    uint8_t *data_a;
+    uint8_t *data_b;
+    duplex_info_t info;
+} duplex_fs_layer_info_t;
+
+typedef struct {
+    uint8_t *map_storage;
+    uint8_t *physical_block_bitmap;
+    uint8_t *virtual_block_bitmap;
+    uint8_t *free_block_bitmap;
+} journal_map_params_t;
+
+typedef struct {
+    uint32_t physical_index;
+    uint32_t virtual_index;
+} journal_map_entry_t;
+
+
+typedef struct {
+    journal_map_header_t *header;
+    journal_map_entry_t *entries;
+    uint8_t *map_storage;
+} journal_map_ctx_t;
+
+typedef struct {
+    journal_map_ctx_t map;
+    journal_header_t *header;
+    uint32_t block_size;
+    uint64_t journal_data_offset;
+    uint64_t _length;
+    FIL *file;
+} journal_storage_ctx_t;
+
+typedef struct {
+    uint64_t data_offset;
+    uint64_t data_size;
+    uint64_t hash_offset;
+    uint32_t hash_block_size;
+    validity_t hash_validity;
+    enum base_storage_type type;
+    save_ctx_t *save_ctx;
+} ivfc_level_save_ctx_t;
+
+typedef struct {
+    ivfc_level_save_ctx_t *data;
+    uint32_t block_size;
+    uint8_t salt[0x20];
+} integrity_verification_info_ctx_t;
+
+
+typedef struct integrity_verification_storage_ctx_t integrity_verification_storage_ctx_t;
+
+struct integrity_verification_storage_ctx_t {
+    ivfc_level_save_ctx_t *hash_storage;
+    ivfc_level_save_ctx_t *base_storage;
+    validity_t *block_validities;
+    uint8_t salt[0x20];
+    uint32_t sector_size;
+    uint32_t sector_count;
+    uint64_t _length;
+    integrity_verification_storage_ctx_t *next_level;
+};
+
+typedef struct {
+    ivfc_level_save_ctx_t levels[5];
+    ivfc_level_save_ctx_t *data_level;
+    validity_t **level_validities;
+    uint64_t _length;
+    integrity_verification_storage_ctx_t integrity_storages[4];
+} hierarchical_integrity_verification_storage_ctx_t;
+
+typedef struct {
+    uint32_t prev;
+    uint32_t next;
+} allocation_table_entry_t;
+
+typedef struct {
+    uint32_t free_list_entry_index;
+    void *base_storage;
+    fat_header_t *header;
+} allocation_table_ctx_t;
+
+typedef struct {
+    hierarchical_integrity_verification_storage_ctx_t *base_storage;
+    uint32_t block_size;
+    uint32_t initial_block;
+    allocation_table_ctx_t *fat;
+    uint64_t _length;
+} allocation_table_storage_ctx_t;
+
+typedef struct {
+    allocation_table_ctx_t *fat;
+    uint32_t virtual_block;
+    uint32_t physical_block;
+    uint32_t current_segment_size;
+    uint32_t next_block;
+    uint32_t prev_block;
+} allocation_table_iterator_ctx_t;
+
+typedef struct {
+    char name[SAVE_FS_LIST_MAX_NAME_LENGTH];
+    uint32_t parent;
+} save_entry_key_t;
+
+#pragma pack(push, 1)
+typedef struct {
+    uint32_t start_block;
+    uint64_t length;
+    uint32_t _0xC[2];
+} save_file_info_t;
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+typedef struct {
+    uint32_t next_directory;
+    uint32_t next_file;
+    uint32_t _0x8[3];
+} save_find_position_t;
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+typedef struct {
+    uint32_t next_sibling;
+    union { /* Save table entry type. Size = 0x14. */
+        save_file_info_t save_file_info;
+        save_find_position_t save_find_position;
+    };
+} save_table_entry_t;
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+typedef struct {
+    uint32_t parent;
+    char name[SAVE_FS_LIST_MAX_NAME_LENGTH];
+    save_table_entry_t value;
+    uint32_t next;
+} save_fs_list_entry_t;
+#pragma pack(pop)
+
+typedef struct {
+    uint32_t free_list_head_index;
+    uint32_t used_list_head_index;
+    allocation_table_storage_ctx_t storage;
+    uint32_t capacity;
+} save_filesystem_list_ctx_t;
+
+typedef struct {
+    save_filesystem_list_ctx_t file_table;
+    save_filesystem_list_ctx_t directory_table;
+} hierarchical_save_file_table_ctx_t;
+
+typedef struct {
+    hierarchical_integrity_verification_storage_ctx_t *base_storage;
+    allocation_table_ctx_t allocation_table;
+    save_fs_header_t *header;
+    hierarchical_save_file_table_ctx_t file_table;
+} save_filesystem_ctx_t;
+
+#define ACTION_VERIFY (1<<2)
+
+struct save_ctx_t {
+    save_header_t header;
+    FIL *file;
+    struct {
+        FIL *file;
+        uint32_t action;
+    } tool_ctx;
+    validity_t header_cmac_validity;
+    validity_t header_hash_validity;
+    uint8_t *data_ivfc_master;
+    uint8_t *fat_ivfc_master;
+    remap_storage_ctx_t data_remap_storage;
+    remap_storage_ctx_t meta_remap_storage;
+    duplex_fs_layer_info_t duplex_layers[3];
+    hierarchical_duplex_storage_ctx_t duplex_storage;
+    journal_storage_ctx_t journal_storage;
+    journal_map_params_t journal_map_info;
+    hierarchical_integrity_verification_storage_ctx_t core_data_ivfc_storage;
+    hierarchical_integrity_verification_storage_ctx_t fat_ivfc_storage;
+    uint8_t *fat_storage;
+    save_filesystem_ctx_t save_filesystem_core;
+    uint8_t save_mac_key[0x10];
+};
+
+static inline uint32_t allocation_table_entry_index_to_block(uint32_t entry_index) {
+    return entry_index - 1;
+}
+
+static inline uint32_t allocation_table_block_to_entry_index(uint32_t block_index) {
+    return block_index + 1;
+}
+
+static inline int allocation_table_is_list_end(allocation_table_entry_t *entry) {
+    return (entry->next & 0x7FFFFFFF) == 0;
+}
+
+static inline int allocation_table_is_list_start(allocation_table_entry_t *entry) {
+    return entry->prev == 0x80000000;
+}
+
+
+static inline int allocation_table_get_next(allocation_table_entry_t *entry) {
+    return entry->next & 0x7FFFFFFF;
+}
+
+static inline int allocation_table_get_prev(allocation_table_entry_t *entry) {
+    return entry->prev & 0x7FFFFFFF;
+}
+
+static inline allocation_table_entry_t *save_allocation_table_read_entry(allocation_table_ctx_t *ctx, uint32_t entry_index) {
+    return (allocation_table_entry_t *)((uint8_t *)ctx->base_storage + entry_index * SAVE_FAT_ENTRY_SIZE);
+}
+
+static inline uint32_t save_allocation_table_get_free_list_entry_index(allocation_table_ctx_t *ctx) {
+    return allocation_table_get_next(save_allocation_table_read_entry(ctx, ctx->free_list_entry_index));
+}
+
+static inline uint32_t save_allocation_table_get_free_list_block_index(allocation_table_ctx_t *ctx) {
+    return allocation_table_entry_index_to_block(save_allocation_table_get_free_list_entry_index(ctx));
+}
+
+bool save_process(save_ctx_t *ctx);
+bool save_process_header(save_ctx_t *ctx);
+
+void save_free_contexts(save_ctx_t *ctx);
+
+void save_open_fat_storage(save_filesystem_ctx_t *ctx, allocation_table_storage_ctx_t *storage_ctx, uint32_t block_index);
+uint32_t save_allocation_table_storage_read(allocation_table_storage_ctx_t *ctx, void *buffer, uint64_t offset, size_t count);
+int save_fs_list_get_value(save_filesystem_list_ctx_t *ctx, uint32_t index, save_fs_list_entry_t *value);
+int save_hierarchical_file_table_get_file_entry_by_path(hierarchical_save_file_table_ctx_t *ctx, const char *path, save_fs_list_entry_t *entry);
+
+#endif

source/libs/compr/blz.c

@@ -15,6 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <stdlib.h>
 #include <string.h>
 
 #include "blz.h"
@@ -32,24 +33,24 @@ const blz_footer *blz_get_footer(const unsigned char *compData, unsigned int com
 }
 
 // From https://github.com/SciresM/hactool/blob/master/kip.c which is exactly how kernel does it, thanks SciresM!
-int blz_uncompress_inplace(unsigned char *dataBuf, unsigned int compSize, const blz_footer *footer) 
+int blz_uncompress_inplace(unsigned char *dataBuf, unsigned int compSize, const blz_footer *footer)
 {
 	u32 addl_size = footer->addl_size;
 	u32 header_size = footer->header_size;
 	u32 cmp_and_hdr_size = footer->cmp_and_hdr_size;
-	
+
 	unsigned char* cmp_start = &dataBuf[compSize] - cmp_and_hdr_size;
 	u32 cmp_ofs = cmp_and_hdr_size - header_size;
 	u32 out_ofs = cmp_and_hdr_size + addl_size;
-	
-	while (out_ofs) 
+
+	while (out_ofs)
 	{
 		unsigned char control = cmp_start[--cmp_ofs];
-		for (unsigned int i=0; i<8; i++) 
+		for (unsigned int i=0; i<8; i++)
 		{
-			if (control & 0x80) 
+			if (control & 0x80)
 			{
-				if (cmp_ofs < 2) 
+				if (cmp_ofs < 2)
 					return 0; // Out of bounds.
 
 				cmp_ofs -= 2;
@@ -64,17 +65,17 @@ int blz_uncompress_inplace(unsigned char *dataBuf, unsigned int compSize, const
 				for (unsigned int j = 0; j < seg_size; j++)
 					cmp_start[out_ofs + j] = cmp_start[out_ofs + j + seg_ofs];
 			}
-			else 
+			else
 			{
 				// Copy directly.
-				if (cmp_ofs < 1) 
+				if (cmp_ofs < 1)
 					return 0; //out of bounds
 
 				cmp_start[--out_ofs] = cmp_start[--cmp_ofs];
 			}
 			control <<= 1;
 			if (out_ofs == 0) // Blz works backwards, so if it reaches byte 0, it's done.
-				return 1; 
+				return 1;
 			}
 		}
 

source/libs/elfload/elf.h

@@ -0,0 +1,589 @@
+/*    $OpenBSD: exec_elf.h,v 1.53 2014/01/03 03:00:39 guenther Exp $    */
+/*
+ * Copyright (c) 1995, 1996 Erik Theisen.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* imported sys/exec_elf.h from OpenBSD */
+
+#ifndef ELF_H
+#define ELF_H
+#include <stdint.h>
+
+typedef uint8_t Elf_Byte;
+
+typedef uint32_t Elf32_Addr; /* Unsigned program address */
+typedef uint32_t Elf32_Off;  /* Unsigned file offset */
+typedef int32_t Elf32_Sword; /* Signed large integer */
+typedef uint32_t Elf32_Word; /* Unsigned large integer */
+typedef uint16_t Elf32_Half; /* Unsigned medium integer */
+
+typedef uint64_t Elf64_Addr;
+typedef uint64_t Elf64_Off;
+typedef int32_t Elf64_Shalf;
+
+#ifdef __alpha__
+typedef int64_t Elf64_Sword;
+typedef uint64_t Elf64_Word;
+#else
+typedef int32_t Elf64_Sword;
+typedef uint32_t Elf64_Word;
+#endif
+
+typedef int64_t Elf64_Sxword;
+typedef uint64_t Elf64_Xword;
+
+typedef uint32_t Elf64_Half;
+typedef uint16_t Elf64_Quarter;
+
+/*
+ * e_ident[] identification indexes
+ * See http://www.sco.com/developers/gabi/latest/ch4.eheader.html
+ */
+#define EI_MAG0 0       /* file ID */
+#define EI_MAG1 1       /* file ID */
+#define EI_MAG2 2       /* file ID */
+#define EI_MAG3 3       /* file ID */
+#define EI_CLASS 4      /* file class */
+#define EI_DATA 5       /* data encoding */
+#define EI_VERSION 6    /* ELF header version */
+#define EI_OSABI 7      /* OS/ABI ID */
+#define EI_ABIVERSION 8 /* ABI version */
+#define EI_PAD 9        /* start of pad bytes */
+#define EI_NIDENT 16    /* Size of e_ident[] */
+
+/* e_ident[] magic number */
+#define ELFMAG0 0x7f     /* e_ident[EI_MAG0] */
+#define ELFMAG1 'E'      /* e_ident[EI_MAG1] */
+#define ELFMAG2 'L'      /* e_ident[EI_MAG2] */
+#define ELFMAG3 'F'      /* e_ident[EI_MAG3] */
+#define ELFMAG "\177ELF" /* magic */
+#define SELFMAG 4        /* size of magic */
+
+/* e_ident[] file class */
+#define ELFCLASSNONE 0 /* invalid */
+#define ELFCLASS32 1   /* 32-bit objs */
+#define ELFCLASS64 2   /* 64-bit objs */
+#define ELFCLASSNUM 3  /* number of classes */
+
+/* e_ident[] data encoding */
+#define ELFDATANONE 0 /* invalid */
+#define ELFDATA2LSB 1 /* Little-Endian */
+#define ELFDATA2MSB 2 /* Big-Endian */
+#define ELFDATANUM 3  /* number of data encode defines */
+
+/* e_ident[] Operating System/ABI */
+#define ELFOSABI_SYSV 0         /* UNIX System V ABI */
+#define ELFOSABI_HPUX 1         /* HP-UX operating system */
+#define ELFOSABI_NETBSD 2       /* NetBSD */
+#define ELFOSABI_LINUX 3        /* GNU/Linux */
+#define ELFOSABI_HURD 4         /* GNU/Hurd */
+#define ELFOSABI_86OPEN 5       /* 86Open common IA32 ABI */
+#define ELFOSABI_SOLARIS 6      /* Solaris */
+#define ELFOSABI_MONTEREY 7     /* Monterey */
+#define ELFOSABI_IRIX 8         /* IRIX */
+#define ELFOSABI_FREEBSD 9      /* FreeBSD */
+#define ELFOSABI_TRU64 10       /* TRU64 UNIX */
+#define ELFOSABI_MODESTO 11     /* Novell Modesto */
+#define ELFOSABI_OPENBSD 12     /* OpenBSD */
+#define ELFOSABI_ARM 97         /* ARM */
+#define ELFOSABI_STANDALONE 255 /* Standalone (embedded) application */
+
+/* e_ident */
+#define IS_ELF(ehdr) ((ehdr).e_ident[EI_MAG0] == ELFMAG0 && \
+                      (ehdr).e_ident[EI_MAG1] == ELFMAG1 && \
+                      (ehdr).e_ident[EI_MAG2] == ELFMAG2 && \
+                      (ehdr).e_ident[EI_MAG3] == ELFMAG3)
+
+/* ELF Header */
+typedef struct
+{
+    unsigned char e_ident[EI_NIDENT]; /* ELF Identification */
+    Elf32_Half e_type;                /* object file type */
+    Elf32_Half e_machine;             /* machine */
+    Elf32_Word e_version;             /* object file version */
+    Elf32_Addr e_entry;               /* virtual entry point */
+    Elf32_Off e_phoff;                /* program header table offset */
+    Elf32_Off e_shoff;                /* section header table offset */
+    Elf32_Word e_flags;               /* processor-specific flags */
+    Elf32_Half e_ehsize;              /* ELF header size */
+    Elf32_Half e_phentsize;           /* program header entry size */
+    Elf32_Half e_phnum;               /* number of program header entries */
+    Elf32_Half e_shentsize;           /* section header entry size */
+    Elf32_Half e_shnum;               /* number of section header entries */
+    Elf32_Half e_shstrndx;            /* section header table's "section
+                                                header string table" entry offset */
+} Elf32_Ehdr;
+
+typedef struct
+{
+    unsigned char e_ident[EI_NIDENT]; /* Id bytes */
+    Elf64_Quarter e_type;             /* file type */
+    Elf64_Quarter e_machine;          /* machine type */
+    Elf64_Half e_version;             /* version number */
+    Elf64_Addr e_entry;               /* entry point */
+    Elf64_Off e_phoff;                /* Program hdr offset */
+    Elf64_Off e_shoff;                /* Section hdr offset */
+    Elf64_Half e_flags;               /* Processor flags */
+    Elf64_Quarter e_ehsize;           /* sizeof ehdr */
+    Elf64_Quarter e_phentsize;        /* Program header entry size */
+    Elf64_Quarter e_phnum;            /* Number of program headers */
+    Elf64_Quarter e_shentsize;        /* Section header entry size */
+    Elf64_Quarter e_shnum;            /* Number of section headers */
+    Elf64_Quarter e_shstrndx;         /* String table index */
+} Elf64_Ehdr;
+
+/* e_type */
+#define ET_NONE 0        /* No file type */
+#define ET_REL 1         /* relocatable file */
+#define ET_EXEC 2        /* executable file */
+#define ET_DYN 3         /* shared object file */
+#define ET_CORE 4        /* core file */
+#define ET_NUM 5         /* number of types */
+#define ET_LOPROC 0xff00 /* reserved range for processor */
+#define ET_HIPROC 0xffff /*  specific e_type */
+
+/* e_machine */
+#define EM_NONE 0  /* No Machine */
+#define EM_M32 1   /* AT&T WE 32100 */
+#define EM_SPARC 2 /* SPARC */
+#define EM_386 3   /* Intel 80386 */
+#define EM_68K 4   /* Motorola 68000 */
+#define EM_88K 5   /* Motorola 88000 */
+#define EM_486 6   /* Intel 80486 - unused? */
+#define EM_860 7   /* Intel 80860 */
+#define EM_MIPS 8  /* MIPS R3000 Big-Endian only */
+/*
+ * Don't know if EM_MIPS_RS4_BE,
+ * EM_SPARC64, EM_PARISC,
+ * or EM_PPC are ABI compliant
+ */
+#define EM_MIPS_RS4_BE 10 /* MIPS R4000 Big-Endian */
+#define EM_SPARC64 11     /* SPARC v9 64-bit unofficial */
+#define EM_PARISC 15      /* HPPA */
+#define EM_SPARC32PLUS 18 /* Enhanced instruction set SPARC */
+#define EM_PPC 20         /* PowerPC */
+#define EM_ARM 40         /* ARM AArch32 */
+#define EM_ALPHA 41       /* DEC ALPHA */
+#define EM_SH 42          /* Hitachi/Renesas Super-H */
+#define EM_SPARCV9 43     /* SPARC version 9 */
+#define EM_IA_64 50       /* Intel IA-64 Processor */
+#define EM_AMD64 62       /* AMD64 architecture */
+#define EM_VAX 75         /* DEC VAX */
+#define EM_AARCH64 183    /* ARM AArch64 */
+
+/* Non-standard */
+#define EM_ALPHA_EXP 0x9026 /* DEC ALPHA */
+
+/* Version */
+#define EV_NONE 0    /* Invalid */
+#define EV_CURRENT 1 /* Current */
+#define EV_NUM 2     /* number of versions */
+
+/* Section Header */
+typedef struct
+{
+    Elf32_Word sh_name;      /* name - index into section header
+                                 * string table section */
+    Elf32_Word sh_type;      /* type */
+    Elf32_Word sh_flags;     /* flags */
+    Elf32_Addr sh_addr;      /* address */
+    Elf32_Off sh_offset;     /* file offset */
+    Elf32_Word sh_size;      /* section size */
+    Elf32_Word sh_link;      /* section header table index link */
+    Elf32_Word sh_info;      /* extra information */
+    Elf32_Word sh_addralign; /* address alignment */
+    Elf32_Word sh_entsize;   /* section entry size */
+} Elf32_Shdr;
+
+typedef struct
+{
+    Elf64_Half sh_name;       /* section name */
+    Elf64_Half sh_type;       /* section type */
+    Elf64_Xword sh_flags;     /* section flags */
+    Elf64_Addr sh_addr;       /* virtual address */
+    Elf64_Off sh_offset;      /* file offset */
+    Elf64_Xword sh_size;      /* section size */
+    Elf64_Half sh_link;       /* link to another */
+    Elf64_Half sh_info;       /* misc info */
+    Elf64_Xword sh_addralign; /* memory alignment */
+    Elf64_Xword sh_entsize;   /* table entry size */
+} Elf64_Shdr;
+
+/* Special Section Indexes */
+#define SHN_UNDEF 0          /* undefined */
+#define SHN_LORESERVE 0xff00 /* lower bounds of reserved indexes */
+#define SHN_LOPROC 0xff00    /* reserved range for processor */
+#define SHN_HIPROC 0xff1f    /*   specific section indexes */
+#define SHN_ABS 0xfff1       /* absolute value */
+#define SHN_COMMON 0xfff2    /* common symbol */
+#define SHN_HIRESERVE 0xffff /* upper bounds of reserved indexes */
+
+/* sh_type */
+#define SHT_NULL 0            /* inactive */
+#define SHT_PROGBITS 1        /* program defined information */
+#define SHT_SYMTAB 2          /* symbol table section */
+#define SHT_STRTAB 3          /* string table section */
+#define SHT_RELA 4            /* relocation section with addends*/
+#define SHT_HASH 5            /* symbol hash table section */
+#define SHT_DYNAMIC 6         /* dynamic section */
+#define SHT_NOTE 7            /* note section */
+#define SHT_NOBITS 8          /* no space section */
+#define SHT_REL 9             /* relation section without addends */
+#define SHT_SHLIB 10          /* reserved - purpose unknown */
+#define SHT_DYNSYM 11         /* dynamic symbol table section */
+#define SHT_NUM 12            /* number of section types */
+#define SHT_LOPROC 0x70000000 /* reserved range for processor */
+#define SHT_HIPROC 0x7fffffff /*  specific section header types */
+#define SHT_LOUSER 0x80000000 /* reserved range for application */
+#define SHT_HIUSER 0xffffffff /*  specific indexes */
+
+/* Section names */
+#define ELF_BSS ".bss"               /* uninitialized data */
+#define ELF_DATA ".data"             /* initialized data */
+#define ELF_DEBUG ".debug"           /* debug */
+#define ELF_DYNAMIC ".dynamic"       /* dynamic linking information */
+#define ELF_DYNSTR ".dynstr"         /* dynamic string table */
+#define ELF_DYNSYM ".dynsym"         /* dynamic symbol table */
+#define ELF_FINI ".fini"             /* termination code */
+#define ELF_GOT ".got"               /* global offset table */
+#define ELF_HASH ".hash"             /* symbol hash table */
+#define ELF_INIT ".init"             /* initialization code */
+#define ELF_REL_DATA ".rel.data"     /* relocation data */
+#define ELF_REL_FINI ".rel.fini"     /* relocation termination code */
+#define ELF_REL_INIT ".rel.init"     /* relocation initialization code */
+#define ELF_REL_DYN ".rel.dyn"       /* relocation dynamic link info */
+#define ELF_REL_RODATA ".rel.rodata" /* relocation read-only data */
+#define ELF_REL_TEXT ".rel.text"     /* relocation code */
+#define ELF_RODATA ".rodata"         /* read-only data */
+#define ELF_SHSTRTAB ".shstrtab"     /* section header string table */
+#define ELF_STRTAB ".strtab"         /* string table */
+#define ELF_SYMTAB ".symtab"         /* symbol table */
+#define ELF_TEXT ".text"             /* code */
+
+/* Section Attribute Flags - sh_flags */
+#define SHF_WRITE 0x1           /* Writable */
+#define SHF_ALLOC 0x2           /* occupies memory */
+#define SHF_EXECINSTR 0x4       /* executable */
+#define SHF_TLS 0x400           /* thread local storage */
+#define SHF_MASKPROC 0xf0000000 /* reserved bits for processor \
+                                 *  specific section attributes */
+
+/* Symbol Table Entry */
+typedef struct elf32_sym
+{
+    Elf32_Word st_name;     /* name - index into string table */
+    Elf32_Addr st_value;    /* symbol value */
+    Elf32_Word st_size;     /* symbol size */
+    unsigned char st_info;  /* type and binding */
+    unsigned char st_other; /* 0 - no defined meaning */
+    Elf32_Half st_shndx;    /* section header index */
+} Elf32_Sym;
+
+typedef struct
+{
+    Elf64_Half st_name;     /* Symbol name index in str table */
+    Elf_Byte st_info;       /* type / binding attrs */
+    Elf_Byte st_other;      /* unused */
+    Elf64_Quarter st_shndx; /* section index of symbol */
+    Elf64_Xword st_value;   /* value of symbol */
+    Elf64_Xword st_size;    /* size of symbol */
+} Elf64_Sym;
+
+/* Symbol table index */
+#define STN_UNDEF 0 /* undefined */
+
+/* Extract symbol info - st_info */
+#define ELF32_ST_BIND(x) ((x) >> 4)
+#define ELF32_ST_TYPE(x) (((unsigned int)x) & 0xf)
+#define ELF32_ST_INFO(b, t) (((b) << 4) + ((t)&0xf))
+
+#define ELF64_ST_BIND(x) ((x) >> 4)
+#define ELF64_ST_TYPE(x) (((unsigned int)x) & 0xf)
+#define ELF64_ST_INFO(b, t) (((b) << 4) + ((t)&0xf))
+
+/* Symbol Binding - ELF32_ST_BIND - st_info */
+#define STB_LOCAL 0   /* Local symbol */
+#define STB_GLOBAL 1  /* Global symbol */
+#define STB_WEAK 2    /* like global - lower precedence */
+#define STB_NUM 3     /* number of symbol bindings */
+#define STB_LOPROC 13 /* reserved range for processor */
+#define STB_HIPROC 15 /*  specific symbol bindings */
+
+/* Symbol type - ELF32_ST_TYPE - st_info */
+#define STT_NOTYPE 0  /* not specified */
+#define STT_OBJECT 1  /* data object */
+#define STT_FUNC 2    /* function */
+#define STT_SECTION 3 /* section */
+#define STT_FILE 4    /* file */
+#define STT_TLS 6     /* thread local storage */
+#define STT_LOPROC 13 /* reserved range for processor */
+#define STT_HIPROC 15 /*  specific symbol types */
+
+/* Relocation entry with implicit addend */
+typedef struct
+{
+    Elf32_Addr r_offset; /* offset of relocation */
+    Elf32_Word r_info;   /* symbol table index and type */
+} Elf32_Rel;
+
+/* Relocation entry with explicit addend */
+typedef struct
+{
+    Elf32_Addr r_offset; /* offset of relocation */
+    Elf32_Word r_info;   /* symbol table index and type */
+    Elf32_Sword r_addend;
+} Elf32_Rela;
+
+/* Extract relocation info - r_info */
+#define ELF32_R_SYM(i) ((i) >> 8)
+#define ELF32_R_TYPE(i) ((unsigned char)(i))
+#define ELF32_R_INFO(s, t) (((s) << 8) + (unsigned char)(t))
+
+typedef struct
+{
+    Elf64_Xword r_offset; /* where to do it */
+    Elf64_Xword r_info;   /* index & type of relocation */
+} Elf64_Rel;
+
+typedef struct
+{
+    Elf64_Xword r_offset;  /* where to do it */
+    Elf64_Xword r_info;    /* index & type of relocation */
+    Elf64_Sxword r_addend; /* adjustment value */
+} Elf64_Rela;
+
+#define ELF64_R_SYM(info) ((info) >> 32)
+#define ELF64_R_TYPE(info) ((info)&0xFFFFFFFF)
+#define ELF64_R_INFO(s, t) (((s) << 32) + (__uint32_t)(t))
+
+#if defined(__mips64__) && defined(__MIPSEL__)
+/*
+ * The 64-bit MIPS ELF ABI uses a slightly different relocation format
+ * than the regular ELF ABI: the r_info field is split into several
+ * pieces (see gnu/usr.bin/binutils/include/elf/mips.h for details).
+ */
+#undef ELF64_R_SYM
+#undef ELF64_R_TYPE
+#undef ELF64_R_INFO
+#define ELF64_R_TYPE(info) (swap32((info) >> 32))
+#define ELF64_R_SYM(info) ((info)&0xFFFFFFFF)
+#define ELF64_R_INFO(s, t) (((__uint64_t)swap32(t) << 32) + (__uint32_t)(s))
+#endif /* __mips64__ && __MIPSEL__ */
+
+/* Program Header */
+typedef struct
+{
+    Elf32_Word p_type;   /* segment type */
+    Elf32_Off p_offset;  /* segment offset */
+    Elf32_Addr p_vaddr;  /* virtual address of segment */
+    Elf32_Addr p_paddr;  /* physical address - ignored? */
+    Elf32_Word p_filesz; /* number of bytes in file for seg. */
+    Elf32_Word p_memsz;  /* number of bytes in mem. for seg. */
+    Elf32_Word p_flags;  /* flags */
+    Elf32_Word p_align;  /* memory alignment */
+} Elf32_Phdr;
+
+typedef struct
+{
+    Elf64_Half p_type;    /* entry type */
+    Elf64_Half p_flags;   /* flags */
+    Elf64_Off p_offset;   /* offset */
+    Elf64_Addr p_vaddr;   /* virtual address */
+    Elf64_Addr p_paddr;   /* physical address */
+    Elf64_Xword p_filesz; /* file size */
+    Elf64_Xword p_memsz;  /* memory size */
+    Elf64_Xword p_align;  /* memory & file alignment */
+} Elf64_Phdr;
+
+/* Segment types - p_type */
+#define PT_NULL 0            /* unused */
+#define PT_LOAD 1            /* loadable segment */
+#define PT_DYNAMIC 2         /* dynamic linking section */
+#define PT_INTERP 3          /* the RTLD */
+#define PT_NOTE 4            /* auxiliary information */
+#define PT_SHLIB 5           /* reserved - purpose undefined */
+#define PT_PHDR 6            /* program header */
+#define PT_TLS 7             /* thread local storage */
+#define PT_LOOS 0x60000000   /* reserved range for OS */
+#define PT_HIOS 0x6fffffff   /*  specific segment types */
+#define PT_LOPROC 0x70000000 /* reserved range for processor */
+#define PT_HIPROC 0x7fffffff /*  specific segment types */
+
+#define PT_OPENBSD_RANDOMIZE 0x65a3dbe6 /* fill with random data */
+#define PT_GANDR_KERNEL 0x67646b6c      /* gdkl */
+
+/* Segment flags - p_flags */
+#define PF_X 0x1               /* Executable */
+#define PF_W 0x2               /* Writable */
+#define PF_R 0x4               /* Readable */
+#define PF_MASKPROC 0xf0000000 /* reserved bits for processor */
+                               /*  specific segment flags */
+
+/* Dynamic structure */
+typedef struct
+{
+    Elf32_Sword d_tag; /* controls meaning of d_val */
+    union {
+        Elf32_Word d_val; /* Multiple meanings - see d_tag */
+        Elf32_Addr d_ptr; /* program virtual address */
+    } d_un;
+} Elf32_Dyn;
+
+typedef struct
+{
+    Elf64_Xword d_tag; /* controls meaning of d_val */
+    union {
+        Elf64_Addr d_ptr;
+        Elf64_Xword d_val;
+    } d_un;
+} Elf64_Dyn;
+
+/* Dynamic Array Tags - d_tag */
+#define DT_NULL 0            /* marks end of _DYNAMIC array */
+#define DT_NEEDED 1          /* string table offset of needed lib */
+#define DT_PLTRELSZ 2        /* size of relocation entries in PLT */
+#define DT_PLTGOT 3          /* address PLT/GOT */
+#define DT_HASH 4            /* address of symbol hash table */
+#define DT_STRTAB 5          /* address of string table */
+#define DT_SYMTAB 6          /* address of symbol table */
+#define DT_RELA 7            /* address of relocation table */
+#define DT_RELASZ 8          /* size of relocation table */
+#define DT_RELAENT 9         /* size of relocation entry */
+#define DT_STRSZ 10          /* size of string table */
+#define DT_SYMENT 11         /* size of symbol table entry */
+#define DT_INIT 12           /* address of initialization func. */
+#define DT_FINI 13           /* address of termination function */
+#define DT_SONAME 14         /* string table offset of shared obj */
+#define DT_RPATH 15          /* string table offset of library \
+                              * search path */
+#define DT_SYMBOLIC 16       /* start sym search in shared obj. */
+#define DT_REL 17            /* address of rel. tbl. w addends */
+#define DT_RELSZ 18          /* size of DT_REL relocation table */
+#define DT_RELENT 19         /* size of DT_REL relocation entry */
+#define DT_PLTREL 20         /* PLT referenced relocation entry */
+#define DT_DEBUG 21          /* bugger */
+#define DT_TEXTREL 22        /* Allow rel. mod. to unwritable seg */
+#define DT_JMPREL 23         /* add. of PLT's relocation entries */
+#define DT_BIND_NOW 24       /* Bind now regardless of env setting */
+#define DT_LOOS 0x6000000d   /* reserved range for OS */
+#define DT_HIOS 0x6ffff000   /*  specific dynamic array tags */
+#define DT_LOPROC 0x70000000 /* reserved range for processor */
+#define DT_HIPROC 0x7fffffff /*  specific dynamic array tags */
+
+/* some other useful tags */
+#define DT_RELACOUNT 0x6ffffff9 /* if present, number of RELATIVE */
+#define DT_RELCOUNT 0x6ffffffa  /* relocs, which must come first */
+#define DT_FLAGS_1 0x6ffffffb
+
+/* Dynamic Flags - DT_FLAGS_1 .dynamic entry */
+#define DF_1_NOW 0x00000001
+#define DF_1_GLOBAL 0x00000002
+#define DF_1_GROUP 0x00000004
+#define DF_1_NODELETE 0x00000008
+#define DF_1_LOADFLTR 0x00000010
+#define DF_1_INITFIRST 0x00000020
+#define DF_1_NOOPEN 0x00000040
+#define DF_1_ORIGIN 0x00000080
+#define DF_1_DIRECT 0x00000100
+#define DF_1_TRANS 0x00000200
+#define DF_1_INTERPOSE 0x00000400
+#define DF_1_NODEFLIB 0x00000800
+#define DF_1_NODUMP 0x00001000
+#define DF_1_CONLFAT 0x00002000
+
+/* ld.so: number of low tags that are used saved internally (0 .. DT_NUM-1) */
+#define DT_NUM (DT_JMPREL + 1)
+
+/*
+ * Note Definitions
+ */
+typedef struct
+{
+    Elf32_Word namesz;
+    Elf32_Word descsz;
+    Elf32_Word type;
+} Elf32_Note;
+
+typedef struct
+{
+    Elf64_Half namesz;
+    Elf64_Half descsz;
+    Elf64_Half type;
+} Elf64_Note;
+
+#if defined(ELFSIZE) && (ELFSIZE == 32)
+#define Elf_Ehdr Elf32_Ehdr
+#define Elf_Phdr Elf32_Phdr
+#define Elf_Shdr Elf32_Shdr
+#define Elf_Sym Elf32_Sym
+#define Elf_Rel Elf32_Rel
+#define Elf_RelA Elf32_Rela
+#define Elf_Dyn Elf32_Dyn
+#define Elf_Half Elf32_Half
+#define Elf_Word Elf32_Word
+#define Elf_Sword Elf32_Sword
+#define Elf_Addr Elf32_Addr
+#define Elf_Off Elf32_Off
+#define Elf_Nhdr Elf32_Nhdr
+#define Elf_Note Elf32_Note
+
+#define ELF_R_SYM ELF32_R_SYM
+#define ELF_R_TYPE ELF32_R_TYPE
+#define ELF_R_INFO ELF32_R_INFO
+#define ELFCLASS ELFCLASS32
+
+#define ELF_ST_BIND ELF32_ST_BIND
+#define ELF_ST_TYPE ELF32_ST_TYPE
+#define ELF_ST_INFO ELF32_ST_INFO
+
+#elif defined(ELFSIZE) && (ELFSIZE == 64)
+
+#define Elf_Ehdr Elf64_Ehdr
+#define Elf_Phdr Elf64_Phdr
+#define Elf_Shdr Elf64_Shdr
+#define Elf_Sym Elf64_Sym
+#define Elf_Rel Elf64_Rel
+#define Elf_RelA Elf64_Rela
+#define Elf_Dyn Elf64_Dyn
+#define Elf_Half Elf64_Half
+#define Elf_Word Elf64_Word
+#define Elf_Sword Elf64_Sword
+#define Elf_Addr Elf64_Addr
+#define Elf_Off Elf64_Off
+#define Elf_Nhdr Elf64_Nhdr
+#define Elf_Note Elf64_Note
+
+#define ELF_R_SYM ELF64_R_SYM
+#define ELF_R_TYPE ELF64_R_TYPE
+#define ELF_R_INFO ELF64_R_INFO
+#define ELFCLASS ELFCLASS64
+
+#define ELF_ST_BIND ELF64_ST_BIND
+#define ELF_ST_TYPE ELF64_ST_TYPE
+#define ELF_ST_INFO ELF64_ST_INFO
+
+#endif
+
+#endif

source/libs/elfload/elfarch.h

@@ -0,0 +1,49 @@
+/*
+ * Copyright © 2014, Owen Shepherd
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
+ * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
+ * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+ * OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+ * PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef ELFARCH_H
+#define ELFARCH_H
+
+#if defined(__i386__)
+#define EM_THIS EM_386
+#define EL_ARCH_USES_REL
+#elif defined(__amd64__)
+#define EM_THIS EM_AMD64
+#define EL_ARCH_USES_RELA
+#elif defined(__arm__)
+#define EM_THIS EM_ARM
+#define EL_ARCH_USES_REL
+#elif defined(__aarch64__)
+#define EM_THIS EM_AARCH64
+#define EL_ARCH_USES_RELA
+#define EL_ARCH_USES_REL
+#else
+#error specify your ELF architecture
+#endif
+
+#if defined(__LP64__) || defined(__LLP64__)
+#define ELFSIZE 64
+#else
+#define ELFSIZE 32
+#endif
+
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+#define ELFDATATHIS ELFDATA2LSB
+#else
+#define ELFDATATHIS ELFDATA2MSB
+#endif
+
+#endif

source/libs/elfload/elfload.c

@@ -0,0 +1,324 @@
+/*
+ * Copyright © 2018, M4xw
+ * Copyright © 2014, Owen Shepherd
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
+ * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
+ * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+ * OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+ * PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <string.h>
+
+#include "elfload.h"
+
+el_status el_pread(el_ctx *ctx, void *def, size_t nb, size_t offset)
+{
+	return ctx->pread(ctx, def, nb, offset) ? EL_OK : EL_EIO;
+}
+
+#define EL_PHOFF(ctx, num) (((ctx)->ehdr.e_phoff + (num) *(ctx)->ehdr.e_phentsize))
+el_status el_findphdr(el_ctx *ctx, Elf_Phdr *phdr, uint32_t type, unsigned *i)
+{
+	el_status rv = EL_OK;
+	for (; *i < ctx->ehdr.e_phnum; (*i)++)
+	{
+		if ((rv = el_pread(ctx, phdr, sizeof *phdr, EL_PHOFF(ctx, *i))))
+			return rv;
+
+		if (phdr->p_type == type)
+		{
+			return rv;
+		}
+	}
+
+	*i = -1;
+	return rv;
+}
+
+#define EL_SHOFF(ctx, num) (((ctx)->ehdr.e_shoff + (num) *(ctx)->ehdr.e_shentsize))
+el_status el_findshdr(el_ctx *ctx, Elf_Shdr *shdr, uint32_t type, unsigned *i)
+{
+	el_status rv = EL_OK;
+
+	for (; *i < ctx->ehdr.e_shnum; (*i)++)
+	{
+		if ((rv = el_pread(ctx, shdr, sizeof *shdr, EL_SHOFF(ctx, *i))))
+
+			return rv;
+
+		if (shdr->sh_type == type)
+		{
+			return rv;
+		}
+	}
+
+	*i = -1;
+
+	return rv;
+}
+
+el_status el_init(el_ctx *ctx)
+{
+	el_status rv = EL_OK;
+	if ((rv = el_pread(ctx, &ctx->ehdr, sizeof ctx->ehdr, 0)))
+		return rv;
+
+	/* validate header */
+
+	if (!IS_ELF(ctx->ehdr))
+		return EL_NOTELF;
+
+	if (ctx->ehdr.e_ident[EI_CLASS] != ELFCLASS)
+		return EL_WRONGBITS;
+
+	if (ctx->ehdr.e_ident[EI_DATA] != ELFDATATHIS)
+		return EL_WRONGENDIAN;
+
+	if (ctx->ehdr.e_ident[EI_VERSION] != EV_CURRENT)
+		return EL_NOTELF;
+
+	if (ctx->ehdr.e_type != ET_EXEC && ctx->ehdr.e_type != ET_DYN)
+		return EL_NOTEXEC;
+
+	if (ctx->ehdr.e_machine != EM_THIS)
+		return EL_WRONGARCH;
+
+	if (ctx->ehdr.e_version != EV_CURRENT)
+		return EL_NOTELF;
+
+	/* load phdrs */
+	Elf_Phdr ph;
+
+	/* iterate through, calculate extents */
+	ctx->base_load_paddr = ctx->base_load_vaddr = 0;
+	ctx->align = 1;
+	ctx->memsz = 0;
+
+	unsigned i = 0;
+	for (;;)
+	{
+		if ((rv = el_findphdr(ctx, &ph, PT_LOAD, &i)))
+			return rv;
+
+		if (i == (unsigned)-1)
+			break;
+
+		Elf_Addr phend = ph.p_vaddr + ph.p_memsz;
+		if (phend > ctx->memsz)
+			ctx->memsz = phend;
+
+		if (ph.p_align > ctx->align)
+			ctx->align = ph.p_align;
+
+		i++;
+	}
+
+	// Program Header
+	if (ctx->ehdr.e_type == ET_DYN)
+	{
+		i = 0;
+
+		if ((rv = el_findphdr(ctx, &ph, PT_DYNAMIC, &i)))
+			return rv;
+
+		if (i == (unsigned)-1)
+			return EL_NODYN;
+
+		ctx->dynoff = ph.p_offset;
+		ctx->dynsize = ph.p_filesz;
+	}
+	else
+	{
+		ctx->dynoff = 0;
+		ctx->dynsize = 0;
+	}
+
+	// Section String Table
+	if (ctx->ehdr.e_type == ET_DYN)
+	{
+		i = ctx->ehdr.e_shstrndx - 1;
+
+		if ((rv = el_findshdr(ctx, &ctx->shstr, SHT_STRTAB, &i)))
+			return rv;
+
+		// Reset
+		i = 0;
+
+		if ((rv = el_findshdr(ctx, &ctx->symtab, SHT_SYMTAB, &i)))
+			return rv;
+
+		if (i == (unsigned)-1)
+			return EL_NODYN;
+	}
+
+	return rv;
+}
+
+/*
+typedef void* (*el_alloc_cb)(
+	el_ctx *ctx,
+	Elf_Addr phys,
+	Elf_Addr virt,
+	Elf_Addr size);
+*/
+
+el_status el_load(el_ctx *ctx, el_alloc_cb alloc)
+{
+	el_status rv = EL_OK;
+
+	/* address deltas */
+	Elf_Addr pdelta = ctx->base_load_paddr;
+	Elf_Addr vdelta = ctx->base_load_vaddr;
+
+	/* iterate paddrs */
+	Elf_Phdr ph;
+	unsigned i = 0;
+	for (;;)
+	{
+		if ((rv = el_findphdr(ctx, &ph, PT_LOAD, &i)))
+			return rv;
+
+		if (i == (unsigned)-1)
+			break;
+
+		Elf_Addr pload = ph.p_paddr + pdelta;
+		Elf_Addr vload = ph.p_vaddr + vdelta;
+
+		/* allocate mem */
+		char *dest = alloc(ctx, pload, vload, ph.p_memsz);
+		if (!dest)
+			return EL_ENOMEM;
+
+		EL_DEBUG("Loading seg fileoff %x, vaddr %x to %p\n",
+				ph.p_offset, ph.p_vaddr, dest);
+
+		/* read loaded portion */
+		if ((rv = el_pread(ctx, dest, ph.p_filesz, ph.p_offset)))
+			return rv;
+
+		/* zero mem-only portion */
+		memset(dest + ph.p_filesz, 0, ph.p_memsz - ph.p_filesz);
+
+		i++;
+	}
+
+	return rv;
+}
+
+el_status el_finddyn(el_ctx *ctx, Elf_Dyn *dyn, uint32_t tag)
+{
+	el_status rv = EL_OK;
+	size_t ndyn = ctx->dynsize / sizeof(Elf_Dyn);
+
+	for (unsigned i = 0; i < ndyn; i++)
+	{
+		if ((rv = el_pread(ctx, dyn, sizeof *dyn, ctx->dynoff + i * sizeof *dyn)))
+			return rv;
+
+		if (dyn->d_tag == tag)
+			return EL_OK;
+	}
+
+	dyn->d_tag = DT_NULL;
+	return EL_OK;
+}
+
+el_status el_findrelocs(el_ctx *ctx, el_relocinfo *ri, uint32_t type)
+{
+	el_status rv = EL_OK;
+
+	Elf_Dyn rel, relsz, relent;
+
+	if ((rv = el_finddyn(ctx, &rel, type)))
+		return rv;
+
+	if ((rv = el_finddyn(ctx, &relsz, type + 1)))
+		return rv;
+
+	if ((rv = el_finddyn(ctx, &relent, type + 2)))
+		return rv;
+
+	if (rel.d_tag == DT_NULL || relsz.d_tag == DT_NULL || relent.d_tag == DT_NULL)
+	{
+		ri->entrysize = 0;
+		ri->tablesize = 0;
+		ri->tableoff = 0;
+	}
+	else
+	{
+		ri->tableoff = rel.d_un.d_ptr;
+		ri->tablesize = relsz.d_un.d_val;
+		ri->entrysize = relent.d_un.d_val;
+	}
+
+	return rv;
+}
+
+extern el_status el_applyrel(el_ctx *ctx, Elf_Rel *rel);
+extern el_status el_applyrela(el_ctx *ctx, Elf_RelA *rela);
+
+el_status el_relocate(el_ctx *ctx)
+{
+	el_status rv = EL_OK;
+
+	// not dynamic
+	if (ctx->ehdr.e_type != ET_DYN)
+		return EL_OK;
+
+	char *base = (char *)ctx->base_load_paddr;
+
+	el_relocinfo ri;
+#ifdef EL_ARCH_USES_REL
+	if ((rv = el_findrelocs(ctx, &ri, DT_REL)))
+		return rv;
+
+	if (ri.entrysize != sizeof(Elf_Rel) && ri.tablesize)
+	{
+		EL_DEBUG("Relocation size %u doesn't match expected %u\n",
+				ri.entrysize, sizeof(Elf_Rel));
+		return EL_BADREL;
+	}
+
+	size_t relcnt = ri.tablesize / sizeof(Elf_Rel);
+	Elf_Rel *reltab = (Elf_Rel *)(base + ri.tableoff);
+	for (size_t i = 0; i < relcnt; i++)
+	{
+		if ((rv = el_applyrel(ctx, &reltab[i])))
+			return rv;
+	}
+#endif
+
+#ifdef EL_ARCH_USES_RELA
+	if ((rv = el_findrelocs(ctx, &ri, DT_RELA)))
+		return rv;
+
+	if (ri.entrysize != sizeof(Elf_RelA) && ri.tablesize)
+	{
+		EL_DEBUG("Relocation size %u doesn't match expected %u\n",
+				ri.entrysize, sizeof(Elf_RelA));
+		return EL_BADREL;
+	}
+
+	size_t relacnt = ri.tablesize / sizeof(Elf_RelA);
+	Elf_RelA *relatab = (Elf_RelA *)(base + ri.tableoff);
+	for (size_t i = 0; i < relacnt; i++)
+	{
+		if ((rv = el_applyrela(ctx, &relatab[i])))
+			return rv;
+	}
+#endif
+
+#if !defined(EL_ARCH_USES_REL) && !defined(EL_ARCH_USES_RELA)
+#error No relocation type defined!
+#endif
+
+	return rv;
+}

source/libs/elfload/elfload.h

@@ -0,0 +1,127 @@
+/*
+ * Copyright © 2018, M4xw
+ * Copyright © 2014, Owen Shepherd
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
+ * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
+ * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+ * OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+ * PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef ELFLOAD_H
+#define ELFLOAD_H
+#include <stddef.h>
+
+#include "elfarch.h"
+#include "elf.h"
+
+#include "../../utils/types.h"
+
+#ifdef DEBUG
+#include "../../gfx/gfx.h"
+#define EL_DEBUG(format, ...) \
+	gfx_printf(format __VA_OPT__(, ) __VA_ARGS__)
+#else
+#define EL_DEBUG(...) \
+	do                \
+	{                 \
+	} while (0)
+#endif
+
+typedef enum
+{
+	EL_OK = 0,
+
+	EL_EIO,
+	EL_ENOMEM,
+
+	EL_NOTELF,
+	EL_WRONGBITS,
+	EL_WRONGENDIAN,
+	EL_WRONGARCH,
+	EL_WRONGOS,
+	EL_NOTEXEC,
+	EL_NODYN,
+	EL_BADREL,
+
+} el_status;
+
+typedef struct el_ctx
+{
+	bool (*pread)(struct el_ctx *ctx, void *dest, size_t nb, size_t offset);
+
+	/* base_load_* -> address we are actually going to load at
+	 */
+	Elf_Addr
+		base_load_paddr,
+		base_load_vaddr;
+
+	/* size in memory of binary */
+	Elf_Addr memsz;
+
+	/* required alignment */
+	Elf_Addr align;
+
+	/* ELF header */
+	Elf_Ehdr ehdr;
+
+	// Section Header Str Table
+	Elf_Shdr shstr;
+	Elf_Shdr symtab;
+
+	/* Offset of dynamic table (0 if not ET_DYN) */
+	Elf_Off dynoff;
+	/* Size of dynamic table (0 if not ET_DYN) */
+	Elf_Addr dynsize;
+} el_ctx;
+
+el_status el_pread(el_ctx *ctx, void *def, size_t nb, size_t offset);
+
+el_status el_init(el_ctx *ctx);
+typedef void *(*el_alloc_cb)(
+	el_ctx *ctx,
+	Elf_Addr phys,
+	Elf_Addr virt,
+	Elf_Addr size);
+
+el_status el_load(el_ctx *ctx, el_alloc_cb alloccb);
+
+/* find the next phdr of type \p type, starting at \p *i.
+ * On success, returns EL_OK with *i set to the phdr number, and the phdr loaded
+ * in *phdr.
+ *
+ * If the end of the phdrs table was reached, *i is set to -1 and the contents
+ * of *phdr are undefined
+ */
+el_status el_findphdr(el_ctx *ctx, Elf_Phdr *phdr, uint32_t type, unsigned *i);
+
+/* Relocate the loaded executable */
+el_status el_relocate(el_ctx *ctx);
+
+/* find a dynamic table entry
+ * returns the entry on success, dyn->d_tag = DT_NULL on failure
+ */
+el_status el_finddyn(el_ctx *ctx, Elf_Dyn *dyn, uint32_t type);
+
+typedef struct
+{
+	Elf_Off tableoff;
+	Elf_Addr tablesize;
+	Elf_Addr entrysize;
+} el_relocinfo;
+
+/* find all information regarding relocations of a specific type.
+ *
+ * pass DT_REL or DT_RELA for type
+ * sets ri->entrysize = 0 if not found
+ */
+el_status el_findrelocs(el_ctx *ctx, el_relocinfo *ri, uint32_t type);
+
+#endif

source/libs/elfload/elfreloc_aarch64.c

@@ -0,0 +1,84 @@
+/*
+ * Copyright © 2014, Owen Shepherd
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
+ * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
+ * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+ * OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+ * PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "elfload.h"
+
+#if defined(__aarch64__)
+
+#define R_AARCH64_NONE 0
+#define R_AARCH64_RELATIVE 1027
+
+el_status el_applyrela(el_ctx *ctx, Elf_RelA *rel)
+{
+	uintptr_t *p = (uintptr_t *)(rel->r_offset + ctx->base_load_paddr);
+	uint32_t type = ELF_R_TYPE(rel->r_info);
+	uint32_t sym = ELF_R_SYM(rel->r_info);
+
+	switch (type)
+	{
+	case R_AARCH64_NONE:
+		EL_DEBUG("R_AARCH64_NONE\n");
+		break;
+	case R_AARCH64_RELATIVE:
+		if (sym)
+		{
+			EL_DEBUG("R_AARCH64_RELATIVE with symbol ref!\n");
+			return EL_BADREL;
+		}
+
+		EL_DEBUG("Applying R_AARCH64_RELATIVE reloc @%p\n", p);
+		*p = rel->r_addend + ctx->base_load_vaddr;
+		break;
+
+	default:
+		EL_DEBUG("Bad relocation %u\n", type);
+		return EL_BADREL;
+	}
+
+	return EL_OK;
+}
+
+el_status el_applyrel(el_ctx *ctx, Elf_Rel *rel)
+{
+	uintptr_t *p = (uintptr_t *)(rel->r_offset + ctx->base_load_paddr);
+	uint32_t type = ELF_R_TYPE(rel->r_info);
+	uint32_t sym = ELF_R_SYM(rel->r_info);
+
+	switch (type)
+	{
+	case R_AARCH64_NONE:
+		EL_DEBUG("R_AARCH64_NONE\n");
+		break;
+	case R_AARCH64_RELATIVE:
+		if (sym)
+		{
+			EL_DEBUG("R_AARCH64_RELATIVE with symbol ref!\n");
+			return EL_BADREL;
+		}
+
+		EL_DEBUG("Applying R_AARCH64_RELATIVE reloc @%p\n", p);
+		*p += ctx->base_load_vaddr;
+		break;
+
+	default:
+		EL_DEBUG("Bad relocation %u\n", type);
+		return EL_BADREL;
+	}
+
+	return EL_OK;
+}
+
+#endif

source/libs/elfload/elfreloc_arm.c

@@ -0,0 +1,66 @@
+/*
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <m4x@m4xw.net> wrote this file. As long as you retain this notice you can do
+ * whatever you want with this stuff. If we meet some day, and you think this
+ * stuff is worth it, you can buy me a beer in return.                     M4xw
+ * ----------------------------------------------------------------------------
+ */
+
+#include "elfload.h"
+
+#if defined(__arm__)
+
+// Taken from http://infocenter.arm.com/help/topic/com.arm.doc.ihi0044f/IHI0044F_aaelf.pdf
+#define R_ARM_NONE 0
+#define R_ARM_ABS32 2
+#define R_ARM_JUMP_SLOT 22
+#define R_ARM_GLOB_DAT 21
+#define R_ARM_RELATIVE 23
+
+el_status el_applyrel(el_ctx *ctx, Elf_Rel *rel)
+{
+	uint32_t sym = ELF_R_SYM(rel->r_info);                              // Symbol offset
+	uint32_t type = ELF_R_TYPE(rel->r_info);                            // Relocation Type
+	uintptr_t *p = (uintptr_t *)(rel->r_offset + ctx->base_load_paddr); // Target Addr
+
+#if 0 // For later symbol usage
+	Elf32_Sym *elfSym;
+	const char *symbolName;
+
+	// We resolve relocs from the originating elf-image
+	elfSym = (Elf32_Sym *)(ctx->symtab.sh_offset + (char *)buffteg) + sym;
+	int strtab_offset = ctx->shstr.sh_offset;
+	char *strtab = (char *)buffteg + strtab_offset;
+	symbolName = strtab + elfSym->st_name;
+	//EL_DEBUG("Str: %s sz: %x val: %x\n", symbolName, elfSym->st_size, elfSym->st_value);
+#endif
+
+	switch (type)
+	{
+	case R_ARM_NONE:
+		EL_DEBUG("R_ARM_NONE\n");
+		break;
+	case R_ARM_JUMP_SLOT:
+	case R_ARM_ABS32:
+	case R_ARM_GLOB_DAT:
+		// Stubbed for later purpose
+		//*p += elfSym->st_value; // + vaddr from sec
+		//*p |= 0; // 1 if Thumb && STT_FUNC, ignored for now
+		break;
+	case R_ARM_RELATIVE: // Needed for PIE
+		if (sym)
+		{
+			return EL_BADREL;
+		}
+		*p += ctx->base_load_vaddr;
+		break;
+
+	default:
+		return EL_BADREL;
+	}
+
+	return EL_OK;
+}
+
+#endif

source/libs/fatfs/diskio.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019 shchmue
+ * Copyright (c) 2019-2020 shchmue
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -24,15 +24,15 @@
 /*-----------------------------------------------------------------------*/
 
 #include <string.h>
+
+#include "../../../common/memory_map.h"
+
 #include "diskio.h"		/* FatFs lower layer API */
 #include "../../mem/heap.h"
 #include "../../sec/se.h"
 #include "../../storage/nx_emmc.h"
 #include "../../storage/sdmmc.h"
 
-#define SDMMC_UPPER_BUFFER 0xB8000000
-#define DRAM_START         0x80000000
-
 extern sdmmc_storage_t sd_storage;
 extern sdmmc_storage_t storage;
 extern emmc_part_t *system_part;
@@ -40,13 +40,16 @@ extern emmc_part_t *system_part;
 typedef struct {
     u32 sector;
     u32 visit_count;
+    u8  align[8];
     u8  tweak[0x10];
     u8  cached_sector[0x200];
 } sector_cache_t;
- 
-#define MAX_SEC_CACHE_ENTRIES 64
-static sector_cache_t *sector_cache = (sector_cache_t*)0x40020000;
-static u32 secindex = 0;
+
+#define MAX_SEC_CACHE_ENTRIES 256
+static sector_cache_t *sector_cache = (sector_cache_t *)(MIXD_BUF_ALIGNED + 0x100000); //NULL;
+u32 secindex = 0;
+bool clear_sector_cache = false;
+bool lock_sector_cache = false;
 
 DSTATUS disk_status (
     BYTE pdrv /* Physical drive number to identify the drive */
@@ -78,8 +81,10 @@ static inline void _gf256_mul_x_le(void *block) {
 
 static inline int _emmc_xts(u32 ks1, u32 ks2, u32 enc, u8 *tweak, bool regen_tweak, u32 tweak_exp, u64 sec, void *dst, void *src, u32 secsize) {
     int res = 0;
-    u8 *pdst = (u8 *)dst;
-    u8 *psrc = (u8 *)src;
+    u8 *temptweak = (u8 *)malloc(0x10);
+	u32 *pdst = (u32 *)dst;
+    u32 *psrc = (u32 *)src;
+    u32 *ptweak = (u32 *)tweak;
 
     if (regen_tweak) {
         for (int i = 0xF; i >= 0; i--) {
@@ -93,34 +98,33 @@ static inline int _emmc_xts(u32 ks1, u32 ks2, u32 enc, u8 *tweak, bool regen_twe
     for (u32 i = 0; i < tweak_exp * 0x20; i++)
         _gf256_mul_x_le(tweak);
 
-    u8 temptweak[0x10];
     memcpy(temptweak, tweak, 0x10);
 
     //We are assuming a 0x10-aligned sector size in this implementation.
     for (u32 i = 0; i < secsize / 0x10; i++) {
-        for (u32 j = 0; j < 0x10; j++)
-            pdst[j] = psrc[j] ^ tweak[j];
+        for (u32 j = 0; j < 4; j++)
+            pdst[j] = psrc[j] ^ ptweak[j];
         _gf256_mul_x_le(tweak);
-        psrc += 0x10;
-        pdst += 0x10;
+        psrc += 4;
+        pdst += 4;
     }
 
-    se_aes_crypt_ecb(ks2, 0, dst, secsize, src, secsize);
+    se_aes_crypt_ecb(ks2, enc, dst, secsize, dst, secsize);
 
-    pdst = (u8 *)dst;
+    pdst = (u32 *)dst;
 
     memcpy(tweak, temptweak, 0x10);
     for (u32 i = 0; i < secsize / 0x10; i++) {
-        for (u32 j = 0; j < 0x10; j++)
-            pdst[j] = pdst[j] ^ tweak[j];
+        for (u32 j = 0; j < 4; j++)
+            pdst[j] = pdst[j] ^ ptweak[j];
         _gf256_mul_x_le(tweak);
-        pdst += 0x10;
+        pdst += 4;
     }
 
-
     res = 1;
 
 out:;
+    free(temptweak);
     return res;
 }
 
@@ -134,17 +138,31 @@ DRESULT disk_read (
     switch (pdrv)
     {
     case 0:
-        return sdmmc_storage_read(&sd_storage, sector, count, buff) ? RES_OK : RES_ERROR;
+        if (((u32)buff >= DRAM_START) && !((u32)buff % 8))
+            return sdmmc_storage_read(&sd_storage, sector, count, buff) ? RES_OK : RES_ERROR;
+        u8 *buf = (u8 *)SDMMC_UPPER_BUFFER;
+        if (sdmmc_storage_read(&sd_storage, sector, count, buf))
+        {
+            memcpy(buff, buf, 512 * count);
+            return RES_OK;
+        }
+        return RES_ERROR;
 
     case 1:;
         __attribute__ ((aligned (16))) static u8 tweak[0x10];
         __attribute__ ((aligned (16))) static u64 prev_cluster = -1;
         __attribute__ ((aligned (16))) static u32 prev_sector = 0;
-        u32 tweak_exp = 0;
-        bool regen_tweak = true, cache_sector = false;
+        bool needs_cache_sector = false;
+
+        if (secindex == 0 || clear_sector_cache) {
+            clear_sector_cache = false;
+            lock_sector_cache = false;
+            secindex = 0;
+        }
 
         u32 s = 0;
-        if (count == 1) {
+        // only attempt to cache single-sector reads as these are most likely to be repeated (eg. rereading FAT)
+        if (!lock_sector_cache && count == 1) {
             for ( ; s < secindex; s++) {
                 if (sector_cache[s].sector == sector) {
                     sector_cache[s].visit_count++;
@@ -159,12 +177,14 @@ DRESULT disk_read (
             if (s == secindex && s < MAX_SEC_CACHE_ENTRIES) {
                 sector_cache[s].sector = sector;
                 sector_cache[s].visit_count++;
-                cache_sector = true;
+                needs_cache_sector = true;
                 secindex++;
             }
         }
 
         if (nx_emmc_part_read(&storage, system_part, sector, count, buff)) {
+            u32 tweak_exp = 0;
+            bool regen_tweak = true;
             if (prev_cluster != sector / 0x20) { // sector in different cluster than last read
                 prev_cluster = sector / 0x20;
                 tweak_exp = sector % 0x20;
@@ -177,7 +197,7 @@ DRESULT disk_read (
 
             // fatfs will never pull more than a cluster
             _emmc_xts(9, 8, 0, tweak, regen_tweak, tweak_exp, prev_cluster, buff, buff, count * 0x200);
-            if (cache_sector) {
+            if (needs_cache_sector) {
                 memcpy(sector_cache[s].cached_sector, buff, 0x200);
                 memcpy(sector_cache[s].tweak, tweak, 0x10);
             }
@@ -198,7 +218,14 @@ DRESULT disk_write (
 {
     if (pdrv == 1)
         return RES_WRPRT;
-    return sdmmc_storage_write(&sd_storage, sector, count, (void *)buff) ? RES_OK : RES_ERROR;
+
+    if (((u32)buff >= DRAM_START) && !((u32)buff % 8))
+        return sdmmc_storage_write(&sd_storage, sector, count, (void *)buff) ? RES_OK : RES_ERROR;
+    u8 *buf = (u8 *)SDMMC_UPPER_BUFFER; //TODO: define this somewhere.
+    memcpy(buf, buff, 512 * count);
+    if (sdmmc_storage_write(&sd_storage, sector, count, buf))
+        return RES_OK;
+    return RES_ERROR;
 }
 
 DRESULT disk_ioctl (

source/libs/fatfs/ff.c

@@ -1,10 +1,25 @@
+/*
+ * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2019 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/>.
+ */
+
 /*----------------------------------------------------------------------------/
-/  FatFs - Generic FAT Filesystem Module  R0.13c (p3)                         /
+/  FatFs - Generic FAT Filesystem Module  R0.13c (p4)                         /
 /-----------------------------------------------------------------------------/
 /
 / Copyright (C) 2018, ChaN, all right reserved.
-/ Copyright (c) 2018 naehrwert
-/ Copyright (C) 2018-2019 CTCaer
 /
 / FatFs module is an open source software. Redistribution and use of FatFs in
 / source and binary forms, with or without modification, are permitted provided
@@ -3472,7 +3487,7 @@ static FRESULT find_volume (	/* FR_OK(0): successful, !=0: an error occurred */
 #if FF_USE_LFN == 1
 	fs->lfnbuf = LfnBuf;	/* Static LFN working buffer */
 #if FF_FS_EXFAT
-	fs->dirbuf = DirBuf;	/* Static directory block scratchpad buuffer */
+	fs->dirbuf = DirBuf;	/* Static directory block scratchpad buffer */
 #endif
 #endif
 #if FF_FS_RPATH != 0
@@ -3891,6 +3906,93 @@ FRESULT f_read (
 
 
 
+#ifdef FF_FASTFS
+/*-----------------------------------------------------------------------*/
+/* Fast Read Aligned Sized File Without a Cache                         */
+/*-----------------------------------------------------------------------*/
+#if FF_USE_FASTSEEK
+FRESULT f_read_fast (
+	FIL* fp,			/* Pointer to the file object */
+	const void* buff,	/* Pointer to the data to be written */
+	UINT btr			/* Number of bytes to read */
+)
+{
+	FRESULT res;
+	FATFS *fs;
+	UINT csize_bytes;
+	DWORD clst;
+	UINT count = 0;
+	FSIZE_t work_sector = 0;
+	FSIZE_t sector_base = 0;
+	BYTE *wbuff = (BYTE*)buff;
+
+	// TODO support sector reading inside a cluster
+
+	res = validate(&fp->obj, &fs);			/* Check validity of the file object */
+	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) {
+		EFSPRINTF("FOV");
+		LEAVE_FF(fs, res);	/* Check validity */
+	}
+
+	if (!(fp->flag & FA_READ)) LEAVE_FF(fs, FR_DENIED); /* Check access mode */
+	FSIZE_t remain = fp->obj.objsize - fp->fptr;
+	if (btr > remain) btr = (UINT)remain;		/* Truncate btr by remaining bytes */
+
+	csize_bytes = fs->csize * SS(fs);
+
+	if (!fp->fptr) {	/* On the top of the file? */
+		clst = fp->obj.sclust;	/* Follow from the origin */
+	} else {
+		if (fp->cltbl) clst = clmt_clust(fp, fp->fptr);	/* Get cluster# from the CLMT */
+		else { EFSPRINTF("CLTBL"); ABORT(fs, FR_CLTBL_NO_INIT); }
+	}
+	if (clst < 2) { EFSPRINTF("CCHK"); ABORT(fs, FR_INT_ERR); }
+	else if (clst == 0xFFFFFFFF) { EFSPRINTF("DSKC"); ABORT(fs, FR_DISK_ERR); }
+
+	fp->clust = clst;	/* Set working cluster */
+
+	sector_base = clst2sect(fs, fp->clust);
+	count += fs->csize;
+	btr -= csize_bytes;
+	fp->fptr += csize_bytes;
+
+	while (btr) {
+		clst = clmt_clust(fp, fp->fptr);	/* Get cluster# from the CLMT */
+
+		if (clst < 2) { EFSPRINTF("CCHK2"); ABORT(fs, FR_INT_ERR); }
+		else if (clst == 0xFFFFFFFF) { EFSPRINTF("DSKC"); ABORT(fs, FR_DISK_ERR); }
+
+		fp->clust = clst;
+
+		work_sector = clst2sect(fs, fp->clust);
+		if ((work_sector - sector_base) == count) count += fs->csize;
+		else {
+			if (disk_read(fs->pdrv, wbuff, sector_base, count) != RES_OK) ABORT(fs, FR_DISK_ERR);
+			wbuff += count * SS(fs);
+
+			sector_base = work_sector;
+			count = fs->csize;
+		}
+
+		fp->fptr += MIN(btr, csize_bytes);
+		btr -= MIN(btr, csize_bytes);
+
+		// TODO: what about if data is smaller than cluster?
+		// Must read-write back that cluster.
+
+		if (!btr) {	/* Final cluster/sectors read. */
+			if (disk_read(fs->pdrv, wbuff, sector_base, count) != RES_OK) ABORT(fs, FR_DISK_ERR);
+		}
+	}
+
+	LEAVE_FF(fs, FR_OK);
+}
+#endif
+#endif
+
+
+
+
 #if !FF_FS_READONLY
 /*-----------------------------------------------------------------------*/
 /* Write File                                                            */
@@ -4030,6 +4132,98 @@ FRESULT f_write (
 
 
 
+#ifdef FF_FASTFS
+/*-----------------------------------------------------------------------*/
+/* Fast Write Aligned Sized File Without a Cache                         */
+/*-----------------------------------------------------------------------*/
+#if FF_USE_FASTSEEK
+FRESULT f_write_fast (
+	FIL* fp,			/* Pointer to the file object */
+	const void* buff,	/* Pointer to the data to be written */
+	UINT btw			/* Number of bytes to write */
+)
+{
+	FRESULT res;
+	FATFS *fs;
+	UINT csize_bytes;
+	DWORD clst;
+	UINT count = 0;
+	FSIZE_t work_sector = 0;
+	FSIZE_t sector_base = 0;
+	const BYTE *wbuff = (const BYTE*)buff;
+
+	// TODO support sector writing inside a cluster
+
+	res = validate(&fp->obj, &fs);			/* Check validity of the file object */
+	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) {
+		EFSPRINTF("FOV");
+		LEAVE_FF(fs, res);	/* Check validity */
+	}
+
+	if (!(fp->flag & FA_WRITE)) LEAVE_FF(fs, FR_DENIED);	/* Check access mode */
+	/* Check fptr wrap-around (file size cannot reach 4 GiB at FAT volume) */
+	if ((!FF_FS_EXFAT || fs->fs_type != FS_EXFAT) && (DWORD)(fp->fptr + btw) < (DWORD)fp->fptr) {
+		btw = (UINT)(0xFFFFFFFF - (DWORD)fp->fptr);
+	}
+
+	csize_bytes = fs->csize * SS(fs);
+
+	if (!fp->fptr) {	/* On the top of the file? */
+		clst = fp->obj.sclust;	/* Follow from the origin */
+	} else {
+		if (fp->cltbl) clst = clmt_clust(fp, fp->fptr);	/* Get cluster# from the CLMT */
+		else { EFSPRINTF("CLTBL"); ABORT(fs, FR_CLTBL_NO_INIT); }
+	}
+
+	if (clst < 2) { EFSPRINTF("CCHK"); ABORT(fs, FR_INT_ERR); }
+	else if (clst == 0xFFFFFFFF) { EFSPRINTF("DERR"); ABORT(fs, FR_DISK_ERR); }
+
+	fp->clust = clst;	/* Set working cluster */
+
+	sector_base = clst2sect(fs, fp->clust);
+	count += fs->csize;
+	btw -= csize_bytes;
+	fp->fptr += csize_bytes;
+
+	while (btw) {
+		clst = clmt_clust(fp, fp->fptr);	/* Get cluster# from the CLMT */
+
+		if (clst < 2) { EFSPRINTF("CCHK2"); ABORT(fs, FR_INT_ERR); }
+		else if (clst == 0xFFFFFFFF) { EFSPRINTF("DERR"); ABORT(fs, FR_DISK_ERR); }
+
+		fp->clust = clst;
+
+		work_sector = clst2sect(fs, fp->clust);
+		if ((work_sector - sector_base) == count) count += fs->csize;
+		else {
+			if (disk_write(fs->pdrv, wbuff, sector_base, count) != RES_OK) ABORT(fs, FR_DISK_ERR);
+			wbuff += count * SS(fs);
+
+			sector_base = work_sector;
+			count = fs->csize;
+		}
+
+		fp->fptr += MIN(btw, csize_bytes);
+		btw -= MIN(btw, csize_bytes);
+
+		// what about if data is smaller than cluster?
+		// Probably must read-write back that cluster.
+		if (!btw) {	/* Final cluster/sectors write. */
+			if (disk_write(fs->pdrv, wbuff, sector_base, count) != RES_OK) ABORT(fs, FR_DISK_ERR);
+			fp->flag &= (BYTE)~FA_DIRTY;
+		}
+	}
+
+	fp->flag |= FA_MODIFIED;	/* Set file change flag */
+
+	LEAVE_FF(fs, FR_OK);
+}
+#endif
+#endif
+
+
+
+
 /*-----------------------------------------------------------------------*/
 /* Synchronize the File                                                  */
 /*-----------------------------------------------------------------------*/
@@ -4243,9 +4437,9 @@ FRESULT f_getcwd (
 	TCHAR *tp = buff;
 #if FF_VOLUMES >= 2
 	UINT vl;
-#endif
 #if FF_STR_VOLUME_ID
 	const char *vp;
+#endif
 #endif
 	FILINFO fno;
 	DEF_NAMBUF
@@ -4486,6 +4680,39 @@ FRESULT f_lseek (
 
 
 
+#ifdef FF_FASTFS
+#if FF_USE_FASTSEEK
+/*-----------------------------------------------------------------------*/
+/* Seek File Read/Write Pointer                                          */
+/*-----------------------------------------------------------------------*/
+
+DWORD *f_expand_cltbl (
+	FIL* fp,		/* Pointer to the file object */
+	UINT tblsz,		/* Size of table */
+	FSIZE_t ofs		/* File pointer from top of file */
+)
+{
+	if (fp->flag & FA_WRITE) f_lseek(fp, ofs);	/* Expand file if write is enabled */
+	if (!fp->cltbl) {	/* Allocate memory for cluster link table */
+		fp->cltbl = (DWORD *)ff_memalloc(tblsz);
+		fp->cltbl[0] = tblsz;
+	}
+	if (f_lseek(fp, CREATE_LINKMAP)) {	/* Create cluster link table */
+		ff_memfree(fp->cltbl);
+		fp->cltbl = NULL;
+		EFSPRINTF("CLTBLSZ");
+		return NULL;
+	}
+	f_lseek(fp, 0);
+
+	return fp->cltbl;
+}
+#endif
+#endif
+
+
+
+
 #if FF_FS_MINIMIZE <= 1
 /*-----------------------------------------------------------------------*/
 /* Create a Directory Object                                             */
@@ -4726,7 +4953,7 @@ FRESULT f_getfree (
 	/* Get logical drive */
 	res = find_volume(&path, &fs, 0);
 	if (res == FR_OK) {
-		*fatfs = fs;				/* Return ptr to the fs object */
+		if (fatfs) *fatfs = fs;	/* Return ptr to the fs object */
 		/* If free_clst is valid, return it without full FAT scan */
 		if (fs->free_clst <= fs->n_fatent - 2) {
 			*nclst = fs->free_clst;
@@ -5615,7 +5842,7 @@ FRESULT f_mkfs (
 	UINT len			/* Size of working buffer [byte] */
 )
 {
-	const UINT n_fats = 1;		/* Number of FATs for FAT/FAT32 volume (1 or 2) */
+	const UINT n_fats = 2;		/* Number of FATs for FAT/FAT32 volume (1 or 2) */
 	const UINT n_rootdir = 512;	/* Number of root directory entries for FAT volume */
 	static const WORD cst[] = {1, 4, 16, 64, 256, 512, 0};	/* Cluster size boundary for FAT volume (4Ks unit) */
 	static const WORD cst32[] = {1, 2, 4, 8, 16, 32, 0};	/* Cluster size boundary for FAT32 volume (128Ks unit) */
@@ -5679,7 +5906,7 @@ FRESULT f_mkfs (
 	} else {
 		/* Create a single-partition in this function */
 		if (disk_ioctl(pdrv, GET_SECTOR_COUNT, &sz_vol) != RES_OK) LEAVE_MKFS(FR_DISK_ERR);
-		b_vol = (opt & FM_SFD) ? 0 : 63;		/* Volume start sector */
+		b_vol = (opt & FM_SFD) ? 0 : 32768;		/* Volume start sector. Align to 16MB */
 		if (sz_vol < b_vol) LEAVE_MKFS(FR_MKFS_ABORTED);
 		sz_vol -= b_vol;						/* Volume size */
 	}
@@ -5903,6 +6130,9 @@ FRESULT f_mkfs (
 			if (fmt == FS_FAT32) {		/* FAT32: Move FAT base */
 				sz_rsv += n; b_fat += n;
 			} else {					/* FAT: Expand FAT size */
+				if (n % n_fats) {	/* Adjust fractional error if needed */
+					n--; sz_rsv++; b_fat++;
+				}
 				sz_fat += n / n_fats;
 			}
 
@@ -5966,13 +6196,13 @@ FRESULT f_mkfs (
 			st_word(buf + BPB_BkBootSec32, 6);			/* Offset of backup VBR (VBR + 6) */
 			buf[BS_DrvNum32] = 0x80;					/* Drive number (for int13) */
 			buf[BS_BootSig32] = 0x29;					/* Extended boot signature */
-			mem_cpy(buf + BS_VolLab32, "NO NAME    " "FAT32   ", 19);	/* Volume label, FAT signature */
+			mem_cpy(buf + BS_VolLab32, "SWITCH SD  " "FAT32   ", 19);	/* Volume label, FAT signature */
 		} else {
 			st_dword(buf + BS_VolID, GET_FATTIME());	/* VSN */
 			st_word(buf + BPB_FATSz16, (WORD)sz_fat);	/* FAT size [sector] */
 			buf[BS_DrvNum] = 0x80;						/* Drive number (for int13) */
 			buf[BS_BootSig] = 0x29;						/* Extended boot signature */
-			mem_cpy(buf + BS_VolLab, "NO NAME    " "FAT     ", 19);	/* Volume label, FAT signature */
+			mem_cpy(buf + BS_VolLab, "SWITCH SD  " "FAT     ", 19);	/* Volume label, FAT signature */
 		}
 		st_word(buf + BS_55AA, 0xAA55);					/* Signature (offset is fixed here regardless of sector size) */
 		if (disk_write(pdrv, buf, b_vol, 1) != RES_OK) LEAVE_MKFS(FR_DISK_ERR);	/* Write it to the VBR sector */
@@ -6632,4 +6862,3 @@ FRESULT f_setcp (
 	return FR_OK;
 }
 #endif	/* FF_CODE_PAGE == 0 */
-

source/libs/fatfs/ff.h

@@ -95,6 +95,7 @@ typedef DWORD FSIZE_t;
 /* Filesystem object structure (FATFS) */
 
 typedef struct {
+	BYTE	win[FF_MAX_SS];	/* Disk access window for Directory, FAT (and file data at tiny cfg) */
 	BYTE	fs_type;		/* Filesystem type (0:not mounted) */
 	BYTE	pdrv;			/* Associated physical drive */
 	BYTE	n_fats;			/* Number of FATs (1 or 2) */
@@ -137,8 +138,6 @@ typedef struct {
 	DWORD	bitbase;		/* Allocation bitmap base sector */
 #endif
 	DWORD	winsect;		/* Current sector appearing in the win[] */
-	DWORD	padding;		/* Ensure window is 16-aligned */
-	BYTE	win[FF_MAX_SS];	/* Disk access window for Directory, FAT (and file data at tiny cfg) */
 } FATFS;
 
 
@@ -169,6 +168,9 @@ typedef struct {
 /* File object structure (FIL) */
 
 typedef struct {
+#if !FF_FS_TINY
+	BYTE	buf[FF_MAX_SS];	/* File private data read/write window */
+#endif
 	FFOBJID	obj;			/* Object identifier (must be the 1st member to detect invalid object pointer) */
 	BYTE	flag;			/* File status flags */
 	BYTE	err;			/* Abort flag (error code) */
@@ -179,9 +181,6 @@ typedef struct {
 	DWORD	dir_sect;		/* Sector number containing the directory entry (not used at exFAT) */
 	BYTE*	dir_ptr;		/* Pointer to the directory entry in the win[] (not used at exFAT) */
 #endif
-#if !FF_FS_TINY
-	BYTE	buf[FF_MAX_SS];	/* File private data read/write window */
-#endif
 #if FF_USE_FASTSEEK
 	DWORD*	cltbl;			/* Pointer to the cluster link map table (nulled on open, set by application) */
 #endif
@@ -247,7 +246,12 @@ typedef enum {
 	FR_LOCKED,				/* (16) The operation is rejected according to the file sharing policy */
 	FR_NOT_ENOUGH_CORE,		/* (17) LFN working buffer could not be allocated */
 	FR_TOO_MANY_OPEN_FILES,	/* (18) Number of open files > FF_FS_LOCK */
+#ifdef FF_FASTFS
+	FR_INVALID_PARAMETER,	/* (19) Given parameter is invalid */
+	FR_CLTBL_NO_INIT	    /* (20) The cluster table for fast seek/read/write was not created */
+#else
 	FR_INVALID_PARAMETER	/* (19) Given parameter is invalid */
+#endif
 } FRESULT;
 
 
@@ -259,6 +263,10 @@ FRESULT f_open (FIL* fp, const TCHAR* path, BYTE mode);				/* Open or create a f
 FRESULT f_close (FIL* fp);											/* Close an open file object */
 FRESULT f_read (FIL* fp, void* buff, UINT btr, UINT* br);			/* Read data from the file */
 FRESULT f_write (FIL* fp, const void* buff, UINT btw, UINT* bw);	/* Write data to the file */
+#ifdef FF_FASTFS
+FRESULT f_read_fast (FIL* fp, const void* buff, UINT btr);			/* Fast read data from the file */
+FRESULT f_write_fast (FIL* fp, const void* buff, UINT btw);         /* Fast write data to the file */
+#endif
 FRESULT f_lseek (FIL* fp, FSIZE_t ofs);								/* Move file pointer of the file object */
 FRESULT f_truncate (FIL* fp);										/* Truncate the file */
 FRESULT f_sync (FIL* fp);											/* Flush cached data of the writing file */
@@ -280,7 +288,10 @@ FRESULT f_getfree (const TCHAR* path, DWORD* nclst, FATFS** fatfs);	/* Get numbe
 FRESULT f_getlabel (const TCHAR* path, TCHAR* label, DWORD* vsn);	/* Get volume label */
 FRESULT f_setlabel (const TCHAR* label);							/* Set volume label */
 FRESULT f_forward (FIL* fp, UINT(*func)(const BYTE*,UINT), UINT btf, UINT* bf);	/* Forward data to the stream */
-FRESULT f_expand (FIL* fp, FSIZE_t szf, BYTE opt);					/* Allocate a contiguous block to the file */
+#ifdef FF_FASTFS
+DWORD  *f_expand_cltbl (FIL* fp, UINT tblsz, FSIZE_t ofs);			/* Expand file and populate cluster table */
+#endif
+FRESULT f_expand (FIL* fp, FSIZE_t fsz, BYTE opt);					/* Allocate a contiguous block to the file */
 FRESULT f_mount (FATFS* fs, const TCHAR* path, BYTE opt);			/* Mount/Unmount a logical drive */
 FRESULT f_mkfs (const TCHAR* path, BYTE opt, DWORD au, void* work, UINT len);	/* Create a FAT volume */
 FRESULT f_fdisk (BYTE pdrv, const DWORD* szt, void* work);			/* Divide a physical drive into some partitions */
@@ -369,8 +380,11 @@ int ff_del_syncobj (FF_SYNC_t sobj);	/* Delete a sync object */
 #define	AM_RDO	0x01	/* Read only */
 #define	AM_HID	0x02	/* Hidden */
 #define	AM_SYS	0x04	/* System */
+#define	AM_VOL	0x08	/* Volume */
 #define AM_DIR	0x10	/* Directory */
 #define AM_ARC	0x20	/* Archive */
+#define AM_DEV	0x40	/* Device */
+#define AM_RVD	0x80	/* Reserved */
 
 
 #ifdef __cplusplus

source/libs/fatfs/ffconf.h

@@ -25,7 +25,7 @@
 /   3: f_lseek() function is removed in addition to 2. */
 
 
-#define FF_USE_STRFUNC	0
+#define FF_USE_STRFUNC	2
 /* This option switches string functions, f_gets(), f_putc(), f_puts() and f_printf().
 /
 /  0: Disable string functions.
@@ -33,7 +33,7 @@
 /  2: Enable with LF-CRLF conversion. */
 
 
-#define FF_USE_FIND		0
+#define FF_USE_FIND		1
 /* This option switches filtered directory read functions, f_findfirst() and
 /  f_findnext(). (0:Disable, 1:Enable 2:Enable with matching altname[] too) */
 
@@ -41,8 +41,13 @@
 #define FF_USE_MKFS		0
 /* This option switches f_mkfs() function. (0:Disable or 1:Enable) */
 
+#define FF_FASTFS 0
 
+#if FF_FASTFS
+#define FF_USE_FASTSEEK	1
+#else
 #define FF_USE_FASTSEEK	0
+#endif
 /* This option switches fast seek function. (0:Disable or 1:Enable) */
 
 
@@ -50,7 +55,7 @@
 /* This option switches f_expand function. (0:Disable or 1:Enable) */
 
 
-#define FF_USE_CHMOD	0
+#define FF_USE_CHMOD	1
 /* This option switches attribute manipulation functions, f_chmod() and f_utime().
 /  (0:Disable or 1:Enable) Also FF_FS_READONLY needs to be 0 to enable this option. */
 
@@ -97,7 +102,7 @@
 */
 
 
-#define FF_USE_LFN		1
+#define FF_USE_LFN		3
 #define FF_MAX_LFN		255
 /* The FF_USE_LFN switches the support for LFN (long file name).
 /

source/libs/fatfs/ffunicode.c

@@ -34,7 +34,6 @@
 #define MERGE2(a, b) a ## b
 #define CVTBL(tbl, cp) MERGE2(tbl, cp)
 
-
 /*------------------------------------------------------------------------*/
 /* Code Conversion Tables                                                 */
 /*------------------------------------------------------------------------*/
@@ -623,5 +622,4 @@ DWORD ff_wtoupper (	/* Returns up-converted code point */
 	return uni;
 }
 
-
 #endif /* #if FF_USE_LFN */

source/main.c

@@ -18,147 +18,365 @@
 
 #include <string.h>
 
+#include "config/config.h"
+#include "config/ini.h"
 #include "gfx/di.h"
 #include "gfx/gfx.h"
+#include "gfx/tui.h"
+#include "hos/pkg1.h"
 #include "libs/fatfs/ff.h"
 #include "mem/heap.h"
+#include "mem/minerva.h"
 #include "power/max77620.h"
 #include "rtc/max77620-rtc.h"
+#include "soc/bpmp.h"
 #include "soc/hw_init.h"
+#include "storage/emummc.h"
+#include "storage/nx_emmc.h"
+#include "storage/nx_sd.h"
 #include "storage/sdmmc.h"
+#include "utils/btn.h"
+#include "utils/dirlist.h"
+#include "utils/sprintf.h"
 #include "utils/util.h"
 
 #include "keys/keys.h"
 
-sdmmc_t sd_sdmmc;
-sdmmc_storage_t sd_storage;
-__attribute__ ((aligned (16))) FATFS sd_fs;
-static bool sd_mounted;
-
+hekate_config h_cfg;
 boot_cfg_t __attribute__((section ("._boot_cfg"))) b_cfg;
 
-bool sd_mount()
-{
-    if (sd_mounted)
-        return true;
-
-    if (!sdmmc_storage_init_sd(&sd_storage, &sd_sdmmc, SDMMC_1, SDMMC_BUS_WIDTH_4, 11))
-    {
-        EPRINTF("Failed to init SD card.\nMake sure that it is inserted.\nOr that SD reader is properly seated!");
-    }
-    else
-    {
-        int res = 0;
-        res = f_mount(&sd_fs, "sd:", 1);
-        if (res == FR_OK)
-        {
-            sd_mounted = 1;
-            return true;
-        }
-        else
-        {
-            EPRINTFARGS("Failed to mount SD card (FatFS Error %d).\nMake sure that a FAT partition exists..", res);
-        }
-    }
-
-    return false;
-}
-
-void sd_unmount()
-{
-    if (sd_mounted)
-    {
-        f_mount(NULL, "sd:", 1);
-        sdmmc_storage_end(&sd_storage);
-        sd_mounted = false;
-    }
-}
-
-void *sd_file_read(const char *path, u32 *fsize)
-{
-	FIL fp;
-	if (f_open(&fp, path, FA_READ) != FR_OK)
-		return NULL;
-
-	u32 size = f_size(&fp);
-	if (fsize)
-		*fsize = size;
-
-	void *buf = malloc(size);
-
-	if (f_read(&fp, buf, size, NULL) != FR_OK)
-	{
-		free(buf);
-		f_close(&fp);
-
-		return NULL;
-	}
-
-	f_close(&fp);
-
-	return buf;
-}
-
-int sd_save_to_file(void *buf, u32 size, const char *filename)
-{
-    FIL fp;
-    u32 res = 0;
-    res = f_open(&fp, filename, FA_CREATE_ALWAYS | FA_WRITE);
-    if (res)
-    {
-        EPRINTFARGS("Error (%d) creating file\n%s.\n", res, filename);
-        return 1;
-    }
-
-    f_write(&fp, buf, size, NULL);
-    f_close(&fp);
-
-    return 0;
-}
+volatile nyx_storage_t *nyx_str = (nyx_storage_t *)NYX_STORAGE_ADDR;
 
 // This is a safe and unused DRAM region for our payloads.
 #define RELOC_META_OFF      0x7C
 #define PATCHED_RELOC_SZ    0x94
 #define PATCHED_RELOC_STACK 0x40007000
-#define COREBOOT_ADDR       (0xD0000000 - 0x100000)
+#define PATCHED_RELOC_ENTRY 0x40010000
+#define EXT_PAYLOAD_ADDR    0xC0000000
+#define RCM_PAYLOAD_ADDR    (EXT_PAYLOAD_ADDR + ALIGN(PATCHED_RELOC_SZ, 0x10))
+#define COREBOOT_END_ADDR   0xD0000000
 #define CBFS_DRAM_EN_ADDR   0x4003e000
 #define  CBFS_DRAM_MAGIC    0x4452414D // "DRAM"
 
+static void *coreboot_addr;
+
 void reloc_patcher(u32 payload_dst, u32 payload_src, u32 payload_size)
 {
-    memcpy((u8 *)payload_src, (u8 *)IPL_LOAD_ADDR, PATCHED_RELOC_SZ);
+	memcpy((u8 *)payload_src, (u8 *)IPL_LOAD_ADDR, PATCHED_RELOC_SZ);
 
-    volatile reloc_meta_t *relocator = (reloc_meta_t *)(payload_src + RELOC_META_OFF);
+	volatile reloc_meta_t *relocator = (reloc_meta_t *)(payload_src + RELOC_META_OFF);
 
-    relocator->start = payload_dst - ALIGN(PATCHED_RELOC_SZ, 0x10);
-    relocator->stack = PATCHED_RELOC_STACK;
-    relocator->end   = payload_dst + payload_size;
-    relocator->ep    = payload_dst;
+	relocator->start = payload_dst - ALIGN(PATCHED_RELOC_SZ, 0x10);
+	relocator->stack = PATCHED_RELOC_STACK;
+	relocator->end   = payload_dst + payload_size;
+	relocator->ep    = payload_dst;
 
-    if (payload_size == 0x7000)
-    {
-        memcpy((u8 *)(payload_src + ALIGN(PATCHED_RELOC_SZ, 0x10)), (u8 *)COREBOOT_ADDR, 0x7000); //Bootblock
-        *(vu32 *)CBFS_DRAM_EN_ADDR = CBFS_DRAM_MAGIC;
-    }
+	if (payload_size == 0x7000)
+	{
+		memcpy((u8 *)(payload_src + ALIGN(PATCHED_RELOC_SZ, 0x10)), coreboot_addr, 0x7000); //Bootblock
+		*(vu32 *)CBFS_DRAM_EN_ADDR = CBFS_DRAM_MAGIC;
+	}
 }
 
-#define IPL_STACK_TOP  0x4003F000
-#define IPL_HEAP_START 0x90020000
+int launch_payload(char *path)
+{
+	gfx_clear_grey(0x1B);
+	gfx_con_setpos(0, 0);
+	if (!path)
+		return 1;
+
+	if (sd_mount())
+	{
+		FIL fp;
+		if (f_open(&fp, path, FA_READ))
+		{
+			EPRINTFARGS("Payload file is missing!\n(%s)", path);
+			sd_unmount();
+
+			return 1;
+		}
+
+		// Read and copy the payload to our chosen address
+		void *buf;
+		u32 size = f_size(&fp);
+
+		if (size < 0x30000)
+			buf = (void *)RCM_PAYLOAD_ADDR;
+		else
+		{
+			coreboot_addr = (void *)(COREBOOT_END_ADDR - size);
+			buf = coreboot_addr;
+		}
+
+		if (f_read(&fp, buf, size, NULL))
+		{
+			f_close(&fp);
+			sd_unmount();
+
+			return 1;
+		}
+
+		f_close(&fp);
+
+		sd_unmount();
+
+		if (size < 0x30000)
+		{
+			reloc_patcher(PATCHED_RELOC_ENTRY, EXT_PAYLOAD_ADDR, ALIGN(size, 0x10));
+
+			reconfig_hw_workaround(false, byte_swap_32(*(u32 *)(buf + size - sizeof(u32))));
+		}
+		else
+		{
+			reloc_patcher(PATCHED_RELOC_ENTRY, EXT_PAYLOAD_ADDR, 0x7000);
+			reconfig_hw_workaround(true, 0);
+		}
+
+		// Some cards (Sandisk U1), do not like a fast power cycle. Wait min 100ms.
+		sdmmc_storage_init_wait_sd();
+
+		void (*ext_payload_ptr)() = (void *)EXT_PAYLOAD_ADDR;
+
+		// Launch our payload.
+		(*ext_payload_ptr)();
+	}
+
+	return 1;
+}
+
+void launch_tools()
+{
+	u8 max_entries = 61;
+	char *filelist = NULL;
+	char *file_sec = NULL;
+	char *dir = NULL;
+
+	ment_t *ments = (ment_t *)malloc(sizeof(ment_t) * (max_entries + 3));
+
+	gfx_clear_grey(0x1B);
+	gfx_con_setpos(0, 0);
+
+	if (sd_mount())
+	{
+		dir = (char *)malloc(256);
+
+		memcpy(dir, "sd:/bootloader/payloads", 24);
+
+		filelist = dirlist(dir, NULL, false);
+
+		u32 i = 0;
+		u32 i_off = 2;
+
+		if (filelist)
+		{
+			// Build configuration menu.
+			u32 color_idx = 0;
+
+			ments[0].type = MENT_BACK;
+			ments[0].caption = "Back";
+			ments[0].color = colors[(color_idx++) % 6];
+			ments[1].type = MENT_CHGLINE;
+			ments[1].color = colors[(color_idx++) % 6];
+			if (!f_stat("sd:/atmosphere/reboot_payload.bin", NULL))
+			{
+				ments[i_off].type = INI_CHOICE;
+				ments[i_off].caption = "reboot_payload.bin";
+				ments[i_off].color = colors[(color_idx++) % 6];
+				ments[i_off].data = "sd:/atmosphere/reboot_payload.bin";
+				i_off++;
+			}
+			if (!f_stat("sd:/ReiNX.bin", NULL))
+			{
+				ments[i_off].type = INI_CHOICE;
+				ments[i_off].caption = "ReiNX.bin";
+				ments[i_off].color = colors[(color_idx++) % 6];
+				ments[i_off].data = "sd:/ReiNX.bin";
+				i_off++;
+			}
+
+			while (true)
+			{
+				if (i > max_entries || !filelist[i * 256])
+					break;
+				ments[i + i_off].type = INI_CHOICE;
+				ments[i + i_off].caption = &filelist[i * 256];
+				ments[i + i_off].color = colors[(color_idx++) % 6];
+				ments[i + i_off].data = &filelist[i * 256];
+
+				i++;
+			}
+		}
+
+		if (i > 0)
+		{
+			memset(&ments[i + i_off], 0, sizeof(ment_t));
+			menu_t menu = { ments, "Choose a file to launch", 0, 0 };
+
+			file_sec = (char *)tui_do_menu(&menu);
+
+			if (!file_sec)
+			{
+				free(ments);
+				free(dir);
+				free(filelist);
+				sd_unmount();
+				return;
+			}
+		}
+		else
+			EPRINTF("No payloads or modules found.");
+
+		free(ments);
+		free(filelist);
+	}
+	else
+	{
+		free(ments);
+		goto out;
+	}
+
+	if (file_sec)
+	{
+		if (memcmp("sd:/", file_sec, 4) != 0)
+		{
+			memcpy(dir + strlen(dir), "/", 2);
+			memcpy(dir + strlen(dir), file_sec, strlen(file_sec) + 1);
+		}
+		else
+			memcpy(dir, file_sec, strlen(file_sec) + 1);
+
+		if (launch_payload(dir))
+		{
+			EPRINTF("Failed to launch payload.");
+			free(dir);
+		}
+	}
+
+out:
+	sd_unmount();
+	free(dir);
+
+	btn_wait();
+}
+
+void dump_sysnand()
+{
+	h_cfg.emummc_force_disable = true;
+	b_cfg.extra_cfg &= ~EXTRA_CFG_DUMP_EMUMMC;
+	dump_keys();
+}
+
+void dump_emunand()
+{
+	if (h_cfg.emummc_force_disable)
+		return;
+	emu_cfg.enabled = 1;
+	b_cfg.extra_cfg |= EXTRA_CFG_DUMP_EMUMMC;
+	dump_keys();
+}
+
+ment_t ment_top[] = {
+	MDEF_HANDLER("Dump from SysNAND | Key generation: unk", dump_sysnand, COLOR_RED),
+	MDEF_HANDLER("Dump from EmuNAND | Key generation: unk", dump_emunand, COLOR_ORANGE),
+	MDEF_CAPTION("---------------", COLOR_YELLOW),
+	MDEF_HANDLER("Payloads...", launch_tools, COLOR_GREEN),
+	MDEF_CAPTION("---------------", COLOR_BLUE),
+	MDEF_HANDLER("Reboot (Normal)", reboot_normal, COLOR_VIOLET),
+	MDEF_HANDLER("Reboot (RCM)", reboot_rcm, COLOR_RED),
+	MDEF_HANDLER("Power off", power_off, COLOR_ORANGE),
+	MDEF_END()
+};
+
+menu_t menu_top = { ment_top, NULL, 0, 0 };
+
+void _get_key_generations(char *sysnand_label, char *emunand_label)
+{
+	sdmmc_t sdmmc;
+	sdmmc_storage_t storage;
+	sdmmc_storage_init_mmc(&storage, &sdmmc, SDMMC_BUS_WIDTH_8, SDHCI_TIMING_MMC_HS400);
+	u8 *pkg1 = (u8 *)malloc(NX_EMMC_BLOCKSIZE);
+	sdmmc_storage_set_mmc_partition(&storage, EMMC_BOOT0);
+	sdmmc_storage_read(&storage, 0x100000 / NX_EMMC_BLOCKSIZE, 1, pkg1);
+	const pkg1_id_t *pkg1_id = pkg1_identify(pkg1);
+	sdmmc_storage_end(&storage);
+
+	if (pkg1_id)
+		sprintf(sysnand_label + 36, "% 3d", pkg1_id->kb);
+	ment_top[0].caption = sysnand_label;
+	if (h_cfg.emummc_force_disable)
+	{
+		free(pkg1);
+		return;
+	}
+
+	emummc_storage_init_mmc(&storage, &sdmmc);
+	memset(pkg1, 0, NX_EMMC_BLOCKSIZE);
+	emummc_storage_set_mmc_partition(&storage, EMMC_BOOT0);
+	emummc_storage_read(&storage, 0x100000 / NX_EMMC_BLOCKSIZE, 1, pkg1);
+	pkg1_id = pkg1_identify(pkg1);
+	emummc_storage_end(&storage);
+
+	if (pkg1_id)
+		sprintf(emunand_label + 36, "% 3d", pkg1_id->kb);
+	free(pkg1);
+	ment_top[1].caption = emunand_label;
+}
 
 extern void pivot_stack(u32 stack_top);
 
 void ipl_main()
 {
-    config_hw();
-    pivot_stack(IPL_STACK_TOP);
-    heap_init(IPL_HEAP_START);
+	// Do initial HW configuration. This is compatible with consecutive reruns without a reset.
+	config_hw();
 
-    display_init();
-    u32 *fb = display_init_framebuffer();
-    gfx_init_ctxt(fb, 720, 1280, 720);
-    gfx_con_init();
-    display_backlight_pwm_init();
+	// Pivot the stack so we have enough space.
+	pivot_stack(IPL_STACK_TOP);
 
-    sd_mount();
-    dump_keys();
+	// Tegra/Horizon configuration goes to 0x80000000+, package2 goes to 0xA9800000, we place our heap in between.
+	heap_init(IPL_HEAP_START);
+
+	// Set bootloader's default configuration.
+	set_default_configuration();
+
+	sd_mount();
+
+	minerva_init();
+	minerva_change_freq(FREQ_1600);
+
+	display_init();
+
+	u32 *fb = display_init_framebuffer();
+	gfx_init_ctxt(fb, 720, 1280, 720);
+
+	gfx_con_init();
+
+	display_backlight_pwm_init();
+
+	// Overclock BPMP.
+	bpmp_clk_rate_set(BPMP_CLK_DEFAULT_BOOST);
+
+	h_cfg.emummc_force_disable = emummc_load_cfg();
+
+	if (b_cfg.boot_cfg & BOOT_CFG_SEPT_RUN)
+	{
+		if (!(b_cfg.extra_cfg & EXTRA_CFG_DUMP_EMUMMC))
+			h_cfg.emummc_force_disable = true;
+		dump_keys();
+	}
+
+	if (h_cfg.emummc_force_disable)
+	{
+		ment_top[1].type = MENT_CAPTION;
+		ment_top[1].color = 0xFF555555;
+		ment_top[1].handler = NULL;
+	}
+
+	_get_key_generations((char *)ment_top[0].caption, (char *)ment_top[1].caption);
+
+	while (true)
+		tui_do_menu(&menu_top);
+
+	// Halt BPMP if we managed to get out of execution.
+	while (true)
+		bpmp_halt();
 }

source/mem/heap.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2020 CTCaer
  * Copyright (c) 2018 M4xw
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -17,6 +18,7 @@
 
 #include <string.h>
 #include "heap.h"
+#include "../gfx/gfx.h"
 #include "../../common/common_heap.h"
 
 static void _heap_create(heap_t *heap, u32 start)
@@ -25,12 +27,13 @@ static void _heap_create(heap_t *heap, u32 start)
 	heap->first = NULL;
 }
 
-static u32 _heap_alloc(heap_t *heap, u32 size, u32 alignment)
+// Node info is before node address.
+static u32 _heap_alloc(heap_t *heap, u32 size)
 {
-	hnode_t *node, *new;
-	int search = 1;
+	hnode_t *node, *new_node;
 
-	size = ALIGN(size, alignment);
+	// Align to cache line size.
+	size = ALIGN(size, sizeof(hnode_t));
 
 	if (!heap->first)
 	{
@@ -45,36 +48,55 @@ static u32 _heap_alloc(heap_t *heap, u32 size, u32 alignment)
 	}
 
 	node = heap->first;
-	while (search)
+	while (true)
 	{
-		if (!node->used && size + sizeof(hnode_t) < node->size)
+		// Check if there's available unused node.
+		if (!node->used && (size <= node->size))
 		{
-			new = (hnode_t *)((u32)node + sizeof(hnode_t) + size);
+			// Size and offset of the new unused node.
+			u32 new_size = node->size - size;
+			new_node = (hnode_t *)((u32)node + sizeof(hnode_t) + size);
+
+			// If there's aligned unused space from the old node,
+			// create a new one and set the leftover size.
+			if (new_size >= (sizeof(hnode_t) << 2))
+			{
+				new_node->size = new_size - sizeof(hnode_t);
+				new_node->used = 0;
+				new_node->next = node->next;
+
+				// Check that we are not on first node.
+				if (new_node->next)
+					new_node->next->prev = new_node;
+
+				new_node->prev = node;
+				node->next = new_node;
+			}
+			else // Unused node size is just enough.
+				size += new_size;
 
-			new->size = node->size - sizeof(hnode_t) - size;
 			node->size = size;
 			node->used = 1;
-			new->used = 0;
-			new->next = node->next;
-			new->prev = node;
-			node->next = new;
 
 			return (u32)node + sizeof(hnode_t);
 		}
+
+		// No unused node found, try the next one.
 		if (node->next)
 			node = node->next;
 		else
-			search = 0;
+			break;
 	}
 
-	new = (hnode_t *)((u32)node + sizeof(hnode_t) + node->size);
-	new->used = 1;
-	new->size = size;
-	new->prev = node;
-	new->next = NULL;
-	node->next = new;
+	// No unused node found, create a new one.
+	new_node = (hnode_t *)((u32)node + sizeof(hnode_t) + node->size);
+	new_node->used = 1;
+	new_node->size = size;
+	new_node->prev = node;
+	new_node->next = NULL;
+	node->next = new_node;
 
-	return (u32)new + sizeof(hnode_t);
+	return (u32)new_node + sizeof(hnode_t);
 }
 
 static void _heap_free(heap_t *heap, u32 addr)
@@ -90,6 +112,7 @@ static void _heap_free(heap_t *heap, u32 addr)
 			{
 				node->prev->size += node->size + sizeof(hnode_t);
 				node->prev->next = node->next;
+
 				if (node->next)
 					node->next->prev = node->prev;
 			}
@@ -107,18 +130,13 @@ void heap_init(u32 base)
 
 void *malloc(u32 size)
 {
-	return (void *)_heap_alloc(&_heap, size, 0x10);
-}
-
-void *memalign(u32 align, u32 size)
-{
-	return (void *)_heap_alloc(&_heap, size, align);
+	return (void *)_heap_alloc(&_heap, size);
 }
 
 void *calloc(u32 num, u32 size)
 {
-	void *res = (void *)_heap_alloc(&_heap, num * size, 0x10);
-	memset(res, 0, num * size);
+	void *res = (void *)_heap_alloc(&_heap, num * size);
+	memset(res, 0, ALIGN(num * size, sizeof(hnode_t))); // Clear the aligned size.
 	return res;
 }
 
@@ -127,3 +145,30 @@ void free(void *buf)
 	if ((u32)buf >= _heap.start)
 		_heap_free(&_heap, (u32)buf);
 }
+
+void heap_monitor(heap_monitor_t *mon, bool print_node_stats)
+{
+	u32 count = 0;
+	memset(mon, 0, sizeof(heap_monitor_t));
+
+	hnode_t *node = _heap.first;
+	while (true)
+	{
+		if (node->used)
+			mon->used += node->size + sizeof(hnode_t);
+		else
+			mon->total += node->size + sizeof(hnode_t);
+
+		if (print_node_stats)
+			gfx_printf("%3d - %d, addr: 0x%08X, size: 0x%X\n",
+				count, node->used, (u32)node + sizeof(hnode_t), node->size);
+
+		count++;
+
+		if (node->next)
+			node = node->next;
+		else
+			break;
+	}
+	mon->total += mon->used;
+}

source/mem/heap.h

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2020 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,
@@ -18,11 +19,12 @@
 #define _HEAP_H_
 
 #include "../utils/types.h"
+#include "../../common/common_heap.h"
 
 void heap_init(u32 base);
 void *malloc(u32 size);
 void *calloc(u32 num, u32 size);
 void free(void *buf);
-void *memalign(u32 align, u32 size);
+void heap_monitor(heap_monitor_t *mon, bool print_node_stats);
 
 #endif

source/mem/mc.c

@@ -127,7 +127,7 @@ void mc_disable_ahb_redirect()
 void mc_enable()
 {
 	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) & 0x1FFFFFFF) | 0x40000000;
-	// Enable MIPI CAL clock.
+	// Enable EMC clock.
 	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = (CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) & 0xFDFFFFFF) | 0x2000000;
 	// Enable MC clock.
 	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = (CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) & 0xFFFFFFFE) | 1;

source/mem/mc_t210.h

@@ -463,4 +463,54 @@
 #define MC_ERR_APB_ASID_UPDATE_STATUS                           0x9d0
 #define MC_DA_CONFIG0                                           0x9dc
 
+// MC_SECURITY_CARVEOUTX_CFG0
+// Mode of LOCK_MODE.
+#define PROTECT_MODE_SHIFT 0
+#define SEC_CARVEOUT_CFG_SECURE    (0 << PROTECT_MODE_SHIFT0)
+#define SEC_CARVEOUT_CFG_TZ_SECURE (1 << PROTECT_MODE_SHIFT0)
+// Enables PROTECT_MODE.
+#define LOCK_MODE_SHIFT 1
+#define SEC_CARVEOUT_CFG_UNLOCKED (0 << LOCK_MODE_SHIFT)
+#define SEC_CARVEOUT_CFG_LOCKED   (1 << LOCK_MODE_SHIFT)
+
+#define ADDRESS_TYPE_SHIFT 2
+#define SEC_CARVEOUT_CFG_ANY_ADDRESS       (0 << ADDRESS_TYPE_SHIFT)
+#define SEC_CARVEOUT_CFG_UNTRANSLATED_ONLY (1 << ADDRESS_TYPE_SHIFT)
+
+#define READ_ACCESS_LEVEL_SHIFT 3
+#define SEC_CARVEOUT_CFG_RD_ALL       (1 << READ_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_RD_UNK       (2 << READ_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_RD_FALCON_LS (4 << READ_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_RD_FALCON_HS (8 << READ_ACCESS_LEVEL_SHIFT)
+
+#define WRITE_ACCESS_LEVEL_SHIFT 7
+#define SEC_CARVEOUT_CFG_WR_ALL       (1 << WRITE_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_WR_UNK       (2 << WRITE_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_WR_FALCON_LS (4 << WRITE_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_WR_FALCON_HS (8 << WRITE_ACCESS_LEVEL_SHIFT)
+
+#define SEC_CARVEOUT_CFG_APERTURE_ID_MASK (3 << 11)
+
+#define DISABLE_READ_CHECK_ACCESS_LEVEL_SHIFT 14
+#define SEC_CARVEOUT_CFG_DIS_RD_CHECK_L0 (1 << DISABLE_READ_CHECK_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_DIS_RD_CHECK_L1 (2 << DISABLE_READ_CHECK_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_DIS_RD_CHECK_L2 (4 << DISABLE_READ_CHECK_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_DIS_RD_CHECK_L3 (8 << DISABLE_READ_CHECK_ACCESS_LEVEL_SHIFT)
+
+#define DISABLE_WRITE_CHECK_ACCESS_LEVEL_SHIFT 18
+#define SEC_CARVEOUT_CFG_DIS_WR_CHECK_L0 (1 << DISABLE_WRITE_CHECK_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_DIS_WR_CHECK_L1 (2 << DISABLE_WRITE_CHECK_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_DIS_WR_CHECK_L2 (4 << DISABLE_WRITE_CHECK_ACCESS_LEVEL_SHIFT)
+#define SEC_CARVEOUT_CFG_DIS_WR_CHECK_L3 (8 << DISABLE_WRITE_CHECK_ACCESS_LEVEL_SHIFT)
+
+#define SEC_CARVEOUT_CFG_SEND_CFG_TO_GPU (1 << 22)
+
+#define SEC_CARVEOUT_CFG_TZ_GLOBAL_WR_EN_BYPASS_CHECK (1 << 23)
+#define SEC_CARVEOUT_CFG_TZ_GLOBAL_RD_EN_BYPASS_CHECK (1 << 24)
+
+#define SEC_CARVEOUT_CFG_ALLOW_APERTURE_ID_MISMATCH (1 << 25)
+#define SEC_CARVEOUT_CFG_FORCE_APERTURE_ID_MATCH    (1 << 26)
+
+#define SEC_CARVEOUT_CFG_IS_WPR (1 << 27)
+
 #endif

source/mem/minerva.c

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2019 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/>.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+
+#include "minerva.h"
+#include "../soc/fuse.h"
+#include "../utils/util.h"
+
+#include "../soc/clock.h"
+#include "../ianos/ianos.h"
+#include "../soc/fuse.h"
+#include "../soc/t210.h"
+
+extern volatile nyx_storage_t *nyx_str;
+void (*minerva_cfg)(mtc_config_t *mtc_cfg, void *);
+
+u32 minerva_init()
+{
+	u32 curr_ram_idx = 0;
+
+	minerva_cfg = NULL;
+	mtc_config_t *mtc_cfg = (mtc_config_t *)&nyx_str->mtc_cfg;
+	memset(mtc_cfg, 0, sizeof(mtc_config_t));
+
+	// Set table to nyx storage.
+	mtc_cfg->mtc_table = (emc_table_t *)nyx_str->mtc_table;
+
+	mtc_cfg->sdram_id = (fuse_read_odm(4) >> 3) & 0x1F;
+	mtc_cfg->init_done = MTC_NEW_MAGIC; // Initialize mtc table.
+
+	u32 ep_addr = ianos_loader(false, "bootloader/sys/libsys_minerva.bso", DRAM_LIB, (void *)mtc_cfg);
+
+	// Ensure that Minerva is new.
+	if (mtc_cfg->init_done == MTC_INIT_MAGIC)
+		minerva_cfg = (void *)ep_addr;
+	else
+		mtc_cfg->init_done = 0;
+
+	if (!minerva_cfg)
+		return 1;
+
+	// Get current frequency
+	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)
+			break;
+	}
+
+	mtc_cfg->rate_from = mtc_cfg->mtc_table[curr_ram_idx].rate_khz;
+	mtc_cfg->rate_to = 204000;
+	mtc_cfg->train_mode = OP_TRAIN;
+	minerva_cfg(mtc_cfg, NULL);
+	mtc_cfg->rate_to = 800000;
+	minerva_cfg(mtc_cfg, NULL);
+	mtc_cfg->rate_to = 1600000;
+	minerva_cfg(mtc_cfg, NULL);
+
+	// FSP WAR.
+	mtc_cfg->train_mode = OP_SWITCH;
+	mtc_cfg->rate_to = 800000;
+	minerva_cfg(mtc_cfg, NULL);
+
+	// Switch to max.
+	mtc_cfg->rate_to = 1600000;
+	minerva_cfg(mtc_cfg, NULL);
+
+	return 0;
+}
+
+void minerva_change_freq(minerva_freq_t freq)
+{
+	if (!minerva_cfg)
+		return;
+
+	mtc_config_t *mtc_cfg = (mtc_config_t *)&nyx_str->mtc_cfg;
+	if (mtc_cfg->rate_from != freq)
+	{
+		mtc_cfg->rate_to = freq;
+		mtc_cfg->train_mode = OP_SWITCH;
+		minerva_cfg(mtc_cfg, NULL);
+	}
+}
+
+void minerva_periodic_training()
+{
+	if (!minerva_cfg)
+		return;
+
+	mtc_config_t *mtc_cfg = (mtc_config_t *)&nyx_str->mtc_cfg;
+	if (mtc_cfg->rate_from == FREQ_1600)
+	{
+		mtc_cfg->train_mode = OP_PERIODIC_TRAIN;
+		minerva_cfg(mtc_cfg, NULL);
+	}
+}

source/mem/minerva.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2019 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 _FE_MINERVA_H_
+#define _FE_MINERVA_H_
+
+#include "mtc_table.h"
+#include "../utils/types.h"
+
+#define MTC_INIT_MAGIC 0x3043544D
+#define MTC_NEW_MAGIC  0x5243544D
+
+#define EMC_PERIODIC_TRAIN_MS 250
+
+typedef struct
+{
+	s32 rate_to;
+	s32 rate_from;
+	emc_table_t *mtc_table;
+	u32 table_entries;
+	emc_table_t *current_emc_table;
+	u32 train_mode;
+	u32 sdram_id;
+	u32 prev_temp;
+	bool emc_2X_clk_src_is_pllmb;
+	bool fsp_for_src_freq;
+	bool train_ram_patterns;
+	bool init_done;
+} mtc_config_t;
+
+enum train_mode_t
+{
+	OP_SWITCH         = 0,
+	OP_TRAIN          = 1,
+	OP_TRAIN_SWITCH   = 2,
+	OP_PERIODIC_TRAIN = 3,
+	OP_TEMP_COMP      = 4
+};
+
+typedef enum
+{
+	FREQ_204  = 204000,
+	FREQ_800  = 800000,
+	FREQ_1600 = 1600000
+} minerva_freq_t;
+
+extern void (*minerva_cfg)(mtc_config_t *mtc_cfg, void *);
+u32  minerva_init();
+void minerva_change_freq(minerva_freq_t freq);
+void minerva_periodic_training();
+
+#endif

source/mem/mtc_table.h

@@ -0,0 +1,560 @@
+/*
+ * Minerva Training Cell
+ * DRAM Training for Tegra X1 SoC. Supports DDR2/3 and LPDDR3/4.
+ *
+ * Copyright (c) 2018 CTCaer  <ctcaer@gmail.com>
+ *
+ * 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 _MTC_TABLE_H_
+#define _MTC_TABLE_H_
+
+#include "../utils/types.h"
+
+typedef struct
+{
+	s32 pll_osc_in;
+	s32 pll_out;
+	u32 pll_feedback_div;
+	u32 pll_input_div;
+	u32 pll_post_div;
+} pllm_clk_config_t;
+
+typedef struct
+{
+	u32 emc_rc_idx;
+	u32 emc_rfc_idx;
+	u32 emc_rfcpb_idx;
+	u32 emc_refctrl2_idx;
+	u32 emc_rfc_slr_idx;
+	u32 emc_ras_idx;
+	u32 emc_rp_idx;
+	u32 emc_r2w_idx;
+	u32 emc_w2r_idx;
+	u32 emc_r2p_idx;
+	u32 emc_w2p_idx;
+	u32 emc_r2r_idx;
+	u32 emc_tppd_idx;
+	u32 emc_ccdmw_idx;
+	u32 emc_rd_rcd_idx;
+	u32 emc_wr_rcd_idx;
+	u32 emc_rrd_idx;
+	u32 emc_rext_idx;
+	u32 emc_wext_idx;
+	u32 emc_wdv_chk_idx;
+	u32 emc_wdv_idx;
+	u32 emc_wsv_idx;
+	u32 emc_wev_idx;
+	u32 emc_wdv_mask_idx;
+	u32 emc_ws_duration_idx;
+	u32 emc_we_duration_idx;
+	u32 emc_quse_idx;
+	u32 emc_quse_width_idx;
+	u32 emc_ibdly_idx;
+	u32 emc_obdly_idx;
+	u32 emc_einput_idx;
+	u32 emc_mrw6_idx;
+	u32 emc_einput_duration_idx;
+	u32 emc_puterm_extra_idx;
+	u32 emc_puterm_width_idx;
+	u32 emc_qrst_idx;
+	u32 emc_qsafe_idx;
+	u32 emc_rdv_idx;
+	u32 emc_rdv_mask_idx;
+	u32 emc_rdv_early_idx;
+	u32 emc_rdv_early_mask_idx;
+	u32 emc_refresh_idx;
+	u32 emc_burst_refresh_num_idx;
+	u32 emc_pre_refresh_req_cnt_idx;
+	u32 emc_pdex2wr_idx;
+	u32 emc_pdex2rd_idx;
+	u32 emc_pchg2pden_idx;
+	u32 emc_act2pden_idx;
+	u32 emc_ar2pden_idx;
+	u32 emc_rw2pden_idx;
+	u32 emc_cke2pden_idx;
+	u32 emc_pdex2cke_idx;
+	u32 emc_pdex2mrr_idx;
+	u32 emc_txsr_idx;
+	u32 emc_txsrdll_idx;
+	u32 emc_tcke_idx;
+	u32 emc_tckesr_idx;
+	u32 emc_tpd_idx;
+	u32 emc_tfaw_idx;
+	u32 emc_trpab_idx;
+	u32 emc_tclkstable_idx;
+	u32 emc_tclkstop_idx;
+	u32 emc_mrw7_idx;
+	u32 emc_trefbw_idx;
+	u32 emc_odt_write_idx;
+	u32 emc_fbio_cfg5_idx;
+	u32 emc_fbio_cfg7_idx;
+	u32 emc_cfg_dig_dll_idx;
+	u32 emc_cfg_dig_dll_period_idx;
+	u32 emc_pmacro_ib_rxrt_idx;
+	u32 emc_cfg_pipe_1_idx;
+	u32 emc_cfg_pipe_2_idx;
+	u32 emc_pmacro_quse_ddll_rank0_4_idx;
+	u32 emc_pmacro_quse_ddll_rank0_5_idx;
+	u32 emc_pmacro_quse_ddll_rank1_4_idx;
+	u32 emc_pmacro_quse_ddll_rank1_5_idx;
+	u32 emc_mrw8_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank1_4_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank1_5_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank0_0_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank0_1_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank0_2_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank0_3_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank0_4_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank0_5_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank1_0_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank1_1_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank1_2_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank1_3_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank1_4_idx;
+	u32 emc_pmacro_ob_ddll_long_dqs_rank1_5_idx;
+	u32 emc_pmacro_ddll_long_cmd_0_idx;
+	u32 emc_pmacro_ddll_long_cmd_1_idx;
+	u32 emc_pmacro_ddll_long_cmd_2_idx;
+	u32 emc_pmacro_ddll_long_cmd_3_idx;
+	u32 emc_pmacro_ddll_long_cmd_4_idx;
+	u32 emc_pmacro_ddll_short_cmd_0_idx;
+	u32 emc_pmacro_ddll_short_cmd_1_idx;
+	u32 emc_pmacro_ddll_short_cmd_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte0_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte1_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte2_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte3_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte4_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte5_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte6_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte7_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd0_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd1_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd2_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd3_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte0_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte1_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte2_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte3_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte4_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte5_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte6_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte7_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd0_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd0_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd0_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd0_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd1_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd1_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd1_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd1_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd2_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd2_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd2_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd2_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd3_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd3_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd3_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_cmd3_3_idx;
+	u32 emc_txdsrvttgen_idx;
+	u32 emc_fdpd_ctrl_dq_idx;
+	u32 emc_fdpd_ctrl_cmd_idx;
+	u32 emc_fbio_spare_idx;
+	u32 emc_zcal_interval_idx;
+	u32 emc_zcal_wait_cnt_idx;
+	u32 emc_mrs_wait_cnt_idx;
+	u32 emc_mrs_wait_cnt2_idx;
+	u32 emc_auto_cal_channel_idx;
+	u32 emc_dll_cfg_0_idx;
+	u32 emc_dll_cfg_1_idx;
+	u32 emc_pmacro_autocal_cfg_common_idx;
+	u32 emc_pmacro_zctrl_idx;
+	u32 emc_cfg_idx;
+	u32 emc_cfg_pipe_idx;
+	u32 emc_dyn_self_ref_control_idx;
+	u32 emc_qpop_idx;
+	u32 emc_dqs_brlshft_0_idx;
+	u32 emc_dqs_brlshft_1_idx;
+	u32 emc_cmd_brlshft_2_idx;
+	u32 emc_cmd_brlshft_3_idx;
+	u32 emc_pmacro_pad_cfg_ctrl_idx;
+	u32 emc_pmacro_data_pad_rx_ctrl_idx;
+	u32 emc_pmacro_cmd_pad_rx_ctrl_idx;
+	u32 emc_pmacro_data_rx_term_mode_idx;
+	u32 emc_pmacro_cmd_rx_term_mode_idx;
+	u32 emc_pmacro_cmd_pad_tx_ctrl_idx;
+	u32 emc_pmacro_data_pad_tx_ctrl_idx;
+	u32 emc_pmacro_common_pad_tx_ctrl_idx;
+	u32 emc_pmacro_vttgen_ctrl_0_idx;
+	u32 emc_pmacro_vttgen_ctrl_1_idx;
+	u32 emc_pmacro_vttgen_ctrl_2_idx;
+	u32 emc_pmacro_brick_ctrl_rfu1_idx;
+	u32 emc_pmacro_cmd_brick_ctrl_fdpd_idx;
+	u32 emc_pmacro_brick_ctrl_rfu2_idx;
+	u32 emc_pmacro_data_brick_ctrl_fdpd_idx;
+	u32 emc_pmacro_bg_bias_ctrl_0_idx;
+	u32 emc_cfg_3_idx;
+	u32 emc_pmacro_tx_pwrd_0_idx;
+	u32 emc_pmacro_tx_pwrd_1_idx;
+	u32 emc_pmacro_tx_pwrd_2_idx;
+	u32 emc_pmacro_tx_pwrd_3_idx;
+	u32 emc_pmacro_tx_pwrd_4_idx;
+	u32 emc_pmacro_tx_pwrd_5_idx;
+	u32 emc_config_sample_delay_idx;
+	u32 emc_pmacro_tx_sel_clk_src_0_idx;
+	u32 emc_pmacro_tx_sel_clk_src_1_idx;
+	u32 emc_pmacro_tx_sel_clk_src_2_idx;
+	u32 emc_pmacro_tx_sel_clk_src_3_idx;
+	u32 emc_pmacro_tx_sel_clk_src_4_idx;
+	u32 emc_pmacro_tx_sel_clk_src_5_idx;
+	u32 emc_pmacro_ddll_bypass_idx;
+	u32 emc_pmacro_ddll_pwrd_0_idx;
+	u32 emc_pmacro_ddll_pwrd_1_idx;
+	u32 emc_pmacro_ddll_pwrd_2_idx;
+	u32 emc_pmacro_cmd_ctrl_0_idx;
+	u32 emc_pmacro_cmd_ctrl_1_idx;
+	u32 emc_pmacro_cmd_ctrl_2_idx;
+	u32 emc_tr_timing_0_idx;
+	u32 emc_tr_dvfs_idx;
+	u32 emc_tr_ctrl_1_idx;
+	u32 emc_tr_rdv_idx;
+	u32 emc_tr_qpop_idx;
+	u32 emc_tr_rdv_mask_idx;
+	u32 emc_mrw14_idx;
+	u32 emc_tr_qsafe_idx;
+	u32 emc_tr_qrst_idx;
+	u32 emc_training_ctrl_idx;
+	u32 emc_training_settle_idx;
+	u32 emc_training_vref_settle_idx;
+	u32 emc_training_ca_fine_ctrl_idx;
+	u32 emc_training_ca_ctrl_misc_idx;
+	u32 emc_training_ca_ctrl_misc1_idx;
+	u32 emc_training_ca_vref_ctrl_idx;
+	u32 emc_training_quse_cors_ctrl_idx;
+	u32 emc_training_quse_fine_ctrl_idx;
+	u32 emc_training_quse_ctrl_misc_idx;
+	u32 emc_training_quse_vref_ctrl_idx;
+	u32 emc_training_read_fine_ctrl_idx;
+	u32 emc_training_read_ctrl_misc_idx;
+	u32 emc_training_read_vref_ctrl_idx;
+	u32 emc_training_write_fine_ctrl_idx;
+	u32 emc_training_write_ctrl_misc_idx;
+	u32 emc_training_write_vref_ctrl_idx;
+	u32 emc_training_mpc_idx;
+	u32 emc_mrw15_idx;
+} burst_regs_t;
+
+
+typedef struct
+{
+	u32 burst_regs[221];
+	u32 burst_reg_per_ch[8];
+	u32 shadow_regs_ca_train[221];
+	u32 shadow_regs_quse_train[221];
+	u32 shadow_regs_rdwr_train[221];
+} burst_regs_table_t;
+
+typedef struct
+{
+	u32 ptfv_dqsosc_movavg_c0d0u0_idx;
+	u32 ptfv_dqsosc_movavg_c0d0u1_idx;
+	u32 ptfv_dqsosc_movavg_c0d1u0_idx;
+	u32 ptfv_dqsosc_movavg_c0d1u1_idx;
+	u32 ptfv_dqsosc_movavg_c1d0u0_idx;
+	u32 ptfv_dqsosc_movavg_c1d0u1_idx;
+	u32 ptfv_dqsosc_movavg_c1d1u0_idx;
+	u32 ptfv_dqsosc_movavg_c1d1u1_idx;
+	u32 ptfv_write_samples_idx;
+	u32 ptfv_dvfs_samples_idx;
+	u32 ptfv_movavg_weight_idx;
+	u32 ptfv_config_ctrl_idx;
+} ptfv_list_table_t;
+
+typedef struct
+{
+	u32 emc0_mrw10_idx;
+	u32 emc1_mrw10_idx;
+	u32 emc0_mrw11_idx;
+	u32 emc1_mrw11_idx;
+	u32 emc0_mrw12_idx;
+	u32 emc1_mrw12_idx;
+	u32 emc0_mrw13_idx;
+	u32 emc1_mrw13_idx;
+} burst_reg_per_ch_t;
+
+typedef struct
+{
+	u32 emc_pmacro_ib_ddll_long_dqs_rank0_0_idx;
+	u32 emc_pmacro_ib_ddll_long_dqs_rank0_1_idx;
+	u32 emc_pmacro_ib_ddll_long_dqs_rank0_2_idx;
+	u32 emc_pmacro_ib_ddll_long_dqs_rank0_3_idx;
+	u32 emc_pmacro_ib_ddll_long_dqs_rank1_0_idx;
+	u32 emc_pmacro_ib_ddll_long_dqs_rank1_1_idx;
+	u32 emc_pmacro_ib_ddll_long_dqs_rank1_2_idx;
+	u32 emc_pmacro_ib_ddll_long_dqs_rank1_3_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte0_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte0_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte0_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte1_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte1_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte1_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte2_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte2_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte2_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte3_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte3_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte3_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte4_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte4_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte4_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte5_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte5_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte5_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte6_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte6_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte6_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte7_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte7_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank0_byte7_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte0_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte0_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte0_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte1_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte1_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte1_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte2_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte2_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte2_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte3_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte3_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte3_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte4_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte4_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte4_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte5_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte5_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte5_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte6_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte6_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte6_2_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte7_0_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte7_1_idx;
+	u32 emc_pmacro_ib_ddll_short_dq_rank1_byte7_2_idx;
+	u32 emc_pmacro_ib_vref_dqs_0_idx;
+	u32 emc_pmacro_ib_vref_dqs_1_idx;
+	u32 emc_pmacro_ib_vref_dq_0_idx;
+	u32 emc_pmacro_ib_vref_dq_1_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank0_0_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank0_1_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank0_2_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank0_3_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank0_4_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank0_5_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank1_0_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank1_1_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank1_2_idx;
+	u32 emc_pmacro_ob_ddll_long_dq_rank1_3_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte0_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte0_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte0_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte1_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte1_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte1_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte2_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte2_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte2_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte3_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte3_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte3_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte4_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte4_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte4_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte5_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte5_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte5_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte6_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte6_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte6_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte7_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte7_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_byte7_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd0_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd0_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd0_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd1_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd1_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd1_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd2_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd2_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd2_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd3_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd3_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank0_cmd3_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte0_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte0_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte0_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte1_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte1_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte1_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte2_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte2_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte2_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte3_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte3_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte3_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte4_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte4_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte4_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte5_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte5_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte5_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte6_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte6_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte6_2_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte7_0_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte7_1_idx;
+	u32 emc_pmacro_ob_ddll_short_dq_rank1_byte7_2_idx;
+	u32 emc_pmacro_quse_ddll_rank0_0_idx;
+	u32 emc_pmacro_quse_ddll_rank0_1_idx;
+	u32 emc_pmacro_quse_ddll_rank0_2_idx;
+	u32 emc_pmacro_quse_ddll_rank0_3_idx;
+	u32 emc_pmacro_quse_ddll_rank1_0_idx;
+	u32 emc_pmacro_quse_ddll_rank1_1_idx;
+	u32 emc_pmacro_quse_ddll_rank1_2_idx;
+	u32 emc_pmacro_quse_ddll_rank1_3_idx;
+} trim_regs_t;
+
+typedef struct
+{
+	u32 emc_cmd_brlshft_0_idx;
+	u32 emc_cmd_brlshft_1_idx;
+	u32 emc0_data_brlshft_0_idx;
+	u32 emc1_data_brlshft_0_idx;
+	u32 emc0_data_brlshft_1_idx;
+	u32 emc1_data_brlshft_1_idx;
+	u32 emc_quse_brlshft_0_idx;
+	u32 emc_quse_brlshft_1_idx;
+	u32 emc_quse_brlshft_2_idx;
+	u32 emc_quse_brlshft_3_idx;
+} trim_perch_regs_t;
+
+typedef struct
+{
+	u32 t_rp;
+	u32 t_fc_lpddr4;
+	u32 t_rfc;
+	u32 t_pdex;
+	u32 rl;
+} dram_timings_t;
+
+typedef struct
+{
+	u32 emc0_training_opt_dqs_ib_vref_rank0_idx;
+	u32 emc1_training_opt_dqs_ib_vref_rank0_idx;
+	u32 emc0_training_opt_dqs_ib_vref_rank1_idx;
+	u32 emc1_training_opt_dqs_ib_vref_rank1_idx;
+} vref_perch_regs_t;
+
+typedef struct
+{
+	u32 trim_regs[138];
+	u32 trim_perch_regs[10];
+	u32 vref_perch_regs[4];
+} trim_regs_table_t;
+
+typedef struct
+{
+	u32 rev;
+	char dvfs_ver[60];
+	u32 rate_khz;
+	u32 min_volt;
+	u32 gpu_min_volt;
+	char clock_src[32];
+	u32 clk_src_emc;
+	u32 needs_training;
+	u32 training_pattern;
+	u32 trained;
+	u32 periodic_training;
+	u32 trained_dram_clktree_c0d0u0;
+	u32 trained_dram_clktree_c0d0u1;
+	u32 trained_dram_clktree_c0d1u0;
+	u32 trained_dram_clktree_c0d1u1;
+	u32 trained_dram_clktree_c1d0u0;
+	u32 trained_dram_clktree_c1d0u1;
+	u32 trained_dram_clktree_c1d1u0;
+	u32 trained_dram_clktree_c1d1u1;
+	u32 current_dram_clktree_c0d0u0;
+	u32 current_dram_clktree_c0d0u1;
+	u32 current_dram_clktree_c0d1u0;
+	u32 current_dram_clktree_c0d1u1;
+	u32 current_dram_clktree_c1d0u0;
+	u32 current_dram_clktree_c1d0u1;
+	u32 current_dram_clktree_c1d1u0;
+	u32 current_dram_clktree_c1d1u1;
+	u32 run_clocks;
+	u32 tree_margin;
+	u32 num_burst;
+	u32 num_burst_per_ch;
+	u32 num_trim;
+	u32 num_trim_per_ch;
+	u32 num_mc_regs;
+	u32 num_up_down;
+	u32 vref_num;
+	u32 training_mod_num;
+	u32 dram_timing_num;
+
+	ptfv_list_table_t ptfv_list;
+
+	burst_regs_t burst_regs;
+	burst_reg_per_ch_t burst_reg_per_ch;
+	burst_regs_t shadow_regs_ca_train;
+	burst_regs_t shadow_regs_quse_train;
+	burst_regs_t shadow_regs_rdwr_train;
+	trim_regs_t trim_regs;
+	trim_perch_regs_t trim_perch_regs;
+	vref_perch_regs_t vref_perch_regs;
+	dram_timings_t dram_timings;
+
+	u32 training_mod_regs[20];
+	u32 save_restore_mod_regs[12];
+	u32 burst_mc_regs[33];
+	u32 la_scale_regs[24];
+
+	u32 min_mrs_wait;
+	u32 emc_mrw;
+	u32 emc_mrw2;
+	u32 emc_mrw3;
+	u32 emc_mrw4;
+	u32 emc_mrw9;
+	u32 emc_mrs;
+	u32 emc_emrs;
+	u32 emc_emrs2;
+	u32 emc_auto_cal_config;
+	u32 emc_auto_cal_config2;
+	u32 emc_auto_cal_config3;
+	u32 emc_auto_cal_config4;
+	u32 emc_auto_cal_config5;
+	u32 emc_auto_cal_config6;
+	u32 emc_auto_cal_config7;
+	u32 emc_auto_cal_config8;
+	u32 emc_cfg_2;
+	u32 emc_sel_dpd_ctrl;
+	u32 emc_fdpd_ctrl_cmd_no_ramp;
+	u32 dll_clk_src;
+	u32 clk_out_enb_x_0_clk_enb_emc_dll;
+	u32 latency;
+} emc_table_t;
+
+#endif

source/mem/sdram.c

@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2018 naehrwert
  * Copyright (c) 2018 balika011
- * Copyright (c) 2019 CTCaer
+ * Copyright (c) 2019-2020 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,
@@ -16,19 +16,22 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "../soc/i2c.h"
-#include "../soc/t210.h"
+#include <string.h>
+
 #include "mc.h"
 #include "emc.h"
 #include "sdram_param_t210.h"
-#include "../soc/pmc.h"
-#include "../utils/util.h"
-#include "../soc/fuse.h"
+#include "../../common/memory_map.h"
 #include "../power/max77620.h"
 #include "../power/max7762x.h"
 #include "../soc/clock.h"
+#include "../soc/fuse.h"
+#include "../soc/i2c.h"
+#include "../soc/pmc.h"
+#include "../soc/t210.h"
+#include "../utils/util.h"
 
-#define CONFIG_SDRAM_COMPRESS_CFG
+#define CONFIG_SDRAM_KEEP_ALIVE
 
 #ifdef CONFIG_SDRAM_COMPRESS_CFG
 #include "../libs/compr/lz.h"
@@ -39,25 +42,91 @@
 
 static u32 _get_sdram_id()
 {
-	return (fuse_read_odm(4) & 0x38) >> 3;
+	return ((fuse_read_odm(4) & 0x38) >> 3);
+}
+
+static bool _sdram_wait_emc_status(u32 reg_offset, u32 bit_mask, bool updated_state, s32 emc_channel)
+{
+	bool err = true;
+
+	for (s32 i = 0; i < EMC_STATUS_UPDATE_TIMEOUT; i++)
+	{
+		if (emc_channel)
+		{
+			if (emc_channel != 1)
+				goto done;
+
+			if (((EMC_CH1(reg_offset) & bit_mask) != 0) == updated_state)
+			{
+				err = false;
+				break;
+			}
+		}
+		else if (((EMC(reg_offset) & bit_mask) != 0) == updated_state)
+		{
+			err = false;
+			break;
+		}
+		usleep(1);
+	}
+
+done:
+	return err;
+}
+
+static void _sdram_req_mrr_data(u32 data, bool dual_channel)
+{
+	EMC(EMC_MRR) = data;
+	_sdram_wait_emc_status(EMC_EMC_STATUS, EMC_STATUS_MRR_DIVLD, true, EMC_CHAN0);
+	if (dual_channel)
+		_sdram_wait_emc_status(EMC_EMC_STATUS, EMC_STATUS_MRR_DIVLD, true, EMC_CHAN1);
+}
+
+emc_mr_data_t sdram_read_mrx(emc_mr_t mrx)
+{
+	emc_mr_data_t data;
+	_sdram_req_mrr_data((1 << 31) | (mrx << 16), EMC_CHAN0);
+	data.dev0_ch0 = EMC(EMC_MRR) & 0xFF;
+	data.dev0_ch1 = (EMC(EMC_MRR) & 0xFF00 >> 8);
+	_sdram_req_mrr_data((1 << 30) | (mrx << 16), EMC_CHAN1);
+	data.dev1_ch0 = EMC(EMC_MRR) & 0xFF;
+	data.dev1_ch1 = (EMC(EMC_MRR) & 0xFF00 >> 8);
+
+	return data;
 }
 
 static void _sdram_config(const sdram_params_t *params)
 {
-	PMC(APBDEV_PMC_IO_DPD3_REQ) = (((4 * params->emc_pmc_scratch1 >> 2) + 0x80000000) ^ 0xFFFF) & 0xC000FFFF;
+	// Program DPD3/DPD4 regs (coldboot path).
+	// Enable sel_dpd on unused pins.
+	u32 dpd_req = (params->emc_pmc_scratch1 & 0x3FFFFFFF) | 0x80000000;
+	PMC(APBDEV_PMC_IO_DPD3_REQ) = (dpd_req ^ 0xFFFF) & 0xC000FFFF;
 	usleep(params->pmc_io_dpd3_req_wait);
 
-	u32 req = (4 * params->emc_pmc_scratch2 >> 2) + 0x80000000;
-	PMC(APBDEV_PMC_IO_DPD4_REQ) = (req >> 16 << 16) ^ 0x3FFF0000;
+	// Disable e_dpd_vttgen.
+	dpd_req = (params->emc_pmc_scratch2 & 0x3FFFFFFF) | 0x80000000;
+	PMC(APBDEV_PMC_IO_DPD4_REQ) = (dpd_req & 0xFFFF0000) ^ 0x3FFF0000;
 	usleep(params->pmc_io_dpd4_req_wait);
-	PMC(APBDEV_PMC_IO_DPD4_REQ) = (req ^ 0xFFFF) & 0xC000FFFF;
+
+	// Disable e_dpd_bg.
+	PMC(APBDEV_PMC_IO_DPD4_REQ) = (dpd_req ^ 0xFFFF) & 0xC000FFFF;
 	usleep(params->pmc_io_dpd4_req_wait);
+
 	PMC(APBDEV_PMC_WEAK_BIAS) = 0;
 	usleep(1);
 
+	// Start clocks.
 	CLOCK(CLK_RST_CONTROLLER_PLLM_MISC1) = params->pllm_setup_control;
 	CLOCK(CLK_RST_CONTROLLER_PLLM_MISC2) = 0;
-	CLOCK(CLK_RST_CONTROLLER_PLLM_BASE) = (params->pllm_feedback_divider << 8) | params->pllm_input_divider | 0x40000000 | ((params->pllm_post_divider & 0xFFFF) << 20);
+
+#ifdef CONFIG_SDRAM_KEEP_ALIVE
+	CLOCK(CLK_RST_CONTROLLER_PLLM_BASE) =
+		(params->pllm_feedback_divider << 8) | params->pllm_input_divider | ((params->pllm_post_divider & 0xFFFF) << 20) | PLLCX_BASE_ENABLE;
+#else
+	u32 pllm_div = (params->pllm_feedback_divider << 8) | params->pllm_input_divider | ((params->pllm_post_divider & 0xFFFF) << 20);
+	CLOCK(CLK_RST_CONTROLLER_PLLM_BASE) = pllm_div;
+	CLOCK(CLK_RST_CONTROLLER_PLLM_BASE) = pllm_div | PLLCX_BASE_ENABLE;
+#endif
 
 	u32 wait_end = get_tmr_us() + 300;
 	while (!(CLOCK(CLK_RST_CONTROLLER_PLLM_BASE) & 0x8000000))
@@ -66,24 +135,35 @@ static void _sdram_config(const sdram_params_t *params)
 			goto break_nosleep;
 	}
 	usleep(10);
-break_nosleep:
 
+break_nosleep:
 	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) = ((params->mc_emem_arb_misc0 >> 11) & 0x10000) | (params->emc_clock_source & 0xFFFEFFFF);
 	if (params->emc_clock_source_dll)
 		CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC_DLL) = params->emc_clock_source_dll;
 	if (params->clear_clock2_mc1)
-		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_W_CLR) = 0x40000000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = 0x2000001;
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) = 0x4000;
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_CLR) = 0x2000001;
+		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_W_CLR) = 0x40000000; // Clear Reset to MC1.
+
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = 0x2000001; // Enable EMC and MEM clocks.
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) = 0x4000;    // Enable EMC_DLL clock.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_CLR) = 0x2000001; // Clear EMC and MEM resets.
+
+	// Set pad macros.
 	EMC(EMC_PMACRO_VTTGEN_CTRL_0) = params->emc_pmacro_vttgen_ctrl0;
 	EMC(EMC_PMACRO_VTTGEN_CTRL_1) = params->emc_pmacro_vttgen_ctrl1;
 	EMC(EMC_PMACRO_VTTGEN_CTRL_2) = params->emc_pmacro_vttgen_ctrl2;
-	EMC(EMC_TIMING_CONTROL) = 1;
-	usleep(1);
+
+	EMC(EMC_TIMING_CONTROL) = 1; // Trigger timing update so above writes take place.
+	usleep(10); // Ensure the regulators settle.
+
+	// Select EMC write mux.
 	EMC(EMC_DBG) = (params->emc_dbg_write_mux << 1) | params->emc_dbg;
+
+	// Patch 2 using BCT spare variables.
 	if (params->emc_bct_spare2)
 		*(vu32 *)params->emc_bct_spare2 = params->emc_bct_spare3;
+
+	// Program CMD mapping. Required before brick mapping, else
+	// we can't guarantee CK will be differential at all times.
 	EMC(EMC_FBIO_CFG7) = params->emc_fbio_cfg7;
 	EMC(EMC_CMD_MAPPING_CMD0_0) = params->emc_cmd_mapping_cmd0_0;
 	EMC(EMC_CMD_MAPPING_CMD0_1) = params->emc_cmd_mapping_cmd0_1;
@@ -98,25 +178,40 @@ break_nosleep:
 	EMC(EMC_CMD_MAPPING_CMD3_1) = params->emc_cmd_mapping_cmd3_1;
 	EMC(EMC_CMD_MAPPING_CMD3_2) = params->emc_cmd_mapping_cmd3_2;
 	EMC(EMC_CMD_MAPPING_BYTE) = params->emc_cmd_mapping_byte;
+
+	// Program brick mapping.
 	EMC(EMC_PMACRO_BRICK_MAPPING_0) = params->emc_pmacro_brick_mapping0;
 	EMC(EMC_PMACRO_BRICK_MAPPING_1) = params->emc_pmacro_brick_mapping1;
 	EMC(EMC_PMACRO_BRICK_MAPPING_2) = params->emc_pmacro_brick_mapping2;
+
 	EMC(EMC_PMACRO_BRICK_CTRL_RFU1) = (params->emc_pmacro_brick_ctrl_rfu1 & 0x1120112) | 0x1EED1EED;
+
+	// This is required to do any reads from the pad macros.
 	EMC(EMC_CONFIG_SAMPLE_DELAY) = params->emc_config_sample_delay;
+
 	EMC(EMC_FBIO_CFG8) = params->emc_fbio_cfg8;
+
+	// Set swizzle for Rank 0.
 	EMC(EMC_SWIZZLE_RANK0_BYTE0) = params->emc_swizzle_rank0_byte0;
 	EMC(EMC_SWIZZLE_RANK0_BYTE1) = params->emc_swizzle_rank0_byte1;
 	EMC(EMC_SWIZZLE_RANK0_BYTE2) = params->emc_swizzle_rank0_byte2;
 	EMC(EMC_SWIZZLE_RANK0_BYTE3) = params->emc_swizzle_rank0_byte3;
+	// Set swizzle for Rank 1.
 	EMC(EMC_SWIZZLE_RANK1_BYTE0) = params->emc_swizzle_rank1_byte0;
 	EMC(EMC_SWIZZLE_RANK1_BYTE1) = params->emc_swizzle_rank1_byte1;
 	EMC(EMC_SWIZZLE_RANK1_BYTE2) = params->emc_swizzle_rank1_byte2;
 	EMC(EMC_SWIZZLE_RANK1_BYTE3) = params->emc_swizzle_rank1_byte3;
+
+	// Patch 4 using BCT spare variables.
 	if (params->emc_bct_spare6)
 		*(vu32 *)params->emc_bct_spare6 = params->emc_bct_spare7;
+
+	// Set pad controls.
 	EMC(EMC_XM2COMPPADCTRL) = params->emc_xm2_comp_pad_ctrl;
 	EMC(EMC_XM2COMPPADCTRL2) = params->emc_xm2_comp_pad_ctrl2;
 	EMC(EMC_XM2COMPPADCTRL3) = params->emc_xm2_comp_pad_ctrl3;
+
+	// Program Autocal controls with shadowed register fields.
 	EMC(EMC_AUTO_CAL_CONFIG2) = params->emc_auto_cal_config2;
 	EMC(EMC_AUTO_CAL_CONFIG3) = params->emc_auto_cal_config3;
 	EMC(EMC_AUTO_CAL_CONFIG4) = params->emc_auto_cal_config4;
@@ -124,6 +219,7 @@ break_nosleep:
 	EMC(EMC_AUTO_CAL_CONFIG6) = params->emc_auto_cal_config6;
 	EMC(EMC_AUTO_CAL_CONFIG7) = params->emc_auto_cal_config7;
 	EMC(EMC_AUTO_CAL_CONFIG8) = params->emc_auto_cal_config8;
+
 	EMC(EMC_PMACRO_RX_TERM) = params->emc_pmacro_rx_term;
 	EMC(EMC_PMACRO_DQ_TX_DRV) = params->emc_pmacro_dq_tx_drive;
 	EMC(EMC_PMACRO_CA_TX_DRV) = params->emc_pmacro_ca_tx_drive;
@@ -131,9 +227,11 @@ break_nosleep:
 	EMC(EMC_PMACRO_AUTOCAL_CFG_COMMON) = params->emc_pmacro_auto_cal_common;
 	EMC(EMC_AUTO_CAL_CHANNEL) = params->emc_auto_cal_channel;
 	EMC(EMC_PMACRO_ZCTRL) = params->emc_pmacro_zcrtl;
+
 	EMC(EMC_DLL_CFG_0) = params->emc_dll_cfg0;
 	EMC(EMC_DLL_CFG_1) = params->emc_dll_cfg1;
 	EMC(EMC_CFG_DIG_DLL_1) = params->emc_cfg_dig_dll_1;
+
 	EMC(EMC_DATA_BRLSHFT_0) = params->emc_data_brlshft0;
 	EMC(EMC_DATA_BRLSHFT_1) = params->emc_data_brlshft1;
 	EMC(EMC_DQS_BRLSHFT_0) = params->emc_dqs_brlshft0;
@@ -146,8 +244,10 @@ break_nosleep:
 	EMC(EMC_QUSE_BRLSHFT_1) = params->emc_quse_brlshft1;
 	EMC(EMC_QUSE_BRLSHFT_2) = params->emc_quse_brlshft2;
 	EMC(EMC_QUSE_BRLSHFT_3) = params->emc_quse_brlshft3;
+
 	EMC(EMC_PMACRO_BRICK_CTRL_RFU1) = (params->emc_pmacro_brick_ctrl_rfu1 & 0x1BF01BF) | 0x1E401E40;
 	EMC(EMC_PMACRO_PAD_CFG_CTRL) = params->emc_pmacro_pad_cfg_ctrl;
+
 	EMC(EMC_PMACRO_CMD_BRICK_CTRL_FDPD) = params->emc_pmacro_cmd_brick_ctrl_fdpd;
 	EMC(EMC_PMACRO_BRICK_CTRL_RFU2) = params->emc_pmacro_brick_ctrl_rfu2 & 0xFF7FFF7F;
 	EMC(EMC_PMACRO_DATA_BRICK_CTRL_FDPD) = params->emc_pmacro_data_brick_ctrl_fdpd;
@@ -158,6 +258,7 @@ break_nosleep:
 	EMC(EMC_PMACRO_DATA_RX_TERM_MODE) = params->emc_pmacro_data_rx_term_mode;
 	EMC(EMC_PMACRO_CMD_RX_TERM_MODE) = params->emc_pmacro_cmd_rx_term_mode;
 	EMC(EMC_PMACRO_CMD_PAD_TX_CTRL) = params->emc_pmacro_cmd_pad_tx_ctrl;
+
 	EMC(EMC_CFG_3) = params->emc_cfg3;
 	EMC(EMC_PMACRO_TX_PWRD_0) = params->emc_pmacro_tx_pwrd0;
 	EMC(EMC_PMACRO_TX_PWRD_1) = params->emc_pmacro_tx_pwrd1;
@@ -183,6 +284,7 @@ break_nosleep:
 	EMC(EMC_PMACRO_IB_VREF_DQS_0) = params->emc_pmacro_ib_vref_dqs_0;
 	EMC(EMC_PMACRO_IB_VREF_DQS_1) = params->emc_pmacro_ib_vref_dqs_1;
 	EMC(EMC_PMACRO_IB_RXRT) = params->emc_pmacro_ib_rxrt;
+
 	EMC(EMC_PMACRO_QUSE_DDLL_RANK0_0) = params->emc_pmacro_quse_ddll_rank0_0;
 	EMC(EMC_PMACRO_QUSE_DDLL_RANK0_1) = params->emc_pmacro_quse_ddll_rank0_1;
 	EMC(EMC_PMACRO_QUSE_DDLL_RANK0_2) = params->emc_pmacro_quse_ddll_rank0_2;
@@ -196,6 +298,7 @@ break_nosleep:
 	EMC(EMC_PMACRO_QUSE_DDLL_RANK1_4) = params->emc_pmacro_quse_ddll_rank1_4;
 	EMC(EMC_PMACRO_QUSE_DDLL_RANK1_5) = params->emc_pmacro_quse_ddll_rank1_5;
 	EMC(EMC_PMACRO_BRICK_CTRL_RFU1) = params->emc_pmacro_brick_ctrl_rfu1;
+
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQ_RANK0_0) = params->emc_pmacro_ob_ddll_long_dq_rank0_0;
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQ_RANK0_1) = params->emc_pmacro_ob_ddll_long_dq_rank0_1;
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQ_RANK0_2) = params->emc_pmacro_ob_ddll_long_dq_rank0_2;
@@ -208,6 +311,7 @@ break_nosleep:
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQ_RANK1_3) = params->emc_pmacro_ob_ddll_long_dq_rank1_3;
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQ_RANK1_4) = params->emc_pmacro_ob_ddll_long_dq_rank1_4;
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQ_RANK1_5) = params->emc_pmacro_ob_ddll_long_dq_rank1_5;
+
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQS_RANK0_0) = params->emc_pmacro_ob_ddll_long_dqs_rank0_0;
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQS_RANK0_1) = params->emc_pmacro_ob_ddll_long_dqs_rank0_1;
 	EMC(EMC_PMACRO_OB_DDLL_LONG_DQS_RANK0_2) = params->emc_pmacro_ob_ddll_long_dqs_rank0_2;
@@ -228,6 +332,7 @@ break_nosleep:
 	EMC(EMC_PMACRO_IB_DDLL_LONG_DQS_RANK1_1) = params->emc_pmacro_ib_ddll_long_dqs_rank1_1;
 	EMC(EMC_PMACRO_IB_DDLL_LONG_DQS_RANK1_2) = params->emc_pmacro_ib_ddll_long_dqs_rank1_2;
 	EMC(EMC_PMACRO_IB_DDLL_LONG_DQS_RANK1_3) = params->emc_pmacro_ib_ddll_long_dqs_rank1_3;
+
 	EMC(EMC_PMACRO_DDLL_LONG_CMD_0) = params->emc_pmacro_ddll_long_cmd_0;
 	EMC(EMC_PMACRO_DDLL_LONG_CMD_1) = params->emc_pmacro_ddll_long_cmd_1;
 	EMC(EMC_PMACRO_DDLL_LONG_CMD_2) = params->emc_pmacro_ddll_long_cmd_2;
@@ -236,10 +341,17 @@ break_nosleep:
 	EMC(EMC_PMACRO_DDLL_SHORT_CMD_0) = params->emc_pmacro_ddll_short_cmd_0;
 	EMC(EMC_PMACRO_DDLL_SHORT_CMD_1) = params->emc_pmacro_ddll_short_cmd_1;
 	EMC(EMC_PMACRO_DDLL_SHORT_CMD_2) = params->emc_pmacro_ddll_short_cmd_2;
+
+	// Common pad macro (cpm).
 	EMC(EMC_PMACRO_COMMON_PAD_TX_CTRL) = (params->emc_pmacro_common_pad_tx_ctrl & 1) | 0xE;
+
+	// Patch 3 using BCT spare variables.
 	if (params->emc_bct_spare4)
 		*(vu32 *)params->emc_bct_spare4 = params->emc_bct_spare5;
-	EMC(EMC_TIMING_CONTROL) = 1;
+
+	EMC(EMC_TIMING_CONTROL) = 1; // Trigger timing update so above writes take place.
+
+	// Initialize MC VPR settings.
 	MC(MC_VIDEO_PROTECT_BOM) = params->mc_video_protect_bom;
 	MC(MC_VIDEO_PROTECT_BOM_ADR_HI) = params->mc_video_protect_bom_adr_hi;
 	MC(MC_VIDEO_PROTECT_SIZE_MB) = params->mc_video_protect_size_mb;
@@ -247,20 +359,32 @@ break_nosleep:
 	MC(MC_VIDEO_PROTECT_VPR_OVERRIDE1) = params->mc_video_protect_vpr_override1;
 	MC(MC_VIDEO_PROTECT_GPU_OVERRIDE_0) = params->mc_video_protect_gpu_override0;
 	MC(MC_VIDEO_PROTECT_GPU_OVERRIDE_1) = params->mc_video_protect_gpu_override1;
+
+	// Program SDRAM geometry parameters.
 	MC(MC_EMEM_ADR_CFG) = params->mc_emem_adr_cfg;
 	MC(MC_EMEM_ADR_CFG_DEV0) = params->mc_emem_adr_cfg_dev0;
 	MC(MC_EMEM_ADR_CFG_DEV1) = params->mc_emem_adr_cfg_dev1;
 	MC(MC_EMEM_ADR_CFG_CHANNEL_MASK) = params->mc_emem_adr_cfg_channel_mask;
+
+	// Program bank swizzling.
 	MC(MC_EMEM_ADR_CFG_BANK_MASK_0) = params->mc_emem_adr_cfg_bank_mask0;
 	MC(MC_EMEM_ADR_CFG_BANK_MASK_1) = params->mc_emem_adr_cfg_bank_mask1;
 	MC(MC_EMEM_ADR_CFG_BANK_MASK_2) = params->mc_emem_adr_cfg_bank_mask2;
+
+	// Program external memory aperture (base and size).
 	MC(MC_EMEM_CFG) = params->mc_emem_cfg;
+
+	// Program SEC carveout (base and size).
 	MC(MC_SEC_CARVEOUT_BOM) = params->mc_sec_carveout_bom;
 	MC(MC_SEC_CARVEOUT_ADR_HI) = params->mc_sec_carveout_adr_hi;
 	MC(MC_SEC_CARVEOUT_SIZE_MB) = params->mc_sec_carveout_size_mb;
+
+	// Program MTS carveout (base and size).
 	MC(MC_MTS_CARVEOUT_BOM) = params->mc_mts_carveout_bom;
 	MC(MC_MTS_CARVEOUT_ADR_HI) = params->mc_mts_carveout_adr_hi;
 	MC(MC_MTS_CARVEOUT_SIZE_MB) = params->mc_mts_carveout_size_mb;
+
+	// Program the memory arbiter.
 	MC(MC_EMEM_ARB_CFG) = params->mc_emem_arb_cfg;
 	MC(MC_EMEM_ARB_OUTSTANDING_REQ) = params->mc_emem_arb_outstanding_req;
 	MC(MC_EMEM_ARB_REFPB_HP_CTRL) = params->emc_emem_arb_refpb_hp_ctrl;
@@ -289,21 +413,38 @@ break_nosleep:
 	MC(MC_EMEM_ARB_OVERRIDE_1) = params->mc_emem_arb_override1;
 	MC(MC_EMEM_ARB_RSV) = params->mc_emem_arb_rsv;
 	MC(MC_DA_CONFIG0) = params->mc_da_cfg0;
-	MC(MC_TIMING_CONTROL) = 1;
+
+	MC(MC_TIMING_CONTROL) = 1; // Trigger MC timing update.
+
+	// Program second-level clock enable overrides.
 	MC(MC_CLKEN_OVERRIDE) = params->mc_clken_override;
+
+	// Program statistics gathering.
 	MC(MC_STAT_CONTROL) = params->mc_stat_control;
+
+	// Program SDRAM geometry parameters.
 	EMC(EMC_ADR_CFG) = params->emc_adr_cfg;
+
+	// Program second-level clock enable overrides.
 	EMC(EMC_CLKEN_OVERRIDE) = params->emc_clken_override;
+
+	// Program EMC pad auto calibration.
 	EMC(EMC_PMACRO_AUTOCAL_CFG_0) = params->emc_pmacro_auto_cal_cfg0;
 	EMC(EMC_PMACRO_AUTOCAL_CFG_1) = params->emc_pmacro_auto_cal_cfg1;
 	EMC(EMC_PMACRO_AUTOCAL_CFG_2) = params->emc_pmacro_auto_cal_cfg2;
+
 	EMC(EMC_AUTO_CAL_VREF_SEL_0) = params->emc_auto_cal_vref_sel0;
 	EMC(EMC_AUTO_CAL_VREF_SEL_1) = params->emc_auto_cal_vref_sel1;
+
 	EMC(EMC_AUTO_CAL_INTERVAL) = params->emc_auto_cal_interval;
 	EMC(EMC_AUTO_CAL_CONFIG) = params->emc_auto_cal_config;
 	usleep(params->emc_auto_cal_wait);
+
+	// Patch 5 using BCT spare variables.
 	if (params->emc_bct_spare8)
 		*(vu32 *)params->emc_bct_spare8 = params->emc_bct_spare9;
+
+	// Program EMC timing configuration.
 	EMC(EMC_CFG_2) = params->emc_cfg2;
 	EMC(EMC_CFG_PIPE) = params->emc_cfg_pipe;
 	EMC(EMC_CFG_PIPE_1) = params->emc_cfg_pipe1;
@@ -348,9 +489,11 @@ break_nosleep:
 	EMC(EMC_EINPUT_DURATION) = params->emc_einput_duration;
 	EMC(EMC_PUTERM_EXTRA) = params->emc_puterm_extra;
 	EMC(EMC_PUTERM_WIDTH) = params->emc_puterm_width;
+
 	EMC(EMC_PMACRO_COMMON_PAD_TX_CTRL) = params->emc_pmacro_common_pad_tx_ctrl;
 	EMC(EMC_DBG) = params->emc_dbg;
 	EMC(EMC_QRST) = params->emc_qrst;
+	EMC(EMC_ISSUE_QRST) = 1;
 	EMC(EMC_ISSUE_QRST) = 0;
 	EMC(EMC_QSAFE) = params->emc_qsafe;
 	EMC(EMC_RDV) = params->emc_rdv;
@@ -383,6 +526,8 @@ break_nosleep:
 	EMC(EMC_ODT_WRITE) = params->emc_odt_write;
 	EMC(EMC_CFG_DIG_DLL) = params->emc_cfg_dig_dll;
 	EMC(EMC_CFG_DIG_DLL_PERIOD) = params->emc_cfg_dig_dll_period;
+
+	// Don't write CFG_ADR_EN (bit 1) here - lock bit written later.
 	EMC(EMC_FBIO_SPARE) = params->emc_fbio_spare & 0xFFFFFFFD;
 	EMC(EMC_CFG_RSV) = params->emc_cfg_rsv;
 	EMC(EMC_PMC_SCRATCH1) = params->emc_pmc_scratch1;
@@ -390,70 +535,104 @@ break_nosleep:
 	EMC(EMC_PMC_SCRATCH3) = params->emc_pmc_scratch3;
 	EMC(EMC_ACPD_CONTROL) = params->emc_acpd_control;
 	EMC(EMC_TXDSRVTTGEN) = params->emc_txdsrvttgen;
+
+	// Set pipe bypass enable bits before sending any DRAM commands.
 	EMC(EMC_CFG) = (params->emc_cfg & 0xE) | 0x3C00000;
+
+	// Patch BootROM.
 	if (params->boot_rom_patch_control & (1 << 31))
 	{
 		*(vu32 *)(APB_MISC_BASE + params->boot_rom_patch_control * 4) = params->boot_rom_patch_data;
-		MC(MC_TIMING_CONTROL) = 1;
+		MC(MC_TIMING_CONTROL) = 1; // Trigger MC timing update.
 	}
-	PMC(APBDEV_PMC_IO_DPD3_REQ) = ((4 * params->emc_pmc_scratch1 >> 2) + 0x40000000) & 0xCFFF0000;
+
+	// Release SEL_DPD_CMD.
+	PMC(APBDEV_PMC_IO_DPD3_REQ) = ((params->emc_pmc_scratch1 & 0x3FFFFFFF) | 0x40000000) & 0xCFFF0000;
 	usleep(params->pmc_io_dpd3_req_wait);
+
+	// Set autocal interval if not configured.
 	if (!params->emc_auto_cal_interval)
 		EMC(EMC_AUTO_CAL_CONFIG) = params->emc_auto_cal_config | 0x200;
+
 	EMC(EMC_PMACRO_BRICK_CTRL_RFU2) = params->emc_pmacro_brick_ctrl_rfu2;
+
+	// ZQ CAL setup (not actually issuing ZQ CAL now).
 	if (params->emc_zcal_warm_cold_boot_enables & 1)
 	{
-		if (params->memory_type == 2)
-			EMC(EMC_ZCAL_WAIT_CNT) = 8 * params->emc_zcal_wait_cnt;
-		if (params->memory_type == 3)
+		if (params->memory_type == MEMORY_TYPE_DDR3L)
+			EMC(EMC_ZCAL_WAIT_CNT) = params->emc_zcal_wait_cnt << 3;
+		if (params->memory_type == MEMORY_TYPE_LPDDR4)
 		{
 			EMC(EMC_ZCAL_WAIT_CNT) = params->emc_zcal_wait_cnt;
 			EMC(EMC_ZCAL_MRW_CMD) = params->emc_zcal_mrw_cmd;
 		}
 	}
-	EMC(EMC_TIMING_CONTROL) = 1;
+
+	EMC(EMC_TIMING_CONTROL) = 1; // Trigger timing update so above writes take place.
 	usleep(params->emc_timing_control_wait);
+
+	// Deassert HOLD_CKE_LOW.
 	PMC(APBDEV_PMC_DDR_CNTRL) &= 0xFFF8007F;
 	usleep(params->pmc_ddr_ctrl_wait);
-	if (params->memory_type == 2)
+
+	// Set clock enable signal.
+	u32 pin_gpio_cfg = (params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12);
+	if (params->memory_type == MEMORY_TYPE_DDR3L || params->memory_type == MEMORY_TYPE_LPDDR4)
 	{
-		EMC(EMC_PIN) = (params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12);
+		EMC(EMC_PIN) = pin_gpio_cfg;
+		(void)EMC(EMC_PIN);
 		usleep(params->emc_pin_extra_wait + 200);
-		EMC(EMC_PIN) = ((params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12)) + 256;
-		usleep(params->emc_pin_extra_wait + 500);
+		EMC(EMC_PIN) = pin_gpio_cfg | 0x100;
+		(void)EMC(EMC_PIN);
 	}
-	if (params->memory_type == 3)
-	{
-		EMC(EMC_PIN) = (params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12);
-		usleep(params->emc_pin_extra_wait + 200);
-		EMC(EMC_PIN) = ((params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12)) + 256;
+
+	if (params->memory_type == MEMORY_TYPE_LPDDR4)
 		usleep(params->emc_pin_extra_wait + 2000);
-	}
-	EMC(EMC_PIN) = ((params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12)) + 0x101;
+	else if (params->memory_type == MEMORY_TYPE_DDR3L)
+		usleep(params->emc_pin_extra_wait + 500);
+
+	// Enable clock enable signal.
+	EMC(EMC_PIN) = pin_gpio_cfg | 0x101;
+	(void)EMC(EMC_PIN);
 	usleep(params->emc_pin_program_wait);
-	if (params->memory_type != 3)
+
+	// Send NOP (trigger just needs to be non-zero).
+	if (params->memory_type != MEMORY_TYPE_LPDDR4)
 		EMC(EMC_NOP) = (params->emc_dev_select << 30) + 1;
-	if (params->memory_type == 1)
+
+	// On coldboot w/LPDDR2/3, wait 200 uSec after asserting CKE high.
+	if (params->memory_type == MEMORY_TYPE_LPDDR2)
 		usleep(params->emc_pin_extra_wait + 200);
-	if (params->memory_type == 3)
+
+	// Init zq calibration,
+	if (params->memory_type == MEMORY_TYPE_LPDDR4)
 	{
+		// Patch 6 using BCT spare variables.
 		if (params->emc_bct_spare10)
 			*(vu32 *)params->emc_bct_spare10 = params->emc_bct_spare11;
+
+		// Write mode registers.
 		EMC(EMC_MRW2) = params->emc_mrw2;
 		EMC(EMC_MRW) = params->emc_mrw1;
 		EMC(EMC_MRW3) = params->emc_mrw3;
 		EMC(EMC_MRW4) = params->emc_mrw4;
 		EMC(EMC_MRW6) = params->emc_mrw6;
 		EMC(EMC_MRW14) = params->emc_mrw14;
+
 		EMC(EMC_MRW8) = params->emc_mrw8;
 		EMC(EMC_MRW12) = params->emc_mrw12;
 		EMC(EMC_MRW9) = params->emc_mrw9;
 		EMC(EMC_MRW13) = params->emc_mrw13;
+
 		if (params->emc_zcal_warm_cold_boot_enables & 1)
 		{
+			// Issue ZQCAL start, device 0.
 			EMC(EMC_ZQ_CAL) = params->emc_zcal_init_dev0;
 			usleep(params->emc_zcal_init_wait);
+
+			// Issue ZQCAL latch.
 			EMC(EMC_ZQ_CAL) = params->emc_zcal_init_dev0 ^ 3;
+			// Same for device 1.
 			if (!(params->emc_dev_select & 2))
 			{
 				EMC(EMC_ZQ_CAL) = params->emc_zcal_init_dev1;
@@ -462,46 +641,97 @@ break_nosleep:
 			}
 		}
 	}
+
+	// Set package and DPD pad control.
 	PMC(APBDEV_PMC_DDR_CFG) = params->pmc_ddr_cfg;
-	if (params->memory_type - 1 <= 2)
+
+	// Start periodic ZQ calibration (LPDDRx only).
+	if (params->memory_type && params->memory_type <= MEMORY_TYPE_LPDDR4)
 	{
 		EMC(EMC_ZCAL_INTERVAL) = params->emc_zcal_interval;
 		EMC(EMC_ZCAL_WAIT_CNT) = params->emc_zcal_wait_cnt;
 		EMC(EMC_ZCAL_MRW_CMD) = params->emc_zcal_mrw_cmd;
 	}
+
+	// Patch 7 using BCT spare variables.
 	if (params->emc_bct_spare12)
 		*(vu32 *)params->emc_bct_spare12 = params->emc_bct_spare13;
-	EMC(EMC_TIMING_CONTROL) = 1;
+
+	EMC(EMC_TIMING_CONTROL) = 1; // Trigger timing update so above writes take place.
+
 	if (params->emc_extra_refresh_num)
-		EMC(EMC_REF) = ((1 << params->emc_extra_refresh_num << 8) - 0xFD) | (params->emc_pin_gpio << 30);
+		EMC(EMC_REF) = (((1 << params->emc_extra_refresh_num) - 1) << 8) | (params->emc_dev_select << 30) | 3;
+
+	// Enable refresh.
 	EMC(EMC_REFCTRL) = params->emc_dev_select | 0x80000000;
+
 	EMC(EMC_DYN_SELF_REF_CONTROL) = params->emc_dyn_self_ref_control;
 	EMC(EMC_CFG_UPDATE) = params->emc_cfg_update;
 	EMC(EMC_CFG) = params->emc_cfg;
 	EMC(EMC_FDPD_CTRL_DQ) = params->emc_fdpd_ctrl_dq;
 	EMC(EMC_FDPD_CTRL_CMD) = params->emc_fdpd_ctrl_cmd;
 	EMC(EMC_SEL_DPD_CTRL) = params->emc_sel_dpd_ctrl;
+
+	// Write addr swizzle lock bit.
 	EMC(EMC_FBIO_SPARE) = params->emc_fbio_spare | 2;
-	EMC(EMC_TIMING_CONTROL) = 1;
+
+	EMC(EMC_TIMING_CONTROL) = 1; // Re-trigger timing to latch power saving functions.
+
+	// Enable EMC pipe clock gating.
 	EMC(EMC_CFG_PIPE_CLK) = params->emc_cfg_pipe_clk;
+
+	// Depending on freqency, enable CMD/CLK fdpd.
 	EMC(EMC_FDPD_CTRL_CMD_NO_RAMP) = params->emc_fdpd_ctrl_cmd_no_ramp;
-	SYSREG(AHB_ARBITRATION_XBAR_CTRL) = (SYSREG(AHB_ARBITRATION_XBAR_CTRL) & 0xFFFEFFFF) | ((params->ahb_arbitration_xbar_ctrl_meminit_done & 0xFFFF) << 16);
+
+	// Enable arbiter.
+	SYSREG(AHB_ARBITRATION_XBAR_CTRL) = (SYSREG(AHB_ARBITRATION_XBAR_CTRL) & 0xFFFEFFFF) | (params->ahb_arbitration_xbar_ctrl_meminit_done << 16);
+
+	// Lock carveouts per BCT cfg.
 	MC(MC_VIDEO_PROTECT_REG_CTRL) = params->mc_video_protect_write_access;
 	MC(MC_SEC_CARVEOUT_REG_CTRL) = params->mc_sec_carveout_protect_write_access;
 	MC(MC_MTS_CARVEOUT_REG_CTRL) = params->mc_mts_carveout_reg_ctrl;
-	MC(MC_EMEM_CFG_ACCESS_CTRL) = 1; //Disable write access to a bunch of EMC registers.
+
+	// Disable write access to a bunch of EMC registers.
+	MC(MC_EMEM_CFG_ACCESS_CTRL) = 1;
 }
 
+#ifndef CONFIG_SDRAM_COMPRESS_CFG
+static void _sdram_patch_model_params(u32 dramid, u32 *params)
+{
+	for (u32 i = 0; i < sizeof(sdram_cfg_vendor_patches) / sizeof(sdram_vendor_patch_t); i++)
+		if (sdram_cfg_vendor_patches[i].dramid & DRAM_ID(dramid))
+			params[sdram_cfg_vendor_patches[i].addr] = sdram_cfg_vendor_patches[i].val;
+}
+#endif
+
 sdram_params_t *sdram_get_params()
 {
-	//TODO: sdram_id should be in [0, 7].
+	// Check if id is proper.
+	u32 dramid = _get_sdram_id();
+	if (dramid > 6)
+		dramid = 0;
 
 #ifdef CONFIG_SDRAM_COMPRESS_CFG
-	u8 *buf = (u8 *)0x40030000;
+	u8 *buf = (u8 *)SDRAM_PARAMS_ADDR;
 	LZ_Uncompress(_dram_cfg_lz, buf, sizeof(_dram_cfg_lz));
-	return (sdram_params_t *)&buf[sizeof(sdram_params_t) * _get_sdram_id()];
+	return (sdram_params_t *)&buf[sizeof(sdram_params_t) * dramid];
 #else
-	return _dram_cfgs[_get_sdram_id()];
+	sdram_params_t *buf = (sdram_params_t *)SDRAM_PARAMS_ADDR;
+	memcpy(buf, &_dram_cfg_0_samsung_4gb, sizeof(sdram_params_t));
+	switch (dramid)
+	{
+	case DRAM_4GB_SAMSUNG_K4F6E304HB_MGCH:
+	case DRAM_4GB_MICRON_MT53B512M32D2NP_062_WT:
+		break;
+	case DRAM_4GB_HYNIX_H9HCNNNBPUMLHR_NLN:
+	case DRAM_4GB_COPPER_UNK_3:
+	case DRAM_6GB_SAMSUNG_K4FHE3D4HM_MFCH:
+	case DRAM_4GB_COPPER_UNK_5:
+	case DRAM_4GB_COPPER_UNK_6:
+		_sdram_patch_model_params(dramid, (u32 *)buf);
+		break;
+	}
+	return buf;
 #endif
 }
 
@@ -529,25 +759,42 @@ sdram_params_t *sdram_get_params_patched()
 	// Disable Warmboot signature check.
 	sdram_params->boot_rom_patch_control = (1 << 31) | (((IPATCH_BASE + 4) - APB_MISC_BASE) / 4);
 	sdram_params->boot_rom_patch_data = IPATCH_CONFIG(0x10459E, 0x2000);
+/*
+	// Disable SBK lock.
+	sdram_params->emc_bct_spare8 = (IPATCH_BASE + 7 * 4);
+	sdram_params->emc_bct_spare9 = IPATCH_CONFIG(0x10210E, 0x2000);
 
+	// Disable bootrom read lock.
+	sdram_params->emc_bct_spare10 = (IPATCH_BASE + 10 * 4);
+	sdram_params->emc_bct_spare11 = IPATCH_CONFIG(0x100FDC, 0xF000);
+	sdram_params->emc_bct_spare12 = (IPATCH_BASE + 11 * 4);
+	sdram_params->emc_bct_spare13 = IPATCH_CONFIG(0x100FDE, 0xE320);
+*/
 	return sdram_params;
 }
 
 void sdram_init()
 {
-	//TODO: sdram_id should be in [0,4].
 	const sdram_params_t *params = (const sdram_params_t *)sdram_get_params();
 
+	// Set DRAM voltage.
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_SD_CFG2, 0x05);
 	max77620_regulator_set_voltage(REGULATOR_SD1, 1100000);
 
+	// VDDP Select.
 	PMC(APBDEV_PMC_VDDP_SEL) = params->pmc_vddp_sel;
 	usleep(params->pmc_vddp_sel_wait);
+
+	// Set DDR pad voltage.
 	PMC(APBDEV_PMC_DDR_PWR) = PMC(APBDEV_PMC_DDR_PWR);
+
+	// Turn on MEM IO Power.
 	PMC(APBDEV_PMC_NO_IOPOWER) = params->pmc_no_io_power;
 	PMC(APBDEV_PMC_REG_SHORT) = params->pmc_reg_short;
+
 	PMC(APBDEV_PMC_DDR_CNTRL) = params->pmc_ddr_ctrl;
 
+	// Patch 1 using BCT spare variables
 	if (params->emc_bct_spare0)
 		*(vu32 *)params->emc_bct_spare0 = params->emc_bct_spare1;
 

source/mem/sdram.h

@@ -17,11 +17,13 @@
 #ifndef _SDRAM_H_
 #define _SDRAM_H_
 
+#include "emc.h"
 #include "sdram_param_t210.h"
 
 void sdram_init();
 sdram_params_t *sdram_get_params();
 sdram_params_t *sdram_get_params_patched();
 void sdram_lp0_save_params(const void *params);
+emc_mr_data_t sdram_read_mrx(emc_mr_t mrx);
 
 #endif

source/mem/sdram_lp0_param_t210.h

@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2013-2015, NVIDIA CORPORATION.  All rights reserved.
  * Copyright 2014 Google Inc.
- * Copyright (C) 2018 CTCaer
+ * 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,

source/mem/sdram_param_t210.h

@@ -26,12 +26,12 @@
 #ifndef _SDRAM_PARAM_T210_H_
 #define _SDRAM_PARAM_T210_H_
 
-#define MEMORY_TYPE_NONE 0
-#define MEMORY_TYPE_DDR 0
-#define MEMORY_TYPE_LPDDR 0
-#define MEMORY_TYPE_DDR2 0
+#define MEMORY_TYPE_NONE   0
+#define MEMORY_TYPE_DDR    0
+#define MEMORY_TYPE_LPDDR  0
+#define MEMORY_TYPE_DDR2   0
 #define MEMORY_TYPE_LPDDR2 1
-#define MEMORY_TYPE_DDR3 2
+#define MEMORY_TYPE_DDR3L  2
 #define MEMORY_TYPE_LPDDR4 3
 
 /**

source/power/max17050.c

@@ -1,9 +1,9 @@
 /*
  * Fuel gauge driver for Nintendo Switch's Maxim 17050
  *
- * Copyright (C) 2011 Samsung Electronics
+ * Copyright (c) 2011 Samsung Electronics
  * MyungJoo Ham <myungjoo.ham@samsung.com>
- * Copyright (C) 2018 CTCaer
+ * Copyright (c) 2018 CTCaer
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -43,6 +43,9 @@
 
 #define MAX17050_VMAX_TOLERANCE 50 /* 50 mV */
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 int max17050_get_property(enum MAX17050_reg reg, int *value)
 {
 	u16 data;
@@ -259,8 +262,10 @@ int max17050_fix_configuration()
 	/* Init complete, Clear the POR bit */
 	//_max17050_set_por_bit(0); // Should we? Or let the switch to reconfigure POR?
 
-	// Sets POR, BI, BR. 
+	// Sets POR, BI, BR.
 	_max17050_set_por_bit(0x8801);
 
 	return 0;
 }
+
+#pragma GCC pop_options

source/power/max17050.h

@@ -2,9 +2,9 @@
  * Fuel gauge driver for Nintendo Switch's Maxim 17050
  *  Note that Maxim 8966 and 8997 are mfd and this is its subdevice.
  *
- * Copyright (C) 2011 Samsung Electronics
+ * Copyright (c) 2011 Samsung Electronics
  * MyungJoo Ham <myungjoo.ham@samsung.com>
- * Copyright (C) 2018 CTCaer
+ * Copyright (c) 2018-2020 CTCaer
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -24,6 +24,8 @@
 #ifndef __MAX17050_H_
 #define __MAX17050_H_
 
+#include "../utils/types.h"
+
 #define MAX17050_STATUS_BattAbsent    (1 << 3)
 #define MAX17050_DEFAULT_SNS_RESISTOR 10000
 
@@ -96,14 +98,17 @@ enum MAX17050_reg {
 	MAX17050_K_empty0	= 0x3B,
 	MAX17050_TaskPeriod	= 0x3C,
 	MAX17050_FSTAT		= 0x3D,
-
+	MAX17050_TIMER		= 0x3E,
 	MAX17050_SHDNTIMER	= 0x3F,
+
 	MAX17050_QRTbl30	= 0x42,
+
 	MAX17050_dQacc		= 0x45,
 	MAX17050_dPacc		= 0x46,
 
 	MAX17050_VFSOC0		= 0x48,
 
+	Max17050_QH0		= 0x4C,
 	MAX17050_QH			= 0x4D,
 	MAX17050_QL			= 0x4E,
 
@@ -111,6 +116,8 @@ enum MAX17050_reg {
 	MAX17050_MaxVolt	= 0x51, // Custom ID. Not to be sent to i2c.
 
 	MAX17050_VFSOC0Enable	= 0x60,
+	MAX17050_MODELEnable1	= 0x62,
+	MAX17050_MODELEnable2	= 0x63,
 
 	MAX17050_MODELChrTbl	= 0x80,
 
@@ -123,5 +130,6 @@ enum MAX17050_reg {
 
 int max17050_get_property(enum MAX17050_reg reg, int *value);
 int max17050_fix_configuration();
+u32 max17050_get_cached_batt_volt();
 
 #endif /* __MAX17050_H_ */

source/power/max77620.h

@@ -1,7 +1,7 @@
 /*
  * Defining registers address and its bit definitions of MAX77620 and MAX20024
  *
- * Copyright (C) 2016 NVIDIA CORPORATION. All rights reserved.
+ * Copyright (c) 2016 NVIDIA CORPORATION. All rights reserved.
  * Copyright (c) 2019 CTCaer
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -19,9 +19,19 @@
 #define  MAX77620_CNFGGLBL1_LBDAC_EN          (1 << 7)
 #define  MAX77620_CNFGGLBL1_MPPLD             (1 << 6)
 #define  MAX77620_CNFGGLBL1_LBHYST            ((1 << 5) | (1 << 4))
-#define  MAX77620_CNFGGLBL1_LBHYST_N          (1 << 4)
-#define  MAX77620_CNFGGLBL1_LBDAC             0x0E
-#define  MAX77620_CNFGGLBL1_LBDAC_N           (1 << 1)
+#define  MAX77620_CNFGGLBL1_LBHYST_100        (0 << 4)
+#define  MAX77620_CNFGGLBL1_LBHYST_200        (1 << 4)
+#define  MAX77620_CNFGGLBL1_LBHYST_300        (2 << 4)
+#define  MAX77620_CNFGGLBL1_LBHYST_400        (3 << 4)
+#define  MAX77620_CNFGGLBL1_LBDAC_MASK        0x0E
+#define  MAX77620_CNFGGLBL1_LBDAC_2700        (0 << 1)
+#define  MAX77620_CNFGGLBL1_LBDAC_2800        (1 << 1)
+#define  MAX77620_CNFGGLBL1_LBDAC_2900        (2 << 1)
+#define  MAX77620_CNFGGLBL1_LBDAC_3000        (3 << 1)
+#define  MAX77620_CNFGGLBL1_LBDAC_3100        (4 << 1)
+#define  MAX77620_CNFGGLBL1_LBDAC_3200        (5 << 1)
+#define  MAX77620_CNFGGLBL1_LBDAC_3300        (6 << 1)
+#define  MAX77620_CNFGGLBL1_LBDAC_3400        (7 << 1)
 #define  MAX77620_CNFGGLBL1_LBRSTEN           (1 << 0)
 
 #define MAX77620_REG_CNFGGLBL2      0x01
@@ -130,7 +140,7 @@
 #define  MAX77620_POWER_MODE_DISABLE          0
 #define  MAX20024_LDO_CFG2_MPOK_MASK          (1 << 2)
 #define  MAX77620_LDO_CFG2_ADE_MASK           (1 << 1)
-#define  MAX77620_LDO_CFG2_ADE_DISABLE        0
+#define  MAX77620_LDO_CFG2_ADE_DISABLE        (0 << 1)
 #define  MAX77620_LDO_CFG2_ADE_ENABLE         (1 << 1)
 #define  MAX77620_LDO_CFG2_SS_MASK            (1 << 0)
 #define  MAX77620_LDO_CFG2_SS_FAST            (1 << 0)
@@ -153,6 +163,24 @@
 #define MAX77620_REG_PUE_GPIO       0x3E
 #define MAX77620_REG_PDE_GPIO       0x3F
 #define MAX77620_REG_AME_GPIO       0x40
+#define  MAX77620_CNFG_GPIO_DRV_MASK          (1 << 0)
+#define  MAX77620_CNFG_GPIO_DRV_PUSHPULL      (1 << 0)
+#define  MAX77620_CNFG_GPIO_DRV_OPENDRAIN     (0 << 0)
+#define  MAX77620_CNFG_GPIO_DIR_MASK          (1 << 1)
+#define  MAX77620_CNFG_GPIO_DIR_INPUT         (1 << 1)
+#define  MAX77620_CNFG_GPIO_DIR_OUTPUT        (0 << 1)
+#define  MAX77620_CNFG_GPIO_INPUT_VAL_MASK    (1 << 2)
+#define  MAX77620_CNFG_GPIO_OUTPUT_VAL_MASK   (1 << 3)
+#define  MAX77620_CNFG_GPIO_OUTPUT_VAL_HIGH   (1 << 3)
+#define  MAX77620_CNFG_GPIO_OUTPUT_VAL_LOW    (0 << 3)
+#define  MAX77620_CNFG_GPIO_INT_MASK          (0x3 << 4)
+#define  MAX77620_CNFG_GPIO_INT_FALLING       (1 << 4)
+#define  MAX77620_CNFG_GPIO_INT_RISING        (1 << 5)
+#define  MAX77620_CNFG_GPIO_DBNC_MASK         (0x3 << 6)
+#define  MAX77620_CNFG_GPIO_DBNC_None         (0x0 << 6)
+#define  MAX77620_CNFG_GPIO_DBNC_8ms          (0x1 << 6)
+#define  MAX77620_CNFG_GPIO_DBNC_16ms         (0x2 << 6)
+#define  MAX77620_CNFG_GPIO_DBNC_32ms         (0x3 << 6)
 
 #define MAX77620_REG_ONOFFCNFG1     0x41
 #define  MAX77620_ONOFFCNFG1_SFT_RST          (1 << 7)
@@ -259,25 +287,6 @@
 #define MAX77620_SD_CFG1_FSRADE_SD_DISABLE    0
 #define MAX77620_SD_CFG1_FSRADE_SD_ENABLE     (1 << 0)
 
-#define MAX77620_CNFG_GPIO_DRV_MASK           (1 << 0)
-#define MAX77620_CNFG_GPIO_DRV_PUSHPULL       (1 << 0)
-#define MAX77620_CNFG_GPIO_DRV_OPENDRAIN      0
-#define MAX77620_CNFG_GPIO_DIR_MASK           (1 << 1)
-#define MAX77620_CNFG_GPIO_DIR_INPUT          (1 << 1)
-#define MAX77620_CNFG_GPIO_DIR_OUTPUT         0
-#define MAX77620_CNFG_GPIO_INPUT_VAL_MASK     (1 << 2)
-#define MAX77620_CNFG_GPIO_OUTPUT_VAL_MASK    (1 << 3)
-#define MAX77620_CNFG_GPIO_OUTPUT_VAL_HIGH    (1 << 3)
-#define MAX77620_CNFG_GPIO_OUTPUT_VAL_LOW     0
-#define MAX77620_CNFG_GPIO_INT_MASK           (0x3 << 4)
-#define MAX77620_CNFG_GPIO_INT_FALLING        (1 << 4)
-#define MAX77620_CNFG_GPIO_INT_RISING         (1 << 5)
-#define MAX77620_CNFG_GPIO_DBNC_MASK          (0x3 << 6)
-#define MAX77620_CNFG_GPIO_DBNC_None          (0x0 << 6)
-#define MAX77620_CNFG_GPIO_DBNC_8ms           (0x1 << 6)
-#define MAX77620_CNFG_GPIO_DBNC_16ms          (0x2 << 6)
-#define MAX77620_CNFG_GPIO_DBNC_32ms          (0x3 << 6)
-
 #define MAX77620_IRQ_LVL2_GPIO_EDGE0          (1 << 0)
 #define MAX77620_IRQ_LVL2_GPIO_EDGE1          (1 << 1)
 #define MAX77620_IRQ_LVL2_GPIO_EDGE2          (1 << 2)

source/power/max7762x.c

@@ -64,6 +64,16 @@ static const max77620_regulator_t _pmic_regulators[] = {
 	{ REGULATOR_LDO, "ldo8", 0x00,  50000, 800000, 1050000, 1050000, MAX77620_REG_LDO8_CFG, MAX77620_REG_LDO8_CFG2, MAX77620_LDO_VOLT_MASK, MAX77620_LDO_POWER_MODE_MASK, MAX77620_LDO_POWER_MODE_SHIFT, 0x00,  MAX77620_REG_FPS_LDO8, 3, 7, 0 }
 };
 
+static void _max77620_try_set_reg(u8 reg, u8 val)
+{
+	u8 tmp;
+	do
+	{
+		i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, reg, val);
+		tmp = i2c_recv_byte(I2C_5, MAX77620_I2C_ADDR, reg);
+	} while (val != tmp);
+}
+
 int max77620_regulator_get_status(u32 id)
 {
 	if (id > REGULATOR_MAX)
@@ -83,7 +93,7 @@ int max77620_regulator_config_fps(u32 id)
 
 	const max77620_regulator_t *reg = &_pmic_regulators[id];
 
-	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, reg->fps_addr,
+	_max77620_try_set_reg(reg->fps_addr,
 		(reg->fps_src << MAX77620_FPS_SRC_SHIFT) | (reg->pu_period << MAX77620_FPS_PU_PERIOD_SHIFT) | (reg->pd_period));
 
 	return 1;
@@ -102,7 +112,7 @@ int max77620_regulator_set_voltage(u32 id, u32 mv)
 	u32 mult = (mv + reg->mv_step - 1 - reg->mv_min) / reg->mv_step;
 	u8 val = i2c_recv_byte(I2C_5, MAX77620_I2C_ADDR, reg->volt_addr);
 	val = (val & ~reg->volt_mask) | (mult & reg->volt_mask);
-	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, reg->volt_addr, val);
+	_max77620_try_set_reg(reg->volt_addr, val);
 	usleep(1000);
 
 	return 1;
@@ -121,12 +131,13 @@ int max77620_regulator_enable(u32 id, int enable)
 		val = (val & ~reg->enable_mask) | ((MAX77620_POWER_MODE_NORMAL << reg->enable_shift) & reg->enable_mask);
 	else
 		val &= ~reg->enable_mask;
-	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, addr, val);
+	_max77620_try_set_reg(addr, val);
 	usleep(1000);
 
 	return 1;
 }
 
+// LDO only.
 int max77620_regulator_set_volt_and_flags(u32 id, u32 mv, u8 flags)
 {
 	if (id > REGULATOR_MAX)
@@ -139,7 +150,7 @@ int max77620_regulator_set_volt_and_flags(u32 id, u32 mv, u8 flags)
 
 	u32 mult = (mv + reg->mv_step - 1 - reg->mv_min) / reg->mv_step;
 	u8 val = ((flags << reg->enable_shift) & ~reg->volt_mask) | (mult & reg->volt_mask);
-	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, reg->volt_addr, val);
+	_max77620_try_set_reg(reg->volt_addr, val);
 	usleep(1000);
 
 	return 1;
@@ -155,11 +166,12 @@ void max77620_config_default()
 		if (_pmic_regulators[i].fps_src != MAX77620_FPS_SRC_NONE)
 			max77620_regulator_enable(i, 1);
 	}
-	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_SD_CFG2, 4);
+	_max77620_try_set_reg(MAX77620_REG_SD_CFG2, 4);
 }
 
-void max77620_low_battery_monitor_config()
+void max77620_low_battery_monitor_config(bool enable)
 {
-	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_CNFGGLBL1,
-		MAX77620_CNFGGLBL1_LBDAC_EN | MAX77620_CNFGGLBL1_LBHYST_N | MAX77620_CNFGGLBL1_LBDAC_N);
+	_max77620_try_set_reg(MAX77620_REG_CNFGGLBL1,
+		MAX77620_CNFGGLBL1_LBDAC_EN | (enable ? MAX77620_CNFGGLBL1_MPPLD : 0) |
+		MAX77620_CNFGGLBL1_LBHYST_200 | MAX77620_CNFGGLBL1_LBDAC_2800);
 }

source/power/max7762x.h

@@ -24,16 +24,16 @@
 * Switch Power domains (max77620):
 * Name  | Usage         | uV step | uV min | uV default | uV max  | Init
 *-------+---------------+---------+--------+------------+---------+------------------
-*  sd0  | core          | 12500   | 600000 |  625000    | 1400000 | 1.125V (pkg1.1)
+*  sd0  | SoC           | 12500   | 600000 |  625000    | 1400000 | 1.125V (pkg1.1)
 *  sd1  | SDRAM         | 12500   | 600000 | 1125000    | 1125000 | 1.1V   (pkg1.1)
 *  sd2  | ldo{0-1, 7-8} | 12500   | 600000 | 1325000    | 1350000 | 1.325V (pcv)
 *  sd3  | 1.8V general  | 12500   | 600000 | 1800000    | 1800000 |
 *  ldo0 | Display Panel | 25000   | 800000 | 1200000    | 1200000 | 1.2V   (pkg1.1)
 *  ldo1 | XUSB, PCIE    | 25000   | 800000 | 1050000    | 1050000 | 1.05V  (pcv)
 *  ldo2 | SDMMC1        | 50000   | 800000 | 1800000    | 3300000 |
-*  ldo3 | GC ASIC       | 50000   | 800000 | 3100000    | 3100000 | 3.1V  (pcv)
+*  ldo3 | GC ASIC       | 50000   | 800000 | 3100000    | 3100000 | 3.1V   (pcv)
 *  ldo4 | RTC           | 12500   | 800000 |  850000    |  850000 |
-*  ldo5 | GC ASIC       | 50000   | 800000 | 1800000    | 1800000 | 1.8V  (pcv)
+*  ldo5 | GC Card       | 50000   | 800000 | 1800000    | 1800000 | 1.8V   (pcv)
 *  ldo6 | Touch, ALS    | 50000   | 800000 | 2900000    | 2900000 | 2.9V
 *  ldo7 | XUSB          | 50000   | 800000 | 1050000    | 1050000 |
 *  ldo8 | XUSB, DC      | 50000   | 800000 | 1050000    | 1050000 |
@@ -71,6 +71,8 @@
 /* MAX77621_VOUT */
 #define MAX77621_VOUT_ENABLE  (1 << 7)
 #define MAX77621_VOUT_MASK    0x7F
+#define  MAX77621_VOUT_0_95V  0x37
+#define  MAX77621_VOUT_1_09V  0x4F
 
 /* MAX77621_VOUT_DVC_DVS */
 #define MAX77621_DVS_VOUT_MASK 0x7F
@@ -111,6 +113,6 @@ int max77620_regulator_set_voltage(u32 id, u32 mv);
 int max77620_regulator_enable(u32 id, int enable);
 int max77620_regulator_set_volt_and_flags(u32 id, u32 mv, u8 flags);
 void max77620_config_default();
-void max77620_low_battery_monitor_config();
+void max77620_low_battery_monitor_config(bool enable);
 
 #endif

source/rtc/max77620-rtc.c

@@ -1,7 +1,8 @@
 /*
  * PMIC Real Time Clock driver for Nintendo Switch's MAX77620-RTC
  *
- * Copyright (c) 2018 CTCaer
+ * Copyright (c) 2018-2019 CTCaer
+ * Copyright (c) 2019-2020 shchmue
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -36,7 +37,7 @@ void max77620_rtc_get_time(rtc_time_t *time)
 	time->min  = i2c_recv_byte(I2C_5, MAX77620_RTC_I2C_ADDR, MAX77620_RTC_MIN_REG) & 0x7F;
 
 	time->hour = i2c_recv_byte(I2C_5, MAX77620_RTC_I2C_ADDR, MAX77620_RTC_HOUR_REG) & 0x1F;
-	
+
 	if (!(val & MAX77620_RTC_24H) && time->hour & MAX77620_RTC_HOUR_PM_MASK)
 		time->hour = (time->hour & 0xF) + 12;
 
@@ -52,7 +53,7 @@ void max77620_rtc_get_time(rtc_time_t *time)
 	}
 
 	// Get date.
-	time->date  = i2c_recv_byte(I2C_5, MAX77620_RTC_I2C_ADDR, MAX77620_RTC_DATE_REG) & 0x1f;
+	time->day  = i2c_recv_byte(I2C_5, MAX77620_RTC_I2C_ADDR, MAX77620_RTC_DATE_REG) & 0x1f;
 	time->month = (i2c_recv_byte(I2C_5, MAX77620_RTC_I2C_ADDR, MAX77620_RTC_MONTH_REG) & 0xF) - 1;
 	time->year  = (i2c_recv_byte(I2C_5, MAX77620_RTC_I2C_ADDR, MAX77620_RTC_YEAR_REG) & 0x7F) + 2000;
 }
@@ -75,3 +76,94 @@ void max77620_rtc_stop_alarm()
 	// Update RTC clock from RTC regs.
 	i2c_send_byte(I2C_5, MAX77620_RTC_I2C_ADDR, MAX77620_RTC_UPDATE0_REG, MAX77620_RTC_WRITE_UPDATE);
 }
+
+void max77620_rtc_epoch_to_date(u32 epoch, rtc_time_t *time)
+{
+	u32 tmp, edays, year, month, day;
+
+	// Set time.
+	time->sec = epoch % 60;
+	epoch /= 60;
+	time->min = epoch % 60;
+	epoch /= 60;
+	time->hour = epoch % 24;
+	epoch /= 24;
+
+	// Calculate base date values.
+	tmp = (u32)(((u64)4 * epoch + 102032) / 146097 + 15);
+	tmp = (u32)((u64)epoch + 2442113 + tmp - (tmp >> 2));
+
+	year = (20 * tmp - 2442) / 7305;
+	edays = tmp - 365 * year - (year >> 2);
+	month = edays * 1000 / 30601;
+	day = edays - month * 30 - month * 601 / 1000;
+
+	// Month/Year offset.
+	if(month < 14)
+	{
+		year -= 4716;
+		month--;
+	}
+	else
+	{
+		year -= 4715;
+		month -= 13;
+	}
+
+	// Set date.
+	time->year = year;
+	time->month = month;
+	time->day = day;
+
+	// Set weekday.
+	time->weekday = 0; //! TODO.
+}
+
+u32 max77620_rtc_date_to_epoch(const rtc_time_t *time, bool hos_encoding)
+{
+	u32 year, month, epoch;
+
+	//Year
+	year = time->year;
+	//Month of year
+	month = time->month;
+
+	if (!hos_encoding)
+	{
+		// Month/Year offset.
+		if(month < 3)
+		{
+			month += 12;
+			year--;
+		}
+	}
+	else
+	{
+		year -= 2000;
+		month++;
+
+		// Month/Year offset.
+		if(month < 3)
+		{
+			month += 9;
+			year--;
+		}
+		else
+			month -= 3;
+	}
+
+	epoch = (365 * year) + (year >> 2) - (year / 100) + (year / 400); // Years to days.
+
+	if (!hos_encoding)
+	{
+		epoch += (30 * month) + (3 * (month + 1) / 5) + time->day; // Months to days.
+		epoch -= 719561; // Epoch time is 1/1/1970.
+	}
+	else
+		epoch += (30 * month) + ((3 * month + 2) / 5) + 59 + time->day; // Months to days.
+
+	epoch *= 86400; // Days to seconds.
+	epoch += (3600 * time->hour) + (60 * time->min) + time->sec; // Add hours, minutes and seconds.
+
+	return epoch;
+}

source/rtc/max77620-rtc.h

@@ -64,12 +64,14 @@ typedef struct _rtc_time_t {
 	u8  sec;
 	u8  min;
 	u8  hour;
-	u8  date;
+	u8  day;
 	u8  month;
 	u16 year;
 } rtc_time_t;
 
 void max77620_rtc_get_time(rtc_time_t *time);
 void max77620_rtc_stop_alarm();
+void max77620_rtc_epoch_to_date(u32 epoch, rtc_time_t *time);
+u32 max77620_rtc_date_to_epoch(const rtc_time_t *time, bool hos_encoding);
 
 #endif /* _MFD_MAX77620_RTC_H_ */

source/sec/se.c

@@ -2,7 +2,7 @@
  * Copyright (c) 2018 naehrwert
  * Copyright (c) 2018 CTCaer
  * Copyright (c) 2018 Atmosphère-NX
- * Copyright (c) 2019 shchmue
+ * Copyright (c) 2019-2020 shchmue
  *
  * 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,8 +19,10 @@
 
 #include <string.h>
 
+#include "../../common/memory_map.h"
 #include "../sec/se.h"
 #include "../mem/heap.h"
+#include "../soc/bpmp.h"
 #include "../soc/t210.h"
 #include "../sec/se_t210.h"
 #include "../utils/util.h"
@@ -32,6 +34,9 @@ typedef struct _se_ll_t
 	vu32 size;
 } se_ll_t;
 
+static u32 _se_rsa_mod_sizes[TEGRA_SE_RSA_KEYSLOT_COUNT];
+static u32 _se_rsa_exp_sizes[TEGRA_SE_RSA_KEYSLOT_COUNT];
+
 static void _gf256_mul_x(void *block)
 {
 	u8 *pdata = (u8 *)block;
@@ -82,25 +87,28 @@ static int _se_wait()
 	while (!(SE(SE_INT_STATUS_REG_OFFSET) & SE_INT_OP_DONE(INT_SET)))
 		;
 	if (SE(SE_INT_STATUS_REG_OFFSET) & SE_INT_ERROR(INT_SET) ||
-		SE(SE_STATUS_0) & 3 ||
-		SE(SE_ERR_STATUS_0) != 0)
+		SE(SE_STATUS_0) & SE_STATUS_0_STATE_WAIT_IN ||
+		SE(SE_ERR_STATUS_0) != SE_ERR_STATUS_0_SE_NS_ACCESS_CLEAR)
 		return 0;
 	return 1;
 }
 
 static int _se_execute(u32 op, void *dst, u32 dst_size, const void *src, u32 src_size)
 {
-	se_ll_t *ll_dst = NULL, *ll_src = NULL;
+	static se_ll_t *ll_dst = NULL, *ll_src = NULL;
+	if (!ll_dst)
+	{
+		ll_dst = (se_ll_t *)malloc(sizeof(se_ll_t));
+		ll_src = (se_ll_t *)malloc(sizeof(se_ll_t));
+	}
 
 	if (dst)
 	{
-		ll_dst = (se_ll_t *)malloc(sizeof(se_ll_t));
 		_se_ll_init(ll_dst, (u32)dst, dst_size);
 	}
 
 	if (src)
 	{
-		ll_src = (se_ll_t *)malloc(sizeof(se_ll_t));
 		_se_ll_init(ll_src, (u32)src, src_size);
 	}
 
@@ -108,14 +116,13 @@ static int _se_execute(u32 op, void *dst, u32 dst_size, const void *src, u32 src
 
 	SE(SE_ERR_STATUS_0) = SE(SE_ERR_STATUS_0);
 	SE(SE_INT_STATUS_REG_OFFSET) = SE(SE_INT_STATUS_REG_OFFSET);
-	SE(SE_OPERATION_REG_OFFSET) = SE_OPERATION(op);
 
+	bpmp_mmu_maintenance(BPMP_MMU_MAINT_CLN_INV_WAY, false);
+
+	SE(SE_OPERATION_REG_OFFSET) = SE_OPERATION(op);
 	int res = _se_wait();
 
-	if (src)
-		free(ll_src);
-	if (dst)
-		free(ll_dst);
+	bpmp_mmu_maintenance(BPMP_MMU_MAINT_CLN_INV_WAY, false);
 
 	return res;
 }
@@ -133,7 +140,7 @@ static int _se_execute_one_block(u32 op, void *dst, u32 dst_size, const void *sr
 	memcpy(block, src, src_size);
 	int res = _se_execute(op, block, 0x10, block, 0x10);
 	memcpy(dst, block, dst_size);
-	
+
 	free(block);
 	return res;
 }
@@ -147,17 +154,79 @@ static void _se_aes_ctr_set(void *ctr)
 
 void se_rsa_acc_ctrl(u32 rs, u32 flags)
 {
-	if (flags & 0x7F)
-		SE(SE_RSA_KEYTABLE_ACCESS_REG_OFFSET + 4 * rs) = (((flags >> 4) & 4) | (flags & 3)) ^ 7;
-	if (flags & 0x80)
+	if (flags & SE_RSA_KEY_TBL_DIS_KEY_ALL_FLAG)
+		SE(SE_RSA_KEYTABLE_ACCESS_REG_OFFSET + 4 * rs) =
+			((flags >> SE_RSA_KEY_TBL_DIS_KEYUSE_FLAG_SHIFT) & SE_RSA_KEY_TBL_DIS_KEYUSE_FLAG) |
+			((flags & SE_RSA_KEY_TBL_DIS_KEY_READ_UPDATE_FLAG) ^ SE_RSA_KEY_TBL_DIS_KEY_ALL_COMMON_FLAG);
+	if (flags & SE_RSA_KEY_TBL_DIS_KEY_LOCK_FLAG)
 		SE(SE_RSA_KEYTABLE_ACCESS_LOCK_OFFSET) &= ~(1 << rs);
 }
 
+// se_rsa_key_set() was derived from Atmosphère's set_rsa_keyslot
+void se_rsa_key_set(u32 ks, const void *mod, u32 mod_size, const void *exp, u32 exp_size)
+{
+	u32 *data = (u32 *)mod;
+	for (u32 i = 0; i < mod_size / 4; i++)
+	{
+		SE(SE_RSA_KEYTABLE_ADDR) = RSA_KEY_NUM(ks) | RSA_KEY_TYPE(RSA_KEY_TYPE_MOD) | i;
+		SE(SE_RSA_KEYTABLE_DATA) = byte_swap_32(data[mod_size / 4 - i - 1]);
+	}
+
+	data = (u32 *)exp;
+	for (u32 i = 0; i < exp_size / 4; i++)
+	{
+		SE(SE_RSA_KEYTABLE_ADDR) = RSA_KEY_NUM(ks) | RSA_KEY_TYPE(RSA_KEY_TYPE_EXP) | i;
+		SE(SE_RSA_KEYTABLE_DATA) = byte_swap_32(data[exp_size / 4 - i - 1]);
+	}
+
+	_se_rsa_mod_sizes[ks] = mod_size;
+	_se_rsa_exp_sizes[ks] = exp_size;
+}
+
+// se_rsa_key_clear() was derived from Atmosphère's clear_rsa_keyslot
+void se_rsa_key_clear(u32 ks)
+{
+	for (u32 i = 0; i < TEGRA_SE_RSA2048_DIGEST_SIZE / 4; i++)
+	{
+		SE(SE_RSA_KEYTABLE_ADDR) = RSA_KEY_NUM(ks) | RSA_KEY_TYPE(RSA_KEY_TYPE_MOD) | i;
+		SE(SE_RSA_KEYTABLE_DATA) = 0;
+	}
+	for (u32 i = 0; i < TEGRA_SE_RSA2048_DIGEST_SIZE / 4; i++)
+	{
+		SE(SE_RSA_KEYTABLE_ADDR) = RSA_KEY_NUM(ks) | RSA_KEY_TYPE(RSA_KEY_TYPE_EXP) | i;
+		SE(SE_RSA_KEYTABLE_DATA) = 0;
+	}
+}
+
+// se_rsa_exp_mod() was derived from Atmosphère's se_synchronous_exp_mod and se_get_exp_mod_output
+int se_rsa_exp_mod(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size)
+{
+	int res;
+	u8 stack_buf[TEGRA_SE_RSA2048_DIGEST_SIZE];
+
+	for (u32 i = 0; i < src_size; i++)
+		stack_buf[i] = *((u8 *)src + src_size - i - 1);
+
+	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_RSA) | SE_CONFIG_DST(DST_RSAREG);
+	SE(SE_RSA_CONFIG) = RSA_KEY_SLOT(ks);
+	SE(SE_RSA_KEY_SIZE_REG_OFFSET) = (_se_rsa_mod_sizes[ks] >> 6) - 1;
+	SE(SE_RSA_EXP_SIZE_REG_OFFSET) = _se_rsa_exp_sizes[ks] >> 2;
+
+	res = _se_execute(OP_START, NULL, 0, stack_buf, src_size);
+
+	// Copy output hash.
+	u32 *dst32 = (u32 *)dst;
+	for (u32 i = 0; i < dst_size / 4; i++)
+		dst32[dst_size / 4 - i - 1] = byte_swap_32(SE(SE_RSA_OUTPUT + (i << 2)));
+
+	return res;
+}
+
 void se_key_acc_ctrl(u32 ks, u32 flags)
 {
-	if (flags & 0x7F)
+	if (flags & SE_KEY_TBL_DIS_KEY_ACCESS_FLAG)
 		SE(SE_KEY_TABLE_ACCESS_REG_OFFSET + 4 * ks) = ~flags;
-	if (flags & 0x80)
+	if (flags & SE_KEY_TBL_DIS_KEY_LOCK_FLAG)
 		SE(SE_KEY_TABLE_ACCESS_LOCK_OFFSET) &= ~(1 << ks);
 }
 
@@ -171,6 +240,16 @@ void se_aes_key_set(u32 ks, const void *key, u32 size)
 	}
 }
 
+void se_aes_iv_set(u32 ks, const void *iv, u32 size)
+{
+	u32 *data = (u32 *)iv;
+	for (u32 i = 0; i < size / 4; i++)
+	{
+		SE(SE_KEYTABLE_REG_OFFSET) = SE_KEYTABLE_SLOT(ks) | 8 | i;
+		SE(SE_KEYTABLE_DATA0_REG_OFFSET) = data[i];
+	}
+}
+
 void se_aes_key_read(u32 ks, void *key, u32 size)
 {
 	u32 *data = (u32 *)key;
@@ -227,18 +306,7 @@ int se_aes_crypt_ecb(u32 ks, u32 enc, void *dst, u32 dst_size, const void *src,
 
 int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, const void *src)
 {
-	if (enc)
-	{
-		SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_AES_ENC) | SE_CONFIG_DST(DST_MEMORY);
-		SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | SE_CRYPTO_CORE_SEL(CORE_ENCRYPT);
-	}
-	else
-	{
-		SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_DEC_ALG(ALG_AES_DEC) | SE_CONFIG_DST(DST_MEMORY);
-		SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | SE_CRYPTO_CORE_SEL(CORE_DECRYPT);
-	}
-	SE(SE_BLOCK_COUNT_REG_OFFSET) = 0;
-	return _se_execute(OP_START, dst, 0x10, src, 0x10);
+	return se_aes_crypt_ecb(ks, enc, dst, 0x10, src, 0x10);
 }
 
 int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size, void *ctr)
@@ -246,7 +314,8 @@ int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_s
 	SE(SE_SPARE_0_REG_OFFSET) = 1;
 	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_AES_ENC) | SE_CONFIG_DST(DST_MEMORY);
 	SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | SE_CRYPTO_CORE_SEL(CORE_ENCRYPT) |
-		SE_CRYPTO_XOR_POS(XOR_BOTTOM) | SE_CRYPTO_INPUT_SEL(INPUT_LNR_CTR) | SE_CRYPTO_CTR_VAL(1);
+		SE_CRYPTO_XOR_POS(XOR_BOTTOM) | SE_CRYPTO_INPUT_SEL(INPUT_LNR_CTR) | SE_CRYPTO_CTR_VAL(1) |
+		SE_CRYPTO_VCTRAM_SEL(VCTRAM_AHB);
 	_se_aes_ctr_set(ctr);
 
 	u32 src_size_aligned = src_size & 0xFFFFFFF0;
@@ -267,12 +336,90 @@ int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_s
 	return 1;
 }
 
+// random calls were derived from Atmosphère's
+int se_initialize_rng(u32 ks)
+{
+	u8 *output_buf = (u8 *)malloc(0x10);
+
+	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_RNG) | SE_CONFIG_DST(DST_MEMORY);
+	SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | SE_CRYPTO_CORE_SEL(CORE_ENCRYPT) |
+		SE_CRYPTO_INPUT_SEL(INPUT_RANDOM);
+	SE(SE_RNG_CONFIG_REG_OFFSET) = SE_RNG_CONFIG_MODE(RNG_MODE_FORCE_INSTANTION) | SE_RNG_CONFIG_SRC(RNG_SRC_ENTROPY);
+	SE(SE_RNG_RESEED_INTERVAL_REG_OFFSET) = 70001;
+	SE(SE_RNG_SRC_CONFIG_REG_OFFSET) = SE_RNG_SRC_CONFIG_ENT_SRC_LOCK(RNG_SRC_RO_ENT_LOCK_ENABLE);
+	SE(SE_BLOCK_COUNT_REG_OFFSET) = 0;
+
+	int res =_se_execute(OP_START, output_buf, 0x10, NULL, 0);
+
+	free(output_buf);
+	return res;
+}
+
+int se_generate_random(u32 ks, void *dst, u32 size)
+{
+	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_RNG) | SE_CONFIG_DST(DST_MEMORY);
+	SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | SE_CRYPTO_CORE_SEL(CORE_ENCRYPT) |
+		SE_CRYPTO_INPUT_SEL(INPUT_RANDOM);
+	SE(SE_RNG_CONFIG_REG_OFFSET) = SE_RNG_CONFIG_MODE(RNG_MODE_NORMAL) | SE_RNG_CONFIG_SRC(RNG_SRC_ENTROPY);
+
+	u32 num_blocks = size >> 4;
+	u32 aligned_size = num_blocks << 4;
+	if (num_blocks)
+	{
+		SE(SE_BLOCK_COUNT_REG_OFFSET) = num_blocks - 1;
+		if (!_se_execute(OP_START, dst, aligned_size, NULL, 0))
+			return 0;
+	}
+	if (size > aligned_size)
+		return _se_execute_one_block(OP_START, dst + aligned_size, size - aligned_size, NULL, 0);
+	return 1;
+}
+
+int se_generate_random_key(u32 ks_dst, u32 ks_src)
+{
+	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_RNG) | SE_CONFIG_DST(DST_MEMORY);
+	SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks_src) | SE_CRYPTO_CORE_SEL(CORE_ENCRYPT) |
+		SE_CRYPTO_INPUT_SEL(INPUT_RANDOM);
+	SE(SE_RNG_CONFIG_REG_OFFSET) = SE_RNG_CONFIG_MODE(RNG_MODE_NORMAL) | SE_RNG_CONFIG_SRC(RNG_SRC_ENTROPY);
+
+	SE(SE_CRYPTO_KEYTABLE_DST_REG_OFFSET) = SE_CRYPTO_KEYTABLE_DST_KEY_INDEX(ks_dst);
+	if (!_se_execute(OP_START, NULL, 0, NULL, 0))
+		return 0;
+	SE(SE_CRYPTO_KEYTABLE_DST_REG_OFFSET) = SE_CRYPTO_KEYTABLE_DST_KEY_INDEX(ks_dst) | 1;
+	if (!_se_execute(OP_START, NULL, 0, NULL, 0))
+		return 0;
+
+	return 1;
+}
+
+int se_aes_crypt_cbc(u32 ks, u32 enc, void *dst, u32 dst_size, const void *src, u32 src_size)
+{
+	if (enc)
+	{
+		SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_AES_ENC) | SE_CONFIG_DST(DST_MEMORY);
+		SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | SE_CRYPTO_VCTRAM_SEL(VCTRAM_AESOUT) |
+			SE_CRYPTO_CORE_SEL(CORE_ENCRYPT) | SE_CRYPTO_XOR_POS(XOR_TOP) | SE_CRYPTO_INPUT_SEL(INPUT_AHB) |
+			SE_CRYPTO_IV_SEL(IV_ORIGINAL);
+	}
+	else
+	{
+		SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_DEC_ALG(ALG_AES_DEC) | SE_CONFIG_DST(DST_MEMORY);
+		SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | SE_CRYPTO_VCTRAM_SEL(VCTRAM_PREVAHB) |
+			SE_CRYPTO_CORE_SEL(CORE_DECRYPT) | SE_CRYPTO_XOR_POS(XOR_BOTTOM) | SE_CRYPTO_INPUT_SEL(INPUT_AHB) |
+			SE_CRYPTO_IV_SEL(IV_ORIGINAL);
+	}
+	SE(SE_BLOCK_COUNT_REG_OFFSET) = (src_size >> 4) - 1;
+	return _se_execute(OP_START, dst, dst_size, src, src_size);
+}
+
 int se_aes_xts_crypt_sec(u32 ks1, u32 ks2, u32 enc, u64 sec, void *dst, const void *src, u32 secsize)
 {
 	int res = 0;
 	u8 *tweak = (u8 *)malloc(0x10);
-	u8 *pdst = (u8 *)dst;
-	u8 *psrc = (u8 *)src;
+	u8 *temptweak = (u8 *)malloc(0x10);
+	u32 *pdst = (u32 *)dst;
+    u32 *psrc = (u32 *)src;
+    u32 *ptweak = (u32 *)tweak;
 
 	//Generate tweak.
 	for (int i = 0xF; i >= 0; i--)
@@ -283,23 +430,35 @@ int se_aes_xts_crypt_sec(u32 ks1, u32 ks2, u32 enc, u64 sec, void *dst, const vo
 	if (!se_aes_crypt_block_ecb(ks1, 1, tweak, tweak))
 		goto out;
 
+	memcpy(temptweak, tweak, 0x10);
+
 	//We are assuming a 0x10-aligned sector size in this implementation.
 	for (u32 i = 0; i < secsize / 0x10; i++)
 	{
-		for (u32 j = 0; j < 0x10; j++)
-			pdst[j] = psrc[j] ^ tweak[j];
-		if (!se_aes_crypt_block_ecb(ks2, enc, pdst, pdst))
-			goto out;
-		for (u32 j = 0; j < 0x10; j++)
-			pdst[j] = pdst[j] ^ tweak[j];
+		for (u32 j = 0; j < 4; j++)
+			pdst[j] = psrc[j] ^ ptweak[j];
 		_gf256_mul_x_le(tweak);
-		psrc += 0x10;
-		pdst += 0x10;
+		psrc += 4;
+		pdst += 4;
+	}
+
+	se_aes_crypt_ecb(ks2, enc, dst, secsize, dst, secsize);
+
+	pdst = (u32 *)dst;
+
+	memcpy(tweak, temptweak, 0x10);
+	for (u32 i = 0; i < secsize / 0x10; i++)
+	{
+		for (u32 j = 0; j < 4; j++)
+			pdst[j] = pdst[j] ^ ptweak[j];
+		_gf256_mul_x_le(tweak);
+		pdst += 4;
 	}
 
 	res = 1;
 
 out:;
+	free(temptweak);
 	free(tweak);
 	return res;
 }
@@ -331,21 +490,27 @@ int se_aes_cmac(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size)
 		_gf256_mul_x(key);
 
 	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_AES_ENC) | SE_CONFIG_DST(DST_HASHREG);
-	SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | 0x145;
+	SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks) | SE_CRYPTO_INPUT_SEL(INPUT_AHB) |
+		SE_CRYPTO_XOR_POS(XOR_TOP) | SE_CRYPTO_VCTRAM_SEL(VCTRAM_AESOUT) | SE_CRYPTO_HASH(HASH_ENABLE) |
+		SE_CRYPTO_CORE_SEL(CORE_ENCRYPT);
 	se_aes_key_iv_clear(ks);
 
 	u32 num_blocks = (src_size + 0xf) >> 4;
-	if (num_blocks > 1) {
+	if (num_blocks > 1)
+	{
 		SE(SE_BLOCK_COUNT_REG_OFFSET) = num_blocks - 2;
 		if (!_se_execute(OP_START, NULL, 0, src, src_size))
 			goto out;
 		SE(SE_CRYPTO_REG_OFFSET) |= SE_CRYPTO_IV_SEL(IV_UPDATED);
 	}
 
-	if (src_size & 0xf) {
+	if (src_size & 0xf)
+	{
 		memcpy(last_block, src + (src_size & ~0xf), src_size & 0xf);
 		last_block[src_size & 0xf] = 0x80;
-	} else if (src_size >= 0x10) {
+	}
+	else if (src_size >= 0x10)
+	{
 		memcpy(last_block, src + src_size - 0x10, 0x10);
 	}
 
@@ -371,15 +536,15 @@ int se_calc_sha256(void *dst, const void *src, u32 src_size)
 	int res;
 	// Setup config for SHA256, size = BITS(src_size).
 	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_MODE(MODE_SHA256) | SE_CONFIG_ENC_ALG(ALG_SHA) | SE_CONFIG_DST(DST_HASHREG);
-	SE(SE_SHA_CONFIG_REG_OFFSET) = SHA_ENABLE;
-	SE(SE_SHA_MSG_LENGTH_REG_OFFSET) = (u32)(src_size << 3);
-	SE(SE_SHA_MSG_LENGTH_REG_OFFSET + 4 * 1) = 0;
-	SE(SE_SHA_MSG_LENGTH_REG_OFFSET + 4 * 2) = 0;
-	SE(SE_SHA_MSG_LENGTH_REG_OFFSET + 4 * 3) = 0;
-	SE(SE_SHA_MSG_LEFT_REG_OFFSET) = (u32)(src_size << 3);
-	SE(SE_SHA_MSG_LEFT_REG_OFFSET + 4 * 1) = 0;
-	SE(SE_SHA_MSG_LEFT_REG_OFFSET + 4 * 2) = 0;
-	SE(SE_SHA_MSG_LEFT_REG_OFFSET + 4 * 3) = 0;
+	SE(SE_SHA_CONFIG_REG_OFFSET) = SHA_INIT_HASH;
+	SE(SE_SHA_MSG_LENGTH_0_REG_OFFSET) = (u32)(src_size << 3);
+	SE(SE_SHA_MSG_LENGTH_1_REG_OFFSET) = 0;
+	SE(SE_SHA_MSG_LENGTH_2_REG_OFFSET) = 0;
+	SE(SE_SHA_MSG_LENGTH_3_REG_OFFSET) = 0;
+	SE(SE_SHA_MSG_LEFT_0_REG_OFFSET) = (u32)(src_size << 3);
+	SE(SE_SHA_MSG_LEFT_1_REG_OFFSET) = 0;
+	SE(SE_SHA_MSG_LEFT_2_REG_OFFSET) = 0;
+	SE(SE_SHA_MSG_LEFT_3_REG_OFFSET) = 0;
 
 	// Trigger the operation.
 	res = _se_execute(OP_START, NULL, 0, src, src_size);
@@ -391,3 +556,47 @@ int se_calc_sha256(void *dst, const void *src, u32 src_size)
 
 	return res;
 }
+
+int se_calc_hmac_sha256(void *dst, const void *src, u32 src_size, const void *key, u32 key_size)
+{
+	int res = 0;
+	u8 *secret = (u8 *)malloc(0x40);
+	u8 *ipad = (u8 *)malloc(0x40 + src_size);
+	u8 *opad = (u8 *)malloc(0x60);
+
+	if (key_size > 0x40)
+	{
+		if (!se_calc_sha256(secret, key, key_size))
+			goto out;
+		memset(secret + 0x20, 0, 0x20);
+	}
+	else
+	{
+		memcpy(secret, key, key_size);
+		memset(secret + key_size, 0, 0x40 - key_size);
+	}
+
+	u32 *secret32 = (u32 *)secret;
+	u32 *ipad32 = (u32 *)ipad;
+	u32 *opad32 = (u32 *)opad;
+	for (u32 i = 0; i < 0x10; i++)
+	{
+		ipad32[i] = secret32[i] ^ 0x36363636;
+		opad32[i] = secret32[i] ^ 0x5C5C5C5C;
+	}
+
+	memcpy(ipad + 0x40, src, src_size);
+	if (!se_calc_sha256(dst, ipad, 0x40 + src_size))
+		goto out;
+	memcpy(opad + 0x40, dst, 0x20);
+	if (!se_calc_sha256(dst, opad, 0x60))
+		goto out;
+
+	res = 1;
+
+out:;
+	free(secret);
+	free(ipad);
+	free(opad);
+	return res;
+}

source/sec/se.h

@@ -20,17 +20,26 @@
 #include "../utils/types.h"
 
 void se_rsa_acc_ctrl(u32 rs, u32 flags);
+void se_rsa_key_set(u32 ks, const void *mod, u32 mod_size, const void *exp, u32 exp_size);
+void se_rsa_key_clear(u32 ks);
+int se_rsa_exp_mod(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size);
 void se_key_acc_ctrl(u32 ks, u32 flags);
 void se_aes_key_set(u32 ks, const void *key, u32 size);
+void se_aes_iv_set(u32 ks, const void *iv, u32 size);
 void se_aes_key_read(u32 ks, void *key, u32 size);
 void se_aes_key_clear(u32 ks);
+int se_initialize_rng(u32 ks);
+int se_generate_random(u32 ks, void *dst, u32 size);
+int se_generate_random_key(u32 ks_dst, u32 ks_src);
 int se_aes_unwrap_key(u32 ks_dst, u32 ks_src, const void *input);
 int se_aes_crypt_ecb(u32 ks, u32 enc, void *dst, u32 dst_size, const void *src, u32 src_size);
 int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, const void *src);
 int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size, void *ctr);
+int se_aes_crypt_cbc(u32 ks, u32 enc, void *dst, u32 dst_size, const void *src, u32 src_size);
 int se_aes_xts_crypt_sec(u32 ks1, u32 ks2, u32 enc, u64 sec, void *dst, const void *src, u32 secsize);
 int se_aes_xts_crypt(u32 ks1, u32 ks2, u32 enc, u64 sec, void *dst, const void *src, u32 secsize, u32 num_secs);
 int se_aes_cmac(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size);
 int se_calc_sha256(void *dst, const void *src, u32 src_size);
+int se_calc_hmac_sha256(void *dst, const void *src, u32 src_size, const void *key, u32 key_size);
 
 #endif

source/sec/se_t210.h

@@ -36,6 +36,8 @@
 #define SE_SECURITY_0 0x000
 #define SE_KEY_SCHED_READ_SHIFT 3
 
+#define SE_TZRAM_SECURITY_0 0x004
+
 #define SE_CONFIG_REG_OFFSET		0x014
 #define SE_CONFIG_ENC_ALG_SHIFT		12
 #define SE_CONFIG_DEC_ALG_SHIFT		8
@@ -68,27 +70,26 @@
 #define SE_CONFIG_DEC_MODE(x)		(x << SE_CONFIG_DEC_MODE_SHIFT)
 
 #define SE_RNG_CONFIG_REG_OFFSET		0x340
-#define DRBG_MODE_SHIFT	0
-#define DRBG_MODE_NORMAL	0
-#define DRBG_MODE_FORCE_INSTANTION	1
-#define DRBG_MODE_FORCE_RESEED		2
-#define SE_RNG_CONFIG_MODE(x)		(x << DRBG_MODE_SHIFT)
+#define RNG_MODE_SHIFT	0
+#define RNG_MODE_NORMAL	0
+#define RNG_MODE_FORCE_INSTANTION	1
+#define RNG_MODE_FORCE_RESEED		2
+#define SE_RNG_CONFIG_MODE(x)		(x << RNG_MODE_SHIFT)
+#define RNG_SRC_SHIFT	2
+#define RNG_SRC_NONE	0
+#define RNG_SRC_ENTROPY	1
+#define RNG_SRC_LFSR	2
+#define SE_RNG_CONFIG_SRC(x)		(x << RNG_SRC_SHIFT)
 
 #define SE_RNG_SRC_CONFIG_REG_OFFSET	0x344
-#define DRBG_RO_ENT_SRC_SHIFT	1
-#define DRBG_RO_ENT_SRC_ENABLE	1
-#define DRBG_RO_ENT_SRC_DISABLE	0
-#define SE_RNG_SRC_CONFIG_RO_ENT_SRC(x)	(x << DRBG_RO_ENT_SRC_SHIFT)
-#define DRBG_RO_ENT_SRC_LOCK_SHIFT	0
-#define DRBG_RO_ENT_SRC_LOCK_ENABLE	1
-#define DRBG_RO_ENT_SRC_LOCK_DISABLE	0
-#define SE_RNG_SRC_CONFIG_RO_ENT_SRC_LOCK(x) (x << DRBG_RO_ENT_SRC_LOCK_SHIFT)
-
-#define DRBG_SRC_SHIFT	2
-#define DRBG_SRC_NONE	0
-#define DRBG_SRC_ENTROPY	1
-#define DRBG_SRC_LFSR	2
-#define SE_RNG_CONFIG_SRC(x)		(x << DRBG_SRC_SHIFT)
+#define RNG_SRC_RO_ENT_SHIFT	1
+#define RNG_SRC_RO_ENT_ENABLE	1
+#define RNG_SRC_RO_ENT_DISABLE	0
+#define SE_RNG_SRC_CONFIG_ENT_SRC(x)	(x << RNG_SRC_RO_ENT_SHIFT)
+#define RNG_SRC_RO_ENT_LOCK_SHIFT	 0
+#define RNG_SRC_RO_ENT_LOCK_ENABLE	 1
+#define RNG_SRC_RO_ENT_LOCK_DISABLE 0
+#define SE_RNG_SRC_CONFIG_ENT_SRC_LOCK(x) (x << RNG_SRC_RO_ENT_LOCK_SHIFT)
 
 #define SE_RNG_RESEED_INTERVAL_REG_OFFSET		0x348
 
@@ -117,6 +118,8 @@
 #define OP_DONE	1
 #define SE_OP_DONE(x, y)	((x) && (y << SE_OP_DONE_SHIFT))
 
+#define SE_CRYPTO_LAST_BLOCK	0x080
+
 #define SE_CRYPTO_REG_OFFSET		0x304
 #define SE_CRYPTO_HASH_SHIFT		0
 #define HASH_DISABLE	0
@@ -189,6 +192,7 @@
 #define SRK		6
 
 #define RSA_KEYTABLE	1
+#define AES_KEYTABLE	2
 #define SE_CONTEXT_SAVE_SRC(x)		(x << SE_CONTEXT_SAVE_SRC_SHIFT)
 
 #define SE_CONTEXT_SAVE_RSA_KEY_INDEX_SHIFT	16
@@ -209,8 +213,12 @@
 #define SE_INT_OP_DONE(x)			(x << SE_INT_OP_DONE_SHIFT)
 #define SE_INT_ERROR_SHIFT		16
 #define SE_INT_ERROR(x)			(x << SE_INT_ERROR_SHIFT)
+
 #define SE_STATUS_0 0x800
+#define SE_STATUS_0_STATE_WAIT_IN 3
+
 #define SE_ERR_STATUS_0			0x804
+#define SE_ERR_STATUS_0_SE_NS_ACCESS_CLEAR 0
 
 #define SE_CRYPTO_KEYTABLE_DST_REG_OFFSET	0X330
 #define SE_CRYPTO_KEYTABLE_DST_WORD_QUAD_SHIFT		0
@@ -231,11 +239,17 @@
 #define SE_SPARE_0_REG_OFFSET		0x80c
 
 #define SE_SHA_CONFIG_REG_OFFSET	0x200
-#define SHA_DISABLE		0
-#define SHA_ENABLE		1
+#define  SHA_CONTINUE  0
+#define  SHA_INIT_HASH 1
 
-#define SE_SHA_MSG_LENGTH_REG_OFFSET	0x204
-#define SE_SHA_MSG_LEFT_REG_OFFSET		0x214
+#define SE_SHA_MSG_LENGTH_0_REG_OFFSET	0x204
+#define SE_SHA_MSG_LENGTH_1_REG_OFFSET	0x208
+#define SE_SHA_MSG_LENGTH_2_REG_OFFSET	0x20C
+#define SE_SHA_MSG_LENGTH_3_REG_OFFSET	0x210
+#define SE_SHA_MSG_LEFT_0_REG_OFFSET	0x214
+#define SE_SHA_MSG_LEFT_1_REG_OFFSET	0x218
+#define SE_SHA_MSG_LEFT_2_REG_OFFSET	0x21C
+#define SE_SHA_MSG_LEFT_3_REG_OFFSET	0x220
 
 #define SE_HASH_RESULT_REG_COUNT	16
 #define SE_HASH_RESULT_REG_OFFSET	0x030
@@ -254,13 +268,24 @@
 						TEGRA_SE_RNG_DT_SIZE)
 
 #define TEGRA_SE_AES_CMAC_DIGEST_SIZE	16
-#define TEGRA_SE_RSA512_DIGEST_SIZE	64
+#define TEGRA_SE_RSA512_DIGEST_SIZE	    64
 #define TEGRA_SE_RSA1024_DIGEST_SIZE	128
 #define TEGRA_SE_RSA1536_DIGEST_SIZE	192
 #define TEGRA_SE_RSA2048_DIGEST_SIZE	256
 
 #define SE_KEY_TABLE_ACCESS_LOCK_OFFSET	0x280
+#define SE_KEY_TBL_DIS_KEY_LOCK_FLAG 0x80
+
 #define SE_KEY_TABLE_ACCESS_REG_OFFSET	0x284
+#define SE_KEY_TBL_DIS_KEYREAD_FLAG    (1 << 0)
+#define SE_KEY_TBL_DIS_KEYUPDATE_FLAG  (1 << 1)
+#define SE_KEY_TBL_DIS_OIVREAD_FLAG    (1 << 2)
+#define SE_KEY_TBL_DIS_OIVUPDATE_FLAG  (1 << 3)
+#define SE_KEY_TBL_DIS_UIVREAD_FLAG    (1 << 4)
+#define SE_KEY_TBL_DIS_UIVUPDATE_FLAG  (1 << 5)
+#define SE_KEY_TBL_DIS_KEYUSE_FLAG     (1 << 6)
+#define SE_KEY_TBL_DIS_KEY_ACCESS_FLAG 0x7F
+
 #define SE_KEY_READ_DISABLE_SHIFT		0
 #define SE_KEY_UPDATE_DISABLE_SHIFT		1
 
@@ -312,7 +337,16 @@
 #define TEGRA_SE_RSA_KEYSLOT_COUNT		2
 
 #define SE_RSA_KEYTABLE_ACCESS_LOCK_OFFSET	0x40C
+#define SE_RSA_KEY_TBL_DIS_KEY_LOCK_FLAG    0x80
+
 #define SE_RSA_KEYTABLE_ACCESS_REG_OFFSET	0x410
+#define SE_RSA_KEY_TBL_DIS_KEYREAD_FLAG         (1 << 0)
+#define SE_RSA_KEY_TBL_DIS_KEYUPDATE_FLAG       (1 << 1)
+#define SE_RSA_KEY_TBL_DIS_KEY_READ_UPDATE_FLAG (SE_RSA_KEY_TBL_DIS_KEYREAD_FLAG | SE_RSA_KEY_TBL_DIS_KEYUPDATE_FLAG)
+#define SE_RSA_KEY_TBL_DIS_KEYUSE_FLAG          (1 << 2)
+#define SE_RSA_KEY_TBL_DIS_KEYUSE_FLAG_SHIFT    (1 << 2)
+#define SE_RSA_KEY_TBL_DIS_KEY_ALL_COMMON_FLAG  7
+#define SE_RSA_KEY_TBL_DIS_KEY_ALL_FLAG        0x7F
 
 #define SE_RSA_KEYTABLE_ADDR	0x420
 #define SE_RSA_KEYTABLE_DATA	0x424

source/sec/tsec.c

@@ -18,10 +18,13 @@
 
 #include <string.h>
 
+#include "../hos/hos.h"
 #include "../sec/tsec.h"
 #include "../sec/tsec_t210.h"
 #include "../sec/se_t210.h"
+#include "../soc/bpmp.h"
 #include "../soc/clock.h"
+#include "../soc/kfuse.h"
 #include "../soc/smmu.h"
 #include "../soc/t210.h"
 #include "../mem/heap.h"
@@ -64,14 +67,20 @@ int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt)
 	u32 *pdir, *car, *fuse, *pmc, *flowctrl, *se, *mc, *iram, *evec;
 	u32 *pkg11_magic_off;
 
-	//Enable clocks.
+	bpmp_mmu_disable();
+	bpmp_clk_rate_set(BPMP_CLK_NORMAL);
+
+	// Enable clocks.
 	clock_enable_host1x();
+	usleep(2);
 	clock_enable_tsec();
 	clock_enable_sor_safe();
 	clock_enable_sor0();
 	clock_enable_sor1();
 	clock_enable_kfuse();
 
+	kfuse_wait_ready();
+
 	//Configure Falcon.
 	TSEC(TSEC_DMACTL) = 0;
 	TSEC(TSEC_IRQMSET) =
@@ -149,7 +158,7 @@ int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt)
 		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);
@@ -170,7 +179,7 @@ int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt)
 	}
 
 	//Execute firmware.
-	HOST1X(0x3300) = 0x34C2E1DA;
+	HOST1X(HOST1X_CH0_SYNC_SYNCPT_160) = 0x34C2E1DA;
 	TSEC(TSEC_STATUS) = 0;
 	TSEC(TSEC_BOOTKEYVER) = 1; // HOS uses key version 1.
 	TSEC(TSEC_BOOTVEC) = 0;
@@ -203,7 +212,7 @@ int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt)
 			res = -6;
 			smmu_deinit_for_tsec();
 
-			goto out;
+			goto out_free;
 		}
 
 		// Give some extra time to make sure PKG1.1 is decrypted.
@@ -211,7 +220,7 @@ int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt)
 
 		memcpy(tsec_keys, &key, 0x20);
 		memcpy(tsec_ctxt->pkg1, iram, 0x30000);
-		
+
 		smmu_deinit_for_tsec();
 
 		// for (int i = 0; i < kidx; i++)
@@ -247,7 +256,7 @@ int tsec_query(u8 *tsec_keys, u8 kb, tsec_ctxt_t *tsec_ctxt)
 		}
 
 		//Fetch result.
-		HOST1X(0x3300) = 0;
+		HOST1X(HOST1X_CH0_SYNC_SYNCPT_160) = 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);
@@ -272,7 +281,8 @@ out:;
 	clock_disable_sor0();
 	clock_disable_sor_safe();
 	clock_disable_tsec();
-	clock_disable_host1x();
+	bpmp_mmu_enable();
+	bpmp_clk_rate_set(BPMP_CLK_DEFAULT_BOOST);
 
 	return res;
 }

source/soc/bpmp.c

@@ -0,0 +1,308 @@
+/*
+ * BPMP-Lite Cache/MMU and Frequency driver for Tegra X1
+ *
+ * Copyright (c) 2019-2020 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/>.
+ */
+
+#include "bpmp.h"
+#include "clock.h"
+#include "t210.h"
+#include "../../common/memory_map.h"
+#include "../utils/util.h"
+
+#define BPMP_MMU_CACHE_LINE_SIZE        0x20
+
+#define BPMP_CACHE_CONFIG               0x0
+#define  CFG_ENABLE_CACHE               (1 << 0)
+#define  CFG_ENABLE_SKEW_ASSOC          (1 << 1)
+#define  CFG_DISABLE_RANDOM_ALLOC       (1 << 2)
+#define  CFG_FORCE_WRITE_THROUGH        (1 << 3)
+#define  CFG_NEVER_ALLOCATE             (1 << 6)
+#define  CFG_ENABLE_INTERRUPT           (1 << 7)
+#define  CFG_MMU_TAG_MODE(x)            (x << 8)
+#define   TAG_MODE_PARALLEL             0
+#define   TAG_MODE_TAG_FIRST            1
+#define   TAG_MODE_MMU_FIRST            2
+#define  CFG_DISABLE_WRITE_BUFFER       (1 << 10)
+#define  CFG_DISABLE_READ_BUFFER        (1 << 11)
+#define  CFG_ENABLE_HANG_DETECT         (1 << 12)
+#define  CFG_FULL_LINE_DIRTY            (1 << 13)
+#define  CFG_TAG_CHK_ABRT_ON_ERR        (1 << 14)
+#define  CFG_TAG_CHK_CLR_ERR            (1 << 15)
+#define  CFG_DISABLE_SAMELINE           (1 << 16)
+#define  CFG_OBS_BUS_EN                 (1 << 31)
+
+#define BPMP_CACHE_LOCK                 0x4
+#define  LOCK_LINE(x)                   (1 << x)
+
+#define BPMP_CACHE_SIZE                 0xC
+#define BPMP_CACHE_LFSR                 0x10
+
+#define BPMP_CACHE_TAG_STATUS           0x14
+#define  TAG_STATUS_TAG_CHECK_ERROR     (1 << 0)
+#define  TAG_STATUS_CONFLICT_ADDR_MASK  0xFFFFFFE0
+
+#define BPMP_CACHE_CLKEN_OVERRIDE       0x18
+#define  CLKEN_OVERRIDE_WR_MCCIF_CLKEN  (1 << 0)
+#define  CLKEN_OVERRIDE_RD_MCCIF_CLKEN  (1 << 1)
+
+#define BPMP_CACHE_MAINT_ADDR           0x20
+#define BPMP_CACHE_MAINT_DATA           0x24
+
+#define BPMP_CACHE_MAINT_REQ            0x28
+#define  MAINT_REQ_WAY_BITMAP(x)        ((x) << 8)
+
+#define BPMP_CACHE_INT_MASK             0x40
+#define BPMP_CACHE_INT_CLEAR            0x44
+#define BPMP_CACHE_INT_RAW_EVENT        0x48
+#define BPMP_CACHE_INT_STATUS           0x4C
+#define  INT_MAINT_DONE                 (1 << 0)
+#define  INT_MAINT_ERROR                (1 << 1)
+
+#define BPMP_CACHE_RB_CFG               0x80
+#define BPMP_CACHE_WB_CFG               0x84
+
+#define BPMP_CACHE_MMU_FALLBACK_ENTRY   0xA0
+#define BPMP_CACHE_MMU_SHADOW_COPY_MASK 0xA4
+
+#define BPMP_CACHE_MMU_CFG              0xAC
+#define  MMU_CFG_BLOCK_MAIN_ENTRY_WR    (1 << 0)
+#define  MMU_CFG_SEQ_EN                 (1 << 1)
+#define  MMU_CFG_TLB_EN                 (1 << 2)
+#define  MMU_CFG_SEG_CHECK_ALL_ENTRIES  (1 << 3)
+#define  MMU_CFG_ABORT_STORE_LAST       (1 << 4)
+#define  MMU_CFG_CLR_ABORT              (1 << 5)
+
+#define BPMP_CACHE_MMU_CMD              0xB0
+#define  MMU_CMD_NOP                    0
+#define  MMU_CMD_INIT                   1
+#define  MMU_CMD_COPY_SHADOW            2
+
+#define BPMP_CACHE_MMU_ABORT_STAT       0xB4
+#define  ABORT_STAT_UNIT_MASK           0x7
+#define  ABORT_STAT_UNIT_NONE           0
+#define  ABORT_STAT_UNIT_CACHE          1
+#define  ABORT_STAT_UNIT_SEQ            2
+#define  ABORT_STAT_UNIT_TLB            3
+#define  ABORT_STAT_UNIT_SEG            4
+#define  ABORT_STAT_UNIT_FALLBACK       5
+#define  ABORT_STAT_OVERLAP             (1 << 3)
+#define  ABORT_STAT_ENTRY               (0x1F << 4)
+#define  ABORT_STAT_TYPE_MASK           (3 << 16)
+#define  ABORT_STAT_TYPE_EXE            (0 << 16)
+#define  ABORT_STAT_TYPE_RD             (1 << 16)
+#define  ABORT_STAT_TYPE_WR             (2 << 16)
+#define  ABORT_STAT_SIZE                (3 << 18)
+#define  ABORT_STAT_SEQ                 (1 << 20)
+#define  ABORT_STAT_PROT                (1 << 21)
+
+#define BPMP_CACHE_MMU_ABORT_ADDR       0xB8
+#define BPMP_CACHE_MMU_ACTIVE_ENTRIES   0xBC
+
+#define BPMP_MMU_SHADOW_ENTRY_BASE      (BPMP_CACHE_BASE + 0x400)
+#define BPMP_MMU_MAIN_ENTRY_BASE        (BPMP_CACHE_BASE + 0x800)
+#define  MMU_EN_CACHED                  (1 << 0)
+#define  MMU_EN_EXEC                    (1 << 1)
+#define  MMU_EN_READ                    (1 << 2)
+#define  MMU_EN_WRITE                   (1 << 3)
+
+bpmp_mmu_entry_t mmu_entries[] =
+{
+	{ DRAM_START, 0xFFFFFFFF, MMU_EN_READ | MMU_EN_WRITE | MMU_EN_EXEC | MMU_EN_CACHED, true },
+	{ IRAM_BASE,  0x4003FFFF, MMU_EN_READ | MMU_EN_WRITE | MMU_EN_EXEC | MMU_EN_CACHED, true }
+};
+
+void bpmp_mmu_maintenance(u32 op, bool force)
+{
+	if (!force && !(BPMP_CACHE_CTRL(BPMP_CACHE_CONFIG) & CFG_ENABLE_CACHE))
+		return;
+
+	BPMP_CACHE_CTRL(BPMP_CACHE_INT_CLEAR) = INT_MAINT_DONE;
+
+	// This is a blocking operation.
+	BPMP_CACHE_CTRL(BPMP_CACHE_MAINT_REQ) = MAINT_REQ_WAY_BITMAP(0xF) | op;
+
+	while(!(BPMP_CACHE_CTRL(BPMP_CACHE_INT_RAW_EVENT) & INT_MAINT_DONE))
+		;
+
+	BPMP_CACHE_CTRL(BPMP_CACHE_INT_CLEAR) = BPMP_CACHE_CTRL(BPMP_CACHE_INT_RAW_EVENT);
+}
+
+void bpmp_mmu_set_entry(int idx, bpmp_mmu_entry_t *entry, bool apply)
+{
+	if (idx > 31)
+		return;
+
+	volatile bpmp_mmu_entry_t *mmu_entry = (bpmp_mmu_entry_t *)(BPMP_MMU_SHADOW_ENTRY_BASE + sizeof(bpmp_mmu_entry_t) * idx);
+
+	if (entry->enable)
+	{
+		mmu_entry->start_addr = ALIGN(entry->start_addr, BPMP_MMU_CACHE_LINE_SIZE);
+		mmu_entry->end_addr = ALIGN_DOWN(entry->end_addr, BPMP_MMU_CACHE_LINE_SIZE);
+		mmu_entry->attr = entry->attr;
+
+		BPMP_CACHE_CTRL(BPMP_CACHE_MMU_SHADOW_COPY_MASK) |= (1 << idx);
+
+		if (apply)
+			BPMP_CACHE_CTRL(BPMP_CACHE_MMU_CMD) = MMU_CMD_COPY_SHADOW;
+	}
+}
+
+void bpmp_mmu_enable()
+{
+	if (BPMP_CACHE_CTRL(BPMP_CACHE_CONFIG) & CFG_ENABLE_CACHE)
+		return;
+
+	// Init BPMP MMU.
+	BPMP_CACHE_CTRL(BPMP_CACHE_MMU_CMD) = MMU_CMD_INIT;
+	BPMP_CACHE_CTRL(BPMP_CACHE_MMU_FALLBACK_ENTRY) = MMU_EN_READ | MMU_EN_WRITE | MMU_EN_EXEC; // RWX for non-defined regions.
+	BPMP_CACHE_CTRL(BPMP_CACHE_MMU_CFG) = MMU_CFG_SEQ_EN | MMU_CFG_TLB_EN | MMU_CFG_ABORT_STORE_LAST;
+
+	// Init BPMP MMU entries.
+	BPMP_CACHE_CTRL(BPMP_CACHE_MMU_SHADOW_COPY_MASK) = 0;
+	for (u32 idx = 0; idx < (sizeof(mmu_entries) / sizeof(bpmp_mmu_entry_t)); idx++)
+		bpmp_mmu_set_entry(idx, &mmu_entries[idx], false);
+
+	BPMP_CACHE_CTRL(BPMP_CACHE_MMU_CMD) = MMU_CMD_COPY_SHADOW;
+
+	// Invalidate cache.
+	bpmp_mmu_maintenance(BPMP_MMU_MAINT_INVALID_WAY, true);
+
+	// Enable cache.
+	BPMP_CACHE_CTRL(BPMP_CACHE_CONFIG) = CFG_ENABLE_CACHE | CFG_FORCE_WRITE_THROUGH |
+		CFG_MMU_TAG_MODE(TAG_MODE_PARALLEL) | CFG_TAG_CHK_ABRT_ON_ERR;
+
+	// HW bug. Invalidate cache again.
+	bpmp_mmu_maintenance(BPMP_MMU_MAINT_INVALID_WAY, false);
+}
+
+void bpmp_mmu_disable()
+{
+	if (!(BPMP_CACHE_CTRL(BPMP_CACHE_CONFIG) & CFG_ENABLE_CACHE))
+		return;
+
+	// Clean and invalidate cache.
+	bpmp_mmu_maintenance(BPMP_MMU_MAINT_CLN_INV_WAY, false);
+
+	// Disable cache.
+	BPMP_CACHE_CTRL(BPMP_CACHE_CONFIG) = 0;
+}
+
+// APB clock affects RTC, PWM, MEMFETCH, APE, USB, SOR PWM,
+// I2C host, DC/DSI/DISP. UART gives extra stress.
+// 92: 100% success ratio. 93-94: 595-602MHz has 99% success ratio. 95: 608MHz less.
+const u8 pll_divn[] = {
+	0,   // BPMP_CLK_NORMAL:      408MHz  0% - 136MHz APB.
+	85,  // BPMP_CLK_HIGH_BOOST:  544MHz 33% - 136MHz APB.
+	90,  // BPMP_CLK_SUPER_BOOST: 576MHz 41% - 144MHz APB.
+	92   // BPMP_CLK_HYPER_BOOST: 589MHz 44% - 147MHz APB.
+	// Do not use for public releases!
+	//95   // BPMP_CLK_DEV_BOOST: 608MHz 49% - 152MHz APB.
+};
+
+bpmp_freq_t bpmp_clock_set = BPMP_CLK_NORMAL;
+
+void bpmp_clk_rate_get()
+{
+	bool clk_src_is_pllp = ((CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) >> 4) & 7) == 3;
+
+	if (clk_src_is_pllp)
+		bpmp_clock_set = BPMP_CLK_NORMAL;
+	else
+	{
+		bpmp_clock_set = BPMP_CLK_HIGH_BOOST;
+
+		u8 pll_divn_curr = (CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) >> 10) & 0xFF;
+		for (u32 i = 1; i < sizeof(pll_divn); i++)
+		{
+			if (pll_divn[i] == pll_divn_curr)
+			{
+				bpmp_clock_set = i;
+				break;
+			}
+		}
+	}
+}
+
+void bpmp_clk_rate_set(bpmp_freq_t fid)
+{
+	if (fid > (BPMP_CLK_MAX - 1))
+		fid = BPMP_CLK_MAX - 1;
+
+	if (bpmp_clock_set == fid)
+		return;
+
+	if (fid)
+	{
+		if (bpmp_clock_set)
+		{
+			// Restore to PLLP source during PLLC4 configuration.
+			CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = 0x20003333; // PLLP_OUT.
+			msleep(1); // Wait a bit for clock source change.
+		}
+
+		// Configure and enable PLLC.
+		clock_enable_pllc(pll_divn[fid]);
+
+		// Set SCLK / HCLK / PCLK.
+		CLOCK(CLK_RST_CONTROLLER_CLK_SYSTEM_RATE) = 3; // PCLK = HCLK / (3 + 1). HCLK == SCLK.
+		CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = 0x20003310; // PLLC_OUT1 for active and CLKM for idle.
+	}
+	else
+	{
+		CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = 0x20003330; // PLLP_OUT for active and CLKM for idle.
+		msleep(1); // Wait a bit for clock source change.
+		CLOCK(CLK_RST_CONTROLLER_CLK_SYSTEM_RATE) = 2; // PCLK = HCLK / (2 + 1). HCLK == SCLK.
+
+		// Disable PLLC to save power.
+		clock_disable_pllc();
+	}
+	bpmp_clock_set = fid;
+}
+
+// The following functions halt BPMP to reduce power while sleeping.
+// They are not as accurate as RTC at big values but they guarantee time+ delay.
+void bpmp_usleep(u32 us)
+{
+	u32 delay;
+
+	// Each iteration takes 1us.
+	while (us)
+	{
+		delay = (us > HALT_COP_MAX_CNT) ? HALT_COP_MAX_CNT : us;
+		us -= delay;
+
+		FLOW_CTLR(FLOW_CTLR_HALT_COP_EVENTS) = HALT_COP_WAIT_EVENT | HALT_COP_USEC | delay;
+	}
+}
+
+void bpmp_msleep(u32 ms)
+{
+	u32 delay;
+
+	// Iteration time is variable. ~200 - 1000us.
+	while (ms)
+	{
+		delay = (ms > HALT_COP_MAX_CNT) ? HALT_COP_MAX_CNT : ms;
+		ms -= delay;
+
+		FLOW_CTLR(FLOW_CTLR_HALT_COP_EVENTS) = HALT_COP_WAIT_EVENT | HALT_COP_MSEC | delay;
+	}
+}
+
+void bpmp_halt()
+{
+	FLOW_CTLR(FLOW_CTLR_HALT_COP_EVENTS) = HALT_COP_WAIT_EVENT | HALT_COP_JTAG;
+}

source/soc/bpmp.h

@@ -0,0 +1,68 @@
+/*
+ * BPMP-Lite Cache/MMU and Frequency driver for Tegra X1
+ *
+ * Copyright (c) 2019-2020 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 _BPMP_H_
+#define _BPMP_H_
+
+#include "../utils/types.h"
+
+typedef enum
+{
+	BPMP_MMU_MAINT_NOP                =  0,
+	BPMP_MMU_MAINT_CLEAN_PHY          =  1,
+	BPMP_MMU_MAINT_INVALID_PHY        =  2,
+	BPMP_MMU_MAINT_CLEAN_INVALID_PHY  =  3,
+	BPMP_MMU_MAINT_CLEAN_LINE         =  9,
+	BPMP_MMU_MAINT_INVALID_LINE       = 10,
+	BPMP_MMU_MAINT_CLEAN_INVALID_LINE = 11,
+	BPMP_MMU_MAINT_CLEAN_WAY          = 17,
+	BPMP_MMU_MAINT_INVALID_WAY        = 18,
+	BPMP_MMU_MAINT_CLN_INV_WAY        = 19
+} bpmp_maintenance_t;
+
+typedef struct _bpmp_mmu_entry_t
+{
+	u32 start_addr;
+	u32 end_addr;
+	u32 attr;
+	u32 enable;
+} bpmp_mmu_entry_t;
+
+typedef enum
+{
+	BPMP_CLK_NORMAL,      // 408MHz  0% - 136MHz APB.
+	BPMP_CLK_HIGH_BOOST,  // 544MHz 33% - 136MHz APB.
+	BPMP_CLK_SUPER_BOOST, // 576MHz 41% - 144MHz APB.
+	BPMP_CLK_HYPER_BOOST, // 589MHz 44% - 147MHz APB.
+	//BPMP_CLK_DEV_BOOST, // 608MHz 49% - 152MHz APB.
+	BPMP_CLK_MAX
+} bpmp_freq_t;
+
+#define BPMP_CLK_DEFAULT_BOOST BPMP_CLK_HYPER_BOOST
+
+void bpmp_mmu_maintenance(u32 op, bool force);
+void bpmp_mmu_set_entry(int idx, bpmp_mmu_entry_t *entry, bool apply);
+void bpmp_mmu_enable();
+void bpmp_mmu_disable();
+void bpmp_clk_rate_get();
+void bpmp_clk_rate_set(bpmp_freq_t fid);
+void bpmp_usleep(u32 us);
+void bpmp_msleep(u32 ms);
+void bpmp_halt();
+
+#endif

source/soc/clock.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2020 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,60 +20,77 @@
 #include "../utils/util.h"
 #include "../storage/sdmmc.h"
 
+/*
+ * CLOCK Peripherals:
+ * L   0 -  31
+ * H  32 -  63
+ * U  64 -  95
+ * V  96 - 127
+ * W 128 - 159
+ * X 160 - 191
+ * Y 192 - 223
+ */
+
 /* clock_t: reset, enable, source, index, clk_src, clk_div */
 
 static const clock_t _clock_uart[] = {
-/* UART A */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_UARTA, 6,    0, 0 },
-/* UART B */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_UARTB, 7,    0, 0 },
-/* UART C */ { CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_UARTC, 0x17, 0, 0 },
-/* UART D */ { 0 },
-/* UART E */ { 0 }
+/* UART A */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_UARTA,   6,  0, 2 },
+/* UART B */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_UARTB,   7,  0, 2 },
+/* UART C */ { CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_UARTC,   23, 0, 2 },
+/* UART D */ { CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_UARTD,   1,  0, 2 },
+/* UART E */ { CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_RST_CONTROLLER_CLK_SOURCE_UARTAPE, 20, 0, 2 }
 };
 
+//I2C default parameters - TLOW: 4, THIGH: 2, DEBOUNCE: 0, FM_DIV: 26.
 static const clock_t _clock_i2c[] = {
-/* I2C1 */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_I2C1, 0xC, 6, 0 },
-/* I2C2 */ { 0 },
-/* I2C3 */ { 0 },
-/* I2C4 */ { 0 },
-/* I2C5 */ { CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_I2C5, 0xF, 6, 0 },
-/* I2C6 */ { 0 }
+/* I2C1 */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_I2C1, 12, 0, 19 }, //20.4MHz -> 100KHz
+/* I2C2 */ { CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_I2C2, 22, 0, 4  }, //81.6MHz -> 400KHz
+/* I2C3 */ { CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_I2C3, 3,  0, 4  }, //81.6MHz -> 400KHz
+/* I2C4 */ { CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_I2C4, 7,  0, 19 }, //20.4MHz -> 100KHz
+/* I2C5 */ { CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_I2C5, 15, 0, 4  }, //81.6MHz -> 400KHz
+/* I2C6 */ { CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_RST_CONTROLLER_CLK_SOURCE_I2C6, 6,  0, 19 }  //20.4MHz -> 100KHz
 };
 
 static clock_t _clock_se = {
-	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_SE,     0x1F, 0, 0
+	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_SE,     31, 0, 0
 };
-static clock_t _clock_unk2 = {
-	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_NO_SOURCE,                        0x1E, 0, 0
+
+static clock_t _clock_tzram = {
+	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_NO_SOURCE,                        30, 0, 0
 };
 
 static clock_t _clock_host1x = {
-	CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X, 0x1C, 4, 3
+	CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X, 28, 4, 3
 };
 static clock_t _clock_tsec = {
-	CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_TSEC,   0x13, 0, 2
+	CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_TSEC,   19, 0, 2
 };
 static clock_t _clock_sor_safe = {
-	CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_NO_SOURCE,                        0x1E, 0, 0
+	CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_NO_SOURCE,                        30, 0, 0
 };
 static clock_t _clock_sor0 = {
-	CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_NO_SOURCE,                        0x16, 0, 0
+	CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_NO_SOURCE,                        22, 0, 0
 };
 static clock_t _clock_sor1 = {
-	CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_RST_CONTROLLER_CLK_SOURCE_SOR1,   0x17, 0, 2
+	CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_RST_CONTROLLER_CLK_SOURCE_SOR1,   23, 0, 2
 };
 static clock_t _clock_kfuse = {
-	CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_NO_SOURCE,                        8,    0, 0
+	CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_NO_SOURCE,                        8,  0, 0
 };
 
 static clock_t _clock_cl_dvfs =	{
-	CLK_RST_CONTROLLER_RST_DEVICES_W, CLK_RST_CONTROLLER_CLK_OUT_ENB_W, CLK_NO_SOURCE,                        0x1B, 0, 0
+	CLK_RST_CONTROLLER_RST_DEVICES_W, CLK_RST_CONTROLLER_CLK_OUT_ENB_W, CLK_NO_SOURCE,                        27, 0, 0
 };
 static clock_t _clock_coresight = {
-	CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_CSITE,     9, 0, 4
+	CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_CSITE,   9, 0, 4
 };
 
 static clock_t _clock_pwm = {
-	CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_PWM,    0x11, 6, 4
+	CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_PWM,    17, 6, 4 // Fref: 6.2MHz.
+};
+
+static clock_t _clock_sdmmc_legacy_tm = {
+	CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC_LEGACY_TM, 1, 4, 66
 };
 
 void clock_enable(const clock_t *clk)
@@ -86,6 +104,8 @@ void clock_enable(const clock_t *clk)
 		CLOCK(clk->source) = clk->clk_div | (clk->clk_src << 29);
 	// Enable.
 	CLOCK(clk->enable) = (CLOCK(clk->enable) & ~(1 << clk->index)) | (1 << clk->index);
+	usleep(2);
+
 	// Take clock off reset.
 	CLOCK(clk->reset) &= ~(1 << clk->index);
 }
@@ -108,6 +128,29 @@ void clock_enable_uart(u32 idx)
 	clock_enable(&_clock_uart[idx]);
 }
 
+void clock_disable_uart(u32 idx)
+{
+	clock_disable(&_clock_uart[idx]);
+}
+
+#define UART_SRC_CLK_DIV_EN (1 << 24)
+
+int clock_uart_use_src_div(u32 idx, u32 baud)
+{
+	u32 clk_src_div = CLOCK(_clock_uart[idx].source) & 0xE0000000;
+
+	if (baud == 1000000)
+		CLOCK(_clock_uart[idx].source) = clk_src_div | UART_SRC_CLK_DIV_EN | 49;
+	else
+	{
+		CLOCK(_clock_uart[idx].source) = clk_src_div | 2;
+
+		return 1;
+	}
+
+	return 0;
+}
+
 void clock_enable_i2c(u32 idx)
 {
 	clock_enable(&_clock_i2c[idx]);
@@ -123,9 +166,9 @@ void clock_enable_se()
 	clock_enable(&_clock_se);
 }
 
-void clock_enable_unk2()
+void clock_enable_tzram()
 {
-	clock_enable(&_clock_unk2);
+	clock_enable(&_clock_tzram);
 }
 
 void clock_enable_host1x()
@@ -224,6 +267,51 @@ void clock_disable_pwm()
 	clock_disable(&_clock_pwm);
 }
 
+void clock_enable_pllc(u32 divn)
+{
+	u8 pll_divn_curr = (CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) >> 10) & 0xFF;
+
+	// Check if already enabled and configured.
+	if ((CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) & PLLCX_BASE_ENABLE) && (pll_divn_curr == divn))
+		return;
+
+	// Take PLLC out of reset and set basic misc parameters.
+	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC) = 
+		((CLOCK(CLK_RST_CONTROLLER_PLLC_MISC) & 0xFFF0000F) & ~PLLC_MISC_RESET) | (0x80000 << 4); // PLLC_EXT_FRU.
+	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC_2) |= 0xF0 << 8; // PLLC_FLL_LD_MEM.
+
+	// Disable PLL and IDDQ in case they are on.
+	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) &= ~PLLCX_BASE_ENABLE;
+	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC_1) &= ~PLLC_MISC1_IDDQ;
+	usleep(10);
+
+	// Set PLLC dividers.
+	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) = (divn << 10) | 4; // DIVM: 4, DIVP: 1.
+
+	// Enable PLLC and wait for Phase and Frequency lock.
+	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) |= PLLCX_BASE_ENABLE;
+	while (!(CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) & PLLCX_BASE_LOCK))
+		;
+
+	// Disable PLLC_OUT1, enable reset and set div to 1.5.
+	CLOCK(CLK_RST_CONTROLLER_PLLC_OUT) = (1 << 8);
+
+	// Enable PLLC_OUT1 and bring it out of reset.
+	CLOCK(CLK_RST_CONTROLLER_PLLC_OUT) |= (PLLC_OUT1_CLKEN | PLLC_OUT1_RSTN_CLR);
+	msleep(1); // Wait a bit for PLL to stabilize.
+}
+
+void clock_disable_pllc()
+{
+	// Disable PLLC and PLLC_OUT1.
+	CLOCK(CLK_RST_CONTROLLER_PLLC_OUT) &= ~(PLLC_OUT1_CLKEN | PLLC_OUT1_RSTN_CLR);
+	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) &= ~PLLCX_BASE_ENABLE;
+	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) |= PLLCX_BASE_REF_DIS;
+	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC_1) |= PLLC_MISC1_IDDQ;
+	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC) |= PLLC_MISC_RESET;
+	usleep(10);
+}
+
 #define L_SWR_SDMMC1_RST (1 << 14)
 #define L_SWR_SDMMC2_RST (1 << 9)
 #define L_SWR_SDMMC4_RST (1 << 15)
@@ -362,57 +450,80 @@ static void _clock_sdmmc_clear_enable(u32 id)
 	}
 }
 
-static u32 _clock_sdmmc_table[8] = { 0 };
+static void _clock_sdmmc_config_legacy_tm()
+{
+	clock_t *clk = &_clock_sdmmc_legacy_tm;
+	if (!(CLOCK(clk->enable) & (1 << clk->index)))
+		clock_enable(clk);
+}
 
-#define PLLP_OUT0      0x0
+typedef struct _clock_sdmmc_t
+{
+	u32 clock;
+	u32 real_clock;
+} clock_sdmmc_t;
 
-static int _clock_sdmmc_config_clock_source_inner(u32 *pout, u32 id, u32 val)
+static clock_sdmmc_t _clock_sdmmc_table[4] = { 0 };
+
+#define SDMMC_CLOCK_SRC_PLLP_OUT0      0x0
+#define SDMMC_CLOCK_SRC_PLLC4_OUT2     0x3
+#define SDMMC4_CLOCK_SRC_PLLC4_OUT2_LJ 0x1
+
+static int _clock_sdmmc_config_clock_host(u32 *pclock, u32 id, u32 val)
 {
 	u32 divisor = 0;
-	u32 source = PLLP_OUT0;
+	u32 source = SDMMC_CLOCK_SRC_PLLP_OUT0;
 
+	if (id > SDMMC_4)
+		return 0;
+
+	// Get IO clock divisor.
 	switch (val)
 	{
 	case 25000:
-		*pout = 24728;
-		divisor = 31;
+		*pclock = 24728;
+		divisor = 31; // 16.5 div.
 		break;
 	case 26000:
-		*pout = 25500;
-		divisor = 30;
+		*pclock = 25500;
+		divisor = 30; // 16 div.
 		break;
 	case 40800:
-		*pout = 40800;
-		divisor = 18;
+		*pclock = 40800;
+		divisor = 18; // 10 div.
 		break;
 	case 50000:
-		*pout = 48000;
-		divisor = 15;
+		*pclock = 48000;
+		divisor = 15; // 8.5 div.
 		break;
 	case 52000:
-		*pout = 51000;
-		divisor = 14;
+		*pclock = 51000;
+		divisor = 14; // 8 div.
 		break;
 	case 100000:
-		*pout = 90667;
-		divisor = 7;
+		*pclock = 90667;
+		divisor = 7;  // 4.5 div.
+		break;
+	case 164000:
+		*pclock = 163200;
+		divisor = 3;  // 2.5 div.
 		break;
 	case 200000:
-		*pout = 163200;
-		divisor = 3;
-		break;
-	case 208000:
-		*pout = 204000;
-		divisor = 2;
+		*pclock = 204000;
+		divisor = 2;  // 2 div.
 		break;
 	default:
-		*pout = 24728;
-		divisor = 31;
+		*pclock = 24728;
+		divisor = 31; // 16.5 div.
 	}
 
-	_clock_sdmmc_table[2 * id] = val;
-	_clock_sdmmc_table[2 * id + 1] = *pout;
+	_clock_sdmmc_table[id].clock = val;
+	_clock_sdmmc_table[id].real_clock = *pclock;
 
+	// Set SDMMC legacy timeout clock.
+	_clock_sdmmc_config_legacy_tm();
+
+	// Set SDMMC clock.
 	switch (id)
 	{
 	case SDMMC_1:
@@ -432,69 +543,75 @@ static int _clock_sdmmc_config_clock_source_inner(u32 *pout, u32 id, u32 val)
 	return 1;
 }
 
-void clock_sdmmc_config_clock_source(u32 *pout, u32 id, u32 val)
+void clock_sdmmc_config_clock_source(u32 *pclock, u32 id, u32 val)
 {
-	if (_clock_sdmmc_table[2 * id] == val)
+	if (_clock_sdmmc_table[id].clock == val)
 	{
-		*pout = _clock_sdmmc_table[2 * id + 1];
+		*pclock = _clock_sdmmc_table[id].real_clock;
 	}
 	else
 	{
 		int is_enabled = _clock_sdmmc_is_enabled(id);
 		if (is_enabled)
 			_clock_sdmmc_clear_enable(id);
-		_clock_sdmmc_config_clock_source_inner(pout, id, val);
+		_clock_sdmmc_config_clock_host(pclock, id, val);
 		if (is_enabled)
 			_clock_sdmmc_set_enable(id);
 		_clock_sdmmc_is_reset(id);
 	}
 }
 
-void clock_sdmmc_get_params(u32 *pout, u16 *pdivisor, u32 type)
+void clock_sdmmc_get_card_clock_div(u32 *pclock, u16 *pdivisor, u32 type)
 {
+	// Get Card clock divisor.
 	switch (type)
 	{
-	case 0:
-		*pout = 26000;
+	case SDHCI_TIMING_MMC_ID: // Actual IO Freq: 380.59 KHz.
+		*pclock = 26000;
 		*pdivisor = 66;
 		break;
-	case 1:
-		*pout = 26000;
+	case SDHCI_TIMING_MMC_LS26:
+		*pclock = 26000;
 		*pdivisor = 1;
 		break;
-	case 2:
-		*pout = 52000;
+	case SDHCI_TIMING_MMC_HS52:
+		*pclock = 52000;
 		*pdivisor = 1;
 		break;
-	case 3:
-	case 4:
-	case 11:
-		*pout = 200000;
+	case SDHCI_TIMING_MMC_HS200:
+	case SDHCI_TIMING_MMC_HS400:
+	case SDHCI_TIMING_UHS_SDR104:
+		*pclock = 200000;
 		*pdivisor = 1;
 		break;
-	case 5:
-		*pout = 25000;
+	case SDHCI_TIMING_SD_ID: // Actual IO Freq: 380.43 KHz.
+		*pclock = 25000;
 		*pdivisor = 64;
 		break;
-	case 6:
-	case 8:
-		*pout = 25000;
+	case SDHCI_TIMING_SD_DS12:
+	case SDHCI_TIMING_UHS_SDR12:
+		*pclock = 25000;
 		*pdivisor = 1;
 		break;
-	case 7:
-		*pout = 50000;
+	case SDHCI_TIMING_SD_HS25:
+	case SDHCI_TIMING_UHS_SDR25:
+		*pclock = 50000;
 		*pdivisor = 1;
 		break;
-	case 10:
-		*pout = 100000;
+	case SDHCI_TIMING_UHS_SDR50:
+		*pclock = 100000;
 		*pdivisor = 1;
 		break;
-	case 13:
-		*pout = 40800;
+	case SDHCI_TIMING_UHS_SDR82:
+		*pclock = 164000;
 		*pdivisor = 1;
 		break;
-	case 14:
-		*pout = 200000;
+	case SDHCI_TIMING_UHS_DDR50:
+		*pclock = 40800;
+		*pdivisor = 1;
+		break;
+	case SDHCI_TIMING_MMC_DDR52: // Actual IO Freq: 49.92 MHz.
+		*pclock = 200000;
 		*pdivisor = 2;
 		break;
 	}
@@ -507,15 +624,15 @@ int clock_sdmmc_is_not_reset_and_enabled(u32 id)
 
 void clock_sdmmc_enable(u32 id, u32 val)
 {
-	u32 div = 0;
+	u32 clock = 0;
 
 	if (_clock_sdmmc_is_enabled(id))
 		_clock_sdmmc_clear_enable(id);
 	_clock_sdmmc_set_reset(id);
-	_clock_sdmmc_config_clock_source_inner(&div, id, val);
+	_clock_sdmmc_config_clock_host(&clock, id, val);
 	_clock_sdmmc_set_enable(id);
 	_clock_sdmmc_is_reset(id);
-	usleep((100000 + div - 1) / div);
+	usleep((100000 + clock - 1) / clock);
 	_clock_sdmmc_clear_reset(id);
 	_clock_sdmmc_is_reset(id);
 }

source/soc/clock.h

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2020 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,
@@ -35,21 +36,33 @@
 #define CLK_RST_CONTROLLER_MISC_CLK_ENB 0x48
 #define CLK_RST_CONTROLLER_OSC_CTRL 0x50
 #define CLK_RST_CONTROLLER_PLLC_BASE 0x80
+#define CLK_RST_CONTROLLER_PLLC_OUT 0x84
 #define CLK_RST_CONTROLLER_PLLC_MISC 0x88
+#define CLK_RST_CONTROLLER_PLLC_MISC_1 0x8C
 #define CLK_RST_CONTROLLER_PLLM_BASE 0x90
 #define CLK_RST_CONTROLLER_PLLM_MISC1 0x98
 #define CLK_RST_CONTROLLER_PLLM_MISC2 0x9C
 #define CLK_RST_CONTROLLER_PLLP_BASE 0xA0
+#define CLK_RST_CONTROLLER_PLLA_BASE 0xB0
+#define CLK_RST_CONTROLLER_PLLA_OUT 0xB4
+#define CLK_RST_CONTROLLER_PLLA_MISC1 0xB8
+#define CLK_RST_CONTROLLER_PLLA_MISC 0xBC
+#define CLK_RST_CONTROLLER_PLLU_BASE 0xC0
+#define CLK_RST_CONTROLLER_PLLU_MISC 0xCC
 #define CLK_RST_CONTROLLER_PLLD_BASE 0xD0
+#define CLK_RST_CONTROLLER_PLLD_MISC1 0xD8
+#define CLK_RST_CONTROLLER_PLLD_MISC 0xDC
 #define CLK_RST_CONTROLLER_PLLX_BASE 0xE0
 #define CLK_RST_CONTROLLER_PLLX_MISC 0xE4
 #define CLK_RST_CONTROLLER_PLLE_BASE 0xE8
 #define CLK_RST_CONTROLLER_PLLE_MISC 0xEC
 #define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRA 0xF8
 #define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRB 0xFC
+#define CLK_RST_CONTROLLER_CLK_SOURCE_I2S2 0x100
 #define CLK_RST_CONTROLLER_CLK_SOURCE_PWM 0x110
 #define CLK_RST_CONTROLLER_CLK_SOURCE_I2C1 0x124
 #define CLK_RST_CONTROLLER_CLK_SOURCE_I2C5 0x128
+#define CLK_RST_CONTROLLER_CLK_SOURCE_DISP1 0x138
 #define CLK_RST_CONTROLLER_CLK_SOURCE_VI 0x148
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC1 0x150
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC2 0x154
@@ -57,11 +70,14 @@
 #define CLK_RST_CONTROLLER_CLK_SOURCE_UARTA 0x178
 #define CLK_RST_CONTROLLER_CLK_SOURCE_UARTB 0x17C
 #define CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X 0x180
+#define CLK_RST_CONTROLLER_CLK_SOURCE_I2C2 0x198
+#define CLK_RST_CONTROLLER_CLK_SOURCE_EMC 0x19C
 #define CLK_RST_CONTROLLER_CLK_SOURCE_UARTC 0x1A0
 #define CLK_RST_CONTROLLER_CLK_SOURCE_I2C3 0x1B8
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC3 0x1BC
+#define CLK_RST_CONTROLLER_CLK_SOURCE_UARTD 0x1C0
 #define CLK_RST_CONTROLLER_CLK_SOURCE_CSITE 0x1D4
-#define CLK_RST_CONTROLLER_CLK_SOURCE_EMC 0x19C
+#define CLK_RST_CONTROLLER_CLK_SOURCE_I2S1 0x1D8
 #define CLK_RST_CONTROLLER_CLK_SOURCE_TSEC 0x1F4
 #define CLK_RST_CONTROLLER_CLK_OUT_ENB_X 0x280
 #define CLK_RST_CONTROLLER_CLK_ENB_X_SET 0x284
@@ -95,29 +111,66 @@
 #define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRC 0x3A0
 #define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRD 0x3A4
 #define CLK_RST_CONTROLLER_CLK_SOURCE_MSELECT 0x3B4
+#define CLK_RST_CONTROLLER_CLK_SOURCE_I2C4 0x3C4
+#define CLK_RST_CONTROLLER_CLK_SOURCE_EXTPERIPH1 0x3EC
+#define CLK_RST_CONTROLLER_CLK_SOURCE_SYS 0x400
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SOR1 0x410
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SE 0x42C
+#define CLK_RST_CONTROLLER_RST_DEV_V_SET 0x430
+#define CLK_RST_CONTROLLER_RST_DEV_V_CLR 0x434
+#define CLK_RST_CONTROLLER_RST_DEV_W_SET 0x438
+#define CLK_RST_CONTROLLER_RST_DEV_W_CLR 0x43C
 #define CLK_RST_CONTROLLER_CLK_ENB_V_SET 0x440
+#define CLK_RST_CONTROLLER_CLK_ENB_V_CLR 0x444
 #define CLK_RST_CONTROLLER_CLK_ENB_W_SET 0x448
 #define CLK_RST_CONTROLLER_CLK_ENB_W_CLR 0x44C
 #define CLK_RST_CONTROLLER_RST_CPUG_CMPLX_SET 0x450
 #define CLK_RST_CONTROLLER_RST_CPUG_CMPLX_CLR 0x454
+#define CLK_RST_CONTROLLER_UTMIP_PLL_CFG0 0x480
+#define CLK_RST_CONTROLLER_UTMIP_PLL_CFG1 0x484
 #define CLK_RST_CONTROLLER_UTMIP_PLL_CFG2 0x488
 #define CLK_RST_CONTROLLER_PLLE_AUX 0x48C
 #define CLK_RST_CONTROLLER_AUDIO_SYNC_CLK_I2S0 0x4A0
 #define CLK_RST_CONTROLLER_PLLX_MISC_3 0x518
+#define CLK_RST_CONTROLLER_UTMIPLL_HW_PWRDN_CFG0 0x52C
 #define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRE 0x554
 #define CLK_RST_CONTROLLER_SPARE_REG0 0x55C
+#define CLK_RST_CONTROLLER_PLLC4_BASE 0x5A4
+#define CLK_RST_CONTROLLER_PLLC4_MISC 0x5A8
+#define CLK_RST_CONTROLLER_PLLC_MISC_2 0x5D0
+#define CLK_RST_CONTROLLER_PLLC4_OUT 0x5E4
 #define CLK_RST_CONTROLLER_PLLMB_BASE 0x5E8
 #define CLK_RST_CONTROLLER_CLK_SOURCE_DSIA_LP 0x620
+#define CLK_RST_CONTROLLER_CLK_SOURCE_I2C6 0x65C
 #define CLK_RST_CONTROLLER_CLK_SOURCE_EMC_DLL 0x664
-#define CLK_RST_CONTROLLER_CLK_SOURCE_UART_FST_MIP_CAL 0x66C
+#define CLK_RST_CONTROLLER_CLK_SOURCE_UART_FST_MIPI_CAL 0x66C
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC_LEGACY_TM 0x694
 #define CLK_RST_CONTROLLER_CLK_SOURCE_NVENC 0x6A0
+#define CLK_RST_CONTROLLER_CLK_SOURCE_USB2_HSIC_TRK 0x6CC
 #define CLK_RST_CONTROLLER_SE_SUPER_CLK_DIVIDER 0x704
+#define CLK_RST_CONTROLLER_CLK_SOURCE_UARTAPE 0x710
 
 #define CLK_NO_SOURCE 0x0
 
+/*! PLL control and status bits */
+#define PLLCX_BASE_ENABLE    (1 << 30)
+#define PLLCX_BASE_REF_DIS   (1 << 29)
+#define PLLCX_BASE_LOCK      (1 << 27)
+
+#define PLLA_BASE_IDDQ       (1 << 25)
+#define PLLA_OUT0_CLKEN      (1 << 1)
+#define PLLA_OUT0_RSTN_CLR   (1 << 0)
+
+#define PLLC_MISC_RESET      (1 << 30)
+#define PLLC_MISC1_IDDQ      (1 << 27)
+#define PLLC_OUT1_CLKEN      (1 << 1)
+#define PLLC_OUT1_RSTN_CLR   (1 << 0)
+
+#define PLLC4_MISC_EN_LCKDET (1 << 30)
+#define PLLC4_BASE_IDDQ      (1 << 18)
+#define PLLC4_OUT3_CLKEN     (1 << 1)
+#define PLLC4_OUT3_RSTN_CLR  (1 << 0)
+
 /*! Generic clock descriptor. */
 typedef struct _clock_t
 {
@@ -136,10 +189,12 @@ void clock_disable(const clock_t *clk);
 /*! Clock control for specific hardware portions. */
 void clock_enable_fuse(bool enable);
 void clock_enable_uart(u32 idx);
+void clock_disable_uart(u32 idx);
+int  clock_uart_use_src_div(u32 idx, u32 baud);
 void clock_enable_i2c(u32 idx);
 void clock_disable_i2c(u32 idx);
 void clock_enable_se();
-void clock_enable_unk2();
+void clock_enable_tzram();
 void clock_enable_host1x();
 void clock_disable_host1x();
 void clock_enable_tsec();
@@ -158,9 +213,11 @@ void clock_enable_coresight();
 void clock_disable_coresight();
 void clock_enable_pwm();
 void clock_disable_pwm();
-void clock_sdmmc_config_clock_source(u32 *pout, u32 id, u32 val);
-void clock_sdmmc_get_params(u32 *pout, u16 *pdivisor, u32 type);
-int clock_sdmmc_is_not_reset_and_enabled(u32 id);
+void clock_enable_pllc(u32 divn);
+void clock_disable_pllc();
+void clock_sdmmc_config_clock_source(u32 *pclock, u32 id, u32 val);
+void clock_sdmmc_get_card_clock_div(u32 *pclock, u16 *pdivisor, u32 type);
+int  clock_sdmmc_is_not_reset_and_enabled(u32 id);
 void clock_sdmmc_enable(u32 id, u32 val);
 void clock_sdmmc_disable(u32 id);
 

source/soc/cluster.c

@@ -32,18 +32,21 @@ void _cluster_enable_power()
 	// Enable cores power.
 	// 1-3.x: MAX77621_NFSR_ENABLE.
 	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_CONTROL1_REG,
-		MAX77621_AD_ENABLE | MAX77621_NFSR_ENABLE | MAX77621_SNS_ENABLE);
+		MAX77621_AD_ENABLE | MAX77621_NFSR_ENABLE | MAX77621_SNS_ENABLE | MAX77621_RAMP_12mV_PER_US);
 	// 1.0.0-3.x: MAX77621_T_JUNCTION_120 | MAX77621_CKKADV_TRIP_DISABLE | MAX77621_INDUCTOR_NOMINAL.
 	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_CONTROL2_REG,
 		MAX77621_T_JUNCTION_120 | MAX77621_WDTMR_ENABLE | MAX77621_CKKADV_TRIP_75mV_PER_US| MAX77621_INDUCTOR_NOMINAL);
-	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_VOUT_REG, MAX77621_VOUT_ENABLE | 0x37);
-	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_VOUT_DVC_REG, MAX77621_VOUT_ENABLE | 0x37);
+	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_VOUT_REG, MAX77621_VOUT_ENABLE | MAX77621_VOUT_0_95V);
+	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_VOUT_DVC_REG, MAX77621_VOUT_ENABLE | MAX77621_VOUT_0_95V);
 }
 
-int _cluster_pmc_enable_partition(u32 part, u32 toggle, bool enable)
+int _cluster_pmc_enable_partition(u32 part, int enable)
 {
-	// Check if the partition has already been turned on.
-	if (enable && PMC(APBDEV_PMC_PWRGATE_STATUS) & part)
+	u32 part_mask = 1 << part;
+	u32 desired_state = enable << part;
+
+	// Check if the partition has the state we want.
+	if ((PMC(APBDEV_PMC_PWRGATE_STATUS) & part_mask) == desired_state)
 		return 1;
 
 	u32 i = 5001;
@@ -55,12 +58,13 @@ int _cluster_pmc_enable_partition(u32 part, u32 toggle, bool enable)
 			return 0;
 	}
 
-	PMC(APBDEV_PMC_PWRGATE_TOGGLE) = toggle | (enable ? 0x100 : 0);
+	// Toggle power gating.
+	PMC(APBDEV_PMC_PWRGATE_TOGGLE) = part | 0x100;
 
 	i = 5001;
 	while (i > 0)
 	{
-		if (PMC(APBDEV_PMC_PWRGATE_STATUS) & part)
+		if ((PMC(APBDEV_PMC_PWRGATE_STATUS) & part_mask) == desired_state)
 			break;
 		usleep(1);
 		i--;
@@ -76,9 +80,9 @@ void cluster_boot_cpu0(u32 entry)
 
 	_cluster_enable_power();
 
-	if (!(CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) & 0x40000000))
+	if (!(CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) & 0x40000000)) // PLLX_ENABLE.
 	{
-		CLOCK(CLK_RST_CONTROLLER_PLLX_MISC_3) &= 0xFFFFFFF7;
+		CLOCK(CLK_RST_CONTROLLER_PLLX_MISC_3) &= 0xFFFFFFF7; // Disable IDDQ.
 		usleep(2);
 		CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) = 0x80404E02;
 		CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) = 0x404E02;
@@ -103,11 +107,11 @@ void cluster_boot_cpu0(u32 entry)
 	CLOCK(CLK_RST_CONTROLLER_CPU_SOFTRST_CTRL2) &= 0xFFFFF000;
 
 	// Enable CPU rail.
-	_cluster_pmc_enable_partition(1, 0, true);
+	_cluster_pmc_enable_partition(0, 1);
 	// Enable cluster 0 non-CPU.
-	_cluster_pmc_enable_partition(0x8000, 15, true);
+	_cluster_pmc_enable_partition(15, 1);
 	// Enable CE0.
-	_cluster_pmc_enable_partition(0x4000, 14, true);
+	_cluster_pmc_enable_partition(14, 1);
 
 	// Request and wait for RAM repair.
 	FLOW_CTLR(FLOW_CTLR_RAM_REPAIR) = 1;
@@ -117,12 +121,15 @@ void cluster_boot_cpu0(u32 entry)
 	EXCP_VEC(EVP_CPU_RESET_VECTOR) = 0;
 
 	// Set reset vector.
-	SB(SB_AA64_RESET_LOW) = entry | 1;
+	SB(SB_AA64_RESET_LOW) = entry | SB_AA64_RST_AARCH64_MODE_EN;
 	SB(SB_AA64_RESET_HIGH) = 0;
 	// Non-secure reset vector write disable.
-	SB(SB_CSR) = 2;
+	SB(SB_CSR) = SB_CSR_NS_RST_VEC_WR_DIS;
 	(void)SB(SB_CSR);
 
+	// Tighten up the security aperture.
+	// MC(MC_TZ_SECURITY_CTRL) = 1;
+
 	// Clear MSELECT reset.
 	CLOCK(CLK_RST_CONTROLLER_RST_DEVICES_V) &= 0xFFFFFFF7;
 	// Clear NONCPU reset.

source/soc/cluster.h

@@ -19,19 +19,6 @@
 
 #include "../utils/types.h"
 
-/*! Flow controller registers. */
-#define FLOW_CTLR_HALT_CPU0_EVENTS 0x0
-#define FLOW_CTLR_HALT_CPU1_EVENTS 0x14
-#define FLOW_CTLR_HALT_CPU2_EVENTS 0x1C
-#define FLOW_CTLR_HALT_CPU3_EVENTS 0x24
-#define FLOW_CTLR_HALT_COP_EVENTS 0x4
-#define FLOW_CTLR_CPU0_CSR 0x8
-#define FLOW_CTLR_CPU1_CSR 0x18
-#define FLOW_CTLR_CPU2_CSR 0x20
-#define FLOW_CTLR_CPU3_CSR 0x28
-#define FLOW_CTLR_RAM_REPAIR 0x40
-#define FLOW_CTLR_BPMP_CLUSTER_CONTROL 0x98
-
 void cluster_boot_cpu0(u32 entry);
 
 #endif

source/soc/fuse.c

@@ -22,6 +22,34 @@
 #include "../soc/fuse.h"
 #include "../soc/t210.h"
 
+#define ARRAYSIZE(x) (sizeof(x) / sizeof(*x))
+
+static const u32 evp_thunk_template[] = {
+	0xe92d0007, //   STMFD   SP!, {R0-R2}
+	0xe1a0200e, //   MOV     R2, LR
+	0xe2422002, //   SUB     R2, R2, #2
+	0xe5922000, //   LDR     R2, [R2]
+	0xe20220ff, //   AND     R2, R2, #0xFF
+	0xe1a02082, //   MOV     R2, R2,LSL#1
+	0xe59f001c, //   LDR     R0, =evp_thunk_template
+	0xe59f101c, //   LDR     R1, =thunk_end
+	0xe0411000, //   SUB     R1, R1, R0
+	0xe59f0018, //   LDR     R0, =iram_evp_thunks
+	0xe0800001, //   ADD     R0, R0, R1
+	0xe0822000, //   ADD     R2, R2, R0
+	0xe3822001, //   ORR     R2, R2, #1
+	0xe8bd0003, //   LDMFD   SP!, {R0,R1}
+	0xe12fff12, //   BX      R2
+	0x001007b0, // off_1007EC DCD evp_thunk_template
+	0x001007f8, // off_1007F0 DCD thunk_end
+	0x40004c30, // off_1007F4 DCD iram_evp_thunks
+	// thunk_end is here
+};
+static const u32 evp_thunk_template_len = sizeof(evp_thunk_template);
+
+// treated as 12bit values
+static const u32 hash_vals[] = {1, 2, 4, 8, 0, 3, 5, 6, 7, 9, 10, 11};
+
 void fuse_disable_program()
 {
 	FUSE(FUSE_DISABLEREGPROGRAM) = 1;
@@ -31,3 +59,291 @@ u32 fuse_read_odm(u32 idx)
 {
 	return FUSE(FUSE_RESERVED_ODMX(idx));
 }
+
+void fuse_wait_idle()
+{
+	u32 ctrl;
+	do
+	{
+		ctrl = FUSE(FUSE_CTRL);
+	} while (((ctrl >> 16) & 0x1f) != 4);
+}
+
+u32 fuse_read(u32 addr)
+{
+	FUSE(FUSE_ADDR) = addr;
+	FUSE(FUSE_CTRL) = (FUSE(FUSE_ADDR) & ~FUSE_CMD_MASK) | FUSE_READ;
+	fuse_wait_idle();
+	return FUSE(FUSE_RDATA);
+}
+
+void fuse_read_array(u32 *words)
+{
+	for (u32 i = 0; i < 192; i++)
+		words[i] = fuse_read(i);
+}
+
+static u32 _parity32_even(u32 *words, u32 count)
+{
+	u32 acc = words[0];
+	for (u32 i = 1; i < count; i++)
+	{
+		acc ^= words[i];
+	}
+	u32 lo = ((acc & 0xffff) ^ (acc >> 16)) & 0xff;
+	u32 hi = ((acc & 0xffff) ^ (acc >> 16)) >> 8;
+	u32 x = hi ^ lo;
+	lo = ((x & 0xf) ^ (x >> 4)) & 3;
+	hi = ((x & 0xf) ^ (x >> 4)) >> 2;
+	x = hi ^ lo;
+
+	return (x & 1) ^ (x >> 1);
+}
+
+static int _patch_hash_one(u32 *word)
+{
+	u32 bits20_31 = *word & 0xfff00000;
+	u32 parity_bit = _parity32_even(&bits20_31, 1);
+	u32 hash = 0;
+	for (u32 i = 0; i < 12; i++)
+	{
+		if (*word & (1 << (20 + i)))
+		{
+			hash ^= hash_vals[i];
+		}
+	}
+	if (hash == 0)
+	{
+		if (parity_bit == 0)
+		{
+			return 0;
+		}
+		*word ^= 1 << 24;
+		return 1;
+	}
+	if (parity_bit == 0)
+	{
+		return 3;
+	}
+	for (u32 i = 0; i < ARRAYSIZE(hash_vals); i++)
+	{
+		if (hash_vals[i] == hash)
+		{
+			*word ^= 1 << (20 + i);
+			return 1;
+		}
+	}
+	return 2;
+}
+
+static int _patch_hash_multi(u32 *words, u32 count)
+{
+	u32 parity_bit = _parity32_even(words, count);
+	u32 bits0_14 = words[0] & 0x7fff;
+	u32 bit15 = words[0] & 0x8000;
+	u32 bits16_19 = words[0] & 0xf0000;
+
+	u32 hash = 0;
+	words[0] = bits16_19;
+	for (u32 i = 0; i < count; i++)
+	{
+		u32 w = words[i];
+		if (w)
+		{
+			for (u32 bitpos = 0; bitpos < 32; bitpos++)
+			{
+				if ((w >> bitpos) & 1)
+				{
+					hash ^= 0x4000 + i * 32 + bitpos;
+				}
+			}
+		}
+	}
+	hash ^= bits0_14;
+	// stupid but this is what original code does.
+	// equivalent to original words[0] &= 0xfff00000
+	words[0] = bits16_19 ^ bit15 ^ bits0_14;
+
+	if (hash == 0)
+	{
+		if (parity_bit == 0)
+		{
+			return 0;
+		}
+		words[0] ^= 0x8000;
+		return 1;
+	}
+	if (parity_bit == 0)
+	{
+		return 3;
+	}
+	u32 bitcount = hash - 0x4000;
+	if (bitcount < 16 || bitcount >= count * 32)
+	{
+		u32 num_set = 0;
+		for (u32 bitpos = 0; bitpos < 15; bitpos++)
+		{
+			if ((hash >> bitpos) & 1)
+			{
+				num_set++;
+			}
+		}
+		if (num_set != 1)
+		{
+			return 2;
+		}
+		words[0] ^= hash;
+		return 1;
+	}
+	words[bitcount / 32] ^= 1 << (hash & 0x1f);
+	return 1;
+}
+
+int fuse_read_ipatch(void (*ipatch)(u32 offset, u32 value))
+{
+	u32 words[80];
+	u32 word_count;
+	u32 word_addr;
+	u32 word0 = 0;
+	u32 total_read = 0;
+
+	word_count = FUSE(FUSE_FIRST_BOOTROM_PATCH_SIZE);
+	word_count &= 0x7F;
+	word_addr = 191;
+
+	while (word_count)
+	{
+		total_read += word_count;
+		if (total_read >= ARRAYSIZE(words))
+		{
+			break;
+		}
+
+		for (u32 i = 0; i < word_count; i++)
+			words[i] = fuse_read(word_addr--);
+
+		word0 = words[0];
+		if (_patch_hash_multi(words, word_count) >= 2)
+		{
+			return 1;
+		}
+		u32 ipatch_count = (words[0] >> 16) & 0xF;
+		if (ipatch_count)
+		{
+			for (u32 i = 0; i < ipatch_count; i++)
+			{
+				u32 word = words[i + 1];
+				u32 addr = (word >> 16) * 2;
+				u32 data = word & 0xFFFF;
+
+				ipatch(addr, data);
+			}
+		}
+		words[0] = word0;
+		if ((word0 >> 25) == 0)
+			break;
+		if (_patch_hash_one(&word0) >= 2)
+		{
+			return 3;
+		}
+		word_count = word0 >> 25;
+	}
+
+	return 0;
+}
+
+int fuse_read_evp_thunk(u32 *iram_evp_thunks, u32 *iram_evp_thunks_len)
+{
+	u32 words[80];
+	u32 word_count;
+	u32 word_addr;
+	u32 word0 = 0;
+	u32 total_read = 0;
+	int evp_thunk_written = 0;
+	void *evp_thunk_dst_addr = 0;
+
+	memset(iram_evp_thunks, 0, *iram_evp_thunks_len);
+
+	word_count = FUSE(FUSE_FIRST_BOOTROM_PATCH_SIZE);
+	word_count &= 0x7F;
+	word_addr = 191;
+
+	while (word_count)
+	{
+		total_read += word_count;
+		if (total_read >= ARRAYSIZE(words))
+		{
+			break;
+		}
+
+		for (u32 i = 0; i < word_count; i++)
+			words[i] = fuse_read(word_addr--);
+
+		word0 = words[0];
+		if (_patch_hash_multi(words, word_count) >= 2)
+		{
+			return 1;
+		}
+		u32 ipatch_count = (words[0] >> 16) & 0xF;
+		u32 insn_count = word_count - ipatch_count - 1;
+		if (insn_count)
+		{
+			if (!evp_thunk_written)
+			{
+				evp_thunk_dst_addr = (void *)iram_evp_thunks;
+
+				memcpy(evp_thunk_dst_addr, (void *)evp_thunk_template, evp_thunk_template_len);
+				evp_thunk_dst_addr += evp_thunk_template_len;
+				evp_thunk_written = 1;
+				*iram_evp_thunks_len = evp_thunk_template_len;
+
+				//write32(TEGRA_EXCEPTION_VECTORS_BASE + 0x208, iram_evp_thunks);
+			}
+
+			u32 thunk_patch_len = insn_count * sizeof(u32);
+			memcpy(evp_thunk_dst_addr, &words[ipatch_count + 1], thunk_patch_len);
+			evp_thunk_dst_addr += thunk_patch_len;
+			*iram_evp_thunks_len += thunk_patch_len;
+		}
+		words[0] = word0;
+		if ((word0 >> 25) == 0)
+			break;
+		if (_patch_hash_one(&word0) >= 2)
+		{
+			return 3;
+		}
+		word_count = word0 >> 25;
+	}
+
+	return 0;
+}
+
+bool fuse_check_patched_rcm()
+{
+	// Check if XUSB in use.
+	if (FUSE(FUSE_RESERVED_SW) & (1<<7))
+		return true;
+
+	// Check if RCM is ipatched.
+	u32 word_count = FUSE(FUSE_FIRST_BOOTROM_PATCH_SIZE) & 0x7F;
+	u32 word_addr = 191;
+
+	while (word_count)
+	{
+		u32 word0 = fuse_read(word_addr);
+		u32 ipatch_count = (word0 >> 16) & 0xF;
+
+		for (u32 i = 0; i < ipatch_count; i++)
+		{
+			u32 word = fuse_read(word_addr - (i + 1));
+			u32 addr = (word >> 16) * 2;
+			if (addr == 0x769A)
+				return true;
+		}
+
+		word_addr -= word_count;
+		word_count = word0 >> 25;
+	}
+
+	return false;
+}

source/soc/fuse.h

@@ -37,12 +37,34 @@
 #define FUSE_WRITE_ACCESS_SW 0x30
 #define FUSE_PWR_GOOD_SW 0x34
 #define FUSE_SKU_INFO 0x110
+#define FUSE_CPU_SPEEDO_0_CALIB 0x114
+#define FUSE_CPU_IDDQ_CALIB 0x118
+#define FUSE_OPT_FT_REV 0x128
+#define FUSE_CPU_SPEEDO_1_CALIB 0x12C
+#define FUSE_CPU_SPEEDO_2_CALIB 0x130
+#define FUSE_SOC_SPEEDO_0_CALIB 0x134
+#define FUSE_SOC_SPEEDO_1_CALIB 0x138
+#define FUSE_SOC_SPEEDO_2_CALIB 0x13C
+#define FUSE_SOC_IDDQ_CALIB 0x140
+#define FUSE_OPT_CP_REV 0x190
 #define FUSE_FIRST_BOOTROM_PATCH_SIZE 0x19c
 #define FUSE_PRIVATE_KEY0 0x1A4
 #define FUSE_PRIVATE_KEY1 0x1A8
 #define FUSE_PRIVATE_KEY2 0x1AC
 #define FUSE_PRIVATE_KEY3 0x1B0
+#define FUSE_PRIVATE_KEY4 0x1B4
 #define FUSE_RESERVED_SW 0x1C0
+#define FUSE_USB_CALIB 0x1F0
+#define FUSE_SKU_DIRECT_CONFIG 0x1F4
+#define FUSE_OPT_VENDOR_CODE 0x200
+#define FUSE_OPT_FAB_CODE 0x204
+#define FUSE_OPT_LOT_CODE_0 0x208
+#define FUSE_OPT_LOT_CODE_1 0x20C
+#define FUSE_OPT_WAFER_ID 0x210
+#define FUSE_OPT_X_COORDINATE 0x214
+#define FUSE_OPT_Y_COORDINATE 0x218
+#define FUSE_GPU_IDDQ_CALIB	0x228
+#define FUSE_USB_CALIB_EXT 0x350
 
 /*! Fuse commands. */
 #define FUSE_READ 0x1
@@ -55,5 +77,10 @@
 
 void fuse_disable_program();
 u32 fuse_read_odm(u32 idx);
+void fuse_wait_idle();
+int fuse_read_ipatch(void (*ipatch)(u32 offset, u32 value));
+int fuse_read_evp_thunk(u32 *iram_evp_thunks, u32 *iram_evp_thunks_len);
+void fuse_read_array(u32 *words);
+bool fuse_check_patched_rcm();
 
 #endif

source/soc/gpio.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2019 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,
@@ -14,81 +15,146 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "../soc/gpio.h"
-#include "../soc/t210.h"
+#include "gpio.h"
+#include "irq.h"
+#include "t210.h"
 
-static const u16 _gpio_cnf[31] = {
-	0x000, 0x004, 0x008, 0x00C,
-	0x100, 0x104, 0x108, 0x10C,
-	0x200, 0x204, 0x208, 0x20C,
-	0x300, 0x304, 0x308, 0x30C,
-	0x400, 0x404, 0x408, 0x40C,
-	0x500, 0x504, 0x508, 0x50C,
-	0x600, 0x604, 0x608, 0x60C,
-	0x700, 0x704, 0x708
-};
+#define GPIO_BANK_IDX(port)      (port >> 2)
 
-static const u16 _gpio_oe[31] = {
-	0x010, 0x014, 0x018, 0x01C,
-	0x110, 0x114, 0x118, 0x11C,
-	0x210, 0x214, 0x218, 0x21C,
-	0x310, 0x314, 0x318, 0x31C,
-	0x410, 0x414, 0x418, 0x41C,
-	0x510, 0x514, 0x518, 0x51C,
-	0x610, 0x614, 0x618, 0x61C,
-	0x710, 0x714, 0x718
-};
+#define GPIO_CNF_OFFSET(port)     (0x00 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_OE_OFFSET(port)      (0x10 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_OUT_OFFSET(port)     (0x20 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_IN_OFFSET(port)      (0x30 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_INT_STA_OFFSET(port) (0x40 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_INT_ENB_OFFSET(port) (0x50 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_INT_LVL_OFFSET(port) (0x60 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_INT_CLR_OFFSET(port) (0x70 + ((port >> 2) << 8) + ((port % 4) << 2))
 
-static const u16 _gpio_out[31] = {
-	0x020, 0x024, 0x028, 0x02C,
-	0x120, 0x124, 0x128, 0x12C,
-	0x220, 0x224, 0x228, 0x22C,
-	0x320, 0x324, 0x328, 0x32C,
-	0x420, 0x424, 0x428, 0x42C,
-	0x520, 0x524, 0x528, 0x52C,
-	0x620, 0x624, 0x628, 0x62C,
-	0x720, 0x724, 0x728
-};
+#define GPIO_CNF_MASKED_OFFSET(port)     (0x80 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_OE_MASKED_OFFSET(port)      (0x90 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_OUT_MASKED_OFFSET(port)     (0xA0 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_INT_STA_MASKED_OFFSET(port) (0xC0 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_INT_ENB_MASKED_OFFSET(port) (0xD0 + ((port >> 2) << 8) + ((port % 4) << 2))
+#define GPIO_INT_LVL_MASKED_OFFSET(port) (0xE0 + ((port >> 2) << 8) + ((port % 4) << 2))
 
-static const u16 _gpio_in[31] = {
-	0x030, 0x034, 0x038, 0x03C,
-	0x130, 0x134, 0x138, 0x13C,
-	0x230, 0x234, 0x238, 0x23C,
-	0x330, 0x334, 0x338, 0x33C,
-	0x430, 0x434, 0x438, 0x43C,
-	0x530, 0x534, 0x538, 0x53C,
-	0x630, 0x634, 0x638, 0x63C,
-	0x730, 0x734, 0x738
+static u8 gpio_bank_irq_ids[8] = {
+	IRQ_GPIO1, IRQ_GPIO2, IRQ_GPIO3, IRQ_GPIO4,
+	IRQ_GPIO5, IRQ_GPIO6, IRQ_GPIO7, IRQ_GPIO8
 };
 
 void gpio_config(u32 port, u32 pins, int mode)
 {
+	u32 offset = GPIO_CNF_OFFSET(port);
+
 	if (mode)
-		GPIO(_gpio_cnf[port]) |= pins;
+		GPIO(offset) |= pins;
 	else
-		GPIO(_gpio_cnf[port]) &= ~pins;
-	(void)GPIO(_gpio_cnf[port]);
+		GPIO(offset) &= ~pins;
+
+	(void)GPIO(offset); // Commit the write.
 }
 
 void gpio_output_enable(u32 port, u32 pins, int enable)
 {
+	u32 port_offset = GPIO_OE_OFFSET(port);
+
 	if (enable)
-		GPIO(_gpio_oe[port]) |= pins;
+		GPIO(port_offset) |= pins;
 	else
-		GPIO(_gpio_oe[port]) &= ~pins;
-	(void)GPIO(_gpio_oe[port]);
+		GPIO(port_offset) &= ~pins;
+
+	(void)GPIO(port_offset); // Commit the write.
 }
 
 void gpio_write(u32 port, u32 pins, int high)
 {
+	u32 port_offset = GPIO_OUT_OFFSET(port);
+
 	if (high)
-		GPIO(_gpio_out[port]) |= pins;
+		GPIO(port_offset) |= pins;
 	else
-		GPIO(_gpio_out[port]) &= ~pins;
-	(void)GPIO(_gpio_out[port]);
+		GPIO(port_offset) &= ~pins;
+
+	(void)GPIO(port_offset); // Commit the write.
 }
 
 int gpio_read(u32 port, u32 pins)
 {
-	return (GPIO(_gpio_in[port]) & pins) ? 1 : 0;
+	u32 port_offset = GPIO_IN_OFFSET(port);
+
+	return (GPIO(port_offset) & pins) ? 1 : 0;
 }
+
+static void _gpio_interrupt_clear(u32 port, u32 pins)
+{
+	u32 port_offset = GPIO_INT_CLR_OFFSET(port);
+
+	GPIO(port_offset) |= pins;
+
+	(void)GPIO(port_offset); // Commit the write.
+}
+
+int gpio_interrupt_status(u32 port, u32 pins)
+{
+	u32 port_offset = GPIO_INT_STA_OFFSET(port);
+	u32 enabled = GPIO(GPIO_INT_ENB_OFFSET(port)) & pins;
+
+	int status = ((GPIO(port_offset) & pins) && enabled) ? 1 : 0;
+
+	// Clear the interrupt status.
+	if (status)
+		_gpio_interrupt_clear(port, pins);
+
+	return status;
+}
+
+void gpio_interrupt_enable(u32 port, u32 pins, int enable)
+{
+	u32 port_offset = GPIO_INT_ENB_OFFSET(port);
+
+	// Clear any possible stray interrupt.
+	_gpio_interrupt_clear(port, pins);
+
+	if (enable)
+		GPIO(port_offset) |= pins;
+	else
+		GPIO(port_offset) &= ~pins;
+
+	(void)GPIO(port_offset); // Commit the write.
+}
+
+void gpio_interrupt_level(u32 port, u32 pins, int high, int edge, int delta)
+{
+	u32 port_offset = GPIO_INT_LVL_OFFSET(port);
+
+	u32 val = GPIO(port_offset);
+
+	if (high)
+		val |= pins;
+	else
+		val &= ~pins;
+
+	if (edge)
+		val |= pins << 8;
+	else
+		val &= ~(pins << 8);
+
+	if (delta)
+		val |= pins << 16;
+	else
+		val &= ~(pins << 16);
+
+	GPIO(port_offset) = val;
+
+	(void)GPIO(port_offset); // Commit the write.
+
+	// Clear any possible stray interrupt.
+	_gpio_interrupt_clear(port, pins);
+}
+
+u32 gpio_get_bank_irq_id(u32 port)
+{
+	u32 bank_idx = GPIO_BANK_IDX(port);
+
+	return gpio_bank_irq_ids[bank_idx];
+}

source/soc/gpio.h

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2019 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,
@@ -21,10 +22,23 @@
 
 #define GPIO_MODE_SPIO 0
 #define GPIO_MODE_GPIO 1
+
 #define GPIO_OUTPUT_DISABLE 0
 #define GPIO_OUTPUT_ENABLE 1
+
+#define GPIO_IRQ_DISABLE 0
+#define GPIO_IRQ_ENABLE 1
+
 #define GPIO_LOW 0
 #define GPIO_HIGH 1
+#define GPIO_FALLING 0
+#define GPIO_RISING 1
+
+#define GPIO_LEVEL 0
+#define GPIO_EDGE 1
+
+#define GPIO_CONFIGURED_EDGE 0
+#define GPIO_ANY_EDGE_CHANGE 1
 
 /*! GPIO pins (0-7 for each port). */
 #define GPIO_PIN_0 (1 << 0)
@@ -72,6 +86,10 @@
 void gpio_config(u32 port, u32 pins, int mode);
 void gpio_output_enable(u32 port, u32 pins, int enable);
 void gpio_write(u32 port, u32 pins, int high);
-int gpio_read(u32 port, u32 pins);
+int  gpio_read(u32 port, u32 pins);
+int  gpio_interrupt_status(u32 port, u32 pins);
+void gpio_interrupt_enable(u32 port, u32 pins, int enable);
+void gpio_interrupt_level(u32 port, u32 pins, int high, int edge, int delta);
+u32  gpio_get_bank_irq_id(u32 port);
 
 #endif

source/soc/hw_init.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (c) 2018-2019 CTCaer
+ * Copyright (c) 2018-2020 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,
@@ -18,6 +18,7 @@
 #include <string.h>
 
 #include "hw_init.h"
+#include "bpmp.h"
 #include "clock.h"
 #include "fuse.h"
 #include "gpio.h"
@@ -27,33 +28,49 @@
 #include "t210.h"
 #include "../gfx/di.h"
 #include "../mem/mc.h"
+#include "../mem/minerva.h"
 #include "../mem/sdram.h"
 #include "../power/max77620.h"
 #include "../power/max7762x.h"
 #include "../sec/se.h"
 #include "../sec/se_t210.h"
+#include "../storage/nx_sd.h"
 #include "../storage/sdmmc.h"
 #include "../utils/util.h"
 
-extern sdmmc_t sd_sdmmc;
 extern boot_cfg_t b_cfg;
+extern volatile nyx_storage_t *nyx_str;
+
+/*
+ * CLK_OSC - 38.4 MHz crystal.
+ * CLK_M   - 19.2 MHz (osc/2).
+ * CLK_S   - 32.768 KHz (from PMIC).
+ * SCLK    - 204MHz init (-> 408MHz -> OC).
+ * HCLK    - 204MHz init (-> 408MHz -> OC).
+ * PCLK    - 68MHz  init (-> 136MHz -> OC/4).
+ */
 
 void _config_oscillators()
 {
-	CLOCK(CLK_RST_CONTROLLER_SPARE_REG0) = (CLOCK(CLK_RST_CONTROLLER_SPARE_REG0) & 0xFFFFFFF3) | 4;
-	SYSCTR0(SYSCTR0_CNTFID0) = 19200000;
-	TMR(TIMERUS_USEC_CFG) = 0x45F; // For 19.2MHz clk_m.
-	CLOCK(CLK_RST_CONTROLLER_OSC_CTRL) = 0x50000071;
-	PMC(APBDEV_PMC_OSC_EDPD_OVER) = (PMC(APBDEV_PMC_OSC_EDPD_OVER) & 0xFFFFFF81) | 0xE;
-	PMC(APBDEV_PMC_OSC_EDPD_OVER) = (PMC(APBDEV_PMC_OSC_EDPD_OVER) & 0xFFBFFFFF) | 0x400000;
-	PMC(APBDEV_PMC_CNTRL2) = (PMC(APBDEV_PMC_CNTRL2) & 0xFFFFEFFF) | 0x1000;
-	PMC(APBDEV_PMC_SCRATCH188) = (PMC(APBDEV_PMC_SCRATCH188) & 0xFCFFFFFF) | 0x2000000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_SYSTEM_RATE) = 0x10;
-	CLOCK(CLK_RST_CONTROLLER_PLLMB_BASE) &= 0xBFFFFFFF;
+	CLOCK(CLK_RST_CONTROLLER_SPARE_REG0) = (CLOCK(CLK_RST_CONTROLLER_SPARE_REG0) & 0xFFFFFFF3) | 4; // Set CLK_M_DIVISOR to 2.
+	SYSCTR0(SYSCTR0_CNTFID0) = 19200000;             // Set counter frequency.
+	TMR(TIMERUS_USEC_CFG) = 0x45F;                   // For 19.2MHz clk_m.
+	CLOCK(CLK_RST_CONTROLLER_OSC_CTRL) = 0x50000071; // Set OSC to 38.4MHz and drive strength.
+
+	PMC(APBDEV_PMC_OSC_EDPD_OVER) = (PMC(APBDEV_PMC_OSC_EDPD_OVER) & 0xFFFFFF81) | 0xE; // Set LP0 OSC drive strength.
+	PMC(APBDEV_PMC_OSC_EDPD_OVER) = (PMC(APBDEV_PMC_OSC_EDPD_OVER) & 0xFFBFFFFF) | PMC_OSC_EDPD_OVER_OSC_CTRL_OVER;
+	PMC(APBDEV_PMC_CNTRL2) = (PMC(APBDEV_PMC_CNTRL2) & 0xFFFFEFFF) | PMC_CNTRL2_HOLD_CKE_LOW_EN;
+	PMC(APBDEV_PMC_SCRATCH188) = (PMC(APBDEV_PMC_SCRATCH188) & 0xFCFFFFFF) | (4 << 23); // LP0 EMC2TMC_CFG_XM2COMP_PU_VREF_SEL_RANGE.
+
+	CLOCK(CLK_RST_CONTROLLER_CLK_SYSTEM_RATE) = 0x10;   // Set HCLK div to 2 and PCLK div to 1.
+	CLOCK(CLK_RST_CONTROLLER_PLLMB_BASE) &= 0xBFFFFFFF; // PLLMB disable.
+
 	PMC(APBDEV_PMC_TSC_MULT) = (PMC(APBDEV_PMC_TSC_MULT) & 0xFFFF0000) | 0x249F; //0x249F = 19200000 * (16 / 32.768 kHz)
-	CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = 0x20004444;
-	CLOCK(CLK_RST_CONTROLLER_SUPER_SCLK_DIVIDER) = 0x80000000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_SYSTEM_RATE) = 2;
+
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SYS) = 0;              // Set SCLK div to 1.
+	CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = 0x20004444;  // Set clk source to Run and PLLP_OUT2 (204MHz).
+	CLOCK(CLK_RST_CONTROLLER_SUPER_SCLK_DIVIDER) = 0x80000000; // Enable SUPER_SDIV to 1.
+	CLOCK(CLK_RST_CONTROLLER_CLK_SYSTEM_RATE) = 2;             // Set HCLK div to 1 and PCLK div to 3.
 }
 
 void _config_gpios()
@@ -61,11 +78,22 @@ void _config_gpios()
 	PINMUX_AUX(PINMUX_AUX_UART2_TX) = 0;
 	PINMUX_AUX(PINMUX_AUX_UART3_TX) = 0;
 
-	PINMUX_AUX(PINMUX_AUX_GPIO_PE6) = PINMUX_INPUT_ENABLE;
-	PINMUX_AUX(PINMUX_AUX_GPIO_PH6) = PINMUX_INPUT_ENABLE;
+	// Set Joy-Con IsAttached direction.
+	PINMUX_AUX(PINMUX_AUX_GPIO_PE6) = PINMUX_INPUT_ENABLE | PINMUX_TRISTATE;
+	PINMUX_AUX(PINMUX_AUX_GPIO_PH6) = PINMUX_INPUT_ENABLE | PINMUX_TRISTATE;
 
+	// Set pin mode for Joy-Con IsAttached and UARTB/C TX pins.
+#if !defined (DEBUG_UART_PORT) || DEBUG_UART_PORT != UART_B
+	gpio_config(GPIO_PORT_G, GPIO_PIN_0, GPIO_MODE_GPIO);
+#endif
+#if !defined (DEBUG_UART_PORT) || DEBUG_UART_PORT != UART_C
+	gpio_config(GPIO_PORT_D, GPIO_PIN_1, GPIO_MODE_GPIO);
+#endif
+	// Set Joy-Con IsAttached mode.
 	gpio_config(GPIO_PORT_E, GPIO_PIN_6, GPIO_MODE_GPIO);
 	gpio_config(GPIO_PORT_H, GPIO_PIN_6, GPIO_MODE_GPIO);
+
+	// Enable input logic for Joy-Con IsAttached and UARTB/C TX pins.
 	gpio_output_enable(GPIO_PORT_G, GPIO_PIN_0, GPIO_OUTPUT_DISABLE);
 	gpio_output_enable(GPIO_PORT_D, GPIO_PIN_1, GPIO_OUTPUT_DISABLE);
 	gpio_output_enable(GPIO_PORT_E, GPIO_PIN_6, GPIO_OUTPUT_DISABLE);
@@ -80,27 +108,36 @@ void _config_gpios()
 	gpio_config(GPIO_PORT_X, GPIO_PIN_7, GPIO_MODE_GPIO);
 	gpio_output_enable(GPIO_PORT_X, GPIO_PIN_6, GPIO_OUTPUT_DISABLE);
 	gpio_output_enable(GPIO_PORT_X, GPIO_PIN_7, GPIO_OUTPUT_DISABLE);
+
+	// Configure HOME as inputs.
+	// PINMUX_AUX(PINMUX_AUX_BUTTON_HOME) = PINMUX_INPUT_ENABLE | PINMUX_TRISTATE;
+	// gpio_config(GPIO_PORT_Y, GPIO_PIN_1, GPIO_MODE_GPIO);
 }
 
 void _config_pmc_scratch()
 {
-	PMC(APBDEV_PMC_SCRATCH20) &= 0xFFF3FFFF;
-	PMC(APBDEV_PMC_SCRATCH190) &= 0xFFFFFFFE;
-	PMC(APBDEV_PMC_SECURE_SCRATCH21) |= 0x10;
+	PMC(APBDEV_PMC_SCRATCH20) &= 0xFFF3FFFF;  // Unset Debug console from Customer Option.
+	PMC(APBDEV_PMC_SCRATCH190) &= 0xFFFFFFFE; // Unset DATA_DQ_E_IVREF EMC_PMACRO_DATA_PAD_TX_CTRL
+	PMC(APBDEV_PMC_SECURE_SCRATCH21) |= PMC_FUSE_PRIVATEKEYDISABLE_TZ_STICKY_BIT;
 }
 
 void _mbist_workaround()
 {
 	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_V) |= (1 << 10); // Enable AHUB clock.
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_Y) |= (1 <<  6);  // Enable APE clock.
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_Y) |= (1 <<  6); // Enable APE clock.
 
+	// Set mux output to SOR1 clock switch.
 	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SOR1) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SOR1) | 0x8000) & 0xFFFFBFFF;
-	CLOCK(CLK_RST_CONTROLLER_PLLD_BASE) |= 0x40800000u;
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_Y_CLR) = 0x40;
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_X_CLR) = 0x40000;
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_CLR) = 0x18000000;
+	// Enabled PLLD and set csi to PLLD for test pattern generation.
+	CLOCK(CLK_RST_CONTROLLER_PLLD_BASE) |= 0x40800000;
+
+	// Clear per-clock resets.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_Y_CLR) = 0x40;       // Clear reset APE.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_X_CLR) = 0x40000;    // Clear reset VIC.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_CLR) = 0x18000000; // Clear reset DISP1, HOST1X.
 	usleep(2);
 
+	// I2S channels to master and disable SLCG.
 	I2S(I2S1_CTRL) |= I2S_CTRL_MASTER_EN;
 	I2S(I2S1_CG)   &= ~I2S_CG_SLCG_ENABLE;
 	I2S(I2S2_CTRL) |= I2S_CTRL_MASTER_EN;
@@ -111,39 +148,51 @@ void _mbist_workaround()
 	I2S(I2S4_CG)   &= ~I2S_CG_SLCG_ENABLE;
 	I2S(I2S5_CTRL) |= I2S_CTRL_MASTER_EN;
 	I2S(I2S5_CG)   &= ~I2S_CG_SLCG_ENABLE;
-	DISPLAY_A(_DIREG(DC_COM_DSC_TOP_CTL)) |= 4;
+
+	DISPLAY_A(_DIREG(DC_COM_DSC_TOP_CTL)) |= 4; // DSC_SLCG_OVERRIDE.
 	VIC(0x8C) = 0xFFFFFFFF;
 	usleep(2);
 
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_Y_SET) = 0x40;
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_SET) = 0x18000000;
-	CLOCK(CLK_RST_CONTROLLER_RST_DEV_X_SET) = 0x40000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_H) = 0xC0;
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_L) = 0x80000130;
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_U) = 0x1F00200;
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_V) = 0x80400808;
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_W) = 0x402000FC;
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_X) = 0x23000780;
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_Y) = 0x300;
+	// Set per-clock reset.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_Y_SET) = 0x40;       // Set reset APE.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_SET) = 0x18000000; // Set reset DISP1, HOST1x.
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_X_SET) = 0x40000;    // Set reset VIC.
+
+	// Enable specific clocks and disable all others.
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_H) = 0xC0;       // Enable clock PMC, FUSE.
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_L) = 0x80000130; // Enable clock RTC, TMR, GPIO, BPMP_CACHE.
+	//CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_L) = 0x80400130; // Keep USBD ON.
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_U) = 0x1F00200;  // Enable clock CSITE, IRAMA, IRAMB, IRAMC, IRAMD, BPMP_CACHE_RAM.
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_V) = 0x80400808; // Enable clock MSELECT, APB2APE, SPDIF_DOUBLER, SE.
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_W) = 0x402000FC; // Enable clock PCIERX0, PCIERX1, PCIERX2, PCIERX3, PCIERX4, PCIERX5, ENTROPY, MC1.
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_X) = 0x23000780; // Enable clock MC_CAPA, MC_CAPB, MC_CPU, MC_BBC, DBGAPB, HPLL_ADSP, PLLG_REF.
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_Y) = 0x300;      // Enable clock MC_CDPA, MC_CCPA.
+
+	// Disable clock gate overrides.
 	CLOCK(CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRA) = 0;
 	CLOCK(CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRB) = 0;
 	CLOCK(CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRC) = 0;
 	CLOCK(CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRD) = 0;
 	CLOCK(CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRE) = 0;
-	CLOCK(CLK_RST_CONTROLLER_PLLD_BASE) &= 0x1F7FFFFF;
-	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SOR1) &= 0xFFFF3FFF;
-	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_VI) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_VI) & 0x1FFFFFFF) | 0x80000000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X) & 0x1FFFFFFF) | 0x80000000;
-	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_NVENC) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_NVENC) & 0x1FFFFFFF) | 0x80000000;
+
+	// Set child clock sources.
+	CLOCK(CLK_RST_CONTROLLER_PLLD_BASE) &= 0x1F7FFFFF; // Disable PLLD and set reference clock and csi clock.
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SOR1) &= 0xFFFF3FFF; // Set SOR1 to automatic muxing of safe clock (24MHz) or SOR1 clk switch.
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_VI) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_VI) & 0x1FFFFFFF) | 0x80000000; // Set clock source to PLLP_OUT.
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X) & 0x1FFFFFFF) | 0x80000000; // Set clock source to PLLP_OUT.
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_NVENC) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_NVENC) & 0x1FFFFFFF) | 0x80000000; // Set clock source to PLLP_OUT.
 }
 
 void _config_se_brom()
 {
+	// Enable fuse clock.
+	clock_enable_fuse(true);
+
 	// Skip SBK/SSK if sept was run.
 	if (!(b_cfg.boot_cfg & BOOT_CFG_SEPT_RUN))
 	{
 		// Bootrom part we skipped.
-		u32 sbk[4] = { 
+		u32 sbk[4] = {
 			FUSE(FUSE_PRIVATE_KEY0),
 			FUSE(FUSE_PRIVATE_KEY1),
 			FUSE(FUSE_PRIVATE_KEY2),
@@ -170,42 +219,21 @@ void _config_se_brom()
 	APB_MISC(APB_MISC_PP_STRAPPING_OPT_A) = (APB_MISC(APB_MISC_PP_STRAPPING_OPT_A) & 0xF0) | (7 << 10);
 }
 
-void config_hw()
+void _config_regulators()
 {
-	// Bootrom stuff we skipped by going through rcm.
-	_config_se_brom();
-	//FUSE(FUSE_PRIVATEKEYDISABLE) = 0x11;
-	SYSREG(AHB_AHB_SPARE_REG) &= 0xFFFFFF9F;
-	PMC(APBDEV_PMC_SCRATCH49) = ((PMC(APBDEV_PMC_SCRATCH49) >> 1) << 1) & 0xFFFFFFFD;
+	// Disable low battery shutdown monitor.
+	max77620_low_battery_monitor_config(false);
 
-	_mbist_workaround();
-	clock_enable_se();
-
-	// Enable fuse clock.
-	clock_enable_fuse(true);
-	// Disable fuse programming.
-	fuse_disable_program();
-
-	mc_enable();
-
-	_config_oscillators();
-	APB_MISC(APB_MISC_PP_PINMUX_GLOBAL) = 0;
-	_config_gpios();
-
-	clock_enable_cl_dvfs();
-
-	clock_enable_i2c(I2C_1);
-	clock_enable_i2c(I2C_5);
-
-	clock_enable_unk2();
-
-	i2c_init(I2C_1);
-	i2c_init(I2C_5);
+	// Disable SDMMC1 IO power.
+	gpio_output_enable(GPIO_PORT_E, GPIO_PIN_4, GPIO_OUTPUT_DISABLE);
+	max77620_regulator_enable(REGULATOR_LDO2, 0);
+	sd_power_cycle_time_start = get_tmr_ms();
 
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_CNFGBBC, MAX77620_CNFGBBC_RESISTOR_1K);
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_ONOFFCNFG1,
 		(1 << 6) | (3 << MAX77620_ONOFFCNFG1_MRT_SHIFT)); // PWR delay for forced shutdown off.
 
+	// Configure all Flexible Power Sequencers.
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_FPS_CFG0,
 		(7 << MAX77620_FPS_TIME_PERIOD_SHIFT));
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_FPS_CFG1,
@@ -221,25 +249,84 @@ void config_hw()
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_FPS_GPIO3,
 		(4 << MAX77620_FPS_TIME_PERIOD_SHIFT) | (2 << MAX77620_FPS_PD_PERIOD_SHIFT)); // 3.x+
 
+	// Set vdd_core voltage to 1.125V.
 	max77620_regulator_set_voltage(REGULATOR_SD0, 1125000);
 
-	// Fix GPU after warmboot for Linux.
+	// Fix CPU/GPU after a L4T warmboot.
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_GPIO5, 2);
 	i2c_send_byte(I2C_5, MAX77620_I2C_ADDR, MAX77620_REG_GPIO6, 2);
 
-	// Disable low battery shutdown monitor.
-	max77620_low_battery_monitor_config();
+	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_VOUT_REG, MAX77621_VOUT_0_95V); // Disable power.
+	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_VOUT_DVC_REG, MAX77621_VOUT_ENABLE | MAX77621_VOUT_1_09V); // Enable DVS power.
+	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_CONTROL1_REG, MAX77621_RAMP_50mV_PER_US);
+	i2c_send_byte(I2C_5, MAX77621_CPU_I2C_ADDR, MAX77621_CONTROL2_REG,
+		MAX77621_T_JUNCTION_120 | MAX77621_FT_ENABLE | MAX77621_CKKADV_TRIP_75mV_PER_US_HIST_DIS |
+		MAX77621_CKKADV_TRIP_150mV_PER_US | MAX77621_INDUCTOR_NOMINAL);
+
+	i2c_send_byte(I2C_5, MAX77621_GPU_I2C_ADDR, MAX77621_VOUT_REG, MAX77621_VOUT_0_95V); // Disable power.
+	i2c_send_byte(I2C_5, MAX77621_GPU_I2C_ADDR, MAX77621_VOUT_DVC_REG, MAX77621_VOUT_ENABLE | MAX77621_VOUT_1_09V); // Enable DVS power.
+	i2c_send_byte(I2C_5, MAX77621_GPU_I2C_ADDR, MAX77621_CONTROL1_REG, MAX77621_RAMP_50mV_PER_US);
+	i2c_send_byte(I2C_5, MAX77621_GPU_I2C_ADDR, MAX77621_CONTROL2_REG,
+		MAX77621_T_JUNCTION_120 | MAX77621_FT_ENABLE | MAX77621_CKKADV_TRIP_75mV_PER_US_HIST_DIS |
+		MAX77621_CKKADV_TRIP_150mV_PER_US | MAX77621_INDUCTOR_NOMINAL);
+}
+
+void config_hw()
+{
+	// Bootrom stuff we skipped by going through rcm.
+	_config_se_brom();
+	//FUSE(FUSE_PRIVATEKEYDISABLE) = 0x11;
+	SYSREG(AHB_AHB_SPARE_REG) &= 0xFFFFFF9F; // Unset APB2JTAG_OVERRIDE_EN and OBS_OVERRIDE_EN.
+	PMC(APBDEV_PMC_SCRATCH49) = PMC(APBDEV_PMC_SCRATCH49) & 0xFFFFFFFC;
+
+	_mbist_workaround();
+	clock_enable_se();
+
+	// Enable fuse clock.
+	clock_enable_fuse(true);
+
+	// Disable fuse programming.
+	fuse_disable_program();
+
+	mc_enable();
+
+	_config_oscillators();
+	APB_MISC(APB_MISC_PP_PINMUX_GLOBAL) = 0;
+	_config_gpios();
+
+	clock_enable_cl_dvfs();
+
+	clock_enable_i2c(I2C_1);
+	clock_enable_i2c(I2C_5);
+
+	clock_enable_tzram();
+
+	i2c_init(I2C_1);
+	i2c_init(I2C_5);
+
+	_config_regulators();
 
 	_config_pmc_scratch(); // Missing from 4.x+
 
-	CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = (CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) & 0xFFFF8888) | 0x3333;
+	CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = 0x20003333; // Set SCLK to PLLP_OUT (408MHz).
 
 	sdram_init();
+
+	bpmp_mmu_enable();
 	mc_enable_ahb_redirect();
+
+	// Clear flags from PMC_SCRATCH0
+	PMC(APBDEV_PMC_SCRATCH0) &= ~PMC_SCRATCH0_MODE_PAYLOAD;
 }
 
 void reconfig_hw_workaround(bool extra_reconfig, u32 magic)
 {
+	// Flush and disable MMU.
+	bpmp_mmu_disable();
+	bpmp_clk_rate_set(BPMP_CLK_NORMAL);
+	minerva_change_freq(FREQ_204);
+	nyx_str->mtc_cfg.init_done = 0;
+
 	// Re-enable clocks to Audio Processing Engine as a workaround to hanging.
 	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_V) |= (1 << 10); // Enable AHUB clock.
 	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_Y) |= (1 <<  6);  // Enable APE clock.

source/soc/i2c.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2020 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,7 +20,7 @@
 #include "i2c.h"
 #include "../utils/util.h"
 
-static u32 i2c_addrs[] = {
+static const u32 i2c_addrs[] = {
 	0x7000C000, 0x7000C400, 0x7000C500,
 	0x7000C700, 0x7000D000, 0x7000D100
 };
@@ -37,21 +38,39 @@ static void _i2c_wait(vu32 *base)
 
 static int _i2c_send_pkt(u32 idx, u32 x, u8 *buf, u32 size)
 {
-	if (size > 4)
+	if (size > 8)
 		return 0;
 
 	u32 tmp = 0;
-	memcpy(&tmp, buf, size);
 
 	vu32 *base = (vu32 *)i2c_addrs[idx];
 	base[I2C_CMD_ADDR0] = x << 1; //Set x (send mode).
-	base[I2C_CMD_DATA1] = tmp;    //Set value.
-	base[I2C_CNFG] = (2 * size - 2) | 0x2800; //Set size and send mode.
-	_i2c_wait(base);  //Kick transaction.
+
+	if (size > 4)
+	{
+		memcpy(&tmp, buf, 4);
+		base[I2C_CMD_DATA1] = tmp; //Set value.
+		tmp = 0;
+		memcpy(&tmp, buf + 4, size - 4);
+		base[I2C_CMD_DATA2] = tmp;
+	}
+	else
+	{
+		memcpy(&tmp, buf, size);
+		base[I2C_CMD_DATA1] = tmp; //Set value.
+	}
+
+	base[I2C_CNFG] = ((size - 1) << 1) | 0x2800; //Set size and send mode.
+	_i2c_wait(base);                             //Kick transaction.
 
 	base[I2C_CNFG] = (base[I2C_CNFG] & 0xFFFFFDFF) | 0x200;
+
+	u32 timeout = get_tmr_ms() + 1500;
 	while (base[I2C_STATUS] & 0x100)
-		;
+	{
+		if (get_tmr_ms() > timeout)
+			return 0;
+	}
 
 	if (base[I2C_STATUS] << 28)
 		return 0;
@@ -65,13 +84,18 @@ static int _i2c_recv_pkt(u32 idx, u8 *buf, u32 size, u32 x)
 		return 0;
 
 	vu32 *base = (vu32 *)i2c_addrs[idx];
-	base[I2C_CMD_ADDR0] = (x << 1) | 1; // Set x (recv mode).
-	base[I2C_CNFG] = (size - 1) << 1 | 0x2840; // Set size and recv mode.
-	_i2c_wait(base);        // Kick transaction.
+	base[I2C_CMD_ADDR0] = (x << 1) | 1;          // Set x (recv mode).
+	base[I2C_CNFG] = ((size - 1) << 1) | 0x2840; // Set size and recv mode.
+	_i2c_wait(base);                             // Kick transaction.
 
 	base[I2C_CNFG] = (base[I2C_CNFG] & 0xFFFFFDFF) | 0x200;
+
+	u32 timeout = get_tmr_ms() + 1500;
 	while (base[I2C_STATUS] & 0x100)
-		;
+	{
+		if (get_tmr_ms() > timeout)
+			return 0;
+	}
 
 	if (base[I2C_STATUS] << 28)
 		return 0;
@@ -112,7 +136,7 @@ int i2c_send_buf_small(u32 idx, u32 x, u32 y, u8 *buf, u32 size)
 {
 	u8 tmp[4];
 
-	if (size > 3)
+	if (size > 7)
 		return 0;
 
 	tmp[0] = y;
@@ -121,6 +145,11 @@ int i2c_send_buf_small(u32 idx, u32 x, u32 y, u8 *buf, u32 size)
 	return _i2c_send_pkt(idx, x, tmp, size + 1);
 }
 
+int i2c_recv_buf(u8 *buf, u32 size, u32 idx, u32 x)
+{
+	return _i2c_recv_pkt(idx, buf, size, x);
+}
+
 int i2c_recv_buf_small(u8 *buf, u32 size, u32 idx, u32 x, u32 y)
 {
 	int res = _i2c_send_pkt(idx, x, (u8 *)&y, 1);

source/soc/i2c.h

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2020 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,
@@ -39,6 +40,7 @@
 
 void i2c_init(u32 idx);
 int i2c_send_buf_small(u32 idx, u32 x, u32 y, u8 *buf, u32 size);
+int i2c_recv_buf(u8 *buf, u32 size, u32 idx, u32 x);
 int i2c_recv_buf_small(u8 *buf, u32 size, u32 idx, u32 x, u32 y);
 int i2c_send_byte(u32 idx, u32 x, u32 y, u8 b);
 u8 i2c_recv_byte(u32 idx, u32 x, u32 y);

source/soc/irq.c

@@ -0,0 +1,263 @@
+/*
+ * BPMP-Lite IRQ driver for Tegra X1
+ *
+ * Copyright (c) 2019 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/>.
+ */
+
+#include <string.h>
+
+#include "irq.h"
+#include "t210.h"
+#include "../gfx/gfx.h"
+#include "../mem/heap.h"
+
+//#define DPRINTF(...) gfx_printf(__VA_ARGS__)
+#define DPRINTF(...)
+
+extern void irq_disable();
+extern void irq_enable_cpu_irq_exceptions();
+extern void irq_disable_cpu_irq_exceptions();
+
+typedef struct _irq_ctxt_t
+{
+	u32  irq;
+	int  (*handler)(u32 irq, void *data);
+	void *data;
+	u32  flags;
+} irq_ctxt_t;
+
+bool irq_init_done = false;
+irq_ctxt_t irqs[IRQ_MAX_HANDLERS];
+
+static void _irq_enable_source(u32 irq)
+{
+	u32 ctrl_idx = irq >> 5;
+	u32 bit = irq % 32;
+
+	// Set as normal IRQ.
+	ICTLR(ctrl_idx, PRI_ICTLR_COP_IEP_CLASS) &= ~(1 << bit);
+
+	// Enable IRQ source.
+	ICTLR(ctrl_idx, PRI_ICTLR_COP_IER_SET) = 1 << bit;
+}
+
+static void _irq_disable_source(u32 irq)
+{
+	u32 ctrl_idx = irq >> 5;
+	u32 bit = irq % 32;
+
+	// Disable IRQ source.
+	ICTLR(ctrl_idx, PRI_ICTLR_COP_IER_CLR) = 1 << bit;
+}
+
+static void _irq_disable_and_ack_all()
+{
+	// Disable and ack all IRQ sources.
+	for (u32 ctrl_idx = 0; ctrl_idx < 6; ctrl_idx++)
+	{
+		u32 enabled_irqs = ICTLR(ctrl_idx, PRI_ICTLR_COP_IER);
+		ICTLR(ctrl_idx, PRI_ICTLR_COP_IER_CLR) = enabled_irqs;
+		ICTLR(ctrl_idx, PRI_ICTLR_FIR_CLR) = enabled_irqs;
+	}
+}
+
+static void _irq_ack_source(u32 irq)
+{
+	u32 ctrl_idx = irq >> 5;
+	u32 bit = irq % 32;
+
+	// Force stop the interrupt as it's serviced here.
+	ICTLR(ctrl_idx, PRI_ICTLR_FIR_CLR) = 1 << bit;
+}
+
+void irq_free(u32 irq)
+{
+	for (u32 idx = 0; idx < IRQ_MAX_HANDLERS; idx++)
+	{
+		if (irqs[idx].irq == irq && irqs[idx].handler)
+		{
+			irqs[idx].irq = 0;
+			irqs[idx].handler = NULL;
+			irqs[idx].data = NULL;
+			irqs[idx].flags = 0;
+
+			_irq_disable_source(irq);
+		}
+	}
+}
+
+static void _irq_free_all()
+{
+	for (u32 idx = 0; idx < IRQ_MAX_HANDLERS; idx++)
+	{
+		if (irqs[idx].handler)
+		{
+			_irq_disable_source(irqs[idx].irq);
+
+			irqs[idx].irq = 0;
+			irqs[idx].handler = NULL;
+			irqs[idx].data = NULL;
+			irqs[idx].flags = 0;
+		}
+	}
+}
+
+static irq_status_t _irq_handle_source(u32 irq)
+{
+	int status = IRQ_NONE;
+
+	_irq_disable_source(irq);
+	_irq_ack_source(irq);
+
+	u32 idx;
+	for (idx = 0; idx < IRQ_MAX_HANDLERS; idx++)
+	{
+		if (irqs[idx].irq == irq)
+		{
+			status = irqs[idx].handler(irqs[idx].irq, irqs[idx].data);
+
+			if (status == IRQ_HANDLED)
+				break;
+		}
+	}
+
+	if (irqs[idx].flags & IRQ_FLAG_ONE_OFF)
+		irq_free(irq);
+	else
+		_irq_enable_source(irq);
+
+	return status;
+}
+
+void irq_handler()
+{
+	// Get IRQ source.
+	u32 irq = EXCP_VEC(EVP_COP_IRQ_STS) & 0xFF;
+
+	if (!irq_init_done)
+	{
+		_irq_ack_source(irq);
+		return;
+	}
+
+	DPRINTF("IRQ: %d\n", irq);
+
+	int err = _irq_handle_source(irq);
+
+	//TODO: disable if unhandhled.
+	if (err == IRQ_NONE)
+		gfx_printf("Unhandled IRQ: %d\n", irq);
+}
+
+static void _irq_init()
+{
+	_irq_disable_and_ack_all();
+	memset(irqs, 0, sizeof(irq_ctxt_t) * IRQ_MAX_HANDLERS);
+	irq_init_done = true;
+}
+
+void irq_end()
+{
+	_irq_free_all();
+	irq_disable_cpu_irq_exceptions();
+	irq_init_done = false;
+}
+
+void irq_wait_event(u32 irq)
+{
+	irq_disable_cpu_irq_exceptions();
+
+	_irq_enable_source(irq);
+
+	// Halt BPMP and wait for the IRQ. No need to use WAIT_EVENT + LIC_IRQ when BPMP serves the IRQ.
+	FLOW_CTLR(FLOW_CTLR_HALT_COP_EVENTS) = HALT_COP_STOP_UNTIL_IRQ;
+
+	_irq_disable_source(irq);
+	_irq_ack_source(irq);
+
+	irq_enable_cpu_irq_exceptions();
+}
+
+void irq_disable_wait_event()
+{
+	irq_enable_cpu_irq_exceptions();
+}
+
+irq_status_t irq_request(u32 irq, irq_handler_t handler, void *data, irq_flags_t flags)
+{
+	if (!irq_init_done)
+		_irq_init();
+
+	for (u32 idx = 0; idx < IRQ_MAX_HANDLERS; idx++)
+	{
+		if (irqs[idx].handler == NULL ||
+			(irqs[idx].irq == irq && irqs[idx].flags & IRQ_FLAG_REPLACEABLE))
+		{
+			DPRINTF("Registered handler, IRQ: %d, Slot: %d\n", irq, idx);
+			DPRINTF("Handler: %08p, Flags: %x\n", (u32)handler, flags);
+
+			irqs[idx].irq = irq;
+			irqs[idx].handler = handler;
+			irqs[idx].data = data;
+			irqs[idx].flags = flags;
+
+			_irq_enable_source(irq);
+
+			return IRQ_ENABLED;
+		}
+		else if (irqs[idx].irq == irq)
+			return IRQ_ALREADY_REGISTERED;
+	}
+
+	return IRQ_NO_SLOTS_AVAILABLE;
+}
+
+void __attribute__ ((target("arm"))) fiq_setup()
+{
+/*
+	asm volatile("mrs r12, cpsr\n\t"
+		"bic r12, r12, #0x1F\n\t"
+		"orr r12, r12, #0x11\n\t"
+		"msr cpsr_c, r12\n\t");
+
+	register volatile char *text asm ("r8");
+	register volatile char *uart_tx asm ("r9");
+	register int len asm ("r10");
+
+	len = 5;
+	uart_tx = (char *)0x70006040;
+	memcpy((char *)text, "FIQ\r\n", len);
+	*uart_tx = 0;
+
+	asm volatile("mrs r12, cpsr\n"
+		"orr r12, r12, #0x1F\n"
+		"msr cpsr_c, r12");
+*/
+}
+
+void  __attribute__ ((target("arm"), interrupt ("FIQ"))) fiq_handler()
+{
+/*
+	register volatile char *text asm ("r8");
+	register volatile char *uart_tx asm ("r9");
+	register int len asm ("r10");
+
+	while (len)
+	{
+		*uart_tx = *text++;
+		len--;
+	}
+*/
+}

source/soc/irq.h

@@ -0,0 +1,222 @@
+/*
+ * BPMP-Lite IRQ driver for Tegra X1
+ *
+ * Copyright (c) 2019 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 IRQ_H
+#define IRQ_H
+
+#include "../utils/types.h"
+
+#define IRQ_MAX_HANDLERS 16
+
+/* Primary interrupt controller ids */
+#define IRQ_TMR1 0
+#define IRQ_TMR2 1
+#define IRQ_RTC 2
+#define IRQ_CEC 3
+#define IRQ_SHR_SEM_INBOX_FULL 4
+#define IRQ_SHR_SEM_INBOX_EMPTY 5
+#define IRQ_SHR_SEM_OUTBOX_FULL 6
+#define IRQ_SHR_SEM_OUTBOX_EMPTY 7
+#define IRQ_NVJPEG 8
+#define IRQ_NVDEC 9
+#define IRQ_QUAD_SPI 10
+#define IRQ_DPAUX_INT1 11
+#define IRQ_SATA_RX_STAT 13
+#define IRQ_SDMMC1 14
+#define IRQ_SDMMC2 15
+#define IRQ_VGPIO_INT 16
+#define IRQ_VII2C_INT 17
+#define IRQ_SDMMC3 19
+#define IRQ_USB 20
+#define IRQ_USB2 21
+#define IRQ_SATA_CTL 23
+#define IRQ_PMC_INT 24
+#define IRQ_FC_INT 25
+#define IRQ_APB_DMA_CPU 26
+#define IRQ_ARB_SEM_GNT_COP 28
+#define IRQ_ARB_SEM_GNT_CPU 29
+#define IRQ_SDMMC4 31
+
+/* Secondary interrupt controller ids */
+#define IRQ_GPIO1 32
+#define IRQ_GPIO2 33
+#define IRQ_GPIO3 34
+#define IRQ_GPIO4 35
+#define IRQ_UARTA 36
+#define IRQ_UARTB 37
+#define IRQ_I2C 38
+#define IRQ_USB3_HOST_INT 39
+#define IRQ_USB3_HOST_SMI 40
+#define IRQ_TMR3 41
+#define IRQ_TMR4 42
+#define IRQ_USB3_HOST_PME 43
+#define IRQ_USB3_DEV_HOST 44
+#define IRQ_ACTMON 45
+#define IRQ_UARTC 46
+#define IRQ_THERMAL 48
+#define IRQ_XUSB_PADCTL 49
+#define IRQ_TSEC 50
+#define IRQ_EDP 51
+#define IRQ_I2C5 53
+#define IRQ_GPIO5 55
+#define IRQ_USB3_DEV_SMI 56
+#define IRQ_USB3_DEV_PME 57
+#define IRQ_SE 58
+#define IRQ_SPI1 59
+#define IRQ_APB_DMA_COP 60
+#define IRQ_CLDVFS 62
+#define IRQ_I2C6 63
+
+/* Tertiary interrupt controller ids */
+#define IRQ_HOST1X_SYNCPT_COP 64
+#define IRQ_HOST1X_SYNCPT_CPU 65
+#define IRQ_HOST1X_GEN_COP 66
+#define IRQ_HOST1X_GEN_CPU 67
+#define IRQ_NVENC 68
+#define IRQ_VI 69
+#define IRQ_ISPB 70
+#define IRQ_ISP 71
+#define IRQ_VIC 72
+#define IRQ_DISPLAY 73
+#define IRQ_DISPLAYB 74
+#define IRQ_SOR1 75
+#define IRQ_SOR 76
+#define IRQ_MC 77
+#define IRQ_EMC 78
+#define IRQ_TSECB 80
+#define IRQ_HDA 81
+#define IRQ_SPI2 82
+#define IRQ_SPI3 83
+#define IRQ_I2C2 84
+#define IRQ_PMU_EXT 86
+#define IRQ_GPIO6 87
+#define IRQ_GPIO7 89
+#define IRQ_UARTD 90
+#define IRQ_I2C3 92
+#define IRQ_SPI4 93
+
+/* Quaternary interrupt controller ids */
+#define IRQ_DTV 96
+#define IRQ_PCIE_INT 98
+#define IRQ_PCIE_MSI 99
+#define IRQ_AVP_CACHE 101
+#define IRQ_APE_INT1 102
+#define IRQ_APE_INT0 103
+#define IRQ_APB_DMA_CH0 104
+#define IRQ_APB_DMA_CH1 105
+#define IRQ_APB_DMA_CH2 106
+#define IRQ_APB_DMA_CH3 107
+#define IRQ_APB_DMA_CH4 108
+#define IRQ_APB_DMA_CH5 109
+#define IRQ_APB_DMA_CH6 110
+#define IRQ_APB_DMA_CH7 111
+#define IRQ_APB_DMA_CH8 112
+#define IRQ_APB_DMA_CH9 113
+#define IRQ_APB_DMA_CH10 114
+#define IRQ_APB_DMA_CH11 115
+#define IRQ_APB_DMA_CH12 116
+#define IRQ_APB_DMA_CH13 117
+#define IRQ_APB_DMA_CH14 118
+#define IRQ_APB_DMA_CH15 119
+#define IRQ_I2C4 120
+#define IRQ_TMR5 121
+#define IRQ_WDT_CPU 123
+#define IRQ_WDT_AVP 124
+#define IRQ_GPIO8 125
+#define IRQ_CAR 126
+
+/* Quinary interrupt controller ids */
+#define IRQ_APB_DMA_CH16 128
+#define IRQ_APB_DMA_CH17 129
+#define IRQ_APB_DMA_CH18 130
+#define IRQ_APB_DMA_CH19 131
+#define IRQ_APB_DMA_CH20 132
+#define IRQ_APB_DMA_CH21 133
+#define IRQ_APB_DMA_CH22 134
+#define IRQ_APB_DMA_CH23 135
+#define IRQ_APB_DMA_CH24 136
+#define IRQ_APB_DMA_CH25 137
+#define IRQ_APB_DMA_CH26 138
+#define IRQ_APB_DMA_CH27 139
+#define IRQ_APB_DMA_CH28 140
+#define IRQ_APB_DMA_CH29 141
+#define IRQ_APB_DMA_CH30 142
+#define IRQ_APB_DMA_CH31 143
+#define IRQ_CPU0_PMU_INTR 144
+#define IRQ_CPU1_PMU_INTR 145
+#define IRQ_CPU2_PMU_INTR 146
+#define IRQ_CPU3_PMU_INTR 147
+#define IRQ_SDMMC1_SYS 148
+#define IRQ_SDMMC2_SYS 149
+#define IRQ_SDMMC3_SYS 150
+#define IRQ_SDMMC4_SYS 151
+#define IRQ_TMR6 152
+#define IRQ_TMR7 153
+#define IRQ_TMR8 154
+#define IRQ_TMR9 155
+#define IRQ_TMR0 156
+#define IRQ_GPU_STALL 157
+#define IRQ_GPU_NONSTALL 158
+#define IRQ_DPAUX 159
+
+/* Senary interrupt controller ids */
+#define IRQ_MPCORE_AXIERRIRQ 160
+#define IRQ_MPCORE_INTERRIRQ 161
+#define IRQ_EVENT_GPIO_A 162
+#define IRQ_EVENT_GPIO_B 163
+#define IRQ_EVENT_GPIO_C 164
+#define IRQ_FLOW_RSM_CPU 168
+#define IRQ_FLOW_RSM_COP 169
+#define IRQ_TMR_SHARED 170
+#define IRQ_MPCORE_CTIIRQ0 171
+#define IRQ_MPCORE_CTIIRQ1 172
+#define IRQ_MPCORE_CTIIRQ2 173
+#define IRQ_MPCORE_CTIIRQ3 174
+#define IRQ_MSELECT_ERROR 175
+#define IRQ_TMR10 176
+#define IRQ_TMR11 177
+#define IRQ_TMR12 178
+#define IRQ_TMR13 179
+
+typedef int (*irq_handler_t)(u32 irq, void *data);
+
+typedef enum _irq_status_t
+{
+	IRQ_NONE    = 0,
+	IRQ_HANDLED = 1,
+	IRQ_ERROR   = 2,
+
+	IRQ_ENABLED            = 0,
+	IRQ_NO_SLOTS_AVAILABLE = 1,
+	IRQ_ALREADY_REGISTERED = 2
+} irq_status_t;
+
+typedef enum _irq_flags_t
+{
+	IRQ_FLAG_NONE        = 0,
+	IRQ_FLAG_ONE_OFF     = (1 << 0),
+	IRQ_FLAG_REPLACEABLE = (1 << 1)
+} irq_flags_t;
+
+void irq_end();
+void irq_free(u32 irq);
+void irq_wait_event();
+void irq_disable_wait_event();
+irq_status_t irq_request(u32 irq, irq_handler_t handler, void *data, irq_flags_t flags);
+
+#endif

source/soc/kfuse.c

@@ -0,0 +1,56 @@
+/*
+ * 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 "../soc/kfuse.h"
+#include "../soc/clock.h"
+#include "../soc/t210.h"
+
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
+int kfuse_wait_ready()
+{
+	// Wait for KFUSE to finish init and verification of data.
+	while (!(KFUSE(KFUSE_STATE) & KFUSE_STATE_DONE))
+		;
+
+	if (!(KFUSE(KFUSE_STATE) & KFUSE_STATE_CRCPASS))
+		return 0;
+
+	return 1;
+}
+
+int kfuse_read(u32 *buf)
+{
+	int res = 0;
+
+	clock_enable_kfuse();
+
+	if (!kfuse_wait_ready())
+		goto out;
+
+	KFUSE(KFUSE_KEYADDR) = KFUSE_KEYADDR_AUTOINC;
+	for (int i = 0; i < KFUSE_NUM_WORDS; i++)
+		buf[i] = KFUSE(KFUSE_KEYS);
+
+	res = 1;
+
+out:;
+	clock_disable_kfuse();
+	return res;
+}
+
+#pragma GCC pop_options

source/soc/kfuse.h

@@ -0,0 +1,42 @@
+/*
+ * 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/>.
+ */
+
+#ifndef _KFUSE_H_
+#define _KFUSE_H_
+
+#include "../utils/types.h"
+
+#define KFUSE_STATE_SOFTRESET      (1 << 31)
+#define KFUSE_STATE_STOP           (1 << 25)
+#define KFUSE_STATE_RESTART        (1 << 24)
+#define KFUSE_STATE_CRCPASS        (1 << 17)
+#define KFUSE_STATE_DONE           (1 << 16)
+#define KFUSE_STATE_ERRBLOCK_MASK  0x3F00
+#define KFUSE_STATE_ERRBLOCK_SHIFT 8
+#define KFUSE_STATE_CURBLOCK_MASK  0x3F
+
+#define KFUSE_KEYADDR_AUTOINC (1<<16)
+
+#define KFUSE_STATE 0x80
+#define KFUSE_KEYADDR 0x88
+#define KFUSE_KEYS 0x8C
+
+#define KFUSE_NUM_WORDS 144
+
+int kfuse_wait_ready();
+int kfuse_read(u32 *buf);
+
+#endif

source/soc/pinmux.h

@@ -26,31 +26,51 @@
 #define APB_MISC_GP_VGPIO_GPIO_MUX_SEL      0xB74
 
 /*! Pinmux registers. */
-#define PINMUX_AUX_SDMMC1_CLK  0x00
-#define PINMUX_AUX_SDMMC1_CMD  0x04
-#define PINMUX_AUX_SDMMC1_DAT3 0x08
-#define PINMUX_AUX_SDMMC1_DAT2 0x0C
-#define PINMUX_AUX_SDMMC1_DAT1 0x10
-#define PINMUX_AUX_SDMMC1_DAT0 0x14
-#define PINMUX_AUX_SDMMC3_CLK  0x1C
-#define PINMUX_AUX_SDMMC3_CMD  0x20
-#define PINMUX_AUX_SDMMC3_DAT0 0x24
-#define PINMUX_AUX_SDMMC3_DAT1 0x28
-#define PINMUX_AUX_SDMMC3_DAT2 0x2C
-#define PINMUX_AUX_SDMMC3_DAT3 0x30
-#define PINMUX_AUX_DMIC3_CLK   0xB4
-#define PINMUX_AUX_UART2_TX    0xF4
-#define PINMUX_AUX_UART3_TX    0x104
-#define PINMUX_AUX_WIFI_EN     0x1B4
-#define PINMUX_AUX_WIFI_RST    0x1B8
-#define PINMUX_AUX_NFC_EN      0x1D0
-#define PINMUX_AUX_NFC_INT     0x1D4
-#define PINMUX_AUX_LCD_BL_PWM  0x1FC
-#define PINMUX_AUX_LCD_BL_EN   0x200
-#define PINMUX_AUX_LCD_RST     0x204
-#define PINMUX_AUX_GPIO_PE6    0x248
-#define PINMUX_AUX_GPIO_PH6    0x250
-#define PINMUX_AUX_GPIO_PZ1    0x280
+#define PINMUX_AUX_SDMMC1_CLK      0x00
+#define PINMUX_AUX_SDMMC1_CMD      0x04
+#define PINMUX_AUX_SDMMC1_DAT3     0x08
+#define PINMUX_AUX_SDMMC1_DAT2     0x0C
+#define PINMUX_AUX_SDMMC1_DAT1     0x10
+#define PINMUX_AUX_SDMMC1_DAT0     0x14
+#define PINMUX_AUX_SDMMC3_CLK      0x1C
+#define PINMUX_AUX_SDMMC3_CMD      0x20
+#define PINMUX_AUX_SDMMC3_DAT0     0x24
+#define PINMUX_AUX_SDMMC3_DAT1     0x28
+#define PINMUX_AUX_SDMMC3_DAT2     0x2C
+#define PINMUX_AUX_SDMMC3_DAT3     0x30
+#define PINMUX_AUX_SATA_LED_ACTIVE 0x4C
+#define PINMUX_AUX_DMIC3_CLK       0xB4
+#define PINMUX_AUX_DMIC3_DAT       0xB8
+#define PINMUX_AUX_CAM_I2C_SCL     0xD4
+#define PINMUX_AUX_CAM_I2C_SDA     0xD8
+#define PINMUX_AUX_UART2_TX        0xF4
+#define PINMUX_AUX_UART3_TX        0x104
+#define PINMUX_AUX_DAP4_DIN        0x148
+#define PINMUX_AUX_DAP4_SCLK       0x150
+#define PINMUX_AUX_GPIO_X1_AUD     0x18C
+#define PINMUX_AUX_GPIO_X3_AUD     0x190
+#define PINMUX_AUX_SPDIF_IN        0x1A4
+#define PINMUX_AUX_USB_VBUS_EN0    0x1A8
+#define PINMUX_AUX_USB_VBUS_EN1    0x1AC
+#define PINMUX_AUX_WIFI_EN         0x1B4
+#define PINMUX_AUX_WIFI_RST        0x1B8
+#define PINMUX_AUX_AP_WAKE_NFC     0x1CC
+#define PINMUX_AUX_NFC_EN          0x1D0
+#define PINMUX_AUX_NFC_INT         0x1D4
+#define PINMUX_AUX_CAM1_PWDN       0x1EC
+#define PINMUX_AUX_CAM2_PWDN       0x1F0
+#define PINMUX_AUX_LCD_BL_PWM      0x1FC
+#define PINMUX_AUX_LCD_BL_EN       0x200
+#define PINMUX_AUX_LCD_RST         0x204
+#define PINMUX_AUX_LCD_GPIO1       0x208
+#define PINMUX_AUX_LCD_GPIO2       0x20C
+#define PINMUX_AUX_TOUCH_INT       0x220
+#define PINMUX_AUX_MOTION_INT      0x224
+#define PINMUX_AUX_BUTTON_HOME     0x240
+#define PINMUX_AUX_GPIO_PE6        0x248
+#define PINMUX_AUX_GPIO_PH6        0x250
+#define PINMUX_AUX_GPIO_PK3        0x260
+#define PINMUX_AUX_GPIO_PZ1        0x280
 /*! 0:UART-A, 1:UART-B, 3:UART-C, 3:UART-D */
 #define PINMUX_AUX_UARTX_TX(x)  (0xE4 + 0x10 * (x))
 #define PINMUX_AUX_UARTX_RX(x)  (0xE8 + 0x10 * (x))
@@ -78,7 +98,7 @@
 #define PINMUX_OPEN_DRAIN   (1 << 11)
 #define PINMUX_SCHMT        (1 << 12)
 
-#define PINMUX_DRIVE_1X     (0 << 13) 
+#define PINMUX_DRIVE_1X     (0 << 13)
 #define PINMUX_DRIVE_2X     (1 << 13)
 #define PINMUX_DRIVE_3X     (2 << 13)
 #define PINMUX_DRIVE_4X     (3 << 13)

source/soc/pmc.h

@@ -25,18 +25,32 @@
 #define APBDEV_PMC_PWRGATE_TOGGLE 0x30
 #define APBDEV_PMC_PWRGATE_STATUS 0x38
 #define APBDEV_PMC_NO_IOPOWER 0x44
+#define  PMC_NO_IOPOWER_GPIO_IO_EN   (1 << 21)
+#define  PMC_NO_IOPOWER_AUDIO_HV     (1 << 18)
+#define  PMC_NO_IOPOWER_SDMMC1_IO_EN (1 << 12)
 #define APBDEV_PMC_SCRATCH0 0x50
+#define  PMC_SCRATCH0_MODE_RECOVERY (1 << 31)
+#define  PMC_SCRATCH0_MODE_FASTBOOT (1 << 30)
+#define  PMC_SCRATCH0_MODE_PAYLOAD  (1 << 29)
+#define  PMC_SCRATCH0_MODE_RCM      (1 << 1)
+#define  PMC_SCRATCH0_MODE_WARMBOOT (1 << 0)
 #define APBDEV_PMC_SCRATCH1 0x54
 #define APBDEV_PMC_SCRATCH20 0xA0
 #define APBDEV_PMC_PWR_DET_VAL 0xE4
+#define  PMC_PWR_DET_GPIO_IO_EN   (1 << 21)
+#define  PMC_PWR_DET_AUDIO_HV     (1 << 18)
 #define  PMC_PWR_DET_SDMMC1_IO_EN (1 << 12)
 #define APBDEV_PMC_DDR_PWR 0xE8
+#define APBDEV_PMC_USB_AO 0xF0
 #define APBDEV_PMC_CRYPTO_OP 0xF4
 #define  PMC_CRYPTO_OP_SE_ENABLE  0
 #define  PMC_CRYPTO_OP_SE_DISABLE 1
 #define APBDEV_PMC_SCRATCH33 0x120
 #define APBDEV_PMC_SCRATCH40 0x13C
 #define APBDEV_PMC_OSC_EDPD_OVER 0x1A4
+#define  PMC_OSC_EDPD_OVER_OSC_CTRL_OVER 0x400000
+#define APBDEV_PMC_CLK_OUT_CNTRL 0x1A8
+#define  PMC_CLK_OUT_CNTRL_CLK1_FORCE_EN (1 << 2)
 #define APBDEV_PMC_RST_STATUS 0x1B4
 #define APBDEV_PMC_IO_DPD_REQ 0x1B8
 #define APBDEV_PMC_IO_DPD2_REQ 0x1C0
@@ -51,9 +65,11 @@
 #define APBDEV_PMC_REG_SHORT 0x2CC
 #define APBDEV_PMC_SEC_DISABLE3 0x2D8
 #define APBDEV_PMC_SECURE_SCRATCH21 0x334
+#define  PMC_FUSE_PRIVATEKEYDISABLE_TZ_STICKY_BIT 0x10
 #define APBDEV_PMC_SECURE_SCRATCH32 0x360
 #define APBDEV_PMC_SECURE_SCRATCH49 0x3A4
 #define APBDEV_PMC_CNTRL2 0x440
+#define  PMC_CNTRL2_HOLD_CKE_LOW_EN 0x1000
 #define APBDEV_PMC_IO_DPD3_REQ 0x45C
 #define APBDEV_PMC_IO_DPD4_REQ 0x464
 #define APBDEV_PMC_UTMIP_PAD_CFG1 0x4C4

source/soc/smmu.c

@@ -106,7 +106,7 @@ bool smmu_is_used()
 
 void smmu_exit()
 {
-	*(uint32_t *)(smmu_payload + 0x14) = _NOP();
+	*(u32 *)(smmu_payload + 0x14) = _NOP();
 }
 
 u32 *smmu_init_domain4(u32 dev_base, u32 asid)

source/soc/t210.h

@@ -20,6 +20,7 @@
 #include "../utils/types.h"
 
 #define BOOTROM_BASE 0x100000
+#define IRAM_BASE 0x40000000
 #define HOST1X_BASE 0x50000000
 #define BPMP_CACHE_BASE 0x50040000
 #define DISPLAY_A_BASE 0x54200000
@@ -27,9 +28,11 @@
 #define VIC_BASE 0x54340000
 #define TSEC_BASE 0x54500000
 #define SOR1_BASE 0x54580000
+#define ICTLR_BASE 0x60004000
 #define TMR_BASE 0x60005000
 #define CLOCK_BASE 0x60006000
 #define FLOW_CTLR_BASE 0x60007000
+#define AHBDMA_BASE 0x60008000
 #define SYSREG_BASE 0x6000C000
 #define SB_BASE (SYSREG_BASE + 0x200)
 #define GPIO_BASE 0x6000D000
@@ -43,6 +46,7 @@
 #define GPIO_8_BASE (GPIO_BASE + 0x700)
 #define EXCP_VEC_BASE 0x6000F000
 #define IPATCH_BASE 0x6001DC00
+#define APBDMA_BASE 0x60020000
 #define APB_MISC_BASE 0x70000000
 #define PINMUX_AUX_BASE 0x70003000
 #define UART_BASE 0x70006000
@@ -55,10 +59,16 @@
 #define SE_BASE 0x70012000
 #define MC_BASE 0x70019000
 #define EMC_BASE 0x7001B000
+#define EMC0_BASE 0x7001E000
+#define EMC1_BASE 0x7001F000
 #define MIPI_CAL_BASE 0x700E3000
 #define CL_DVFS_BASE 0x70110000
 #define I2S_BASE 0x702D1000
+#define ADMA_BASE 0x702E2000
 #define TZRAM_BASE 0x7C010000
+#define USB_BASE 0x7D000000
+#define USB_OTG_BASE USB_BASE
+#define USB1_BASE 0x7D004000
 
 #define _REG(base, off) *(vu32 *)((base) + (off))
 
@@ -69,10 +79,12 @@
 #define VIC(off) _REG(VIC_BASE, off)
 #define TSEC(off) _REG(TSEC_BASE, off)
 #define SOR1(off) _REG(SOR1_BASE, off)
+#define ICTLR(cidx, off) _REG(ICTLR_BASE + (0x100 * cidx), off)
 #define TMR(off) _REG(TMR_BASE, off)
 #define CLOCK(off) _REG(CLOCK_BASE, off)
 #define FLOW_CTLR(off) _REG(FLOW_CTLR_BASE, off)
 #define SYSREG(off) _REG(SYSREG_BASE, off)
+#define AHB_GIZMO(off) _REG(SYSREG_BASE, off)
 #define SB(off) _REG(SB_BASE, off)
 #define GPIO(off) _REG(GPIO_BASE, off)
 #define GPIO_1(off) _REG(GPIO_1_BASE, off)
@@ -95,20 +107,67 @@
 #define SE(off) _REG(SE_BASE, off)
 #define MC(off) _REG(MC_BASE, off)
 #define EMC(off) _REG(EMC_BASE, off)
+#define EMC_CH0(off) _REG(EMC0_BASE, off)
+#define EMC_CH1(off) _REG(EMC1_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 I2S(off) _REG(I2S_BASE, off)
+#define ADMA(off) _REG(ADMA_BASE, off)
+#define USB(off) _REG(USB_BASE, off)
+#define USB1(off) _REG(USB1_BASE, off)
 #define TEST_REG(off) _REG(0x0, off)
 
+/* HOST1X registers. */
+#define HOST1X_CH0_SYNC_BASE 0x2100
+#define HOST1X_CH0_SYNC_SYNCPT_9   (HOST1X_CH0_SYNC_BASE + 0xFA4)
+#define HOST1X_CH0_SYNC_SYNCPT_160 (HOST1X_CH0_SYNC_BASE + 0x1200)
+
 /*! EVP registers. */
 #define EVP_CPU_RESET_VECTOR 0x100
+#define EVP_COP_RESET_VECTOR 0x200
+#define EVP_COP_UNDEF_VECTOR 0x204
+#define EVP_COP_SWI_VECTOR 0x208
+#define EVP_COP_PREFETCH_ABORT_VECTOR 0x20C
+#define EVP_COP_DATA_ABORT_VECTOR 0x210
+#define EVP_COP_RSVD_VECTOR 0x214
+#define EVP_COP_IRQ_VECTOR 0x218
+#define EVP_COP_FIQ_VECTOR 0x21C
+#define EVP_COP_IRQ_STS 0x220
+
+/*! Primary Interrupt Controller registers. */
+#define PRI_ICTLR_FIR 0x14
+#define PRI_ICTLR_FIR_SET 0x18
+#define PRI_ICTLR_FIR_CLR 0x1C
+#define PRI_ICTLR_CPU_IER 0x20
+#define PRI_ICTLR_CPU_IER_SET 0x24
+#define PRI_ICTLR_CPU_IER_CLR 0x28
+#define PRI_ICTLR_CPU_IEP_CLASS 0x2C
+#define PRI_ICTLR_COP_IER 0x30
+#define PRI_ICTLR_COP_IER_SET 0x34
+#define PRI_ICTLR_COP_IER_CLR 0x38
+#define PRI_ICTLR_COP_IEP_CLASS 0x3C
+
+/*! AHB Gizmo registers. */
+#define AHB_ARBITRATION_PRIORITY_CTRL 0x8
+#define  ARBITRATION_PRIORITY_CTRL_ENB_FAST_REARBITRATE (1 << 6)
+#define AHB_GIZMO_AHB_MEM 0x10
+#define  AHB_MEM_ENB_FAST_REARBITRATE (1 << 2)
+#define AHB_GIZMO_USB 0x20
+#define  AHB_GIZMO_USB_IMMEDIATE (1 << 18)
+#define AHB_AHB_MEM_PREFETCH_CFG1 0xF0
+#define  MEM_PREFETCH_ENABLE (1 << 31)
+#define  MEM_PREFETCH_AHB_MST_USB 6
 
 /*! Misc registers. */
 #define APB_MISC_PP_STRAPPING_OPT_A 0x08
 #define APB_MISC_PP_PINMUX_GLOBAL 0x40
+#define APB_MISC_GP_HIDREV 0x804
+#define APB_MISC_GP_AUD_MCLK_CFGPADCTRL 0x8F4
 #define APB_MISC_GP_LCD_BL_PWM_CFGPADCTRL 0xA34
 #define APB_MISC_GP_SDMMC1_PAD_CFGPADCTRL 0xA98
+#define APB_MISC_GP_EMMC2_PAD_CFGPADCTRL 0xA9C
 #define APB_MISC_GP_EMMC4_PAD_CFGPADCTRL 0xAB4
+#define APB_MISC_GP_EMMC4_PAD_PUPD_CFGPADCTRL 0xABC
 #define APB_MISC_GP_WIFI_EN_CFGPADCTRL 0xB64
 #define APB_MISC_GP_WIFI_RST_CFGPADCTRL 0xB68
 
@@ -118,7 +177,10 @@
 
 /*! Secure boot registers. */
 #define SB_CSR 0x0
+#define  SB_CSR_NS_RST_VEC_WR_DIS    (1 << 1)
+#define  SB_CSR_PIROM_DISABLE        (1 << 4)
 #define SB_AA64_RESET_LOW  0x30
+#define  SB_AA64_RST_AARCH64_MODE_EN (1 << 0)
 #define SB_AA64_RESET_HIGH 0x34
 
 /*! SOR registers. */
@@ -145,15 +207,20 @@
 #define SYSCTR0_COUNTERID7  0xFDC
 #define SYSCTR0_COUNTERID8  0xFF0
 #define SYSCTR0_COUNTERID9  0xFF4
-#define SYSCTR0_COUNTERID10 0xFF8 
+#define SYSCTR0_COUNTERID10 0xFF8
 #define SYSCTR0_COUNTERID11 0xFFC
 
 /*! TMR registers. */
 #define TIMERUS_CNTR_1US          (0x10 + 0x0)
 #define TIMERUS_USEC_CFG          (0x10 + 0x4)
+#define TIMER_TMR8_TMR_PTV        0x78
 #define TIMER_TMR9_TMR_PTV        0x80
-#define  TIMER_EN     (1 << 31)
-#define  TIMER_PER_EN (1 << 30)
+#define  TIMER_EN          (1 << 31)
+#define  TIMER_PER_EN      (1 << 30)
+#define TIMER_TMR8_TMR_PCR        0x7C
+#define TIMER_TMR9_TMR_PCR        0x8C
+#define  TIMER_INTR_CLR    (1 << 30)
+
 #define TIMER_WDT4_CONFIG         (0x100 + 0x80)
 #define  TIMER_SRC(TMR) (TMR & 0xF)
 #define  TIMER_PER(PER) ((PER & 0xFF) << 4)
@@ -182,10 +249,38 @@
 /*! PWM registers. */
 #define PWM_CONTROLLER_PWM_CSR_0 0x00
 #define PWM_CONTROLLER_PWM_CSR_1 0x10
+#define  PWM_CSR_EN (1 << 31)
 
 /*! Special registers. */
 #define EMC_SCRATCH0 0x324
 #define  EMC_HEKA_UPD (1 << 30)
 #define  EMC_SEPT_RUN (1 << 31)
 
+/*! Flow controller registers. */
+#define FLOW_CTLR_HALT_COP_EVENTS  0x4
+#define  HALT_COP_GIC_IRQ        (1 << 9)
+#define  HALT_COP_LIC_IRQ        (1 << 11)
+#define  HALT_COP_SEC            (1 << 23)
+#define  HALT_COP_MSEC           (1 << 24)
+#define  HALT_COP_USEC           (1 << 25)
+#define  HALT_COP_JTAG           (1 << 28)
+#define  HALT_COP_WAIT_EVENT     (1 << 30)
+#define  HALT_COP_STOP_UNTIL_IRQ (1 << 31)
+#define  HALT_COP_MAX_CNT        0xFF
+#define FLOW_CTLR_HALT_CPU0_EVENTS 0x0
+#define FLOW_CTLR_HALT_CPU1_EVENTS 0x14
+#define FLOW_CTLR_HALT_CPU2_EVENTS 0x1C
+#define FLOW_CTLR_HALT_CPU3_EVENTS 0x24
+#define FLOW_CTLR_CPU0_CSR 0x8
+#define FLOW_CTLR_CPU1_CSR 0x18
+#define FLOW_CTLR_CPU2_CSR 0x20
+#define FLOW_CTLR_CPU3_CSR 0x28
+#define FLOW_CTLR_RAM_REPAIR 0x40
+#define FLOW_CTLR_BPMP_CLUSTER_CONTROL 0x98
+
+/*! USB controller registers. */
+#define USB1_UTMIP_BAT_CHRG_CFG0 0x830
+#define  BAT_CHRG_CFG0_OP_SRC_EN    (1 << 3)
+#define  BAT_CHRG_CFG0_PWRDOWN_CHRG (1 << 0)
+
 #endif

source/storage/emummc.c

@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2019 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/>.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+
+#include "emummc.h"
+#include "sdmmc.h"
+#include "../config/config.h"
+#include "../config/ini.h"
+#include "../gfx/gfx.h"
+#include "../libs/fatfs/ff.h"
+#include "../mem/heap.h"
+#include "../storage/nx_sd.h"
+#include "../utils/list.h"
+#include "../utils/types.h"
+
+extern hekate_config h_cfg;
+emummc_cfg_t emu_cfg;
+
+bool emummc_load_cfg()
+{
+	sd_mount();
+	emu_cfg.enabled = 0;
+	emu_cfg.path = NULL;
+	emu_cfg.nintendo_path = NULL;
+	emu_cfg.sector = 0;
+	emu_cfg.id = 0;
+	emu_cfg.file_based_part_size = 0;
+	emu_cfg.active_part = 0;
+	emu_cfg.fs_ver = 0;
+	if (!emu_cfg.emummc_file_based_path)
+		emu_cfg.emummc_file_based_path = (char *)malloc(0x80);
+
+	LIST_INIT(ini_sections);
+	if (ini_parse(&ini_sections, "emuMMC/emummc.ini", false))
+	{
+		LIST_FOREACH_ENTRY(ini_sec_t, ini_sec, &ini_sections, link)
+		{
+			if (ini_sec->type == INI_CHOICE)
+			{
+				if (strcmp(ini_sec->name, "emummc"))
+					continue;
+
+				LIST_FOREACH_ENTRY(ini_kv_t, kv, &ini_sec->kvs, link)
+				{
+					if (!strcmp("enabled", kv->key))
+						emu_cfg.enabled = atoi(kv->val);
+					else if (!strcmp("sector", kv->key))
+						emu_cfg.sector = strtol(kv->val, NULL, 16);
+					else if (!strcmp("id", kv->key))
+						emu_cfg.id = strtol(kv->val, NULL, 16);
+					else if (!strcmp("path", kv->key))
+						emu_cfg.path = kv->val;
+					else if (!strcmp("nintendo_path", kv->key))
+						emu_cfg.nintendo_path = kv->val;
+				}
+				break;
+			}
+		}
+		return 0;
+	}
+	return 1;
+}
+
+static int emummc_raw_get_part_off(int part_idx)
+{
+	switch (part_idx)
+	{
+	case 0:
+		return 2;
+	case 1:
+		return 0;
+	case 2:
+		return 1;
+	}
+	return 2;
+}
+
+int emummc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc)
+{
+	FILINFO fno;
+	if (!sdmmc_storage_init_mmc(storage, sdmmc, SDMMC_BUS_WIDTH_8, SDHCI_TIMING_MMC_HS400))
+		return 2;
+
+	if (h_cfg.emummc_force_disable)
+		return 0;
+
+	emu_cfg.active_part = 0;
+	if (!sd_mount())
+		goto out;
+
+	if (emu_cfg.enabled && !emu_cfg.sector)
+	{
+		strcpy(emu_cfg.emummc_file_based_path, emu_cfg.path);
+		strcat(emu_cfg.emummc_file_based_path, "/eMMC");
+
+		if (f_stat(emu_cfg.emummc_file_based_path, &fno))
+		{
+			EPRINTF("Failed to open eMMC folder.");
+			goto out;
+		}
+		f_chmod(emu_cfg.emummc_file_based_path, AM_ARC, AM_ARC);
+
+		strcat(emu_cfg.emummc_file_based_path, "/00");
+		if (f_stat(emu_cfg.emummc_file_based_path, &fno))
+		{
+			EPRINTF("Failed to open emuMMC rawnand.");
+			goto out;
+		}
+		emu_cfg.file_based_part_size = fno.fsize >> 9;
+	}
+
+	return 0;
+
+out:
+	return 1;
+}
+
+int emummc_storage_end(sdmmc_storage_t *storage)
+{
+	sd_unmount();
+	sdmmc_storage_end(storage);
+
+	return 1;
+}
+
+int emummc_storage_read(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf)
+{
+	FIL fp;
+	if (!emu_cfg.enabled || h_cfg.emummc_force_disable)
+		return sdmmc_storage_read(storage, sector, num_sectors, buf);
+	else if (emu_cfg.sector)
+	{
+		sector += emu_cfg.sector;
+		sector += emummc_raw_get_part_off(emu_cfg.active_part) * 0x2000;
+		return sdmmc_storage_read(&sd_storage, sector, num_sectors, buf);
+	}
+	else
+	{
+		if (!emu_cfg.active_part)
+		{
+			u32 file_part = sector / emu_cfg.file_based_part_size;
+			sector = sector % emu_cfg.file_based_part_size;
+			if (file_part >= 10)
+				itoa(file_part, emu_cfg.emummc_file_based_path + strlen(emu_cfg.emummc_file_based_path) - 2, 10);
+			else
+			{
+				emu_cfg.emummc_file_based_path[strlen(emu_cfg.emummc_file_based_path) - 2] = '0';
+				itoa(file_part, emu_cfg.emummc_file_based_path + strlen(emu_cfg.emummc_file_based_path) - 1, 10);
+			}
+		}
+		if (f_open(&fp, emu_cfg.emummc_file_based_path, FA_READ))
+		{
+			EPRINTF("Failed to open emuMMC image.");
+			return 0;
+		}
+		f_lseek(&fp, (u64)sector << 9);
+		if (f_read(&fp, buf, (u64)num_sectors << 9, NULL))
+		{
+			EPRINTF("Failed to read emuMMC image.");
+			f_close(&fp);
+			return 0;
+		}
+
+		f_close(&fp);
+		return 1;
+	}
+
+	return 1;
+}
+
+int emummc_storage_write(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf)
+{
+	FIL fp;
+	if (!emu_cfg.enabled || h_cfg.emummc_force_disable)
+		return sdmmc_storage_write(storage, sector, num_sectors, buf);
+	else if (emu_cfg.sector)
+	{
+		sector += emu_cfg.sector;
+		sector += emummc_raw_get_part_off(emu_cfg.active_part) * 0x2000;
+		return sdmmc_storage_write(&sd_storage, sector, num_sectors, buf);
+	}
+	else
+	{
+		if (!emu_cfg.active_part)
+		{
+			u32 file_part = sector / emu_cfg.file_based_part_size;
+			sector = sector % emu_cfg.file_based_part_size;
+			if (file_part >= 10)
+				itoa(file_part, emu_cfg.emummc_file_based_path + strlen(emu_cfg.emummc_file_based_path) - 2, 10);
+			else
+			{
+				emu_cfg.emummc_file_based_path[strlen(emu_cfg.emummc_file_based_path) - 2] = '0';
+				itoa(file_part, emu_cfg.emummc_file_based_path + strlen(emu_cfg.emummc_file_based_path) - 1, 10);
+			}
+		}
+		if (f_open(&fp, emu_cfg.emummc_file_based_path, FA_WRITE))
+		{
+			gfx_printf("e5\n");
+			return 0;
+		}
+		f_lseek(&fp, (u64)sector << 9);
+		if (f_write(&fp, buf, (u64)num_sectors << 9, NULL))
+		{
+			gfx_printf("e6\n");
+			f_close(&fp);
+			return 0;
+		}
+
+		f_close(&fp);
+		return 1;
+	}
+}
+
+int emummc_storage_set_mmc_partition(sdmmc_storage_t *storage, u32 partition)
+{
+	emu_cfg.active_part = partition;
+
+	if (!emu_cfg.enabled || h_cfg.emummc_force_disable)
+		sdmmc_storage_set_mmc_partition(storage, partition);
+	else if (emu_cfg.sector)
+		return 1;
+	else
+	{
+		strcpy(emu_cfg.emummc_file_based_path, emu_cfg.path);
+		strcat(emu_cfg.emummc_file_based_path, "/eMMC");
+
+		switch (partition)
+		{
+		case 0:
+			strcat(emu_cfg.emummc_file_based_path, "/00");
+			break;
+		case 1:
+			strcat(emu_cfg.emummc_file_based_path, "/BOOT0");
+			break;
+		case 2:
+			strcat(emu_cfg.emummc_file_based_path, "/BOOT1");
+			break;
+		}
+
+		return 1;
+	}
+
+	return 1;
+}

source/storage/emummc.h

@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2019 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 EMUMMC_H
+#define EMUMMC_H
+
+#include "sdmmc.h"
+#include "../utils/types.h"
+
+typedef enum
+{
+	EMUMMC_TYPE_NONE      = 0,
+	EMUMMC_TYPE_PARTITION = 1,
+	EMUMMC_TYPE_FILES     = 2,
+} emummc_type_t;
+
+typedef enum {
+	EMUMMC_MMC_NAND = 0,
+	EMUMMC_MMC_SD   = 1,
+	EMUMMC_MMC_GC   = 2,
+} emummc_mmc_t;
+
+typedef struct _emummc_cfg_t
+{
+	int   enabled;
+	u64   sector;
+	u16   id;
+	char *path;
+	char *nintendo_path;
+	// Internal.
+	char *emummc_file_based_path;
+	u32 file_based_part_size;
+	u32 active_part;
+	int fs_ver;
+} emummc_cfg_t;
+
+extern emummc_cfg_t emu_cfg;
+
+bool emummc_load_cfg();
+int emummc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc);
+int emummc_storage_end(sdmmc_storage_t *storage);
+int emummc_storage_read(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf);
+int emummc_storage_write(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf);
+int emummc_storage_set_mmc_partition(sdmmc_storage_t *storage, u32 partition);
+
+#endif

source/storage/mbr_gpt.h

@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2019 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 MBR_GPT_H
+#define MBR_GPT_H
+
+#include "../utils/types.h"
+
+typedef struct _mbr_chs_t
+{
+	u8 head;
+	u8 sector;
+	u8 cylinder;
+} __attribute__((packed)) mbr_chs_t;
+
+typedef struct _mbr_part_t
+{
+	u8  status;
+	mbr_chs_t start_sct_chs;
+	u8  type;
+	mbr_chs_t end_sct_chs;
+	u32 start_sct;
+	u32 size_sct;
+} __attribute__((packed)) mbr_part_t;
+
+typedef struct _mbr_t
+{
+	u8  bootstrap[440];
+	u32 signature;
+	u16 copy_protected;
+	mbr_part_t partitions[4];
+	u16 boot_signature;
+} __attribute__((packed)) mbr_t;
+
+typedef struct _gpt_entry_t
+{
+	u8  type_guid[0x10];
+	u8  part_guid[0x10];
+	u64 lba_start;
+	u64 lba_end;
+	u64 attrs;
+	u16 name[36];
+} gpt_entry_t;
+
+typedef struct _gpt_header_t
+{
+	u64 signature; // "EFI PART"
+	u32 revision;
+	u32 size;
+	u32 crc32;
+	u32 res1;
+	u64 my_lba;
+	u64 alt_lba;
+	u64 first_use_lba;
+	u64 last_use_lba;
+	u8  disk_guid[0x10];
+	u64 part_ent_lba;
+	u32 num_part_ents;
+	u32 part_ent_size;
+	u32 part_ents_crc32;
+	u8  res2[420]; // Used as first 3 partition entries backup for HOS.
+} gpt_header_t;
+
+typedef struct _gpt_t
+{
+	gpt_header_t header;
+	gpt_entry_t  entries[128];
+} gpt_t;
+
+#endif

source/storage/nx_emmc.c

@@ -16,34 +16,39 @@
 
 #include <string.h>
 
+#include "mbr_gpt.h"
 #include "nx_emmc.h"
+#include "emummc.h"
 #include "../mem/heap.h"
 #include "../utils/list.h"
 
 void nx_emmc_gpt_parse(link_t *gpt, sdmmc_storage_t *storage)
 {
-	u8 *buf = (u8 *)malloc(NX_GPT_NUM_BLOCKS * NX_EMMC_BLOCKSIZE);
+	gpt_t *gpt_buf = (gpt_t *)calloc(NX_GPT_NUM_BLOCKS, NX_EMMC_BLOCKSIZE);
 
-	sdmmc_storage_read(storage, NX_GPT_FIRST_LBA, NX_GPT_NUM_BLOCKS, buf);
+	emummc_storage_read(storage, NX_GPT_FIRST_LBA, NX_GPT_NUM_BLOCKS, gpt_buf);
 
-	gpt_header_t *hdr = (gpt_header_t *)buf;
-	for (u32 i = 0; i < hdr->num_part_ents; i++)
+	for (u32 i = 0; i < gpt_buf->header.num_part_ents; i++)
 	{
-		gpt_entry_t *ent = (gpt_entry_t *)(buf + (hdr->part_ent_lba - 1) * NX_EMMC_BLOCKSIZE + i * sizeof(gpt_entry_t));
-		emmc_part_t *part = (emmc_part_t *)malloc(sizeof(emmc_part_t));
-		part->lba_start = ent->lba_start;
-		part->lba_end = ent->lba_end;
-		part->attrs = ent->attrs;
+		emmc_part_t *part = (emmc_part_t *)calloc(sizeof(emmc_part_t), 1);
 
-		//HACK
-		for (u32 i = 0; i < 36; i++)
-			part->name[i] = ent->name[i];
-		part->name[36] = 0;
+		if (gpt_buf->entries[i].lba_start < gpt_buf->header.first_use_lba)
+			continue;
+
+		part->index = i;
+		part->lba_start = gpt_buf->entries[i].lba_start;
+		part->lba_end = gpt_buf->entries[i].lba_end;
+		part->attrs = gpt_buf->entries[i].attrs;
+
+		// ASCII conversion. Copy only the LSByte of the UTF-16LE name.
+		for (u32 j = 0; j < 36; j++)
+			part->name[j] = gpt_buf->entries[i].name[j];
+		part->name[35] = 0;
 
 		list_append(gpt, &part->link);
 	}
 
-	free(buf);
+	free(gpt_buf);
 }
 
 void nx_emmc_gpt_free(link_t *gpt)
@@ -65,7 +70,7 @@ int nx_emmc_part_read(sdmmc_storage_t *storage, emmc_part_t *part, u32 sector_of
 	// The last LBA is inclusive.
 	if (part->lba_start + sector_off > part->lba_end)
 		return 0;
-	return sdmmc_storage_read(storage, part->lba_start + sector_off, num_sectors, buf);
+	return emummc_storage_read(storage, part->lba_start + sector_off, num_sectors, buf);
 }
 
 int nx_emmc_part_write(sdmmc_storage_t *storage, emmc_part_t *part, u32 sector_off, u32 num_sectors, void *buf)

source/storage/nx_emmc.h

@@ -17,38 +17,9 @@
 #ifndef _NX_EMMC_H_
 #define _NX_EMMC_H_
 
+#include "sdmmc.h"
 #include "../utils/types.h"
 #include "../utils/list.h"
-#include "sdmmc.h"
-
-typedef struct _gpt_entry_t
-{
-	u8 type_guid[0x10];
-	u8 part_guid[0x10];
-	u64 lba_start;
-	u64 lba_end;
-	u64 attrs;
-	u16 name[36];
-} gpt_entry_t;
-
-typedef struct _gpt_header_t
-{
-	u64 signature;
-	u32 revision;
-	u32 size;
-	u32 crc32;
-	u32 res1;
-	u64 my_lba;
-	u64 alt_lba;
-	u64 first_use_lba;
-	u64 last_use_lba;
-	u8 disk_guid[0x10];
-	u64 part_ent_lba;
-	u32 num_part_ents;
-	u32 part_ent_size;
-	u32 part_ents_crc32;
-	u8 res2[420];
-} gpt_header_t;
 
 #define NX_GPT_FIRST_LBA 1
 #define NX_GPT_NUM_BLOCKS 33
@@ -56,6 +27,7 @@ typedef struct _gpt_header_t
 
 typedef struct _emmc_part_t
 {
+	u32 index;
 	u32 lba_start;
 	u32 lba_end;
 	u64 attrs;

source/storage/nx_sd.c

@@ -0,0 +1,184 @@
+/*
+ * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2019 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/>.
+ */
+
+#include "nx_sd.h"
+#include "sdmmc.h"
+#include "sdmmc_driver.h"
+#include "../gfx/gfx.h"
+#include "../libs/fatfs/ff.h"
+#include "../mem/heap.h"
+
+static bool sd_mounted = false;
+static u32  sd_mode = SD_UHS_SDR82;
+
+
+sdmmc_t sd_sdmmc;
+sdmmc_storage_t sd_storage;
+FATFS sd_fs;
+
+u32 sd_get_mode()
+{
+	return sd_mode;
+}
+
+int sd_init_retry(bool power_cycle)
+{
+	u32 bus_width = SDMMC_BUS_WIDTH_4;
+	u32 type = SDHCI_TIMING_UHS_SDR82;
+
+	// Power cycle SD card.
+	if (power_cycle)
+	{
+		sd_mode--;
+		sdmmc_storage_end(&sd_storage);
+	}
+
+	// Get init parameters.
+	switch (sd_mode)
+	{
+	case SD_INIT_FAIL: // Reset to max.
+		return 0;
+	case SD_1BIT_HS25:
+		bus_width = SDMMC_BUS_WIDTH_1;
+		type = SDHCI_TIMING_SD_HS25;
+		break;
+	case SD_4BIT_HS25:
+		type = SDHCI_TIMING_SD_HS25;
+		break;
+	case SD_UHS_SDR82:
+		type = SDHCI_TIMING_UHS_SDR82;
+		break;
+	default:
+		sd_mode = SD_UHS_SDR82;
+	}
+
+	return sdmmc_storage_init_sd(&sd_storage, &sd_sdmmc, bus_width, type);
+}
+
+bool sd_initialize(bool power_cycle)
+{
+	if (power_cycle)
+		sdmmc_storage_end(&sd_storage);
+
+	int res = !sd_init_retry(false);
+
+	while (true)
+	{
+		if (!res)
+			return true;
+		else if (!sdmmc_get_sd_inserted()) // SD Card is not inserted.
+		{
+			sd_mode = SD_UHS_SDR82;
+			break;
+		}
+		else if (sd_mode == SD_INIT_FAIL)
+			break;
+		else
+			res = !sd_init_retry(true);
+	}
+
+	sdmmc_storage_end(&sd_storage);
+
+	return false;
+}
+
+bool sd_mount()
+{
+	if (sd_mounted)
+		return true;
+
+	int res = !sd_initialize(false);
+
+	if (res)
+	{
+		gfx_con.mute = false;
+		EPRINTF("Failed to init SD card.");
+		if (!sdmmc_get_sd_inserted())
+			EPRINTF("Make sure that it is inserted.");
+		else
+			EPRINTF("SD Card Reader is not properly seated!");
+	}
+	else
+	{
+		res = f_mount(&sd_fs, "", 1);
+		if (res == FR_OK)
+		{
+			sd_mounted = true;
+			return true;
+		}
+		else
+		{
+			gfx_con.mute = false;
+			EPRINTFARGS("Failed to mount SD card (FatFS Error %d).\nMake sure that a FAT partition exists..", res);
+		}
+	}
+
+	return false;
+}
+
+void sd_unmount()
+{
+	sd_mode = SD_UHS_SDR82;
+	if (sd_mounted)
+	{
+		f_mount(NULL, "", 1);
+		sdmmc_storage_end(&sd_storage);
+		sd_mounted = false;
+	}
+}
+
+void *sd_file_read(const char *path, u32 *fsize)
+{
+	FIL fp;
+	if (f_open(&fp, path, FA_READ) != FR_OK)
+		return NULL;
+
+	u32 size = f_size(&fp);
+	if (fsize)
+		*fsize = size;
+
+	void *buf = malloc(size);
+
+	if (f_read(&fp, buf, size, NULL) != FR_OK)
+	{
+		free(buf);
+		f_close(&fp);
+
+		return NULL;
+	}
+
+	f_close(&fp);
+
+	return buf;
+}
+
+int sd_save_to_file(void *buf, u32 size, const char *filename)
+{
+	FIL fp;
+	u32 res = 0;
+	res = f_open(&fp, filename, FA_CREATE_ALWAYS | FA_WRITE);
+	if (res)
+	{
+		EPRINTFARGS("Error (%d) creating file\n%s.\n", res, filename);
+		return res;
+	}
+
+	f_write(&fp, buf, size, NULL);
+	f_close(&fp);
+
+	return 0;
+}

source/storage/nx_sd.h

@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2018 naehrwert
+ * Copyright (c) 2018-2019 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 NX_SD_H
+#define NX_SD_H
+
+#include "sdmmc.h"
+#include "sdmmc_driver.h"
+#include "../libs/fatfs/ff.h"
+
+enum
+{
+	SD_INIT_FAIL  = 0,
+	SD_1BIT_HS25  = 1,
+	SD_4BIT_HS25  = 2,
+	SD_UHS_SDR82  = 3,
+};
+
+extern sdmmc_t sd_sdmmc;
+extern sdmmc_storage_t sd_storage;
+extern FATFS sd_fs;
+
+u32  sd_get_mode();
+int  sd_init_retry(bool power_cycle);
+bool sd_initialize(bool power_cycle);
+bool sd_mount();
+void sd_unmount();
+void *sd_file_read(const char *path, u32 *fsize);
+int  sd_save_to_file(void *buf, u32 size, const char *filename);
+
+#endif

source/storage/sd.h

@@ -1,8 +1,8 @@
 /*
  *  include/linux/mmc/sd.h
  *
- *  Copyright (C) 2005-2007 Pierre Ossman, All Rights Reserved.
- *  Copyright (C) 2018 CTCaer
+ *  Copyright (c) 2005-2007 Pierre Ossman, All Rights Reserved.
+ *  Copyright (c) 2018 CTCaer
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -40,7 +40,9 @@
 #define SD_ROCR_S18A        SD_OCR_S18R  /* 1.8V switching accepted by card */
 #define SD_OCR_XPC          (1 << 28)    /* SDXC power control */
 #define SD_OCR_CCS          (1 << 30)    /* Card Capacity Status */
+#define SD_OCR_VDD_27_34    (0x7F << 15) /* VDD voltage 2.7 ~ 3.4 */
 #define SD_OCR_VDD_32_33    (1 << 20)	 /* VDD voltage 3.2 ~ 3.3 */
+#define SD_OCR_VDD_18       (1 << 7)	 /* VDD voltage 1.8 */
 
 /*
 * SD_SWITCH argument format:
@@ -104,6 +106,11 @@
 #define SD_SET_CURRENT_LIMIT_600	2
 #define SD_SET_CURRENT_LIMIT_800	3
 
+#define SD_MAX_CURRENT_200 (1 << SD_SET_CURRENT_LIMIT_200)
+#define SD_MAX_CURRENT_400 (1 << SD_SET_CURRENT_LIMIT_400)
+#define SD_MAX_CURRENT_600 (1 << SD_SET_CURRENT_LIMIT_600)
+#define SD_MAX_CURRENT_800 (1 << SD_SET_CURRENT_LIMIT_800)
+
 /*
 * SD_SWITCH mode
 */

source/storage/sdmmc.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (C) 2018 CTCaer
+ * Copyright (c) 2018-2019 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,
@@ -18,7 +18,9 @@
 #include <string.h>
 #include "sdmmc.h"
 #include "mmc.h"
+#include "nx_sd.h"
 #include "sd.h"
+#include "../../common/memory_map.h"
 #include "../gfx/gfx.h"
 #include "../mem/heap.h"
 #include "../utils/util.h"
@@ -26,8 +28,6 @@
 //#define DPRINTF(...) gfx_printf(__VA_ARGS__)
 #define DPRINTF(...)
 
-extern boot_cfg_t b_cfg;
-
 static inline u32 unstuff_bits(u32 *resp, u32 start, u32 size)
 {
 	const u32 mask = (size < 32 ? 1 << size : 0) - 1;
@@ -71,6 +71,7 @@ static int _sdmmc_storage_execute_cmd_type1_ex(sdmmc_storage_t *storage, u32 *re
 	if (_sdmmc_storage_check_result(*resp))
 		if (expected_state == 0x10 || R1_CURRENT_STATE(*resp) == expected_state)
 			return 1;
+
 	return 0;
 }
 
@@ -84,6 +85,7 @@ static int _sdmmc_storage_go_idle_state(sdmmc_storage_t *storage)
 {
 	sdmmc_cmd_t cmd;
 	sdmmc_init_cmd(&cmd, MMC_GO_IDLE_STATE, 0, SDMMC_RSP_TYPE_0, 0);
+
 	return sdmmc_execute_cmd(storage->sdmmc, &cmd, 0, 0);
 }
 
@@ -93,7 +95,9 @@ static int _sdmmc_storage_get_cid(sdmmc_storage_t *storage, void *buf)
 	sdmmc_init_cmd(&cmd, MMC_ALL_SEND_CID, 0, SDMMC_RSP_TYPE_2, 0);
 	if (!sdmmc_execute_cmd(storage->sdmmc, &cmd, 0, 0))
 		return 0;
+
 	sdmmc_get_rsp(storage->sdmmc, buf, 0x10, SDMMC_RSP_TYPE_2);
+
 	return 1;
 }
 
@@ -108,7 +112,9 @@ static int _sdmmc_storage_get_csd(sdmmc_storage_t *storage, void *buf)
 	sdmmc_init_cmd(&cmdbuf, MMC_SEND_CSD, storage->rca << 16, SDMMC_RSP_TYPE_2, 0);
 	if (!sdmmc_execute_cmd(storage->sdmmc, &cmdbuf, 0, 0))
 		return 0;
+
 	sdmmc_get_rsp(storage->sdmmc, buf, 0x10, SDMMC_RSP_TYPE_2);
+
 	return 1;
 }
 
@@ -130,10 +136,12 @@ static int _sdmmc_storage_check_status(sdmmc_storage_t *storage)
 
 static int _sdmmc_storage_readwrite_ex(sdmmc_storage_t *storage, u32 *blkcnt_out, u32 sector, u32 num_sectors, void *buf, u32 is_write)
 {
+	u32 tmp = 0;
 	sdmmc_cmd_t cmdbuf;
+	sdmmc_req_t reqbuf;
+
 	sdmmc_init_cmd(&cmdbuf, is_write ? MMC_WRITE_MULTIPLE_BLOCK : MMC_READ_MULTIPLE_BLOCK, sector, SDMMC_RSP_TYPE_1, 0);
 
-	sdmmc_req_t reqbuf;
 	reqbuf.buf = buf;
 	reqbuf.num_sectors = num_sectors;
 	reqbuf.blksize = 512;
@@ -143,11 +151,12 @@ static int _sdmmc_storage_readwrite_ex(sdmmc_storage_t *storage, u32 *blkcnt_out
 
 	if (!sdmmc_execute_cmd(storage->sdmmc, &cmdbuf, &reqbuf, blkcnt_out))
 	{
-		u32 tmp = 0;
 		sdmmc_stop_transmission(storage->sdmmc, &tmp);
 		_sdmmc_storage_get_status(storage, &tmp, 0);
+
 		return 0;
 	}
+
 	return 1;
 }
 
@@ -155,47 +164,90 @@ int sdmmc_storage_end(sdmmc_storage_t *storage)
 {
 	if (!_sdmmc_storage_go_idle_state(storage))
 		return 0;
+
 	sdmmc_end(storage->sdmmc);
+
 	return 1;
 }
 
 static int _sdmmc_storage_readwrite(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf, u32 is_write)
 {
 	u8 *bbuf = (u8 *)buf;
-
+	bool first_reinit = false;
 	while (num_sectors)
 	{
 		u32 blkcnt = 0;
-		//Retry 9 times on error.
-		u32 retries = 10;
+		// Retry 5 times if failed.
+		u32 retries = 5;
 		do
 		{
+reinit_try:
 			if (_sdmmc_storage_readwrite_ex(storage, &blkcnt, sector, MIN(num_sectors, 0xFFFF), bbuf, is_write))
 				goto out;
 			else
 				retries--;
 
-			msleep(100);
+			msleep(50);
 		} while (retries);
+
+		// Disk IO failure! Reinit SD Card to a lower speed.
+		if (storage->sdmmc->id == SDMMC_1)
+		{
+			int res;
+			if (!first_reinit)
+				res = sd_initialize(true);
+			else
+				res = sd_init_retry(true);
+
+			retries = 3;
+			first_reinit = true;
+
+			if (res)
+				goto reinit_try;
+		}
+
 		return 0;
 
-out:;
-		DPRINTF("readwrite: %08X\n", blkcnt);
+out:
+DPRINTF("readwrite: %08X\n", blkcnt);
 		sector += blkcnt;
 		num_sectors -= blkcnt;
 		bbuf += 512 * blkcnt;
 	}
+
 	return 1;
 }
 
 int sdmmc_storage_read(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf)
 {
-	return _sdmmc_storage_readwrite(storage, sector, num_sectors, buf, 0);
+	// Ensure that buffer resides in DRAM and it's DMA aligned.
+	if (((u32)buf >= DRAM_START) && !((u32)buf % 8))
+		return _sdmmc_storage_readwrite(storage, sector, num_sectors, buf, 0);
+
+	if (num_sectors > (SDMMC_UP_BUF_SZ / 512))
+		return 0;
+
+	u8 *tmp_buf = (u8 *)SDMMC_UPPER_BUFFER;
+	if (_sdmmc_storage_readwrite(storage, sector, num_sectors, tmp_buf, 0))
+	{
+		memcpy(buf, tmp_buf, 512 * num_sectors);
+		return 1;
+	}
+	return 0;
 }
 
 int sdmmc_storage_write(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf)
 {
-	return _sdmmc_storage_readwrite(storage, sector, num_sectors, buf, 1);
+	// Ensure that buffer resides in DRAM and it's DMA aligned.
+	if (((u32)buf >= DRAM_START) && !((u32)buf % 8))
+		return _sdmmc_storage_readwrite(storage, sector, num_sectors, buf, 1);
+
+	if (num_sectors > (SDMMC_UP_BUF_SZ / 512))
+		return 0;
+
+	u8 *tmp_buf = (u8 *)SDMMC_UPPER_BUFFER;
+	memcpy(tmp_buf, buf, 512 * num_sectors);
+	return _sdmmc_storage_readwrite(storage, sector, num_sectors, tmp_buf, 1);
 }
 
 /*
@@ -210,10 +262,10 @@ static int _mmc_storage_get_op_cond_inner(sdmmc_storage_t *storage, u32 *pout, u
 	switch (power)
 	{
 	case SDMMC_POWER_1_8:
-		arg = 0x40000080; //Sector access, voltage.
+		arg = SD_OCR_CCS | SD_OCR_VDD_18;
 		break;
 	case SDMMC_POWER_3_3:
-		arg = 0x403F8000; //Sector access, voltage.
+		arg = SD_OCR_CCS | SD_OCR_VDD_27_34;
 		break;
 	default:
 		return 0;
@@ -235,14 +287,17 @@ static int _mmc_storage_get_op_cond(sdmmc_storage_t *storage, u32 power)
 		u32 cond = 0;
 		if (!_mmc_storage_get_op_cond_inner(storage, &cond, power))
 			break;
+
 		if (cond & MMC_CARD_BUSY)
 		{
-			if (cond & 0x40000000)
+			if (cond & SD_OCR_CCS)
 				storage->has_sector_access = 1;
+
 			return 1;
 		}
 		if (get_tmr_ms() > timeout)
 			break;
+
 		usleep(1000);
 	}
 
@@ -372,6 +427,7 @@ static int _mmc_storage_switch_buswidth(sdmmc_storage_t *storage, u32 bus_width)
 		if (_sdmmc_storage_check_status(storage))
 		{
 			sdmmc_set_bus_width(storage->sdmmc, bus_width);
+
 			return 1;
 		}
 
@@ -382,14 +438,19 @@ static int _mmc_storage_enable_HS(sdmmc_storage_t *storage, int check)
 {
 	if (!_mmc_storage_switch(storage, SDMMC_SWITCH(MMC_SWITCH_MODE_WRITE_BYTE, EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS)))
 		return 0;
+
 	if (check && !_sdmmc_storage_check_status(storage))
 		return 0;
-	if (!sdmmc_setup_clock(storage->sdmmc, 2))
+
+	if (!sdmmc_setup_clock(storage->sdmmc, SDHCI_TIMING_MMC_HS52))
 		return 0;
-	DPRINTF("[MMC] switched to HS\n");
+
+DPRINTF("[MMC] switched to HS\n");
 	storage->csd.busspeed = 52;
+
 	if (check || _sdmmc_storage_check_status(storage))
 		return 1;
+
 	return 0;
 }
 
@@ -397,12 +458,16 @@ static int _mmc_storage_enable_HS200(sdmmc_storage_t *storage)
 {
 	if (!_mmc_storage_switch(storage, SDMMC_SWITCH(MMC_SWITCH_MODE_WRITE_BYTE, EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS200)))
 		return 0;
-	if (!sdmmc_setup_clock(storage->sdmmc, 3))
+
+	if (!sdmmc_setup_clock(storage->sdmmc, SDHCI_TIMING_MMC_HS200))
 		return 0;
-	if (!sdmmc_config_tuning(storage->sdmmc, 3, MMC_SEND_TUNING_BLOCK_HS200))
+
+	if (!sdmmc_tuning_execute(storage->sdmmc, SDHCI_TIMING_MMC_HS200, MMC_SEND_TUNING_BLOCK_HS200))
 		return 0;
-	DPRINTF("[MMC] switched to HS200\n");
+
+DPRINTF("[MMC] switched to HS200\n");
 	storage->csd.busspeed = 200;
+
 	return _sdmmc_storage_check_status(storage);
 }
 
@@ -410,41 +475,46 @@ static int _mmc_storage_enable_HS400(sdmmc_storage_t *storage)
 {
 	if (!_mmc_storage_enable_HS200(storage))
 		return 0;
-	sdmmc_get_venclkctl(storage->sdmmc);
+
+	sdmmc_set_tap_value(storage->sdmmc);
+
 	if (!_mmc_storage_enable_HS(storage, 0))
 		return 0;
+
 	if (!_mmc_storage_switch(storage, SDMMC_SWITCH(MMC_SWITCH_MODE_WRITE_BYTE, EXT_CSD_BUS_WIDTH, EXT_CSD_DDR_BUS_WIDTH_8)))
 		return 0;
+
 	if (!_mmc_storage_switch(storage, SDMMC_SWITCH(MMC_SWITCH_MODE_WRITE_BYTE, EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS400)))
 		return 0;
-	if (!sdmmc_setup_clock(storage->sdmmc, 4))
+
+	if (!sdmmc_setup_clock(storage->sdmmc, SDHCI_TIMING_MMC_HS400))
 		return 0;
-	DPRINTF("[MMC] switched to HS400\n");
+
+DPRINTF("[MMC] switched to HS400\n");
 	storage->csd.busspeed = 400;
+
 	return _sdmmc_storage_check_status(storage);
 }
 
 static int _mmc_storage_enable_highspeed(sdmmc_storage_t *storage, u32 card_type, u32 type)
 {
-	//TODO: this should be a config item.
-	// --v
-	if (!1 || sdmmc_get_voltage(storage->sdmmc) != SDMMC_POWER_1_8)
+	if (sdmmc_get_io_power(storage->sdmmc) != SDMMC_POWER_1_8)
 		goto out;
 
 	if (sdmmc_get_bus_width(storage->sdmmc) == SDMMC_BUS_WIDTH_8 &&
-		card_type & EXT_CSD_CARD_TYPE_HS400_1_8V &&
-		type == 4)
+		card_type & EXT_CSD_CARD_TYPE_HS400_1_8V && type == SDHCI_TIMING_MMC_HS400)
 		return _mmc_storage_enable_HS400(storage);
 
 	if (sdmmc_get_bus_width(storage->sdmmc) == SDMMC_BUS_WIDTH_8 ||
 		(sdmmc_get_bus_width(storage->sdmmc) == SDMMC_BUS_WIDTH_4
 		&& card_type & EXT_CSD_CARD_TYPE_HS200_1_8V
-		&& (type == 4 || type == 3)))
+		&& (type == SDHCI_TIMING_MMC_HS400 || type == SDHCI_TIMING_MMC_HS200)))
 		return _mmc_storage_enable_HS200(storage);
 
-out:;
+out:
 	if (card_type & EXT_CSD_CARD_TYPE_HS_52)
 		return _mmc_storage_enable_HS(storage, 1);
+
 	return 1;
 }
 
@@ -452,53 +522,54 @@ static int _mmc_storage_enable_bkops(sdmmc_storage_t *storage)
 {
 	if (!_mmc_storage_switch(storage, SDMMC_SWITCH(MMC_SWITCH_MODE_SET_BITS, EXT_CSD_BKOPS_EN, EXT_CSD_BKOPS_LEVEL_2)))
 		return 0;
+
 	return _sdmmc_storage_check_status(storage);
 }
 
-int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32 bus_width, u32 type)
+int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 bus_width, u32 type)
 {
 	memset(storage, 0, sizeof(sdmmc_storage_t));
 	storage->sdmmc = sdmmc;
 	storage->rca = 2; //TODO: this could be a config item.
 
-	if (!sdmmc_init(sdmmc, id, SDMMC_POWER_1_8, SDMMC_BUS_WIDTH_1, 0, 0))
+	if (!sdmmc_init(sdmmc, SDMMC_4, SDMMC_POWER_1_8, SDMMC_BUS_WIDTH_1, SDHCI_TIMING_MMC_ID, SDMMC_AUTO_CAL_DISABLE))
 		return 0;
-	DPRINTF("[MMC] after init\n");
+DPRINTF("[MMC] after init\n");
 
 	usleep(1000 + (74000 + sdmmc->divisor - 1) / sdmmc->divisor);
 
 	if (!_sdmmc_storage_go_idle_state(storage))
 		return 0;
-	DPRINTF("[MMC] went to idle state\n");
+DPRINTF("[MMC] went to idle state\n");
 
 	if (!_mmc_storage_get_op_cond(storage, SDMMC_POWER_1_8))
 		return 0;
-	DPRINTF("[MMC] got op cond\n");
+DPRINTF("[MMC] got op cond\n");
 
 	if (!_sdmmc_storage_get_cid(storage, storage->raw_cid))
 		return 0;
-	DPRINTF("[MMC] got cid\n");
+DPRINTF("[MMC] got cid\n");
 
 	if (!_mmc_storage_set_relative_addr(storage))
 		return 0;
-	DPRINTF("[MMC] set relative addr\n");
+DPRINTF("[MMC] set relative addr\n");
 
 	if (!_sdmmc_storage_get_csd(storage, storage->raw_csd))
 		return 0;
-	DPRINTF("[MMC] got csd\n");
+DPRINTF("[MMC] got csd\n");
 	_mmc_storage_parse_csd(storage);
 
-	if (!sdmmc_setup_clock(storage->sdmmc, 1))
+	if (!sdmmc_setup_clock(storage->sdmmc, SDHCI_TIMING_MMC_LS26))
 		return 0;
-	DPRINTF("[MMC] after setup clock\n");
+DPRINTF("[MMC] after setup clock\n");
 
 	if (!_sdmmc_storage_select_card(storage))
 		return 0;
-	DPRINTF("[MMC] card selected\n");
+DPRINTF("[MMC] card selected\n");
 
 	if (!_sdmmc_storage_set_blocklen(storage, 512))
 		return 0;
-	DPRINTF("[MMC] set blocklen to 512\n");
+DPRINTF("[MMC] set blocklen to 512\n");
 
 	u32 *csd = (u32 *)storage->raw_csd;
 	//Check system specification version, only version 4.0 and later support below features.
@@ -510,37 +581,29 @@ int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 
 	if (!_mmc_storage_switch_buswidth(storage, bus_width))
 		return 0;
-	DPRINTF("[MMC] switched buswidth\n");
+DPRINTF("[MMC] switched buswidth\n");
 
-	u8 *ext_csd = (u8 *)malloc(512);
-	if (!_mmc_storage_get_ext_csd(storage, ext_csd))
-	{
-		free(ext_csd);
+	if (!_mmc_storage_get_ext_csd(storage, (u8 *)SDMMC_UPPER_BUFFER))
 		return 0;
-	}
-	free(ext_csd);
-	DPRINTF("[MMC] got ext_csd\n");
+DPRINTF("[MMC] got ext_csd\n");
+
 	_mmc_storage_parse_cid(storage); //This needs to be after csd and ext_csd
 	//gfx_hexdump(0, ext_csd, 512);
 
 	/* When auto BKOPS is enabled the mmc device should be powered all the time until we disable this and check status.
 	   Disable it for now until BKOPS disable added to power down sequence at sdmmc_storage_end().
 	   Additionally this works only when we put the device in idle mode which we don't after enabling it. */
-	if (storage->ext_csd.bkops & 0x1 && !(storage->ext_csd.bkops_en & EXT_CSD_BKOPS_LEVEL_2) && 0)
+	if (0 && storage->ext_csd.bkops & 0x1 && !(storage->ext_csd.bkops_en & EXT_CSD_BKOPS_LEVEL_2))
 	{
 		_mmc_storage_enable_bkops(storage);
-		DPRINTF("[MMC] BKOPS enabled\n");
-	}
-	else
-	{
-		DPRINTF("[MMC] BKOPS disabled\n");
+DPRINTF("[MMC] BKOPS enabled\n");
 	}
 
 	if (!_mmc_storage_enable_highspeed(storage, storage->ext_csd.card_type, type))
 		return 0;
-	DPRINTF("[MMC] succesfully switched to highspeed mode\n");
+DPRINTF("[MMC] succesfully switched to HS mode\n");
 
-	sdmmc_sd_clock_ctrl(storage->sdmmc, 1);
+	sdmmc_card_clock_ctrl(storage->sdmmc, SDMMC_AUTO_CAL_ENABLE);
 
 	return 1;
 }
@@ -549,8 +612,10 @@ int sdmmc_storage_set_mmc_partition(sdmmc_storage_t *storage, u32 partition)
 {
 	if (!_mmc_storage_switch(storage, SDMMC_SWITCH(MMC_SWITCH_MODE_WRITE_BYTE, EXT_CSD_PART_CONFIG, partition)))
 		return 0;
+
 	if (!_sdmmc_storage_check_status(storage))
 		return 0;
+
 	storage->partition = partition;
 	return 1;
 }
@@ -564,6 +629,7 @@ static int _sd_storage_execute_app_cmd(sdmmc_storage_t *storage, u32 expected_st
 	u32 tmp;
 	if (!_sdmmc_storage_execute_cmd_type1_ex(storage, &tmp, MMC_APP_CMD, storage->rca << 16, 0, expected_state, mask))
 		return 0;
+
 	return sdmmc_execute_cmd(storage->sdmmc, cmd, req, blkcnt_out);
 }
 
@@ -571,6 +637,7 @@ static int _sd_storage_execute_app_cmd_type1(sdmmc_storage_t *storage, u32 *resp
 {
 	if (!_sdmmc_storage_execute_cmd_type1(storage, MMC_APP_CMD, storage->rca << 16, 0, R1_STATE_TRAN))
 		return 0;
+
 	return _sdmmc_storage_execute_cmd_type1_ex(storage, resp, cmd, arg, check_busy, expected_state, 0);
 }
 
@@ -602,6 +669,7 @@ static int _sd_storage_get_op_cond_once(sdmmc_storage_t *storage, u32 *cond, int
 	sdmmc_init_cmd(&cmdbuf, SD_APP_OP_COND, arg, SDMMC_RSP_TYPE_3, 0);
 	if (!_sd_storage_execute_app_cmd(storage, 0x10, is_version_1 ? 0x400000 : 0, &cmdbuf, 0, 0))
 		return 0;
+
 	return sdmmc_get_rsp(storage->sdmmc, cond, 4, SDMMC_RSP_TYPE_3);
 }
 
@@ -619,6 +687,7 @@ static int _sd_storage_get_op_cond(sdmmc_storage_t *storage, int is_version_1, i
 			if (cond & SD_OCR_CCS)
 				storage->has_sector_access = 1;
 
+			// Check if card supports 1.8V signaling.
 			if (cond & SD_ROCR_S18A && supports_low_voltage)
 			{
 				//The low voltage regulator configuration is valid for SDMMC1 only.
@@ -629,7 +698,7 @@ static int _sd_storage_get_op_cond(sdmmc_storage_t *storage, int is_version_1, i
 						return 0;
 					storage->is_low_voltage = 1;
 
-					DPRINTF("-> switched to low voltage\n");
+DPRINTF("-> switched to low voltage\n");
 				}
 			}
 
@@ -767,34 +836,37 @@ int _sd_storage_switch(sdmmc_storage_t *storage, void *buf, int mode, int group,
 	return _sdmmc_storage_check_result(tmp);
 }
 
-void _sd_storage_set_current_limit(sdmmc_storage_t *storage, u8 *buf)
+void _sd_storage_set_current_limit(sdmmc_storage_t *storage, u16 current_limit, u8 *buf)
 {
-	u32 pwr = SD_SET_CURRENT_LIMIT_800;
+	u32 pwr = SD_SET_CURRENT_LIMIT_200;
+
+	if (current_limit & SD_MAX_CURRENT_800)
+		pwr = SD_SET_CURRENT_LIMIT_800;
+	else if (current_limit & SD_MAX_CURRENT_600)
+		pwr = SD_SET_CURRENT_LIMIT_600;
+	else if (current_limit & SD_MAX_CURRENT_400)
+		pwr = SD_SET_CURRENT_LIMIT_400;
+
 	_sd_storage_switch(storage, buf, SD_SWITCH_SET, 3, pwr);
 
-	while (pwr > 0)
+	if (((buf[15] >> 4) & 0x0F) == pwr)
 	{
-		pwr--;
-		_sd_storage_switch(storage, buf, SD_SWITCH_SET, 3, pwr);
-		if (((buf[15] >> 4) & 0x0F) == pwr)
+		switch (pwr)
+		{
+		case SD_SET_CURRENT_LIMIT_800:
+DPRINTF("[SD] power limit raised to 800mA\n");
 			break;
-	}
-
-	switch (pwr)
-	{
-	case SD_SET_CURRENT_LIMIT_800:
-		DPRINTF("[SD] Power limit raised to 800mA\n");
-		break;
-	case SD_SET_CURRENT_LIMIT_600:
-		DPRINTF("[SD] Power limit raised to 600mA\n");
-		break;
-	case SD_SET_CURRENT_LIMIT_400:
-		DPRINTF("[SD] Power limit raised to 800mA\n");
-		break;
-	default:
-	case SD_SET_CURRENT_LIMIT_200:
-		DPRINTF("[SD] Power limit defaulted to 200mA\n");
-		break;
+		case SD_SET_CURRENT_LIMIT_600:
+DPRINTF("[SD] power limit raised to 600mA\n");
+			break;
+		case SD_SET_CURRENT_LIMIT_400:
+DPRINTF("[SD] power limit raised to 400mA\n");
+			break;
+		default:
+		case SD_SET_CURRENT_LIMIT_200:
+DPRINTF("[SD] power limit defaulted to 200mA\n");
+			break;
+		}
 	}
 }
 
@@ -802,28 +874,33 @@ int _sd_storage_enable_highspeed(sdmmc_storage_t *storage, u32 hs_type, u8 *buf)
 {
 	if (!_sd_storage_switch(storage, buf, SD_SWITCH_CHECK, 0, hs_type))
 		return 0;
+DPRINTF("[SD] supports switch to (U)HS mode\n");
 
 	u32 type_out = buf[16] & 0xF;
 	if (type_out != hs_type)
 		return 0;
+DPRINTF("[SD] supports selected (U)HS mode\n");
 
-	if ((((u16)buf[0] << 8) | buf[1]) < 0x320)
+	u16 total_pwr_consumption = ((u16)buf[0] << 8) | buf[1];
+DPRINTF("[SD] total max current: %d\n", total_pwr_consumption);
+
+	if (total_pwr_consumption <= 800)
 	{
 		if (!_sd_storage_switch(storage, buf, SD_SWITCH_SET, 0, hs_type))
 			return 0;
 
 		if (type_out != (buf[16] & 0xF))
 			return 0;
-	}
 
-	return 1;
+		return 1;
+	}
+DPRINTF("[SD] card max current over limit\n");
+
+	return 0;
 }
 
-int _sd_storage_enable_highspeed_low_volt(sdmmc_storage_t *storage, u32 type, u8 *buf)
+int _sd_storage_enable_uhs_low_volt(sdmmc_storage_t *storage, u32 type, u8 *buf)
 {
-	// Try to raise the current limit to let the card perform better.
-	_sd_storage_set_current_limit(storage, buf);
-
 	if (sdmmc_get_bus_width(storage->sdmmc) != SDMMC_BUS_WIDTH_4)
 		return 0;
 
@@ -831,34 +908,57 @@ int _sd_storage_enable_highspeed_low_volt(sdmmc_storage_t *storage, u32 type, u8
 		return 0;
 	//gfx_hexdump(0, (u8 *)buf, 64);
 
+	u8  access_mode = buf[13];
+	u16 current_limit = buf[7] | buf[6] << 8;
+
+	// Try to raise the current limit to let the card perform better.
+	_sd_storage_set_current_limit(storage, current_limit, buf);
+
 	u32 hs_type = 0;
 	switch (type)
 	{
-	case 11:
+	case SDHCI_TIMING_UHS_SDR104:
+	case SDHCI_TIMING_UHS_SDR82:
 		// Fall through if not supported.
-		if (buf[13] & SD_MODE_UHS_SDR104)
+		if (access_mode & SD_MODE_UHS_SDR104)
 		{
-			type = 11;
 			hs_type = UHS_SDR104_BUS_SPEED;
-			DPRINTF("[SD] Bus speed set to SDR104\n");
-			storage->csd.busspeed = 104;
+DPRINTF("[SD] bus speed set to SDR104\n");
+			switch (type)
+			{
+			case SDHCI_TIMING_UHS_SDR104:
+				storage->csd.busspeed = 104;
+				break;
+			case SDHCI_TIMING_UHS_SDR82:
+				storage->csd.busspeed = 82;
+				break;
+			}
 			break;
 		}
-	case 10:
-		if (buf[13] & SD_MODE_UHS_SDR50)
+	case SDHCI_TIMING_UHS_SDR50:
+		if (access_mode & SD_MODE_UHS_SDR50)
 		{
-			type = 10;
+			type = SDHCI_TIMING_UHS_SDR50;
 			hs_type = UHS_SDR50_BUS_SPEED;
-			DPRINTF("[SD] Bus speed set to SDR50\n");
+DPRINTF("[SD] bus speed set to SDR50\n");
 			storage->csd.busspeed = 50;
 			break;
 		}
-	case 8:
-		if (!(buf[13] & SD_MODE_UHS_SDR12))
+	case SDHCI_TIMING_UHS_SDR25:
+		if (access_mode & SD_MODE_UHS_SDR25)
+		{
+			type = SDHCI_TIMING_UHS_SDR25;
+			hs_type = UHS_SDR50_BUS_SPEED;
+DPRINTF("[SD] bus speed set to SDR25\n");
+			storage->csd.busspeed = 25;
+			break;
+		}
+	case SDHCI_TIMING_UHS_SDR12:
+		if (!(access_mode & SD_MODE_UHS_SDR12))
 			return 0;
-		type = 8;
+		type = SDHCI_TIMING_UHS_SDR12;
 		hs_type = UHS_SDR12_BUS_SPEED;
-		DPRINTF("[SD] Bus speed set to SDR12\n");
+DPRINTF("[SD] bus speed set to SDR12\n");
 		storage->csd.busspeed = 12;
 		break;
 	default:
@@ -868,26 +968,38 @@ int _sd_storage_enable_highspeed_low_volt(sdmmc_storage_t *storage, u32 type, u8
 
 	if (!_sd_storage_enable_highspeed(storage, hs_type, buf))
 		return 0;
+DPRINTF("[SD] card accepted UHS\n");
 	if (!sdmmc_setup_clock(storage->sdmmc, type))
 		return 0;
-	if (!sdmmc_config_tuning(storage->sdmmc, type, MMC_SEND_TUNING_BLOCK))
+DPRINTF("[SD] setup clock\n");
+	if (!sdmmc_tuning_execute(storage->sdmmc, type, MMC_SEND_TUNING_BLOCK))
 		return 0;
+DPRINTF("[SD] config tuning\n");
 	return _sdmmc_storage_check_status(storage);
 }
 
-int _sd_storage_enable_highspeed_high_volt(sdmmc_storage_t *storage, u8 *buf)
+int _sd_storage_enable_hs_high_volt(sdmmc_storage_t *storage, u8 *buf)
 {
 	if (!_sd_storage_switch_get(storage, buf))
 		return 0;
 	//gfx_hexdump(0, (u8 *)buf, 64);
-	if (!(buf[13] & SD_MODE_HIGH_SPEED))
+
+	u8  access_mode = buf[13];
+	u16 current_limit = buf[7] | buf[6] << 8;
+
+	// Try to raise the current limit to let the card perform better.
+	_sd_storage_set_current_limit(storage, current_limit, buf);
+
+	if (!(access_mode & SD_MODE_HIGH_SPEED))
 		return 1;
 
-	if (!_sd_storage_enable_highspeed(storage, 1, buf))
+	if (!_sd_storage_enable_highspeed(storage, HIGH_SPEED_BUS_SPEED, buf))
 		return 0;
+
 	if (!_sdmmc_storage_check_status(storage))
 		return 0;
-	return sdmmc_setup_clock(storage->sdmmc, 7);
+
+	return sdmmc_setup_clock(storage->sdmmc, SDHCI_TIMING_SD_HS25);
 }
 
 static void _sd_storage_parse_ssr(sdmmc_storage_t *storage)
@@ -949,7 +1061,7 @@ static int _sd_storage_get_ssr(sdmmc_storage_t *storage, u8 *buf)
 
 	if (!(storage->csd.cmdclass & CCC_APP_SPEC))
 	{
-		DPRINTF("[SD] ssr: Card lacks mandatory SD Status function\n");
+DPRINTF("[SD] ssr: Card lacks mandatory SD Status function\n");
 		return 0;
 	}
 
@@ -1011,49 +1123,74 @@ static void _sd_storage_parse_csd(sdmmc_storage_t *storage)
 	}
 }
 
-int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32 bus_width, u32 type)
+static bool _sdmmc_storage_supports_low_voltage(u32 bus_width, u32 type)
+{
+	switch (type)
+	{
+	case SDHCI_TIMING_UHS_SDR12:
+	case SDHCI_TIMING_UHS_SDR25:
+	case SDHCI_TIMING_UHS_SDR50:
+	case SDHCI_TIMING_UHS_SDR104:
+	case SDHCI_TIMING_UHS_SDR82:
+	case SDHCI_TIMING_UHS_DDR50:
+		if (bus_width == SDMMC_BUS_WIDTH_4)
+			return true;
+	default:
+		return false;
+	}
+}
+
+void sdmmc_storage_init_wait_sd()
+{
+	u32 sd_poweroff_time = (u32)get_tmr_ms() - sd_power_cycle_time_start;
+	if (sd_poweroff_time < 100)
+		msleep(100 - sd_poweroff_time);
+}
+
+int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 bus_width, u32 type)
 {
 	int is_version_1 = 0;
+	u8 *buf = (u8 *)SDMMC_UPPER_BUFFER;
 
-	// Some cards (Sandisk U1), do not like a fast power cycle. Wait min 100ms.
-	u32 sd_poweroff_time = (u32)get_tmr_ms() - b_cfg.sd_timeoff;
-	if (id == SDMMC_1 && (sd_poweroff_time < 100))
-		msleep(100 - sd_poweroff_time);
+	// Some cards (SanDisk U1), do not like a fast power cycle. Wait min 100ms.
+	sdmmc_storage_init_wait_sd();
 
 	memset(storage, 0, sizeof(sdmmc_storage_t));
 	storage->sdmmc = sdmmc;
 
-	if (!sdmmc_init(sdmmc, id, SDMMC_POWER_3_3, SDMMC_BUS_WIDTH_1, 5, 0))
+	if (!sdmmc_init(sdmmc, SDMMC_1, SDMMC_POWER_3_3, SDMMC_BUS_WIDTH_1, SDHCI_TIMING_SD_ID, SDMMC_AUTO_CAL_DISABLE))
 		return 0;
-	DPRINTF("[SD] after init\n");
+DPRINTF("[SD] after init\n");
 
 	usleep(1000 + (74000 + sdmmc->divisor - 1) / sdmmc->divisor);
 
 	if (!_sdmmc_storage_go_idle_state(storage))
 		return 0;
-	DPRINTF("[SD] went to idle state\n");
+DPRINTF("[SD] went to idle state\n");
 
 	is_version_1 = _sd_storage_send_if_cond(storage);
 	if (is_version_1 == 2)
 		return 0;
-	DPRINTF("[SD] after send if cond\n");
+DPRINTF("[SD] after send if cond\n");
 
-	if (!_sd_storage_get_op_cond(storage, is_version_1, bus_width == SDMMC_BUS_WIDTH_4 && type == 11))
+	bool supports_low_voltage = _sdmmc_storage_supports_low_voltage(bus_width, type);
+
+	if (!_sd_storage_get_op_cond(storage, is_version_1, supports_low_voltage))
 		return 0;
-	DPRINTF("[SD] got op cond\n");
+DPRINTF("[SD] got op cond\n");
 
 	if (!_sdmmc_storage_get_cid(storage, storage->raw_cid))
 		return 0;
-	DPRINTF("[SD] got cid\n");
+DPRINTF("[SD] got cid\n");
 	_sd_storage_parse_cid(storage);
 
 	if (!_sd_storage_get_rca(storage))
 		return 0;
-	DPRINTF("[SD] got rca (= %04X)\n", storage->rca);
+DPRINTF("[SD] got rca (= %04X)\n", storage->rca);
 
 	if (!_sdmmc_storage_get_csd(storage, storage->raw_csd))
 		return 0;
-	DPRINTF("[SD] got csd\n");
+DPRINTF("[SD] got csd\n");
 
 	//Parse CSD.
 	_sd_storage_parse_csd(storage);
@@ -1066,85 +1203,81 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 		storage->sec_cnt = storage->csd.c_size << 10;
 		break;
 	default:
-		DPRINTF("[SD] Unknown CSD structure %d\n", storage->csd.structure);
+DPRINTF("[SD] unknown CSD structure %d\n", storage->csd.structure);
 		break;
 	}
 
 	if (!storage->is_low_voltage)
 	{
-		if (!sdmmc_setup_clock(storage->sdmmc, 6))
+		if (!sdmmc_setup_clock(storage->sdmmc, SDHCI_TIMING_SD_DS12))
 			return 0;
-		DPRINTF("[SD] after setup clock\n");
+DPRINTF("[SD] after setup clock\n");
 	}
 
 	if (!_sdmmc_storage_select_card(storage))
 		return 0;
-	DPRINTF("[SD] card selected\n");
+DPRINTF("[SD] card selected\n");
 
 	if (!_sdmmc_storage_set_blocklen(storage, 512))
 		return 0;
-	DPRINTF("[SD] set blocklen to 512\n");
+DPRINTF("[SD] set blocklen to 512\n");
 
 	u32 tmp = 0;
 	if (!_sd_storage_execute_app_cmd_type1(storage, &tmp, SD_APP_SET_CLR_CARD_DETECT, 0, 0, R1_STATE_TRAN))
 		return 0;
-	DPRINTF("[SD] cleared card detect\n");
+DPRINTF("[SD] cleared card detect\n");
 
-	u8 *buf = (u8 *)malloc(512);
 	if (!_sd_storage_get_scr(storage, buf))
-	{
-		free(buf);
 		return 0;
-	}
-		
+
 	//gfx_hexdump(0, storage->raw_scr, 8);
-	DPRINTF("[SD] got scr\n");
+DPRINTF("[SD] got scr\n");
 
 	// Check if card supports a wider bus and if it's not SD Version 1.X
 	if (bus_width == SDMMC_BUS_WIDTH_4 && (storage->scr.bus_widths & 4) && (storage->scr.sda_vsn & 0xF))
 	{
 		if (!_sd_storage_execute_app_cmd_type1(storage, &tmp, SD_APP_SET_BUS_WIDTH, SD_BUS_WIDTH_4, 0, R1_STATE_TRAN))
-		{
-			free(buf);
 			return 0;
-		}
+
 		sdmmc_set_bus_width(storage->sdmmc, SDMMC_BUS_WIDTH_4);
-		DPRINTF("[SD] switched to wide bus width\n");
+DPRINTF("[SD] switched to wide bus width\n");
 	}
 	else
 	{
-		DPRINTF("[SD] SD does not support wide bus width\n");
+DPRINTF("[SD] SD does not support wide bus width\n");
 	}
 
 	if (storage->is_low_voltage)
 	{
-		if (!_sd_storage_enable_highspeed_low_volt(storage, type, buf))
-		{
-			free(buf);
+		if (!_sd_storage_enable_uhs_low_volt(storage, type, buf))
 			return 0;
-		}
-		DPRINTF("[SD] enabled highspeed (low voltage)\n");
-	}
-	else if (type != 6 && (storage->scr.sda_vsn & 0xF) != 0)
-	{
-		if (!_sd_storage_enable_highspeed_high_volt(storage, buf))
-		{
-			free(buf);
-			return 0;
-		}
-		DPRINTF("[SD] enabled highspeed (high voltage)\n");
-		storage->csd.busspeed = 25;
-	}
+DPRINTF("[SD] enabled UHS\n");
 
-	sdmmc_sd_clock_ctrl(sdmmc, 1);
+		sdmmc_card_clock_ctrl(sdmmc, SDMMC_AUTO_CAL_ENABLE);
+	}
+	else if (type != SDHCI_TIMING_SD_DS12 && (storage->scr.sda_vsn & 0xF) != 0)
+	{
+		if (!_sd_storage_enable_hs_high_volt(storage, buf))
+			return 0;
+
+DPRINTF("[SD] enabled HS\n");
+		switch (bus_width)
+		{
+		case SDMMC_BUS_WIDTH_4:
+			storage->csd.busspeed = 25;
+			break;
+		case SDMMC_BUS_WIDTH_1:
+			storage->csd.busspeed = 6;
+			break;
+		}
+	}
 
 	// Parse additional card info from sd status.
 	if (_sd_storage_get_ssr(storage, buf))
 	{
-		DPRINTF("[SD] got sd status\n");
+DPRINTF("[SD] got sd status\n");
 	}
 
-	free(buf);
 	return 1;
 }
 
@@ -1184,17 +1317,17 @@ int sdmmc_storage_init_gc(sdmmc_storage_t *storage, sdmmc_t *sdmmc)
 	memset(storage, 0, sizeof(sdmmc_storage_t));
 	storage->sdmmc = sdmmc;
 
-	if (!sdmmc_init(sdmmc, SDMMC_2, SDMMC_POWER_1_8, SDMMC_BUS_WIDTH_8, 14, 0))
+	if (!sdmmc_init(sdmmc, SDMMC_2, SDMMC_POWER_1_8, SDMMC_BUS_WIDTH_8, SDHCI_TIMING_MMC_DDR52, SDMMC_AUTO_CAL_DISABLE))
 		return 0;
-	DPRINTF("[gc] after init\n");
+DPRINTF("[gc] after init\n");
 
 	usleep(1000 + (10000 + sdmmc->divisor - 1) / sdmmc->divisor);
 
-	if (!sdmmc_config_tuning(storage->sdmmc, 14, MMC_SEND_TUNING_BLOCK_HS200))
+	if (!sdmmc_tuning_execute(storage->sdmmc, SDHCI_TIMING_MMC_DDR52, MMC_SEND_TUNING_BLOCK_HS200))
 		return 0;
-	DPRINTF("[gc] after tuning\n");
+DPRINTF("[gc] after tuning\n");
 
-	sdmmc_sd_clock_ctrl(sdmmc, 1);
+	sdmmc_card_clock_ctrl(sdmmc, SDMMC_AUTO_CAL_ENABLE);
 
 	return 1;
 }

source/storage/sdmmc.h

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2018 naehrwert
- * Copyright (C) 2018 CTCaer
+ * 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,
@@ -21,6 +21,18 @@
 #include "../utils/types.h"
 #include "sdmmc_driver.h"
 
+extern u32 sd_power_cycle_time_start;
+
+typedef enum _sdmmc_type
+{
+	MMC_SD   = 0,
+	MMC_EMMC = 1,
+
+	EMMC_GPP   = 0,
+	EMMC_BOOT0 = 1,
+	EMMC_BOOT1 = 2
+} sdmmc_type;
+
 typedef struct _mmc_cid
 {
 	u32 manfid;
@@ -47,7 +59,7 @@ typedef struct _mmc_csd
 	u32 read_blkbits;
 	u32 write_blkbits;
 	u32 capacity;
-	u8  write_protect;  
+	u8  write_protect;
 	u16 busspeed;
 } mmc_csd_t;
 
@@ -105,9 +117,10 @@ typedef struct _sdmmc_storage_t
 int sdmmc_storage_end(sdmmc_storage_t *storage);
 int sdmmc_storage_read(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf);
 int sdmmc_storage_write(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, void *buf);
-int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32 bus_width, u32 type);
+int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 bus_width, u32 type);
 int sdmmc_storage_set_mmc_partition(sdmmc_storage_t *storage, u32 partition);
-int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32 bus_width, u32 type);
+void sdmmc_storage_init_wait_sd();
+int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 bus_width, u32 type);
 int sdmmc_storage_init_gc(sdmmc_storage_t *storage, sdmmc_t *sdmmc);
 
 #endif