Fixed screen brightness problems (tested on 1.0.0 and 2.0.0). Some refactoring.

[INI] Fix memory leaks + other fixes (#38 )
* [INI] Fix memory leaks + other fixes * Add ini_free * check and return if NULL pointer in free() * Add button check with timeout * Fix a bug in menu rendering that tries to parse a string without existing * Change initial cnt value to max (fixes menus with first entry not a selectable entry) * Some & ? clarifications * [INI] Add get selected boot cfg - This allows to free ini as we launch the firmware and have the lowest mem leak * [INI] Rename ini section clone/free functions
2018-06-15 23:38:09 +12:00 · 2018-06-13 11:34:32 +12:00 · 2018-06-12 10:30:40 +12:00 · 2018-06-11 18:28:04 +03:00 · 2018-06-10 00:05:29 +12:00 · 2018-06-08 23:07:25 +12:00
45 changed files with 1917 additions and 3246 deletions
--- a/6
+++ b/6
@@ -2,9 +2,7 @@ ifeq ($(strip $(DEVKITARM)),)
 $(error "Please set DEVKITARM in your environment. export DEVKITARM=<path to>devkitARM")
 endif

-CC = $(DEVKITARM)/bin/arm-none-eabi-gcc
-LD = $(DEVKITARM)/bin/arm-none-eabi-ld
-OBJCOPY = $(DEVKITARM)/bin/arm-none-eabi-objcopy
+include $(DEVKITARM)/base_rules

 TARGET := ipl
 BUILD := build_ipl
@@ -44,7 +42,7 @@ OBJS = $(addprefix $(BUILD)/, \
 OBJS += $(addprefix $(BUILD)/, diskio.o ff.o ffunicode.o)

 ARCH := -march=armv4t -mtune=arm7tdmi -mthumb -mthumb-interwork
-CFLAGS = $(ARCH) -Os -nostdlib -ffunction-sections -fdata-sections -fomit-frame-pointer -fno-inline -std=gnu11# -Wall
+CFLAGS = $(ARCH) -O2 -nostdlib -ffunction-sections -fdata-sections -fomit-frame-pointer -fno-inline -std=gnu11 -Wall
 LDFLAGS = $(ARCH) -nostartfiles -lgcc -Wl,--nmagic,--gc-sections

 .PHONY: all clean
--- a/README.md
+++ b/README.md
@@ -4,13 +4,49 @@

 Nintendo Switch bootloader, firmware patcher, and more.

+
 ## ipl config

 The ipl can be configured via 'hekate_ipl.ini' (if it is present on the SD card). Each ini section represents a boot entry, except for the special section 'config' that controls the global configuration.

-Possible key/value combinations:
+### Possible key/value combinations:

- - warmboot={SD path}
- - secmon={SD path}
- - kernel={SD path}
- - kip1={SD path}
+| Config option      | Description                                                |
+| ------------------ | ---------------------------------------------------------- |
+| warmboot={SD path} | Replaces the warmboot binary                               |
+| secmon={SD path}   | Replaces the security monitor binary                       |
+| kernel={SD path}   | Replaces the kernel binary                                 |
+| kip1={SD path}     | Replaces/Adds kernel initial process. Multiple can be set. |
+| fullsvcperm=1      | Disables SVC verification                                  |
+| debugmode=1        | Enables Debug mode                                         |
+
+
+
+```
+hekate (C) 2018 naehrwert, st4rk.
+
+Thanks to: derrek, nedwill, plutoo, shuffle2, smea, thexyz, yellows8.
+Greetings to: fincs, hexkyz, SciresM, Shiny Quagsire, WinterMute.
+
+Open source and free packages used:
+ - FatFs R0.13a, Copyright (C) 2017, ChaN
+ - bcl-1.2.0,    Copyright (C) 2003-2006, Marcus Geelnard
+
+                         ___
+                      .-'   `'.
+                     /         \
+                     |         ;
+                     |         |           ___.--,
+            _.._     |0) = (0) |    _.---'`__.-( (_.
+     __.--'`_.. '.__.\    '--. \_.-' ,.--'`     `""`
+    ( ,.--'`   ',__ /./;   ;, '.__.'`    __
+    _`) )  .---.__.' / |   |\   \__..--""  """--.,_
+   `---' .'.''-._.-'`_./  /\ '.  \ _.--''````'''--._`-.__.'
+         | |  .' _.-' |  |  \  \  '.               `----`
+          \ \/ .'     \  \   '. '-._)
+           \/ /        \  \    `=.__`'-.
+           / /\         `) )    / / `"".`\
+     , _.-'.'\ \        / /    ( (     / /
+      `--'`   ) )    .-'.'      '.'.  | (
+             (/`    ( (`          ) )  '-;   [switchbrew]
+```
--- a/ipl/arm64.h
+++ b/ipl/arm64.h
@@ -0,0 +1,35 @@
+/*
+* 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 _ARM64_H_
+#define _ARM64_H_
+
+#include "types.h"
+
+#define LSL0 0
+#define LSL16 16
+#define LSL32 32
+
+#define _PAGEOFF(x) ((x) & 0xFFFFF000)
+
+#define _ADRP(r, o) 0x90000000 | ((((o) >> 12) & 0x3) << 29) | ((((o) >> 12) & 0x1FFFFC) << 3) | ((r) & 0x1F)
+#define _BL(a, o) 0x94000000 | ((((o) - (a)) >> 2) & 0x3FFFFFF)
+#define _B(a, o) 0x94000000 | ((((o) - (a)) >> 2) & 0x3FFFFFF)
+#define _MOVKX(r, i, s) 0xF2800000 | (((s) & 0x30) << 17) | (((i) & 0xFFFF) << 5) | ((r) & 0x1F)
+#define _MOVZX(r, i, s) 0xD2800000 | (((s) & 0x30) << 17) | (((i) & 0xFFFF) << 5) | ((r) & 0x1F)
+#define _NOP() 0xD503201F
+
+#endif
--- a/ipl/btn.c
+++ b/ipl/btn.c
@@ -18,6 +18,7 @@
 #include "i2c.h"
 #include "gpio.h"
 #include "t210.h"
+#include "util.h"

 u32 btn_read()
 {
@@ -36,22 +37,38 @@ u32 btn_wait()
 	u32 res = 0, btn = btn_read();
 	int pwr = 0;

-	// Power button down, raise a filter.
+	//Power button down, raise a filter.
 	if (btn & BTN_POWER)
 	{
 		pwr = 1;
-		btn &= 0xFFFFFFFE;
+		btn &= ~BTN_POWER;
 	}

 	do
 	{
 		res = btn_read();
-		// Power button up, remove filter.
+		//Power button up, remove filter.
 		if (!(res & BTN_POWER) && pwr)
 			pwr = 0;
-		// Power button still down.
-		else if (pwr)
-			res &= 0xFFFFFFFE;
+		else if (pwr) //Power button still down.
+			res &= ~BTN_POWER;
 	} while (btn == res);
+
+	return res;
+}
+
+u32 btn_wait_timeout(u32 time_ms)
+{
+	u32 timeout = get_tmr() + (time_ms * 1000);
+	u32 res = btn_read();
+	u32 btn = res;
+
+	do
+	{
+		//Keep the new value until timeout is reached
+		if (btn == res)
+			res = btn_read();
+	} while (get_tmr() < timeout);
+
 	return res;
 }
--- a/ipl/btn.h
+++ b/ipl/btn.h
@@ -25,5 +25,6 @@

 u32 btn_read();
 u32 btn_wait();
+u32 btn_wait_timeout(u32 time_ms);

 #endif
--- a/ipl/clock.c
+++ b/ipl/clock.c
@@ -51,14 +51,14 @@ static clock_t _clock_coresight = { 0xC, 0x18, 0x1D4, 9, 0, 4};
 void clock_enable(const clock_t *clk)
 {
 	//Put clock into reset.
-	CLOCK(clk->reset) = CLOCK(clk->reset) & ~(1 << clk->index) | (1 << clk->index);
+	CLOCK(clk->reset) = (CLOCK(clk->reset) & ~(1 << clk->index)) | (1 << clk->index);
 	//Disable.
 	CLOCK(clk->enable) &= ~(1 << clk->index);
 	//Configure clock source if required.
 	if (clk->source)
 		CLOCK(clk->source) = clk->clk_div | (clk->clk_src << 29);
 	//Enable.
-	CLOCK(clk->enable) = CLOCK(clk->enable) & ~(1 << clk->index) | (1 << clk->index);
+	CLOCK(clk->enable) = (CLOCK(clk->enable) & ~(1 << clk->index)) | (1 << clk->index);
 	//Take clock off reset.
 	CLOCK(clk->reset) &= ~(1 << clk->index);
 }
@@ -66,14 +66,14 @@ void clock_enable(const clock_t *clk)
 void clock_disable(const clock_t *clk)
 {
 	//Put clock into reset.
-	CLOCK(clk->reset) = CLOCK(clk->reset) & ~(1 << clk->index) | (1 << clk->index);
+	CLOCK(clk->reset) = (CLOCK(clk->reset) & ~(1 << clk->index)) | (1 << clk->index);
 	//Disable.
 	CLOCK(clk->enable) &= ~(1 << clk->index);
 }

 void clock_enable_fuse(u32 enable)
 {
-	CLOCK(CLK_RST_CONTROLLER_MISC_CLK_ENB) = CLOCK(CLK_RST_CONTROLLER_MISC_CLK_ENB) & 0xEFFFFFFF | ((enable & 1) << 28) & 0x10000000;
+	CLOCK(CLK_RST_CONTROLLER_MISC_CLK_ENB) = (CLOCK(CLK_RST_CONTROLLER_MISC_CLK_ENB) & 0xEFFFFFFF) | ((enable & 1) << 28);
 }

 void clock_enable_uart(u32 idx)
@@ -144,9 +144,9 @@ void clock_disable_sor1()
 void clock_enable_kfuse()
 {
 	//clock_enable(&_clock_kfuse);
-	CLOCK(0x8) = CLOCK(0x8) & 0xFFFFFEFF | 0x100;
+	CLOCK(0x8) = (CLOCK(0x8) & 0xFFFFFEFF) | 0x100;
 	CLOCK(0x14) &= 0xFFFFFEFF;
-	CLOCK(0x14) = CLOCK(0x14) & 0xFFFFFEFF | 0x100;
+	CLOCK(0x14) = (CLOCK(0x14) & 0xFFFFFEFF) | 0x100;
 	sleep(10);
 	CLOCK(0x8) &= 0xFFFFFEFF;
 	sleep(20);
@@ -259,7 +259,7 @@ static int _clock_sdmmc_is_enabled(u32 id)
 	return 0;
 }

-static int _clock_sdmmc_set_enable(u32 id)
+static void _clock_sdmmc_set_enable(u32 id)
 {
 	switch (id)
 	{
@@ -274,7 +274,7 @@ static int _clock_sdmmc_set_enable(u32 id)
 	}
 }

-static int _clock_sdmmc_clear_enable(u32 id)
+static void _clock_sdmmc_clear_enable(u32 id)
 {
 	switch (id)
 	{
--- a/ipl/cluster.c
+++ b/ipl/cluster.c
@@ -77,15 +77,15 @@ void cluster_boot_cpu0(u32 entry)
 		sleep(2);
 		CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) = 0x80404E02;
 		CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) = 0x404E02;
-		CLOCK(CLK_RST_CONTROLLER_PLLX_MISC) = CLOCK(CLK_RST_CONTROLLER_PLLX_MISC) & 0xFFFBFFFF | 0x40000;
+		CLOCK(CLK_RST_CONTROLLER_PLLX_MISC) = (CLOCK(CLK_RST_CONTROLLER_PLLX_MISC) & 0xFFFBFFFF) | 0x40000;
 		CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) = 0x40404E02;
 	}
 	while (!(CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) & 0x8000000))
 		;

 	//Configure MSELECT source and enable clock.
-	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_MSELECT) = CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_MSELECT) & 0x1FFFFF00 | 6;
-	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_V) = CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_V) & 0xFFFFFFF7 | 8;
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_MSELECT) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_MSELECT) & 0x1FFFFF00) | 6;
+	CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_V) = (CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_V) & 0xFFFFFFF7) | 8;

 	//Configure initial CPU clock frequency and enable clock.
 	CLOCK(CLK_RST_CONTROLLER_CCLK_BURST_POLICY) = 0x20008888;
@@ -95,7 +95,7 @@ void cluster_boot_cpu0(u32 entry)
 	clock_enable_coresight();

 	//CAR2PMC_CPU_ACK_WIDTH should be set to 0.
-	CLOCK(CLK_RST_CONTROLLER_CPU_SOFTRST_CTRL2) = CLOCK(CLK_RST_CONTROLLER_CPU_SOFTRST_CTRL2) & 0xFFFFF000;
+	CLOCK(CLK_RST_CONTROLLER_CPU_SOFTRST_CTRL2) &= 0xFFFFF000;

 	//Enable CPU rail.
 	_cluster_pmc_enable_partition(1, 0);
--- a/ipl/ctc_logo2.h
+++ b/ipl/ctc_logo2.h
--- a/ipl/di.c
+++ b/ipl/di.c
@@ -14,12 +14,16 @@
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

+#include <string.h>
+
 #include "di.h"
 #include "t210.h"
 #include "util.h"
 #include "i2c.h"
 #include "pmc.h"
 #include "max77620.h"
+#include "gpio.h"
+#include "pinmux.h"

 #include "di.inl"

@@ -54,25 +58,25 @@ void display_init()
 	PMC(APBDEV_PMC_IO_DPD2_REQ) = 0x40000000;

 	//Config pins.
-	PINMUX_AUX(0x1D0) &= 0xFFFFFFEF;
-	PINMUX_AUX(0x1D4) &= 0xFFFFFFEF;
-	PINMUX_AUX(0x1FC) &= 0xFFFFFFEF;
-	PINMUX_AUX(0x200) &= 0xFFFFFFEF;
-	PINMUX_AUX(0x204) &= 0xFFFFFFEF;
+	PINMUX_AUX(PINMUX_AUX_NFC_EN) &= 0xFFFFFFEF;
+	PINMUX_AUX(PINMUX_AUX_NFC_INT) &= 0xFFFFFFEF;
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) &= 0xFFFFFFEF;
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_EN) &= 0xFFFFFFEF;
+	PINMUX_AUX(PINMUX_AUX_LCD_RST) &= 0xFFFFFFEF;

-	GPIO_3(0x00) = GPIO_3(0x00) & 0xFFFFFFFC | 0x3;
-	GPIO_3(0x10) = GPIO_3(0x10) & 0xFFFFFFFC | 0x3;
-	GPIO_3(0x20) = GPIO_3(0x20) & 0xFFFFFFFE | 0x1;
-
-	sleep(10000u);
-
-	GPIO_3(0x20) = GPIO_3(0x20) & 0xFFFFFFFD | 0x2;
+	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.
+	gpio_write(GPIO_PORT_I, GPIO_PIN_0, GPIO_HIGH); //Backlight +5V enable.

 	sleep(10000);

-	GPIO_6(0x04) = GPIO_6(0x04) & 0xFFFFFFF8 | 0x7;
-	GPIO_6(0x14) = GPIO_6(0x14) & 0xFFFFFFF8 | 0x7;
-	GPIO_6(0x24) = GPIO_6(0x24) & 0xFFFFFFFD | 0x2;
+	gpio_write(GPIO_PORT_I, GPIO_PIN_1, GPIO_HIGH); //Backlight -5V enable.
+
+	sleep(10000);
+
+	gpio_config(GPIO_PORT_V, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2, GPIO_MODE_GPIO); //Backlight PWM, Enable, Reset.
+	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.

 	//Config display interface and display.
 	MIPI_CAL(0x60) = 0;
@@ -83,35 +87,35 @@ void display_init()

 	sleep(10000);

-	GPIO_6(0x24) = GPIO_6(0x24) & 0xFFFFFFFB | 0x4;
+	gpio_write(GPIO_PORT_V, GPIO_PIN_2, GPIO_HIGH); //Backlight Reset enable.

 	sleep(60000);

-	DSI(_DSIREG(DSI_DSI_BTA_TIMING)) = 0x50204;
-	DSI(_DSIREG(DSI_DSI_WR_DATA)) = 0x337;
-	DSI(_DSIREG(DSI_DSI_TRIGGER)) = 0x2;
-	_display_dsi_wait(250000, _DSIREG(DSI_DSI_TRIGGER), 3);
+	DSI(_DSIREG(DSI_BTA_TIMING)) = 0x50204;
+	DSI(_DSIREG(DSI_WR_DATA)) = 0x337;
+	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+	_display_dsi_wait(250000, _DSIREG(DSI_TRIGGER), DSI_TRIGGER_HOST | DSI_TRIGGER_VIDEO);

-	DSI(_DSIREG(DSI_DSI_WR_DATA)) = 0x406;
-	DSI(_DSIREG(DSI_DSI_TRIGGER)) = 0x2;
-	_display_dsi_wait(250000, _DSIREG(DSI_DSI_TRIGGER), 3);
+	DSI(_DSIREG(DSI_WR_DATA)) = 0x406;
+	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;
+	_display_dsi_wait(250000, _DSIREG(DSI_TRIGGER), DSI_TRIGGER_HOST | DSI_TRIGGER_VIDEO);

-	DSI(_DSIREG(DSI_HOST_DSI_CONTROL)) = 0x200B;
-	_display_dsi_wait(150000, _DSIREG(DSI_HOST_DSI_CONTROL), 8);
+	DSI(_DSIREG(DSI_HOST_CONTROL)) = DSI_HOST_CONTROL_TX_TRIG_HOST | DSI_HOST_CONTROL_IMM_BTA | DSI_HOST_CONTROL_CS | DSI_HOST_CONTROL_ECC;
+	_display_dsi_wait(150000, _DSIREG(DSI_HOST_CONTROL), DSI_HOST_CONTROL_IMM_BTA);

 	sleep(5000);

-	_display_ver = DSI(_DSIREG(DSI_DSI_RD_DATA));
+	_display_ver = DSI(_DSIREG(DSI_RD_DATA));
 	if (_display_ver == 0x10)
 		exec_cfg((u32 *)DSI_BASE, _display_config_4, 43);

-	DSI(_DSIREG(DSI_DSI_WR_DATA)) = 0x1105;
-	DSI(_DSIREG(DSI_DSI_TRIGGER)) = 0x2;
+	DSI(_DSIREG(DSI_WR_DATA)) = 0x1105;
+	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;

 	sleep(180000);

-	DSI(_DSIREG(DSI_DSI_WR_DATA)) = 0x2905;
-	DSI(_DSIREG(DSI_DSI_TRIGGER)) = 0x2;
+	DSI(_DSIREG(DSI_WR_DATA)) = 0x2905;
+	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;

 	sleep(20000);

@@ -131,18 +135,26 @@ void display_init()
 	exec_cfg((u32 *)DISPLAY_A_BASE, _display_config_11, 113);
 }

+void display_backlight(u8 enable)
+{
+	gpio_write(GPIO_PORT_V, GPIO_PIN_0, enable ? GPIO_HIGH : GPIO_LOW); //Backlight PWM.
+}
+
 void display_end()
 {
-	GPIO_6(0x24) &= 0xFFFFFFFE;
-	DSI(_DSIREG(DSI_DSI_VID_MODE_CONTROL)) = 1;
-	DSI(_DSIREG(DSI_DSI_WR_DATA)) = 0x2805;
+	display_backlight(0);
+
+	//TODO: figure out why this freezes.
+
+	/*DSI(_DSIREG(DSI_VIDEO_MODE_CONTROL)) = 1;
+	DSI(_DSIREG(DSI_WR_DATA)) = 0x2805;

 	u32 end = HOST1X(0x30A4) + 5;
 	while (HOST1X(0x30A4) < end)
 		;

-	DISPLAY_A(_DIREG(DC_CMD_STATE_ACCESS)) = 5;
-	DSI(_DSIREG(DSI_DSI_VID_MODE_CONTROL)) = 0;
+	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);
@@ -152,22 +164,22 @@ void display_end()
 	if (_display_ver == 0x10)
 		exec_cfg((u32 *)DSI_BASE, _display_config_14, 22);

-	DSI(_DSIREG(DSI_DSI_WR_DATA)) = 0x1005;
-	DSI(_DSIREG(DSI_DSI_TRIGGER)) = 2;
+	DSI(_DSIREG(DSI_WR_DATA)) = 0x1005;
+	DSI(_DSIREG(DSI_TRIGGER)) = DSI_TRIGGER_HOST;

 	sleep(50000);

-	GPIO_6(0x24) &= 0xFFFFFFFB;
+	//gpio_write(GPIO_PORT_V, GPIO_PIN_2, GPIO_LOW); //Backlight Reset disable.

-	sleep(10000);
+	//sleep(10000);

-	GPIO_3(0x20) &= 0xFFFFFFFD;
+	//gpio_write(GPIO_PORT_I, GPIO_PIN_1, GPIO_LOW); //Backlight -5V disable.

-	sleep(10000);
+	//sleep(10000);

-	GPIO_3(0x20) = (GPIO_3(0x20) >> 1) << 1;
+	//gpio_write(GPIO_PORT_I, GPIO_PIN_0, GPIO_LOW); //Backlight +5V disable.

-	sleep(10000);
+	//sleep(10000);

 	//Disable clocks.
 	CLOCK(0x308) = 0x1010000;
@@ -175,13 +187,13 @@ void display_end()
 	CLOCK(0x300) = 0x18000000;
 	CLOCK(0x324) = 0x18000000;

-	DSI(_DSIREG(DSI_PAD_CONTROL)) = 0x10F010F;
-	DSI(_DSIREG(DSI_DSI_POWER_CONTROL)) = 0;
+	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;*/

-	GPIO_6(0x04) &= 0xFFFFFFFE;
+	gpio_config(GPIO_PORT_V, GPIO_PIN_0, GPIO_MODE_SPIO); //Backlight PWM.

-	PINMUX_AUX(0x1FC) = PINMUX_AUX(0x1FC) & 0xFFFFFFEF | 0x10;
-	PINMUX_AUX(0x1FC) = (PINMUX_AUX(0x1FC) >> 2) << 2 | 1;
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) = (PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) & 0xFFFFFFEF) | 0x10;
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) = (PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) >> 2) << 2 | 1;
 }

 void display_color_screen(u32 color)
@@ -193,21 +205,24 @@ void display_color_screen(u32 color)
 	DISPLAY_A(_DIREG(DC_WIN_BD_WIN_OPTIONS)) = 0;
 	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 | 1;
+	DISPLAY_A(_DIREG(DC_CMD_STATE_CONTROL)) = (DISPLAY_A(_DIREG(DC_CMD_STATE_CONTROL)) & 0xFFFFFFFE) | GENERAL_ACT_REQ;

 	sleep(35000);

-	GPIO_6(0x24) = GPIO_6(0x24) & 0xFFFFFFFE | 1;
+	display_backlight(1);
 }

 u32 *display_init_framebuffer(u32 *fb)
 {
+	//Sanitize framebuffer area. Aligned to 4MB.
+	memset((u32 *)0xC0000000, 0, 0x400000);
 	//This configures the framebuffer @ 0xC0000000 with a resolution of 1280x720 (line stride 768).
 	exec_cfg((u32 *)DISPLAY_A_BASE, cfg_display_framebuffer, 32);

 	sleep(35000);

-	GPIO_6(0x24) = GPIO_6(0x24) & 0xFFFFFFFE | 1;
+	//Enable backlight
+	//display_backlight(1);

 	return (u32 *)0xC0000000;
 }
--- a/ipl/di.h
+++ b/ipl/di.h
@@ -21,44 +21,319 @@

 /*! Display registers. */
 #define _DIREG(reg) ((reg) * 4)
+
+#define DC_CMD_GENERAL_INCR_SYNCPT 0x00
+
+#define DC_CMD_GENERAL_INCR_SYNCPT_CNTRL 0x01
+#define  SYNCPT_CNTRL_NO_STALL   (1 << 8)
+#define  SYNCPT_CNTRL_SOFT_RESET (1 << 0)
+
+#define DC_CMD_CONT_SYNCPT_VSYNC 0x28
+#define  SYNCPT_VSYNC_ENABLE (1 << 8)
+
+#define DC_CMD_DISPLAY_COMMAND_OPTION0 0x031
+
 #define DC_CMD_DISPLAY_COMMAND 0x32
+#define  DISP_CTRL_MODE_STOP       (0 << 5)
+#define  DISP_CTRL_MODE_C_DISPLAY  (1 << 5)
+#define  DISP_CTRL_MODE_NC_DISPLAY (2 << 5)
+#define  DISP_CTRL_MODE_MASK       (3 << 5)
+
+#define DC_CMD_DISPLAY_POWER_CONTROL 0x36
+#define  PW0_ENABLE (1 <<  0)
+#define  PW1_ENABLE (1 <<  2)
+#define  PW2_ENABLE (1 <<  4)
+#define  PW3_ENABLE (1 <<  6)
+#define  PW4_ENABLE (1 <<  8)
+#define  PM0_ENABLE (1 << 16)
+#define  PM1_ENABLE (1 << 18)
+
+#define DC_CMD_INT_MASK 0x38
+#define DC_CMD_INT_ENABLE 0x39
+
 #define DC_CMD_STATE_ACCESS 0x40
+#define  READ_MUX  (1 << 0)
+#define  WRITE_MUX (1 << 2)
+
 #define DC_CMD_STATE_CONTROL 0x41
+#define  GENERAL_ACT_REQ (1 <<  0)
+#define  WIN_A_ACT_REQ   (1 <<  1)
+#define  WIN_B_ACT_REQ   (1 <<  2)
+#define  WIN_C_ACT_REQ   (1 <<  3)
+#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  CURSOR_UPDATE   (1 << 15)
+#define  NC_HOST_TRIG    (1 << 24)
+
 #define DC_CMD_DISPLAY_WINDOW_HEADER 0x42
+#define  WINDOW_A_SELECT (1 << 4)
+#define  WINDOW_B_SELECT (1 << 5)
+#define  WINDOW_C_SELECT (1 << 6)
+
+#define DC_CMD_REG_ACT_CONTROL 0x043
+
+#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_DISP_DISP_WIN_OPTIONS 0x402
+#define  HDMI_ENABLE     (1 << 30)
+#define  DSI_ENABLE      (1 << 29)
+#define  SOR1_TIMING_CYA (1 << 27)
+#define  SOR1_ENABLE     (1 << 26)
+#define  SOR_ENABLE      (1 << 25)
+#define  CURSOR_ENABLE   (1 << 16)
+
+#define DC_DISP_DISP_MEM_HIGH_PRIORITY 0x403
+#define DC_DISP_DISP_MEM_HIGH_PRIORITY_TIMER 0x404
+#define DC_DISP_DISP_TIMING_OPTIONS 0x405
+#define DC_DISP_REF_TO_SYNC 0x406
+#define DC_DISP_SYNC_WIDTH 0x407
+#define DC_DISP_BACK_PORCH 0x408
+#define DC_DISP_ACTIVE 0x409
+#define DC_DISP_FRONT_PORCH 0x40A
+
 #define DC_DISP_DISP_CLOCK_CONTROL 0x42E
+#define  PIXEL_CLK_DIVIDER_PCD1  (0 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD1H (1 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD2  (2 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD3  (3 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD4  (4 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD6  (5 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD8  (6 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD9  (7 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD12 (8 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD16 (9 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD18 (10 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD24 (11 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD13 (12 << 8)
+#define  SHIFT_CLK_DIVIDER(x)    ((x) & 0xff)
+
+#define DC_DISP_DISP_INTERFACE_CONTROL 0x42F
+#define  DISP_DATA_FORMAT_DF1P1C    (0 << 0)
+#define  DISP_DATA_FORMAT_DF1P2C24B (1 << 0)
+#define  DISP_DATA_FORMAT_DF1P2C18B (2 << 0)
+#define  DISP_DATA_FORMAT_DF1P2C16B (3 << 0)
+#define  DISP_DATA_FORMAT_DF2S      (4 << 0)
+#define  DISP_DATA_FORMAT_DF3S      (5 << 0)
+#define  DISP_DATA_FORMAT_DFSPI     (6 << 0)
+#define  DISP_DATA_FORMAT_DF1P3C24B (7 << 0)
+#define  DISP_DATA_FORMAT_DF1P3C18B (8 << 0)
+#define  DISP_ALIGNMENT_MSB         (0 << 8)
+#define  DISP_ALIGNMENT_LSB         (1 << 8)
+#define  DISP_ORDER_RED_BLUE        (0 << 9)
+#define  DISP_ORDER_BLUE_RED        (1 << 9)
+
+#define DC_DISP_DISP_COLOR_CONTROL 0x430
+#define  DITHER_CONTROL_MASK    (3 << 8)
+#define  DITHER_CONTROL_DISABLE (0 << 8)
+#define  DITHER_CONTROL_ORDERED (2 << 8)
+#define  DITHER_CONTROL_ERRDIFF (3 << 8)
+#define  BASE_COLOR_SIZE_MASK   (0xf << 0)
+#define  BASE_COLOR_SIZE_666    (0 << 0)
+#define  BASE_COLOR_SIZE_111    (1 << 0)
+#define  BASE_COLOR_SIZE_222    (2 << 0)
+#define  BASE_COLOR_SIZE_333    (3 << 0)
+#define  BASE_COLOR_SIZE_444    (4 << 0)
+#define  BASE_COLOR_SIZE_555    (5 << 0)
+#define  BASE_COLOR_SIZE_565    (6 << 0)
+#define  BASE_COLOR_SIZE_332    (7 << 0)
+#define  BASE_COLOR_SIZE_888    (8 << 0)
+
+#define DC_DISP_SHIFT_CLOCK_OPTIONS 0x431
+#define  SC1_H_QUALIFIER_NONE	(1 << 16)
+#define  SC0_H_QUALIFIER_NONE	(1 <<  0)
+
+#define DC_DISP_DATA_ENABLE_OPTIONS 0x432
+#define  DE_SELECT_ACTIVE_BLANK  (0 << 0)
+#define  DE_SELECT_ACTIVE        (1 << 0)
+#define  DE_SELECT_ACTIVE_IS     (2 << 0)
+#define  DE_CONTROL_ONECLK       (0 << 2)
+#define  DE_CONTROL_NORMAL       (1 << 2)
+#define  DE_CONTROL_EARLY_EXT    (2 << 2)
+#define  DE_CONTROL_EARLY        (3 << 2)
+#define  DE_CONTROL_ACTIVE_BLANK (4 << 2)
+
+#define DC_DISP_DC_MCCIF_FIFOCTRL 0x480
 #define DC_DISP_BLEND_BACKGROUND_COLOR 0x4E4
+
+#define DC_WIN_CSC_YOF 0x611
+#define DC_WIN_CSC_KYRGB 0x612
+#define DC_WIN_CSC_KUR 0x613
+#define DC_WIN_CSC_KVR 0x614
+#define DC_WIN_CSC_KUG 0x615
+#define DC_WIN_CSC_KVG 0x616
+#define DC_WIN_CSC_KUB 0x617
+#define DC_WIN_CSC_KVB 0x618
 #define DC_WIN_AD_WIN_OPTIONS 0xB80
 #define DC_WIN_BD_WIN_OPTIONS 0xD80
 #define DC_WIN_CD_WIN_OPTIONS 0xF80

 //The following registers are A/B/C shadows of the 0xB80/0xD80/0xF80 registers (see DISPLAY_WINDOW_HEADER).
-#define DC_X_WIN_XD_WIN_OPTIONS 0x700
-#define DC_X_WIN_XD_COLOR_DEPTH 0x703
-#define DC_X_WIN_XD_POSITION 0x704
-#define DC_X_WIN_XD_SIZE 0x705
-#define DC_X_WIN_XD_PRESCALED_SIZE 0x706
-#define DC_X_WIN_XD_H_INITIAL_DDA 0x707
-#define DC_X_WIN_XD_V_INITIAL_DDA 0x708
-#define DC_X_WIN_XD_DDA_INCREMENT 0x709
-#define DC_X_WIN_XD_LINE_STRIDE 0x70A
+#define DC_WIN_WIN_OPTIONS 0x700
+#define  H_DIRECTION  (1 <<  0)
+#define  V_DIRECTION  (1 <<  2)
+#define  COLOR_EXPAND (1 <<  6)
+#define  CSC_ENABLE   (1 << 18)
+#define  WIN_ENABLE   (1 << 30)
+
+#define DC_WIN_COLOR_DEPTH 0x703
+#define  WIN_COLOR_DEPTH_P1             0x0
+#define  WIN_COLOR_DEPTH_P2             0x1
+#define  WIN_COLOR_DEPTH_P4             0x2
+#define  WIN_COLOR_DEPTH_P8             0x3
+#define  WIN_COLOR_DEPTH_B4G4R4A4       0x4
+#define  WIN_COLOR_DEPTH_B5G5R5A        0x5
+#define  WIN_COLOR_DEPTH_B5G6R5         0x6
+#define  WIN_COLOR_DEPTH_AB5G5R5        0x7
+#define  WIN_COLOR_DEPTH_B8G8R8A8       0xC
+#define  WIN_COLOR_DEPTH_R8G8B8A8       0xD
+#define  WIN_COLOR_DEPTH_B6x2G6x2R6x2A8 0xE
+#define  WIN_COLOR_DEPTH_R6x2G6x2B6x2A8 0xF
+#define  WIN_COLOR_DEPTH_YCbCr422       0x10
+#define  WIN_COLOR_DEPTH_YUV422         0x11
+#define  WIN_COLOR_DEPTH_YCbCr420P      0x12
+#define  WIN_COLOR_DEPTH_YUV420P        0x13
+#define  WIN_COLOR_DEPTH_YCbCr422P      0x14
+#define  WIN_COLOR_DEPTH_YUV422P        0x15
+#define  WIN_COLOR_DEPTH_YCbCr422R      0x16
+#define  WIN_COLOR_DEPTH_YUV422R        0x17
+#define  WIN_COLOR_DEPTH_YCbCr422RA     0x18
+#define  WIN_COLOR_DEPTH_YUV422RA       0x19
+
+#define DC_WIN_BUFFER_CONTROL 0x702
+#define DC_WIN_POSITION 0x704
+
+#define DC_WIN_SIZE 0x705
+#define  H_SIZE(x) (((x) & 0x1fff) <<  0)
+#define  V_SIZE(x) (((x) & 0x1fff) << 16)
+
+#define DC_WIN_PRESCALED_SIZE 0x706
+#define  H_PRESCALED_SIZE(x) (((x) & 0x7fff) <<  0)
+#define  V_PRESCALED_SIZE(x) (((x) & 0x1fff) << 16)
+
+#define DC_WIN_H_INITIAL_DDA 0x707
+#define DC_WIN_V_INITIAL_DDA 0x708
+
+#define DC_WIN_DDA_INC 0x709
+#define  H_DDA_INC(x) (((x) & 0xffff) <<  0)
+#define  V_DDA_INC(x) (((x) & 0xffff) << 16)
+
+#define DC_WIN_LINE_STRIDE 0x70A
+#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_X_WINBUF_XD_START_ADDR 0x800
-#define DC_X_WINBUF_XD_ADDR_H_OFFSET 0x806
-#define DC_X_WINBUF_XD_ADDR_V_OFFSET 0x808
-#define DC_X_WINBUF_XD_SURFACE_KIND 0x80B
+#define DC_WINBUF_START_ADDR 0x800
+#define DC_WINBUF_ADDR_H_OFFSET 0x806
+#define DC_WINBUF_ADDR_V_OFFSET 0x808
+#define DC_WINBUF_SURFACE_KIND 0x80B

 /*! Display serial interface registers. */
 #define _DSIREG(reg) ((reg) * 4)
-#define DSI_DSI_RD_DATA 0x9
-#define DSI_DSI_WR_DATA 0xA
-#define DSI_DSI_POWER_CONTROL 0xB
-#define DSI_HOST_DSI_CONTROL 0xF
-#define DSI_DSI_TRIGGER 0x13
-#define DSI_DSI_BTA_TIMING 0x3F
-#define DSI_PAD_CONTROL 0x4B
-#define DSI_DSI_VID_MODE_CONTROL 0x4E
+
+#define DSI_RD_DATA 0x9
+#define DSI_WR_DATA 0xA
+
+#define DSI_POWER_CONTROL 0xB
+#define  DSI_POWER_CONTROL_ENABLE 1
+
+#define DSI_INT_ENABLE 0xC
+#define DSI_INT_STATUS 0xD
+#define DSI_INT_MASK 0xE
+
+#define DSI_HOST_CONTROL 0xF
+#define  DSI_HOST_CONTROL_FIFO_RESET   (1 << 21)
+#define  DSI_HOST_CONTROL_CRC_RESET    (1 << 20)
+#define  DSI_HOST_CONTROL_TX_TRIG_SOL  (0 << 12)
+#define  DSI_HOST_CONTROL_TX_TRIG_FIFO (1 << 12)
+#define  DSI_HOST_CONTROL_TX_TRIG_HOST (2 << 12)
+#define  DSI_HOST_CONTROL_RAW          (1 << 6)
+#define  DSI_HOST_CONTROL_HS           (1 << 5)
+#define  DSI_HOST_CONTROL_FIFO_SEL     (1 << 4)
+#define  DSI_HOST_CONTROL_IMM_BTA      (1 << 3)
+#define  DSI_HOST_CONTROL_PKT_BTA      (1 << 2)
+#define  DSI_HOST_CONTROL_CS           (1 << 1)
+#define  DSI_HOST_CONTROL_ECC          (1 << 0)
+
+#define DSI_CONTROL 0x10
+#define  DSI_CONTROL_HS_CLK_CTRL  (1 << 20)
+#define  DSI_CONTROL_CHANNEL(c)   (((c) & 0x3) << 16)
+#define  DSI_CONTROL_FORMAT(f)    (((f) & 0x3) << 12)
+#define  DSI_CONTROL_TX_TRIG(x)   (((x) & 0x3) <<  8)
+#define  DSI_CONTROL_LANES(n)     (((n) & 0x3) <<  4)
+#define  DSI_CONTROL_DCS_ENABLE   (1 << 3)
+#define  DSI_CONTROL_SOURCE(s)    (((s) & 0x1) <<  2)
+#define  DSI_CONTROL_VIDEO_ENABLE (1 << 1)
+#define  DSI_CONTROL_HOST_ENABLE  (1 << 0)
+
+#define DSI_SOL_DELAY 0x11
+#define DSI_MAX_THRESHOLD 0x12
+
+#define DSI_TRIGGER 0x13
+#define  DSI_TRIGGER_HOST  (1 << 1)
+#define  DSI_TRIGGER_VIDEO (1 << 0)
+
+#define DSI_TX_CRC 0x14
+#define DSI_STATUS 0x15
+#define DSI_INIT_SEQ_CONTROL 0x1A
+#define DSI_INIT_SEQ_DATA_0 0x1B
+#define DSI_INIT_SEQ_DATA_1 0x1C
+#define DSI_INIT_SEQ_DATA_2 0x1D
+#define DSI_INIT_SEQ_DATA_3 0x1E
+#define DSI_PKT_SEQ_0_LO 0x23
+#define DSI_PKT_SEQ_0_HI 0x24
+#define DSI_PKT_SEQ_1_LO 0x25
+#define DSI_PKT_SEQ_1_HI 0x26
+#define DSI_PKT_SEQ_2_LO 0x27
+#define DSI_PKT_SEQ_2_HI 0x28
+#define DSI_PKT_SEQ_3_LO 0x29
+#define DSI_PKT_SEQ_3_HI 0x2A
+#define DSI_PKT_SEQ_4_LO 0x2B
+#define DSI_PKT_SEQ_4_HI 0x2C
+#define DSI_PKT_SEQ_5_LO 0x2D
+#define DSI_PKT_SEQ_5_HI 0x2E
+#define DSI_DCS_CMDS 0x33
+#define DSI_PKT_LEN_0_1 0x34
+#define DSI_PKT_LEN_2_3 0x35
+#define DSI_PKT_LEN_4_5 0x36
+#define DSI_PKT_LEN_6_7 0x37
+#define DSI_PHY_TIMING_0 0x3C
+#define DSI_PHY_TIMING_1 0x3D
+#define DSI_PHY_TIMING_2 0x3E
+#define DSI_BTA_TIMING 0x3F
+
+#define DSI_TIMEOUT_0 0x44
+#define  DSI_TIMEOUT_LRX(x) (((x) & 0xffff) << 16)
+#define  DSI_TIMEOUT_HTX(x) (((x) & 0xffff) <<  0)
+
+#define DSI_TIMEOUT_1 0x45
+#define  DSI_TIMEOUT_PR(x) (((x) & 0xffff) << 16)
+#define  DSI_TIMEOUT_TA(x) (((x) & 0xffff) <<  0)
+
+#define DSI_TO_TALLY 0x46
+
+#define DSI_PAD_CONTROL_0 0x4B
+#define  DSI_PAD_CONTROL_VS1_PULLDN_CLK (1 << 24)
+#define  DSI_PAD_CONTROL_VS1_PULLDN(x)  (((x) & 0xf) << 16)
+#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_VIDEO_MODE_CONTROL 0x4E
+
+#define DSI_PAD_CONTROL_1 0x4F
+#define DSI_PAD_CONTROL_2 0x50
+
+#define DSI_PAD_CONTROL_3 0x51
+#define  DSI_PAD_PREEMP_PD_CLK(x) (((x) & 0x3) << 12)
+#define  DSI_PAD_PREEMP_PU_CLK(x) (((x) & 0x3) << 8)
+#define  DSI_PAD_PREEMP_PD(x)     (((x) & 0x3) << 4)
+#define  DSI_PAD_PREEMP_PU(x)     (((x) & 0x3) << 0)
+
+#define DSI_PAD_CONTROL_4 0x52

 void display_init();
 void display_end();
@@ -66,6 +341,9 @@ void display_end();
 /*! Show one single color on the display. */
 void display_color_screen(u32 color);

+/*! Switches screen backlight ON/OFF. */
+void display_backlight(u8 enable);
+
 /*! Init display in full 1280x720 resolution (32bpp, line stride 768, framebuffer size = 1280*768*4 bytes). */
 u32 *display_init_framebuffer();

--- a/ipl/di.inl
+++ b/ipl/di.inl
@@ -18,263 +18,269 @@
 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
+	{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] = {
-	{0x40, 0},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1},
-	{0x43, 0x54},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1},
-	{0x42, 0x10},
-	{0x42, 0x20},
-	{0x42, 0x40},
-	{0x480, 0},
-	{0x403, 0},
-	{0x404, 0},
-	{0x36, 0x50155},
-	{1, 0x100},
-	{0x28, 0x109},
-	{DC_CMD_STATE_CONTROL, 0xF00},
-	{DC_CMD_STATE_CONTROL, 0xF},
-	{0x40, 0},
-	{0x42, 0x10},
-	{0x700, 0},
-	{0x42, 0x10},
-	{0x70E, 0},
-	{0x700, 0},
-	{0x42, 0x10},
-	{0x42, 0x10},
-	{0x611, 0xF0},
-	{0x612, 0x12A},
-	{0x613, 0},
-	{0x614, 0x198},
-	{0x615, 0x39B},
-	{0x616, 0x32F},
-	{0x617, 0x204},
-	{0x618, 0},
-	{0x42, 0x20},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x70E, 0},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x42, 0x20},
-	{0x611, 0xF0},
-	{0x612, 0x12A},
-	{0x613, 0},
-	{0x614, 0x198},
-	{0x615, 0x39B},
-	{0x616, 0x32F},
-	{0x617, 0x204},
-	{0x618, 0},
-	{0x42, 0x40},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x70E, 0},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x42, 0x40},
-	{0x611, 0xF0},
-	{0x612, 0x12A},
-	{0x613, 0},
-	{0x614, 0x198},
-	{0x615, 0x39B},
-	{0x616, 0x32F},
-	{0x617, 0x204},
-	{0x618, 0},
-	{0x42, 0x10},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x700, 0},
-	{0x430, 8},
-	{0x42F, 0},
-	{0x307, 0x1000000},
-	{0x309, 0},
+	{DC_CMD_STATE_ACCESS, 0},
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
+	{DC_CMD_REG_ACT_CONTROL, 0x54},
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_DISP_DC_MCCIF_FIFOCTRL, 0},
+	{DC_DISP_DISP_MEM_HIGH_PRIORITY, 0},
+	{DC_DISP_DISP_MEM_HIGH_PRIORITY_TIMER, 0},
+	{DC_CMD_DISPLAY_POWER_CONTROL, PW0_ENABLE | PW1_ENABLE | PW2_ENABLE | PW3_ENABLE | PW4_ENABLE | PM0_ENABLE | PM1_ENABLE},
+	{DC_CMD_GENERAL_INCR_SYNCPT_CNTRL, SYNCPT_CNTRL_NO_STALL},
+	{DC_CMD_CONT_SYNCPT_VSYNC, SYNCPT_VSYNC_ENABLE | 0x9}, // 9: SYNCPT
+	{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},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_DV_CONTROL, 0},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	/* Setup default YUV colorspace conversion coefficients */
+	{DC_WIN_CSC_YOF,   0xF0},
+	{DC_WIN_CSC_KYRGB, 0x12A},
+	{DC_WIN_CSC_KUR,   0},
+	{DC_WIN_CSC_KVR,   0x198},
+	{DC_WIN_CSC_KUG,   0x39B},
+	{DC_WIN_CSC_KVG,   0x32F},
+	{DC_WIN_CSC_KUB,   0x204},
+	{DC_WIN_CSC_KVB,   0},
+	/* End of color coefficients */
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_DV_CONTROL, 0},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	/* Setup default YUV colorspace conversion coefficients */
+	{DC_WIN_CSC_YOF,   0xF0},
+	{DC_WIN_CSC_KYRGB, 0x12A},
+	{DC_WIN_CSC_KUR,   0},
+	{DC_WIN_CSC_KVR,   0x198},
+	{DC_WIN_CSC_KUG,   0x39B},
+	{DC_WIN_CSC_KVG,   0x32F},
+	{DC_WIN_CSC_KUB,   0x204},
+	{DC_WIN_CSC_KVB,   0},
+	/* End of color coefficients */
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_WIN_DV_CONTROL, 0},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	/* Setup default YUV colorspace conversion coefficients */
+	{DC_WIN_CSC_YOF,   0xF0},
+	{DC_WIN_CSC_KYRGB, 0x12A},
+	{DC_WIN_CSC_KUR,   0},
+	{DC_WIN_CSC_KVR,   0x198},
+	{DC_WIN_CSC_KUG,   0x39B},
+	{DC_WIN_CSC_KVG,   0x32F},
+	{DC_WIN_CSC_KUB,   0x204},
+	{DC_WIN_CSC_KVB,   0},
+	/* End of color coefficients */
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_DISP_DISP_COLOR_CONTROL, BASE_COLOR_SIZE_888},
+	{DC_DISP_DISP_INTERFACE_CONTROL, DISP_DATA_FORMAT_DF1P1C},
+	{DC_COM_PIN_OUTPUT_POLARITY(1), 0x1000000},
+	{DC_COM_PIN_OUTPUT_POLARITY(3), 0},
 	{0x4E4, 0},
-	{0x300, 0},
-	{DC_CMD_STATE_CONTROL, 0xF00},
-	{DC_CMD_STATE_CONTROL, 0xF},
-	{0x42, 0x10},
+	{DC_COM_CRC_CONTROL, 0},
+	{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_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
 	{0x716, 0x10000FF},
-	{0x42, 0x20},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
 	{0x716, 0x10000FF},
-	{0x42, 0x40},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
 	{0x716, 0x10000FF},
-	{0x31, 0},
-	{0x42, 0x10},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x700, 0},
-	{0x402, 0},
-	{0x32, 0},
-	{DC_CMD_STATE_CONTROL, 0xF00},
-	{DC_CMD_STATE_CONTROL, 0xF}
+	{DC_CMD_DISPLAY_COMMAND_OPTION0, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{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_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 config.
+//DSI Init config.
 static const cfg_op_t _display_config_3[60] = { 
-	{0xA, 0},
-	{0xC, 0},
-	{0xD, 0},
-	{0xE, 0},
-	{0x1B, 0},
-	{0x1C, 0},
-	{0x1D, 0},
-	{0x1E, 0},
-	{0x33, 0},
-	{0x23, 0},
-	{0x25, 0},
-	{0x27, 0},
-	{0x29, 0},
-	{0x2B, 0},
-	{0x2D, 0},
-	{0x24, 0},
-	{0x26, 0},
-	{0x28, 0},
-	{0x2A, 0},
-	{0x2C, 0},
-	{0x2E, 0},
-	{0x10, 0},
-	{0x4C, 0},
-	{0x11, 0x18},
-	{0x12, 0x1E0},
-	{0x13, 0},
-	{0x1A, 0},
-	{0x34, 0},
-	{0x35, 0},
-	{0x36, 0},
-	{0x37, 0},
-	{0x4F, 0},
-	{0x3C, 0x6070601},
-	{0x3D, 0x40A0E05},
-	{0x3E, 0x30109},
-	{0x3F, 0x190A14},
-	{0x44, 0x2000FFFF},
-	{0x45, 0x7652000},
-	{0x46, 0},
-	{0x4B, 0},
-	{DSI_DSI_POWER_CONTROL, 1},
-	{DSI_DSI_POWER_CONTROL, 1},
-	{DSI_DSI_POWER_CONTROL, 0},
-	{DSI_DSI_POWER_CONTROL, 0},
-	{0x4F, 0},
-	{0x3C, 0x6070601},
-	{0x3D, 0x40A0E05},
-	{0x3E, 0x30118},
-	{0x3F, 0x190A14},
-	{0x44, 0x2000FFFF},
-	{0x45, 0x13432000},
-	{0x46, 0},
-	{0xF, 0x102003},
-	{0x10, 0x31},
-	{DSI_DSI_POWER_CONTROL, 1},
-	{DSI_DSI_POWER_CONTROL, 1},
-	{0x12, 0x40},
-	{0x13, 0},
-	{0x14, 0},
-	{0x1A, 0}
+	{DSI_WR_DATA, 0},
+	{DSI_INT_ENABLE, 0},
+	{DSI_INT_STATUS, 0},
+	{DSI_INT_MASK, 0},
+	{DSI_INIT_SEQ_DATA_0, 0},
+	{DSI_INIT_SEQ_DATA_1, 0},
+	{DSI_INIT_SEQ_DATA_2, 0},
+	{DSI_INIT_SEQ_DATA_3, 0},
+	{DSI_DCS_CMDS, 0},
+	{DSI_PKT_SEQ_0_LO, 0},
+	{DSI_PKT_SEQ_1_LO, 0},
+	{DSI_PKT_SEQ_2_LO, 0},
+	{DSI_PKT_SEQ_3_LO, 0},
+	{DSI_PKT_SEQ_4_LO, 0},
+	{DSI_PKT_SEQ_5_LO, 0},
+	{DSI_PKT_SEQ_0_HI, 0},
+	{DSI_PKT_SEQ_1_HI, 0},
+	{DSI_PKT_SEQ_2_HI, 0},
+	{DSI_PKT_SEQ_3_HI, 0},
+	{DSI_PKT_SEQ_4_HI, 0},
+	{DSI_PKT_SEQ_5_HI, 0},
+	{DSI_CONTROL, 0},
+	{DSI_PAD_CONTROL_CD, 0},
+	{DSI_SOL_DELAY, 0x18},
+	{DSI_MAX_THRESHOLD, 0x1E0},
+	{DSI_TRIGGER, 0},
+	{DSI_INIT_SEQ_CONTROL, 0},
+	{DSI_PKT_LEN_0_1, 0},
+	{DSI_PKT_LEN_2_3, 0},
+	{DSI_PKT_LEN_4_5, 0},
+	{DSI_PKT_LEN_6_7, 0},
+	{DSI_PAD_CONTROL_1, 0},
+	{DSI_PHY_TIMING_0, 0x6070601},
+	{DSI_PHY_TIMING_1, 0x40A0E05},
+	{DSI_PHY_TIMING_2, 0x30109},
+	{DSI_BTA_TIMING, 0x190A14},
+	{DSI_TIMEOUT_0, DSI_TIMEOUT_LRX(0x2000) | DSI_TIMEOUT_HTX(0xFFFF)},
+	{DSI_TIMEOUT_1, DSI_TIMEOUT_PR(0x765) | DSI_TIMEOUT_TA(0x2000)},
+	{DSI_TO_TALLY, 0},
+	{DSI_PAD_CONTROL_0, DSI_PAD_CONTROL_VS1_PULLDN(0) | DSI_PAD_CONTROL_VS1_PDIO(0)}, // Enable
+	{DSI_POWER_CONTROL, DSI_POWER_CONTROL_ENABLE},
+	{DSI_POWER_CONTROL, DSI_POWER_CONTROL_ENABLE},
+	{DSI_POWER_CONTROL, 0},
+	{DSI_POWER_CONTROL, 0},
+	{DSI_PAD_CONTROL_1, 0},
+	{DSI_PHY_TIMING_0, 0x6070601},
+	{DSI_PHY_TIMING_1, 0x40A0E05},
+	{DSI_PHY_TIMING_2, 0x30118},
+	{DSI_BTA_TIMING, 0x190A14},
+	{DSI_TIMEOUT_0, DSI_TIMEOUT_LRX(0x2000) | DSI_TIMEOUT_HTX(0xFFFF)},
+	{DSI_TIMEOUT_1, DSI_TIMEOUT_PR(0x1343) | DSI_TIMEOUT_TA(0x2000)},
+	{DSI_TO_TALLY, 0},
+	{DSI_HOST_CONTROL, DSI_HOST_CONTROL_CRC_RESET | DSI_HOST_CONTROL_TX_TRIG_HOST | DSI_HOST_CONTROL_CS | DSI_HOST_CONTROL_ECC},
+	{DSI_CONTROL, DSI_CONTROL_LANES(3) | DSI_CONTROL_HOST_ENABLE},
+	{DSI_POWER_CONTROL, DSI_POWER_CONTROL_ENABLE},
+	{DSI_POWER_CONTROL, DSI_POWER_CONTROL_ENABLE},
+	{DSI_MAX_THRESHOLD, 0x40},
+	{DSI_TRIGGER, 0},
+	{DSI_TX_CRC, 0},
+	{DSI_INIT_SEQ_CONTROL, 0}
 };

 //DSI config (if ver == 0x10).
 static const cfg_op_t _display_config_4[43] = {
-	{DSI_DSI_WR_DATA, 0x439},
-	{DSI_DSI_WR_DATA, 0x9483FFB9},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0xBD15},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0x1939},
-	{DSI_DSI_WR_DATA, 0xAAAAAAD8},
-	{DSI_DSI_WR_DATA, 0xAAAAAAEB},
-	{DSI_DSI_WR_DATA, 0xAAEBAAAA},
-	{DSI_DSI_WR_DATA, 0xAAAAAAAA},
-	{DSI_DSI_WR_DATA, 0xAAAAAAEB},
-	{DSI_DSI_WR_DATA, 0xAAEBAAAA},
-	{DSI_DSI_WR_DATA, 0xAA},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0x1BD15},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0x2739},
-	{DSI_DSI_WR_DATA, 0xFFFFFFD8},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFF},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0x2BD15},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0xF39},
-	{DSI_DSI_WR_DATA, 0xFFFFFFD8},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFFFF},
-	{DSI_DSI_WR_DATA, 0xFFFFFF},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0xBD15},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0x6D915},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0x439},
-	{DSI_DSI_WR_DATA, 0xB9},
-	{DSI_DSI_TRIGGER, 2}
+	{DSI_WR_DATA, 0x439},
+	{DSI_WR_DATA, 0x9483FFB9},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0xBD15},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x1939},
+	{DSI_WR_DATA, 0xAAAAAAD8},
+	{DSI_WR_DATA, 0xAAAAAAEB},
+	{DSI_WR_DATA, 0xAAEBAAAA},
+	{DSI_WR_DATA, 0xAAAAAAAA},
+	{DSI_WR_DATA, 0xAAAAAAEB},
+	{DSI_WR_DATA, 0xAAEBAAAA},
+	{DSI_WR_DATA, 0xAA},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x1BD15},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x2739},
+	{DSI_WR_DATA, 0xFFFFFFD8},
+	{DSI_WR_DATA, 0xFFFFFFFF},
+	{DSI_WR_DATA, 0xFFFFFFFF},
+	{DSI_WR_DATA, 0xFFFFFFFF},
+	{DSI_WR_DATA, 0xFFFFFFFF},
+	{DSI_WR_DATA, 0xFFFFFFFF},
+	{DSI_WR_DATA, 0xFFFFFFFF},
+	{DSI_WR_DATA, 0xFFFFFFFF},
+	{DSI_WR_DATA, 0xFFFFFFFF},
+	{DSI_WR_DATA, 0xFFFFFF},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x2BD15},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0xF39},
+	{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_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x6D915},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x439},
+	{DSI_WR_DATA, 0xB9},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST}
 };

 //DSI config.
 static const cfg_op_t _display_config_5[21] = {
-	{0x4F, 0},
-	{0x3C, 0x6070601},
-	{0x3D, 0x40A0E05},
-	{0x3E, 0x30172},
-	{0x3F, 0x190A14},
-	{0x44, 0x20000A40},
-	{0x45, 0x5A2F2000},
-	{0x46, 0},
-	{0x23, 0x40000208},
-	{0x27, 0x40000308},
-	{0x2B, 0x40000308},
-	{0x25, 0x40000308},
-	{0x29, 0x3F3B2B08},
-	{0x2A, 0x2CC},
-	{0x2D, 0x3F3B2B08},
-	{0x2E, 0x2CC},
-	{0x34, 0xCE0000},
-	{0x35, 0x87001A2},
-	{0x36, 0x190},
-	{0x37, 0x190},
-	{0xF, 0},
+	{DSI_PAD_CONTROL_1, 0},
+	{DSI_PHY_TIMING_0, 0x6070601},
+	{DSI_PHY_TIMING_1, 0x40A0E05},
+	{DSI_PHY_TIMING_2, 0x30172},
+	{DSI_BTA_TIMING, 0x190A14},
+	{DSI_TIMEOUT_0, DSI_TIMEOUT_LRX(0x2000) | DSI_TIMEOUT_HTX(0xA40)},
+	{DSI_TIMEOUT_1, DSI_TIMEOUT_PR(0x5A2F) | DSI_TIMEOUT_TA(0x2000)},
+	{DSI_TO_TALLY, 0},
+	{DSI_PKT_SEQ_0_LO, 0x40000208},
+	{DSI_PKT_SEQ_2_LO, 0x40000308},
+	{DSI_PKT_SEQ_4_LO, 0x40000308},
+	{DSI_PKT_SEQ_1_LO, 0x40000308},
+	{DSI_PKT_SEQ_3_LO, 0x3F3B2B08},
+	{DSI_PKT_SEQ_3_HI, 0x2CC},
+	{DSI_PKT_SEQ_5_LO, 0x3F3B2B08},
+	{DSI_PKT_SEQ_5_HI, 0x2CC},
+	{DSI_PKT_LEN_0_1, 0xCE0000},
+	{DSI_PKT_LEN_2_3, 0x87001A2},
+	{DSI_PKT_LEN_4_5, 0x190},
+	{DSI_PKT_LEN_6_7, 0x190},
+	{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
+	{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] = {
-	{0x13, 0},
-	{0x10, 0},
-	{0x11, 6},
-	{0x12, 0x1E0},
-	{DSI_DSI_POWER_CONTROL, 1},
-	{0x10, 0x103032},
-	{0xF, 0x33},
-	{0x10, 0x103032},
-	{0xF, 3},
-	{0xF, 0x23}
+	{DSI_TRIGGER, 0},
+	{DSI_CONTROL, 0},
+	{DSI_SOL_DELAY, 6},
+	{DSI_MAX_THRESHOLD, 0x1E0},
+	{DSI_POWER_CONTROL, DSI_POWER_CONTROL_ENABLE},
+	{DSI_CONTROL, DSI_CONTROL_HS_CLK_CTRL | DSI_CONTROL_FORMAT(3) | DSI_CONTROL_LANES(3) | DSI_CONTROL_VIDEO_ENABLE},
+	{DSI_HOST_CONTROL, DSI_HOST_CONTROL_HS | DSI_HOST_CONTROL_FIFO_SEL| DSI_HOST_CONTROL_CS | DSI_HOST_CONTROL_ECC},
+	{DSI_CONTROL, DSI_CONTROL_HS_CLK_CTRL | DSI_CONTROL_FORMAT(3) | DSI_CONTROL_LANES(3) | DSI_CONTROL_VIDEO_ENABLE},
+	{DSI_HOST_CONTROL, DSI_HOST_CONTROL_CS | DSI_HOST_CONTROL_ECC},
+	{DSI_HOST_CONTROL, DSI_HOST_CONTROL_HS | DSI_HOST_CONTROL_CS | DSI_HOST_CONTROL_ECC}
 };

 //MIPI CAL config.
@@ -289,10 +295,10 @@ static const cfg_op_t _display_config_8[6] = {

 //DSI config.
 static const cfg_op_t _display_config_9[4] = {
-	{0x4F, 0},
-	{0x50, 0},
-	{0x51, 0x3333},
-	{0x52, 0}
+	{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)},
+	{DSI_PAD_CONTROL_4, 0}
 };

 //MIPI CAL config.
@@ -317,232 +323,240 @@ static const cfg_op_t _display_config_10[16] = {

 //Display A config.
 static const cfg_op_t _display_config_11[113] = {
-	{0x40, 0},
-	{0x42, 0x10},
-	{0x700, 0},
-	{0x42, 0x10},
-	{0x70E, 0},
-	{0x700, 0},
-	{0x42, 0x10},
-	{0x42, 0x10},
-	{0x611, 0xF0},
-	{0x612, 0x12A},
-	{0x613, 0},
-	{0x614, 0x198},
-	{0x615, 0x39B},
-	{0x616, 0x32F},
-	{0x617, 0x204},
-	{0x618, 0},
-	{0x42, 0x20},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x70E, 0},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x42, 0x20},
-	{0x611, 0xF0},
-	{0x612, 0x12A},
-	{0x613, 0},
-	{0x614, 0x198},
-	{0x615, 0x39B},
-	{0x616, 0x32F},
-	{0x617, 0x204},
-	{0x618, 0},
-	{0x42, 0x40},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x70E, 0},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x42, 0x40},
-	{0x611, 0xF0},
-	{0x612, 0x12A},
-	{0x613, 0},
-	{0x614, 0x198},
-	{0x615, 0x39B},
-	{0x616, 0x32F},
-	{0x617, 0x204},
-	{0x618, 0},
-	{0x42, 0x10},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x700, 0},
-	{0x430, 8},
-	{0x42F, 0},
-	{0x307, 0x1000000},
-	{0x309, 0},
+	{DC_CMD_STATE_ACCESS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_DV_CONTROL, 0},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	/* Setup default YUV colorspace conversion coefficients */
+	{DC_WIN_CSC_YOF,   0xF0},
+	{DC_WIN_CSC_KYRGB, 0x12A},
+	{DC_WIN_CSC_KUR,   0},
+	{DC_WIN_CSC_KVR,   0x198},
+	{DC_WIN_CSC_KUG,   0x39B},
+	{DC_WIN_CSC_KVG,   0x32F},
+	{DC_WIN_CSC_KUB,   0x204},
+	{DC_WIN_CSC_KVB,   0},
+	/* End of color coefficients */
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_DV_CONTROL, 0},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	/* Setup default YUV colorspace conversion coefficients */
+	{DC_WIN_CSC_YOF,   0xF0},
+	{DC_WIN_CSC_KYRGB, 0x12A},
+	{DC_WIN_CSC_KUR,   0},
+	{DC_WIN_CSC_KVR,   0x198},
+	{DC_WIN_CSC_KUG,   0x39B},
+	{DC_WIN_CSC_KVG,   0x32F},
+	{DC_WIN_CSC_KUB,   0x204},
+	{DC_WIN_CSC_KVB,   0},
+	/* End of color coefficients */
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_WIN_DV_CONTROL, 0},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	/* Setup default YUV colorspace conversion coefficients */
+	{DC_WIN_CSC_YOF,   0xF0},
+	{DC_WIN_CSC_KYRGB, 0x12A},
+	{DC_WIN_CSC_KUR,   0},
+	{DC_WIN_CSC_KVR,   0x198},
+	{DC_WIN_CSC_KUG,   0x39B},
+	{DC_WIN_CSC_KVG,   0x32F},
+	{DC_WIN_CSC_KUB,   0x204},
+	{DC_WIN_CSC_KVB,   0},
+	/* End of color coefficients */
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_DISP_DISP_COLOR_CONTROL, BASE_COLOR_SIZE_888},
+	{DC_DISP_DISP_INTERFACE_CONTROL, DISP_DATA_FORMAT_DF1P1C},
+	{DC_COM_PIN_OUTPUT_POLARITY(1), 0x1000000},
+	{DC_COM_PIN_OUTPUT_POLARITY(3), 0},
 	{0x4E4, 0},
-	{0x300, 0},
-	{DC_CMD_STATE_CONTROL, 0xF00},
-	{DC_CMD_STATE_CONTROL, 0xF},
-	{0x42, 0x10},
+	{DC_COM_CRC_CONTROL, 0},
+	{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_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
 	{0x716, 0x10000FF},
-	{0x42, 0x20},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
 	{0x716, 0x10000FF},
-	{0x42, 0x40},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
 	{0x716, 0x10000FF},
-	{0x31, 0},
-	{0x42, 0x10},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x700, 0},
-	{0x402, 0},
-	{0x32, 0},
-	{DC_CMD_STATE_CONTROL, 0xF00},
-	{DC_CMD_STATE_CONTROL, 0xF},
-	{0x40, 0},
-	{0x405, 0},
-	{0x406, 0x10000},
-	{0x407, 0x10048},
-	{0x408, 0x90048},
-	{0x409, 0x50002D0},
-	{0x40A, 0xA0088},
-	{0x431, 0x10001},
-	{0x303, 0},
-	{0x432, 5},
-	{0x42F, 0},
-	{0x42E, 0},
-	{0x31, 0},
-	{0x42, 0x10},
-	{0x700, 0},
-	{0x42, 0x20},
-	{0x700, 0},
-	{0x42, 0x40},
-	{0x700, 0},
-	{0x402, 0},
-	{0x32, 0x20},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1},
-	{0x40, 5},
-	{0x40A, 0xA0088},
-	{0x40, 0},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1},
-	{0, 0x301},
-	{0, 0x301},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1},
-	{0x40, 0},
-	{0x42E, 4},
-	{0x430, 8},
-	{0x31, 0}
+	{DC_CMD_DISPLAY_COMMAND_OPTION0, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{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_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},
+	/* Set Display timings */
+	{DC_DISP_DISP_TIMING_OPTIONS, 0},
+	{DC_DISP_REF_TO_SYNC, (1 << 16)}, // h_ref_to_sync = 0, v_ref_to_sync = 1.
+	{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.
+	/* End of Display timings */
+	{DC_DISP_SHIFT_CLOCK_OPTIONS, SC1_H_QUALIFIER_NONE | SC0_H_QUALIFIER_NONE},
+	{DC_COM_PIN_OUTPUT_ENABLE(1), 0},
+	{DC_DISP_DATA_ENABLE_OPTIONS, DE_SELECT_ACTIVE | DE_CONTROL_NORMAL},
+	{DC_DISP_DISP_INTERFACE_CONTROL, DISP_DATA_FORMAT_DF1P1C},
+	{DC_DISP_DISP_CLOCK_CONTROL, 0},
+	{DC_CMD_DISPLAY_COMMAND_OPTION0, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_DISP_DISP_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_COMMAND, DISP_CTRL_MODE_C_DISPLAY},
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
+	{DC_CMD_STATE_ACCESS, READ_MUX | WRITE_MUX},
+	{DC_DISP_FRONT_PORCH, 0xA0088},
+	{DC_CMD_STATE_ACCESS, 0},
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
+	{DC_CMD_GENERAL_INCR_SYNCPT, 0x301},
+	{DC_CMD_GENERAL_INCR_SYNCPT, 0x301},
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
+	{DC_CMD_STATE_ACCESS, 0},
+	{DC_DISP_DISP_CLOCK_CONTROL, PIXEL_CLK_DIVIDER_PCD1 | SHIFT_CLK_DIVIDER(4)},
+	{DC_DISP_DISP_COLOR_CONTROL, BASE_COLOR_SIZE_888},
+	{DC_CMD_DISPLAY_COMMAND_OPTION0, 0}
 };

 ////Display A config.
 static const cfg_op_t _display_config_12[17] = {
-	{0x40A, 0xA0088},
-	{0x38, 0},
-	{0x40, 0},
-	{0x39, 0},
-	{0x28, 0},
-	{0x32, 0},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1},
-	{0, 0x301},
-	{0, 0x301},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1},
-	{0x36, 0},
-	{DC_CMD_STATE_CONTROL, 0x100},
-	{DC_CMD_STATE_CONTROL, 1}
+	{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_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
+	{DC_CMD_GENERAL_INCR_SYNCPT, 0x301},
+	{DC_CMD_GENERAL_INCR_SYNCPT, 0x301},
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
+	{DC_CMD_DISPLAY_POWER_CONTROL, 0},
+	{DC_CMD_STATE_CONTROL, GENERAL_UPDATE},
+	{DC_CMD_STATE_CONTROL, GENERAL_ACT_REQ},
 };

 //DSI config.
 static const cfg_op_t _display_config_13[16] = {
-	{DSI_DSI_POWER_CONTROL, 0},
-	{0x4F, 0},
-	{0x3C, 0x6070601},
-	{0x3D, 0x40A0E05},
-	{0x3E, 0x30109},
-	{0x3F, 0x190A14},
-	{0x44, 0x2000FFFF},
-	{0x45, 0x7652000},
-	{0x46, 0},
-	{0xF, 0x102003},
-	{0x10, 0x31},
-	{DSI_DSI_POWER_CONTROL, 1},
-	{0x12, 0x40},
-	{0x13, 0},
-	{0x14, 0},
-	{0x1A, 0}
+	{DSI_POWER_CONTROL, 0},
+	{DSI_PAD_CONTROL_1, 0},
+	{DSI_PHY_TIMING_0, 0x6070601},
+	{DSI_PHY_TIMING_1, 0x40A0E05},
+	{DSI_PHY_TIMING_2, 0x30109},
+	{DSI_BTA_TIMING, 0x190A14},
+	{DSI_TIMEOUT_0, DSI_TIMEOUT_LRX(0x2000) | DSI_TIMEOUT_HTX(0xFFFF) },
+	{DSI_TIMEOUT_1, DSI_TIMEOUT_PR(0x765) | DSI_TIMEOUT_TA(0x2000)},
+	{DSI_TO_TALLY, 0},
+	{DSI_HOST_CONTROL, DSI_HOST_CONTROL_CRC_RESET | DSI_HOST_CONTROL_TX_TRIG_HOST | DSI_HOST_CONTROL_CS | DSI_HOST_CONTROL_ECC},
+	{DSI_CONTROL, DSI_CONTROL_LANES(3) | DSI_CONTROL_HOST_ENABLE},
+	{DSI_POWER_CONTROL, DSI_POWER_CONTROL_ENABLE},
+	{DSI_MAX_THRESHOLD, 0x40},
+	{DSI_TRIGGER, 0},
+	{DSI_TX_CRC, 0},
+	{DSI_INIT_SEQ_CONTROL, 0}
 };

 //DSI config (if ver == 0x10).
 static const cfg_op_t _display_config_14[22] = {
-	{DSI_DSI_WR_DATA, 0x439},
-	{DSI_DSI_WR_DATA, 0x9483FFB9},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0x2139},
-	{DSI_DSI_WR_DATA, 0x191919D5},
-	{DSI_DSI_WR_DATA, 0x19191919},
-	{DSI_DSI_WR_DATA, 0x19191919},
-	{DSI_DSI_WR_DATA, 0x19191919},
-	{DSI_DSI_WR_DATA, 0x19191919},
-	{DSI_DSI_WR_DATA, 0x19191919},
-	{DSI_DSI_WR_DATA, 0x19191919},
-	{DSI_DSI_WR_DATA, 0x19191919},
-	{DSI_DSI_WR_DATA, 0x19},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0xB39},
-	{DSI_DSI_WR_DATA, 0x4F0F41B1},
-	{DSI_DSI_WR_DATA, 0xF179A433},
-	{DSI_DSI_WR_DATA, 0x2D81},
-	{DSI_DSI_TRIGGER, 2},
-	{DSI_DSI_WR_DATA, 0x439},
-	{DSI_DSI_WR_DATA, 0xB9},
-	{DSI_DSI_TRIGGER, 2}
+	{DSI_WR_DATA, 0x439},
+	{DSI_WR_DATA, 0x9483FFB9},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x2139},
+	{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, 0x19},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0xB39},
+	{DSI_WR_DATA, 0x4F0F41B1},
+	{DSI_WR_DATA, 0xF179A433},
+	{DSI_WR_DATA, 0x2D81},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST},
+	{DSI_WR_DATA, 0x439},
+	{DSI_WR_DATA, 0xB9},
+	{DSI_TRIGGER, DSI_TRIGGER_HOST}
 };

 //Display A config.
 static const cfg_op_t cfg_display_one_color[8] = {
-	{DC_CMD_DISPLAY_WINDOW_HEADER, 0x10}, //Enable window A.
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_WINDOW_HEADER, 0x20}, //Enable window B.
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_WINDOW_HEADER, 0x40}, //Enable window C.
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, 0x20000000}, //DSI_ENABLE
-	{DC_CMD_DISPLAY_COMMAND, 0x20} //DISPLAY_CTRL_MODE: continuous display.
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT}, //Enable window A.
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT}, //Enable window B.
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT}, //Enable window C.
+	{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.
 };

 //Display A config.
 static const cfg_op_t cfg_display_framebuffer[32] = {
-	{DC_CMD_DISPLAY_WINDOW_HEADER, 0x40}, //Enable window C.
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_WINDOW_HEADER, 0x20}, //Enable window B.
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_CMD_DISPLAY_WINDOW_HEADER, 0x10}, //Enable window A.
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, 0x20000000}, //DSI_ENABLE
-	{DC_X_WIN_XD_COLOR_DEPTH, 0xD}, //T_A8B8G8R8
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_X_WIN_XD_POSITION, 0}, //(0,0)
-	{DC_X_WIN_XD_H_INITIAL_DDA, 0},
-	{DC_X_WIN_XD_V_INITIAL_DDA, 0},
-	{DC_X_WIN_XD_PRESCALED_SIZE, 0x5000B40}, //Pre-scaled size: 1280x2880 bytes (= 0x500 vertical lines x 0xB40 bytes).
-	{DC_X_WIN_XD_DDA_INCREMENT, 0x10001000},
-	{DC_X_WIN_XD_SIZE, 0x50002D0}, //Window size: 1280x720 (= 0x500 vertical lines x 0x2D0 horizontal pixels).
-	{DC_X_WIN_XD_LINE_STRIDE, 0x6000C00}, //768*2x768*4 (= 0x600 x 0xC00) bytes, see TRM for alignment requirements.
-	{0x702, 0},
-	{DC_X_WINBUF_XD_SURFACE_KIND, 0}, //Regular surface.
-	{DC_X_WINBUF_XD_START_ADDR, 0xC0000000}, //Framebuffer address.
-	{DC_X_WINBUF_XD_ADDR_H_OFFSET, 0},
-	{DC_X_WINBUF_XD_ADDR_V_OFFSET, 0},
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, 0x20000000}, //DSI_ENABLE
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, 0x20000000}, //DSI_ENABLE
-	{DC_X_WIN_XD_WIN_OPTIONS, 0},
-	{DC_DISP_DISP_WIN_OPTIONS, 0x20000000}, //DSI_ENABLE
-	{DC_X_WIN_XD_WIN_OPTIONS, 0x40000000}, //Enable window AD.
-	{DC_CMD_DISPLAY_COMMAND, 0x20}, //DISPLAY_CTRL_MODE: continuous display.
-	{DC_CMD_STATE_CONTROL, 0x300}, //General update; window A update.
-	{DC_CMD_STATE_CONTROL, 3} //General activation request; window A activation request.
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_C_SELECT}, //Enable window C.
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_B_SELECT}, //Enable window B.
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_CMD_DISPLAY_WINDOW_HEADER, WINDOW_A_SELECT}, //Enable window A.
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE}, //DSI_ENABLE
+	{DC_WIN_COLOR_DEPTH, WIN_COLOR_DEPTH_R8G8B8A8}, //T_A8B8G8R8
+	{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, 0x6000C00}, //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_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_WIN_WIN_OPTIONS, 0},
+	{DC_DISP_DISP_WIN_OPTIONS, DSI_ENABLE}, //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.
 };
--- a/ipl/ff.c
+++ b/ipl/ff.c
@@ -26,7 +26,7 @@
 #include "gfx.h"
 extern gfx_ctxt_t gfx_ctxt;
 extern gfx_con_t gfx_con;
-#define EFSPRINTF(text, ...) gfx_printf(&gfx_con, "\n\n%k[FatFS] "text"%k\n", 0xFF00FFFF, 0xFFFFFFFF)
+#define EFSPRINTF(text, ...) gfx_printf(&gfx_con, "\n\n\n%k[FatFS] Error: "text"%k\n", 0xFF00FFFF, 0xFFFFFFFF)
 //#define EFSPRINTF(...)

 /*--------------------------------------------------------------------------
@@ -3259,7 +3259,7 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		stat = disk_status(fs->pdrv);
 		if (!(stat & STA_NOINIT)) {		/* and the physical drive is kept initialized */
 			if (!FF_FS_READONLY && mode && (stat & STA_PROTECT)) {	/* Check write protection if needed */
-				EFSPRINTF("Error: Write protected!");
+				EFSPRINTF("Write protected!");
 				return FR_WRITE_PROTECTED;
 			}
 			return FR_OK;				/* The filesystem object is valid */
@@ -3273,11 +3273,11 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 	fs->pdrv = LD2PD(vol);				/* Bind the logical drive and a physical drive */
 	stat = disk_initialize(fs->pdrv);	/* Initialize the physical drive */
 	if (stat & STA_NOINIT) { 			/* Check if the initialization succeeded */
-		EFSPRINTF("Error: Medium not ready or hard error!");
+		EFSPRINTF("Medium not ready or hard error!");
 		return FR_NOT_READY;			/* Failed to initialize due to no medium or hard error */
 	}
 	if (!FF_FS_READONLY && mode && (stat & STA_PROTECT)) { /* Check disk write protection if needed */
-		EFSPRINTF("Error: Write protected!");
+		EFSPRINTF("Write protected!");
 		return FR_WRITE_PROTECTED;
 	}
 #if FF_MAX_SS != FF_MIN_SS				/* Get sector size (multiple sector size cfg only) */
@@ -3301,11 +3301,11 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		} while (LD2PT(vol) == 0 && fmt >= 2 && ++i < 4);
 	}
 	if (fmt == 4) {
-		EFSPRINTF("Error: Disk I/O error - Could not load boot record!");
+		EFSPRINTF("Disk I/O error - Could not load boot record!");
 		return FR_DISK_ERR;		/* An error occured in the disk I/O layer */
 	}
 	if (fmt >= 2) {
-		EFSPRINTF("Error: No FAT/FAT32/exFAT filesystem found!");
+		EFSPRINTF("No FAT/FAT32/exFAT filesystem found!");
 		return FR_NO_FILESYSTEM;	/* No FAT volume is found */
 	}

@@ -3316,21 +3316,24 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		QWORD maxlba;

 		for (i = BPB_ZeroedEx; i < BPB_ZeroedEx + 53 && fs->win[i] == 0; i++) ;	/* Check zero filler */
-		if (i < BPB_ZeroedEx + 53) return FR_NO_FILESYSTEM;
+		if (i < BPB_ZeroedEx + 53) {
+			EFSPRINTF("exFAT - Zero filler check failed!");
+			return FR_NO_FILESYSTEM;
+		}

 		if (ld_word(fs->win + BPB_FSVerEx) != 0x100) {
-			EFSPRINTF("Error: exFAT - Version check failed!");
+			EFSPRINTF("exFAT - Version check failed!");
 			return FR_NO_FILESYSTEM;	/* Check exFAT version (must be version 1.0) */
 		}

 		if (1 << fs->win[BPB_BytsPerSecEx] != SS(fs)) {	/* (BPB_BytsPerSecEx must be equal to the physical sector size) */
-			EFSPRINTF("Error: exFAT - Bytes per sector does not match physical sector size!");
+			EFSPRINTF("exFAT - Bytes per sector does not match physical sector size!");
 			return FR_NO_FILESYSTEM;
 		}

 		maxlba = ld_qword(fs->win + BPB_TotSecEx) + bsect;	/* Last LBA + 1 of the volume */
 		if (maxlba >= 0x100000000) {
-			EFSPRINTF("Error: exFAT - Cannot handle volume LBA with 32-bit LBA!");
+			EFSPRINTF("exFAT - Cannot handle volume LBA with 32-bit LBA!");
 			return FR_NO_FILESYSTEM;	/* (It cannot be handled in 32-bit LBA) */
 		}

@@ -3338,19 +3341,19 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */

 		fs->n_fats = fs->win[BPB_NumFATsEx];			/* Number of FATs */
 		if (fs->n_fats != 1) {
-			EFSPRINTF("Error: exFAT - Multiple or no file allocation tables found!");
+			EFSPRINTF("exFAT - Multiple or no file allocation tables found!");
 			return FR_NO_FILESYSTEM;	/* (Supports only 1 FAT) */
 		}

 		fs->csize = 1 << fs->win[BPB_SecPerClusEx];		/* Cluster size */
 		if (fs->csize == 0)	{
-			EFSPRINTF("Error: exFAT - Cluster size is not between 1KB - 32KB!");
+			EFSPRINTF("exFAT - Cluster size is not between 1KB - 32KB!");
 			return FR_NO_FILESYSTEM;	/* (Must be 1..32768) */
 		}

 		nclst = ld_dword(fs->win + BPB_NumClusEx);		/* Number of clusters */
 		if (nclst > MAX_EXFAT) {
-			EFSPRINTF("Error: exFAT - Total clusters exceed allowed!");
+			EFSPRINTF("exFAT - Total clusters exceed allowed!");
 			return FR_NO_FILESYSTEM;	/* (Too many clusters) */
 		}
 		fs->n_fatent = nclst + 2;
@@ -3360,20 +3363,23 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		fs->database = bsect + ld_dword(fs->win + BPB_DataOfsEx);
 		fs->fatbase = bsect + ld_dword(fs->win + BPB_FatOfsEx);
 		if (maxlba < (QWORD)fs->database + nclst * fs->csize) {
-			EFSPRINTF("Error: exFAT - Volume size is lower than required!");
+			EFSPRINTF("exFAT - Volume size is lower than required!");
 			return FR_NO_FILESYSTEM;	/* (Volume size must not be smaller than the size required) */
 		}
 		fs->dirbase = ld_dword(fs->win + BPB_RootClusEx);

 		/* Check if bitmap location is in assumption (at the first cluster) */
 		if (move_window(fs, clst2sect(fs, fs->dirbase)) != FR_OK) {
-			EFSPRINTF("Error: exFAT - Bitmap location not at first cluster!");
+			EFSPRINTF("exFAT - Bitmap location not at first cluster!");
 			return FR_DISK_ERR;
 		}
 		for (i = 0; i < SS(fs); i += SZDIRE) {
 			if (fs->win[i] == 0x81 && ld_dword(fs->win + i + 20) == 2) break;	/* 81 entry with cluster #2? */
 		}
-		if (i == SS(fs)) return FR_NO_FILESYSTEM;
+		if (i == SS(fs)) {
+			EFSPRINTF("exFAT - Bitmap allocation is missing!");
+			return FR_NO_FILESYSTEM;
+		}
 #if !FF_FS_READONLY
 		fs->last_clst = fs->free_clst = 0xFFFFFFFF;		/* Initialize cluster allocation information */
 #endif
@@ -3382,7 +3388,7 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 #endif	/* FF_FS_EXFAT */
 	{
 		if (ld_word(fs->win + BPB_BytsPerSec) != SS(fs)) {
-			EFSPRINTF("Error: FAT - Bytes per sector does not match physical sector size!");
+			EFSPRINTF("FAT - Bytes per sector does not match physical sector size!");
 			return FR_NO_FILESYSTEM;	/* (BPB_BytsPerSec must be equal to the physical sector size) */
 		}

@@ -3392,20 +3398,20 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */

 		fs->n_fats = fs->win[BPB_NumFATs];				/* Number of FATs */
 		if (fs->n_fats != 1 && fs->n_fats != 2) {
-			EFSPRINTF("Error: FAT - No or more than 2 file allocation tables found!");
+			EFSPRINTF("FAT - No or more than 2 file allocation tables found!");
 			return FR_NO_FILESYSTEM;	/* (Must be 1 or 2) */
 		}
 		fasize *= fs->n_fats;							/* Number of sectors for FAT area */

 		fs->csize = fs->win[BPB_SecPerClus];			/* Cluster size */
 		if (fs->csize == 0 || (fs->csize & (fs->csize - 1))) {
-			EFSPRINTF("Error: FAT - Cluster size is not a power of 2!");
+			EFSPRINTF("FAT - Cluster size is not a power of 2!");
 			return FR_NO_FILESYSTEM;	/* (Must be power of 2) */
 		}

 		fs->n_rootdir = ld_word(fs->win + BPB_RootEntCnt);	/* Number of root directory entries */
 		if (fs->n_rootdir % (SS(fs) / SZDIRE)) {
-			EFSPRINTF("Error: FAT - Root directory entries are not sector aligned!");
+			EFSPRINTF("FAT - Root directory entries are not sector aligned!");
 			return FR_NO_FILESYSTEM;	/* (Must be sector aligned) */
 		}

@@ -3414,19 +3420,19 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */

 		nrsv = ld_word(fs->win + BPB_RsvdSecCnt);		/* Number of reserved sectors */
 		if (nrsv == 0) {
-			EFSPRINTF("Error: FAT - Zero reserved sectors!");
+			EFSPRINTF("FAT - Zero reserved sectors!");
 			return FR_NO_FILESYSTEM;			/* (Must not be 0) */
 		}

 		/* Determine the FAT sub type */
 		sysect = nrsv + fasize + fs->n_rootdir / (SS(fs) / SZDIRE);	/* RSV + FAT + DIR */
 		if (tsect < sysect) {
-			EFSPRINTF("Error: FAT - Invalid volume size!");
+			EFSPRINTF("FAT - Invalid volume size!");
 			return FR_NO_FILESYSTEM;	/* (Invalid volume size) */
 		}
 		nclst = (tsect - sysect) / fs->csize;			/* Number of clusters */
 		if (nclst == 0) {
-			EFSPRINTF("Error: FAT - Invalid volume size!");
+			EFSPRINTF("FAT - Invalid volume size!");
 			return FR_NO_FILESYSTEM;		/* (Invalid volume size) */
 		}
 		fmt = 0;
@@ -3434,7 +3440,7 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		if (nclst <= MAX_FAT16) fmt = FS_FAT16;
 		if (nclst <= MAX_FAT12) fmt = FS_FAT12;
 		if (fmt == 0) {
-			EFSPRINTF("Error: FAT - Not compatible FAT12/16/32 filesystem!");
+			EFSPRINTF("FAT - Not compatible FAT12/16/32 filesystem!");
 			return FR_NO_FILESYSTEM;
 		}

@@ -3445,18 +3451,18 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		fs->database = bsect + sysect;					/* Data start sector */
 		if (fmt == FS_FAT32) {
 			if (ld_word(fs->win + BPB_FSVer32) != 0) {
-				EFSPRINTF("Error: FAT32 - Not a 0.0 revision!");
+				EFSPRINTF("FAT32 - Not a 0.0 revision!");
 				return FR_NO_FILESYSTEM;	/* (Must be FAT32 revision 0.0) */
 			}
 			if (fs->n_rootdir != 0) {
-				EFSPRINTF("Error: FAT32 - Root entry sector is not 0!");
+				EFSPRINTF("FAT32 - Root entry sector is not 0!");
 				return FR_NO_FILESYSTEM;	/* (BPB_RootEntCnt must be 0) */
 			}
 			fs->dirbase = ld_dword(fs->win + BPB_RootClus32);	/* Root directory start cluster */
 			szbfat = fs->n_fatent * 4;					/* (Needed FAT size) */
 		} else {
 			if (fs->n_rootdir == 0)	{
-				EFSPRINTF("Error: FAT - Root entry sector is 0!");
+				EFSPRINTF("FAT - Root entry sector is 0!");
 				return FR_NO_FILESYSTEM;	/* (BPB_RootEntCnt must not be 0) */
 			}
 			fs->dirbase = fs->fatbase + fasize;			/* Root directory start sector */
@@ -3464,7 +3470,7 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 				fs->n_fatent * 2 : fs->n_fatent * 3 / 2 + (fs->n_fatent & 1);
 		}
 		if (fs->fsize < (szbfat + (SS(fs) - 1)) / SS(fs)) {
-			EFSPRINTF("Error: FAT - FAT size is not the required size!");
+			EFSPRINTF("FAT - FAT size is not the required size!");
 			return FR_NO_FILESYSTEM;	/* (BPB_FATSz must not be less than the size needed) */
 		}

@@ -3578,7 +3584,7 @@ FRESULT f_mount (
 	/* Get logical drive number */
 	vol = get_ldnumber(&rp);
 	if (vol < 0) {
-		EFSPRINTF("Error: Invalid drive!");
+		EFSPRINTF("Invalid drive!");
 		return FR_INVALID_DRIVE;
 	}
 	cfs = FatFs[vol];					/* Pointer to fs object */
@@ -3822,11 +3828,11 @@ FRESULT f_read (
 	*br = 0;	/* Clear read byte counter */
 	res = validate(&fp->obj, &fs);				/* Check validity of the file object */
 	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) {
-		EFSPRINTF("Error: File object Validation!");
+		EFSPRINTF("File object Validation!");
 		LEAVE_FF(fs, res);	/* Check validity */
 	}
 	if (!(fp->flag & FA_READ)) {
-		EFSPRINTF("Error: Access denied!");
+		EFSPRINTF("Access denied!");
 		LEAVE_FF(fs, FR_DENIED);	/* Check access mode */
 	}
 	remain = fp->obj.objsize - fp->fptr;
@@ -3850,18 +3856,18 @@ FRESULT f_read (
 					}
 				}
 				if (clst < 2) {
-					EFSPRINTF("Error: Cluster status check or Internal error!");
+					EFSPRINTF("Cluster status check or Internal error!");
 					ABORT(fs, FR_INT_ERR);
 				}
 				if (clst == 0xFFFFFFFF) {
-					EFSPRINTF("Error: Disk error (cluster hard error)!");
+					EFSPRINTF("Disk error (cluster hard error)!");
 					ABORT(fs, FR_DISK_ERR);
 				}
 				fp->clust = clst;				/* Update current cluster */
 			}
 			sect = clst2sect(fs, fp->clust);	/* Get current sector */
 			if (sect == 0) {
-				EFSPRINTF("Error: Get current sector error!");
+				EFSPRINTF("Get current sector error!");
 				ABORT(fs, FR_INT_ERR);
 			}
 			sect += csect;
@@ -3871,7 +3877,7 @@ FRESULT f_read (
 					cc = fs->csize - csect;
 				}
 				if (disk_read(fs->pdrv, rbuff, sect, cc) != RES_OK) {
-					EFSPRINTF("Error: Read - Low level disk I/O!");
+					EFSPRINTF("Read - Low level disk I/O!");
 					ABORT(fs, FR_DISK_ERR);
 				}
 #if !FF_FS_READONLY && FF_FS_MINIMIZE <= 2		/* Replace one of the read sectors with cached data if it contains a dirty sector */
@@ -3893,14 +3899,14 @@ FRESULT f_read (
 #if !FF_FS_READONLY
 				if (fp->flag & FA_DIRTY) {		/* Write-back dirty sector cache */
 					if (disk_write(fs->pdrv, fp->buf, fp->sect, 1) != RES_OK) {
-						EFSPRINTF("Error: Write-back dirty sector cache!");
+						EFSPRINTF("Write-back dirty sector cache!");
 						ABORT(fs, FR_DISK_ERR);
 					}
 					fp->flag &= (BYTE)~FA_DIRTY;
 				}
 #endif
 				if (disk_read(fs->pdrv, fp->buf, sect, 1) != RES_OK) {
-					EFSPRINTF("Error: Read - Low level disk I/O!\n(fill sector cache)");
+					EFSPRINTF("Read - Low level disk I/O!\n(fill sector cache)");
 					ABORT(fs, FR_DISK_ERR);	/* Fill sector cache */
 				}
 			}
@@ -3945,11 +3951,11 @@ FRESULT f_write (
 	*bw = 0;	/* Clear write byte counter */
 	res = validate(&fp->obj, &fs);			/* Check validity of the file object */
 	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) {
-		EFSPRINTF("Error: File object Validation!");
+		EFSPRINTF("File object Validation!");
 		LEAVE_FF(fs, res);	/* Check validity */
 	}
 	if (!(fp->flag & FA_WRITE)) {
-		EFSPRINTF("Error: Access denied!");
+		EFSPRINTF("Access denied!");
 		LEAVE_FF(fs, FR_DENIED);	/* Check access mode */
 	}

@@ -3979,15 +3985,15 @@ FRESULT f_write (
 					}
 				}
 				if (clst == 0) {
-					EFSPRINTF("Error: Could not allocate a new cluster\n(disk full or low level disk I/O error)!");
+					EFSPRINTF("Could not allocate a new cluster\n(disk full or low level disk I/O error)!");
 					break;		/* Could not allocate a new cluster (disk full) */
 				}
 				if (clst == 1) {
-					EFSPRINTF("Error: Cluster status check or Internal error!");
+					EFSPRINTF("Cluster status check or Internal error!");
 					ABORT(fs, FR_INT_ERR);
 				}
 				if (clst == 0xFFFFFFFF) {
-					EFSPRINTF("Error: Disk error (cluster hard error)!");
+					EFSPRINTF("Disk error (cluster hard error)!");
 					ABORT(fs, FR_DISK_ERR);
 				}
 				fp->clust = clst;			/* Update current cluster */
@@ -3998,7 +4004,7 @@ FRESULT f_write (
 #else
 			if (fp->flag & FA_DIRTY) {		/* Write-back sector cache */
 				if (disk_write(fs->pdrv, fp->buf, fp->sect, 1) != RES_OK) {
-					EFSPRINTF("Error: Write-back sector cache!");
+					EFSPRINTF("Write-back sector cache!");
 					ABORT(fs, FR_DISK_ERR);
 				}
 				fp->flag &= (BYTE)~FA_DIRTY;
@@ -4006,7 +4012,7 @@ FRESULT f_write (
 #endif
 			sect = clst2sect(fs, fp->clust);	/* Get current sector */
 			if (sect == 0) {
-				EFSPRINTF("Error: Get current sector error!");
+				EFSPRINTF("Get current sector error!");
 				ABORT(fs, FR_INT_ERR);
 			}
 			sect += csect;
@@ -4016,7 +4022,7 @@ FRESULT f_write (
 					cc = fs->csize - csect;
 				}
 				if (disk_write(fs->pdrv, wbuff, sect, cc) != RES_OK) {
-					EFSPRINTF("Error: Write - Low level disk I/O!");
+					EFSPRINTF("Write - Low level disk I/O!");
 					ABORT(fs, FR_DISK_ERR);
 				}
 #if FF_FS_MINIMIZE <= 2
@@ -4044,7 +4050,7 @@ FRESULT f_write (
 			if (fp->sect != sect && 		/* Fill sector cache with file data */
 				fp->fptr < fp->obj.objsize &&
 				disk_read(fs->pdrv, fp->buf, sect, 1) != RES_OK) {
-					EFSPRINTF("Error: Read - Low level disk I/O!\n(Could not fill sector cache with file data)");
+					EFSPRINTF("Read - Low level disk I/O!\n(Could not fill sector cache with file data)");
 					ABORT(fs, FR_DISK_ERR);
 			}
 #endif
--- a/ipl/ffconf.h
+++ b/ipl/ffconf.h
@@ -50,7 +50,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. */

--- a/ipl/gfx.c
+++ b/ipl/gfx.c
@@ -15,6 +15,7 @@
 */

 #include <stdarg.h>
+#include <string.h>
 #include "gfx.h"

 static const u8 _gfx_font[] = {
@@ -76,7 +77,12 @@ void gfx_init_ctxt(gfx_ctxt_t *ctxt, u32 *fb, u32 width, u32 height, u32 stride)
 	ctxt->stride = stride;
 }

-void gfx_clear(gfx_ctxt_t *ctxt, u32 color)
+void gfx_clear_grey(gfx_ctxt_t *ctxt, u8 color)
+{
+	memset(ctxt->fb, color, 0x400000);
+}
+
+void gfx_clear_color(gfx_ctxt_t *ctxt, u32 color)
 {
 	for (u32 i = 0; i < ctxt->height * ctxt->stride; i++)
 		ctxt->fb[i] = color;
@@ -265,7 +271,7 @@ void gfx_hexdump(gfx_con_t *con, u32 base, const u8 *buf, u32 len)
 				for(u32 j = 0; j < 0x10; j++)
 				{
 					u8 c = buf[i - 0x10 + j];
-					if(c >= 32 && c < 128)
+					if(c >= 32 && c <= 126)
 						gfx_putc(con, c);
 					else
 						gfx_putc(con, '.');
@@ -275,6 +281,27 @@ void gfx_hexdump(gfx_con_t *con, u32 base, const u8 *buf, u32 len)
 			gfx_printf(con, "%08x: ", base + i);
 		}
 		gfx_printf(con, "%02x ", buf[i]);
+		if (i == len - 1)
+		{
+			int ln = len % 0x10 != 0;
+			u32 k = 0x10 - 1;
+			if (ln)
+			{
+				k = (len & 0xF) - 1;
+				for (u32 j = 0; j < 0x10 - k; j++)
+					gfx_puts(con, "   ");
+			}
+			gfx_puts(con, "| ");
+			for(u32 j = 0; j < (ln ? k : k + 1); j++)
+			{
+				u8 c = buf[i - k + j];
+				if(c >= 32 && c <= 126)
+					gfx_putc(con, c);
+				else
+					gfx_putc(con, '.');
+			}
+			gfx_putc(con, '\n');
+		}
 	}
 	gfx_putc(con, '\n');
 }
@@ -293,8 +320,8 @@ void gfx_set_pixel(gfx_ctxt_t *ctxt, u32 x, u32 y, u32 color)

 void gfx_line(gfx_ctxt_t *ctxt, int x0, int y0, int x1, int y1, u32 color)
 {
-	int dx = abs(x1-x0), sx = x0 < x1 ? 1 : -1;
-	int dy = abs(y1-y0), sy = y0 < y1 ? 1 : -1; 
+	int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
+	int dy = abs(y1 - y0), sy = y0 < y1 ? 1 : -1; 
 	int err = (dx > dy ? dx : -dy) / 2, e2;

 	while (1)
@@ -303,20 +330,15 @@ void gfx_line(gfx_ctxt_t *ctxt, int x0, int y0, int x1, int y1, u32 color)
 		if (x0 == x1 && y0 == y1)
 			break;
 		e2 = err;
-		if (e2 >-dx) { err -= dy; x0 += sx; }
-		if (e2 < dy) { err += dx; y0 += sy; }
-	}
-}
-
-void gfx_set_logo(gfx_ctxt_t *ctxt, const u8 *buf)
-{
-	u32 pos = 0;
-	for (u32 y = 1180; y < 1256; y++)
-	{
-		for (u32 x = 538; x < 696; x++)
+		if (e2 >-dx)
 		{
-			ctxt->fb[x + y*ctxt->stride] = (0xFF << 24) | buf[pos] | (buf[pos + 1] << 8) | (buf[pos + 2] << 16);
-			pos+=3;
+			err -= dy;
+			x0 += sx;
+		}
+		if (e2 < dy)
+		{
+			err += dx;
+			y0 += sy;
 		}
 	}
 }
--- a/ipl/gfx.h
+++ b/ipl/gfx.h
@@ -38,7 +38,8 @@ typedef struct _gfx_con_t
 } gfx_con_t;

 void gfx_init_ctxt(gfx_ctxt_t *ctxt, u32 *fb, u32 width, u32 height, u32 stride);
-void gfx_clear(gfx_ctxt_t *ctxt, u32 color);
+void gfx_clear_grey(gfx_ctxt_t *ctxt, u8 color);
+void gfx_clear_color(gfx_ctxt_t *ctxt, u32 color);
 void gfx_con_init(gfx_con_t *con, gfx_ctxt_t *ctxt);
 void gfx_con_setcol(gfx_con_t *con, u32 fgcol, int fillbg, u32 bgcol);
 void gfx_con_getpos(gfx_con_t *con, u32 *x, u32 *y);
@@ -50,6 +51,5 @@ void gfx_hexdump(gfx_con_t *con, u32 base, const u8 *buf, u32 len);

 void gfx_set_pixel(gfx_ctxt_t *ctxt, u32 x, u32 y, u32 color);
 void gfx_line(gfx_ctxt_t *ctxt, int x0, int y0, int x1, int y1, u32 color);
-void gfx_set_logo(gfx_ctxt_t *ctxt, const u8 *buf);

 #endif
--- a/ipl/gpio.c
+++ b/ipl/gpio.c
@@ -90,5 +90,5 @@ void gpio_write(u32 port, u32 pins, int high)

 int gpio_read(u32 port, u32 pins)
 {
-	return GPIO(_gpio_in[port]) & pins ? 1 : 0;
+	return (GPIO(_gpio_in[port]) & pins) ? 1 : 0;
 }
--- a/ipl/heap.c
+++ b/ipl/heap.c
@@ -97,6 +97,7 @@ static void _heap_free(heap_t *heap, u32 addr)
 	while (node)
 	{
 		if (!node->used)
+		{
 			if (node->prev && !node->prev->used)
 			{
 				node->prev->size += node->size + sizeof(hnode_t);
@@ -104,6 +105,7 @@ static void _heap_free(heap_t *heap, u32 addr)
 				if (node->next)
 					node->next->prev = node->prev;
 			}
+		}
 		node = node->next;
 	}
 }
@@ -129,5 +131,6 @@ void *calloc(u32 num, u32 size)

 void free(void *buf)
 {
-	_heap_free(&_heap, (u32)buf);
+	if (buf != NULL)
+		_heap_free(&_heap, (u32)buf);
 }
--- a/ipl/hos.c
+++ b/ipl/hos.c
@@ -31,24 +31,52 @@
 #include "pkg1.h"
 #include "pkg2.h"
 #include "ff.h"
+#include "di.h"

 #include "gfx.h"
 extern gfx_ctxt_t gfx_ctxt;
 extern gfx_con_t gfx_con;
-#define DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)
-//#define DPRINTF(...)
+//#define DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)
+#define DPRINTF(...)

-enum KB_FIRMWARE_VERSION {
-	KB_FIRMWARE_VERSION_100_200 = 0,
-	KB_FIRMWARE_VERSION_300 = 1,
-	KB_FIRMWARE_VERSION_301 = 2,
-	KB_FIRMWARE_VERSION_400 = 3,
-	KB_FIRMWARE_VERSION_500 = 4,
-	KB_FIRMWARE_VERSION_MAX
-};
+typedef struct _launch_ctxt_t
+{
+	void *keyblob;

-#define NUM_KEYBLOB_KEYS 5
-static const u8 keyblob_keyseeds[NUM_KEYBLOB_KEYS][0x10] = {
+	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;
+
+	void *kernel;
+	u32 kernel_size;
+	link_t kip1_list;
+
+	int svcperm;
+	int debugmode;
+} launch_ctxt_t;
+
+typedef struct _merge_kip_t
+{
+	void *kip1;
+	link_t link;
+} merge_kip_t;
+
+#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_MAX KB_FIRMWARE_VERSION_500
+
+static const u8 keyblob_keyseeds[][0x10] = {
 	{ 0xDF, 0x20, 0x6F, 0x59, 0x44, 0x54, 0xEF, 0xDC, 0x70, 0x74, 0x48, 0x3B, 0x0D, 0xED, 0x9F, 0xD3 }, //1.0.0
 	{ 0x0C, 0x25, 0x61, 0x5D, 0x68, 0x4C, 0xEB, 0x42, 0x1C, 0x23, 0x79, 0xEA, 0x82, 0x25, 0x12, 0xAC }, //3.0.0
 	{ 0x33, 0x76, 0x85, 0xEE, 0x88, 0x4A, 0xAE, 0x0A, 0xC2, 0x8A, 0xFD, 0x7D, 0x63, 0xC0, 0x43, 0x3B }, //3.0.1
@@ -68,10 +96,10 @@ static const u8 console_keyseed[0x10] =
 static const u8 key8_keyseed[] =
 	{ 0xFB, 0x8B, 0x6A, 0x9C, 0x79, 0x00, 0xC8, 0x49, 0xEF, 0xD2, 0x4D, 0x85, 0x4D, 0x30, 0xA0, 0xC7 };

-static const u8 master_keyseed_4xx[0x10] = 
+static const u8 master_keyseed_4xx_5xx[0x10] = 
 	{ 0x2D, 0xC1, 0xF4, 0x8D, 0xF3, 0x5B, 0x69, 0x33, 0x42, 0x10, 0xAC, 0x65, 0xDA, 0x90, 0x46, 0x66 };

-static const u8 console_keyseed_4xx[0x10] = 
+static const u8 console_keyseed_4xx_5xx[0x10] = 
 	{ 0x0C, 0x91, 0x09, 0xDB, 0x93, 0x93, 0x07, 0x81, 0x07, 0x3C, 0xC4, 0x16, 0x22, 0x7C, 0x6C, 0x28 };


@@ -102,28 +130,30 @@ static void _se_lock()
 	gfx_hexdump(&gfx_con, SE_BASE, (void *)SE_BASE, 0x400);*/
 }

-// <-- key derivation algorithm
 int keygen(u8 *keyblob, u32 kb, void *tsec_fw)
 {
 	u8 tmp[0x10];

-	se_key_acc_ctrl(0x0D, 0x15);
-	se_key_acc_ctrl(0x0E, 0x15);
+	if (kb > KB_FIRMWARE_VERSION_MAX)
+		return 0;
+
+	se_key_acc_ctrl(13, 0x15);
+	se_key_acc_ctrl(14, 0x15);

 	//Get TSEC key.
 	if (tsec_query(tmp, 1, tsec_fw) < 0)
 		return 0;

-	se_aes_key_set(0x0D, tmp, 0x10);
+	se_aes_key_set(13, tmp, 0x10);

 	//Derive keyblob keys from TSEC+SBK.
-	se_aes_crypt_block_ecb(0x0D, 0x00, tmp, keyblob_keyseeds[0]);
-	se_aes_unwrap_key(0x0F, 0x0E, tmp);
-	se_aes_crypt_block_ecb(0xD, 0x00, tmp, keyblob_keyseeds[kb]);
-	se_aes_unwrap_key(0x0D, 0x0E, tmp);
+	se_aes_crypt_block_ecb(13, 0, tmp, keyblob_keyseeds[0]);
+	se_aes_unwrap_key(15, 14, tmp);
+	se_aes_crypt_block_ecb(13, 0, tmp, keyblob_keyseeds[kb]);
+	se_aes_unwrap_key(13, 14, tmp);

-	// Clear SBK
-	se_aes_key_clear(0x0E);
+	//Clear SBK.
+	se_aes_key_clear(14);

 	//TODO: verify keyblob CMAC.
 	//se_aes_unwrap_key(11, 13, cmac_keyseed);
@@ -131,72 +161,46 @@ int keygen(u8 *keyblob, u32 kb, void *tsec_fw)
 	//if (!memcmp(keyblob, tmp, 0x10))
 	//	return 0;

-	se_aes_crypt_block_ecb(0x0D, 0, tmp, cmac_keyseed);
-	se_aes_unwrap_key(0x0B, 0x0D, cmac_keyseed);
+	se_aes_crypt_block_ecb(13, 0, tmp, cmac_keyseed);
+	se_aes_unwrap_key(11, 13, cmac_keyseed);

 	//Decrypt keyblob and set keyslots.
-	se_aes_crypt_ctr(0x0D, keyblob + 0x20, 0x90, keyblob + 0x20, 0x90, keyblob + 0x10);
-	se_aes_key_set(0x0B, keyblob + 0x20 + 0x80, 0x10); // package1 key
-	se_aes_key_set(0x0C, keyblob + 0x20, 0x10);
-	se_aes_key_set(0x0D, keyblob + 0x20, 0x10);
+	se_aes_crypt_ctr(13, keyblob + 0x20, 0x90, keyblob + 0x20, 0x90, keyblob + 0x10);
+	se_aes_key_set(11, keyblob + 0x20 + 0x80, 0x10); //Package1 key.
+	se_aes_key_set(12, keyblob + 0x20, 0x10);
+	se_aes_key_set(13, keyblob + 0x20, 0x10);

-	se_aes_crypt_block_ecb(0x0C, 0, tmp, master_keyseed_retail);
+	se_aes_crypt_block_ecb(12, 0, tmp, master_keyseed_retail);

-	switch (kb) {
+	switch (kb)
+	{
 		case KB_FIRMWARE_VERSION_100_200:
 		case KB_FIRMWARE_VERSION_300:
-			se_aes_unwrap_key(0x0D, 0x0F, console_keyseed);
-			se_aes_unwrap_key(0x0C, 0x0C, master_keyseed_retail);
+		case KB_FIRMWARE_VERSION_301:
+			se_aes_unwrap_key(13, 15, console_keyseed);
+			se_aes_unwrap_key(12, 12, master_keyseed_retail);
 		break;
-
 		case KB_FIRMWARE_VERSION_400:
-			se_aes_unwrap_key(0x0D, 0x0F, console_keyseed_4xx);
-			se_aes_unwrap_key(0x0F, 0x0F, console_keyseed);
-			se_aes_unwrap_key(0x0E, 0x0C, master_keyseed_4xx);
-			se_aes_unwrap_key(0x0C, 0x0C, master_keyseed_retail);
+			se_aes_unwrap_key(13, 15, console_keyseed_4xx_5xx);
+			se_aes_unwrap_key(15, 15, console_keyseed);
+			se_aes_unwrap_key(14, 12, master_keyseed_4xx_5xx);
+			se_aes_unwrap_key(12, 12, master_keyseed_retail);
 		break;
-
 		case KB_FIRMWARE_VERSION_500:
-		default:
-			se_aes_unwrap_key(0x0A, 0x0F, console_keyseed_4xx);
-			se_aes_unwrap_key(0x0F, 0x0F, console_keyseed);
-			se_aes_unwrap_key(0x0E, 0x0C, master_keyseed_4xx);
-			se_aes_unwrap_key(0x0C, 0x0C, master_keyseed_retail);
+			se_aes_unwrap_key(10, 15, console_keyseed_4xx_5xx);
+			se_aes_unwrap_key(15, 15, console_keyseed);
+			se_aes_unwrap_key(14, 12, master_keyseed_4xx_5xx);
+			se_aes_unwrap_key(12, 12, master_keyseed_retail);
 		break;
 	}

-	// Package2 key 
-	se_key_acc_ctrl(0x08, 0x15);
-	se_aes_unwrap_key(0x08, 0x0C, key8_keyseed);
+	//Package2 key.
+	se_key_acc_ctrl(8, 0x15);
+	se_aes_unwrap_key(8, 12, key8_keyseed);
+
+	return 1;
 }

-
-typedef struct _launch_ctxt_t
-{
-	void *keyblob;
-
-	void *pkg1;
-	const pkg1_id_t *pkg1_id;
-
-	void *warmboot;
-	u32 warmboot_size;
-	void *secmon;
-	u32 secmon_size;
-
-	void *pkg2;
-	u32 pkg2_size;
-
-	void *kernel;
-	u32 kernel_size;
-	link_t kip1_list;
-} launch_ctxt_t;
-
-typedef struct _merge_kip_t
-{
-	void *kip1;
-	link_t link;
-} merge_kip_t;
-
 static int _read_emmc_pkg1(launch_ctxt_t *ctxt)
 {
 	int res = 0;
@@ -212,10 +216,10 @@ static int _read_emmc_pkg1(launch_ctxt_t *ctxt)
 	ctxt->pkg1_id = pkg1_identify(ctxt->pkg1);
 	if (!ctxt->pkg1_id)
 	{
-		DPRINTF("%kCould not identify package 1 version (= '%s').%k\n", 0xFF0000FF, (char *)ctxt->pkg1 + 0x10, 0xFFFFFFFF);
+		gfx_printf(&gfx_con, "%kCould not identify package1 version (= '%s').%k\n", 0xFF0000FF, (char *)ctxt->pkg1 + 0x10, 0xFFFFFFFF);
 		goto out;
 	}
-	DPRINTF("Identified package1 ('%s'), keyblob version %d\n", (char *)(ctxt->pkg1 + 0x10), ctxt->pkg1_id->kb);
+	gfx_printf(&gfx_con, "Identified package1 ('%s'), Keyblob version %d\n\n", (char *)(ctxt->pkg1 + 0x10), ctxt->pkg1_id->kb);

 	//Read the correct keyblob.
 	ctxt->keyblob = (u8 *)malloc(NX_EMMC_BLOCKSIZE);
@@ -240,7 +244,7 @@ static int _read_emmc_pkg2(launch_ctxt_t *ctxt)
 	//Parse eMMC GPT.
 	LIST_INIT(gpt);
 	nx_emmc_gpt_parse(&gpt, &storage);
-DPRINTF("parsed GPT\n");
+	DPRINTF("Parsed GPT\n");
 	//Find package2 partition.
 	emmc_part_t *pkg2_part = nx_emmc_part_find(&gpt, "BCPKG2-1-Normal-Main");
 	if (!pkg2_part)
@@ -253,10 +257,10 @@ DPRINTF("parsed GPT\n");
 	u32 *hdr = (u32 *)(tmp + 0x100);
 	u32 pkg2_size = hdr[0] ^ hdr[2] ^ hdr[3];
 	free(tmp);
-DPRINTF("pkg2 size on emmc is %08X\n", pkg2_size);
+	DPRINTF("pkg2 size on emmc is %08X\n", pkg2_size);
 	//Read in package2.
 	u32 pkg2_size_aligned = ALIGN(pkg2_size, NX_EMMC_BLOCKSIZE);
-DPRINTF("pkg2 size aligned is %08X\n", pkg2_size_aligned);
+	DPRINTF("pkg2 size aligned is %08X\n", pkg2_size_aligned);
 	ctxt->pkg2 = malloc(pkg2_size_aligned);
 	ctxt->pkg2_size = pkg2_size;
 	nx_emmc_part_read(&storage, pkg2_part, 0x4000 / NX_EMMC_BLOCKSIZE, 
@@ -314,12 +318,32 @@ static int _config_kip1(launch_ctxt_t *ctxt, const char *value)
 	merge_kip_t *mkip1 = (merge_kip_t *)malloc(sizeof(merge_kip_t));
 	mkip1->kip1 = malloc(f_size(&fp));
 	f_read(&fp, mkip1->kip1, f_size(&fp), NULL);
-DPRINTF("loaded kip from SD (size %08X)\n", f_size(&fp));
+	DPRINTF("Loaded kip1 from SD (size %08X)\n", f_size(&fp));
 	f_close(&fp);
 	list_append(&ctxt->kip1_list, &mkip1->link);
 	return 1;
 }

+static int _config_svcperm(launch_ctxt_t *ctxt, const char *value)
+{
+	if (*value == '1')
+	{
+		DPRINTF("Disabled SVC verification\n");
+		ctxt->svcperm = 1;
+	}
+	return 1;
+}
+
+static int _config_debugmode(launch_ctxt_t *ctxt, const char *value)
+{
+	if (*value == '1')
+	{
+		DPRINTF("Enabled Debug mode\n");
+		ctxt->debugmode = 1;
+	}
+	return 1;
+}
+
 typedef struct _cfg_handler_t
 {
 	const char *key;
@@ -331,6 +355,8 @@ static const cfg_handler_t _config_handlers[] = {
 	{ "secmon", _config_secmon },
 	{ "kernel", _config_kernel },
 	{ "kip1", _config_kip1 },
+	{ "fullsvcperm", _config_svcperm },
+	{ "debugmode", _config_debugmode },
 	{ NULL, NULL },
 };

@@ -346,124 +372,153 @@ static int _config(launch_ctxt_t *ctxt, ini_sec_t *cfg)

 int hos_launch(ini_sec_t *cfg)
 {
+	int bootStateDramPkg2;
+	int bootStatePkg2Continue;
+	int end_di = 0;
 	launch_ctxt_t ctxt;
+
 	memset(&ctxt, 0, sizeof(launch_ctxt_t));
 	list_init(&ctxt.kip1_list);

+	gfx_clear_grey(&gfx_ctxt, 0x1B);
+	gfx_con_setpos(&gfx_con, 0, 0);
+
+	//Try to parse config if present.
 	if (cfg && !_config(&ctxt, cfg))
 		return 0;

+	gfx_printf(&gfx_con, "Initializing...\n\n");
+
 	//Read package1 and the correct keyblob.
 	if (!_read_emmc_pkg1(&ctxt))
 		return 0;

-	//XXX: remove this once we support 3+.
-	//if (ctxt.pkg1_id->kb > 0)
-	//	return 0;
+	gfx_printf(&gfx_con, "Loaded package1 and keyblob\n");

-DPRINTF("loaded pkg1 and keyblob\n");
 	//Generate keys.
 	keygen(ctxt.keyblob, ctxt.pkg1_id->kb, (u8 *)ctxt.pkg1 + ctxt.pkg1_id->tsec_off);
-DPRINTF("generated keys\n");
+	DPRINTF("Generated keys\n");
+
 	//Decrypt and unpack package1 if we require parts of it.
 	if (!ctxt.warmboot || !ctxt.secmon)
 	{
 		pkg1_decrypt(ctxt.pkg1_id, ctxt.pkg1);
-		pkg1_unpack((void *)0x8000D000, (void *)ctxt.pkg1_id->secmon_base, ctxt.pkg1_id, ctxt.pkg1);
-		//gfx_hexdump(&gfx_con, 0x8000D000, (void *)0x8000D000, 0x100);
-		//gfx_hexdump(&gfx_con, ctxt.pkg1_id->secmon_base, (void *)ctxt.pkg1_id->secmon_base, 0x100);
-DPRINTF("decrypted and unpacked pkg1\n");
+		pkg1_unpack((void *)ctxt.pkg1_id->warmboot_base, (void *)ctxt.pkg1_id->secmon_base, NULL, ctxt.pkg1_id, ctxt.pkg1);
+		gfx_printf(&gfx_con, "Decrypted and unpacked package1\n");
 	}
+
 	//Replace 'warmboot.bin' if requested.
 	if (ctxt.warmboot)
-		memcpy((void *)0x8000D000, ctxt.warmboot, ctxt.warmboot_size);
-	//Set warmboot address in PMC.
-	PMC(APBDEV_PMC_SCRATCH1) = 0x8000D000;
+		memcpy((void *)ctxt.pkg1_id->warmboot_base, ctxt.warmboot, ctxt.warmboot_size);
+	//Set warmboot address in PMC if required.
+	if (ctxt.pkg1_id->set_warmboot)
+		PMC(APBDEV_PMC_SCRATCH1) = ctxt.pkg1_id->warmboot_base;
+
 	//Replace 'SecureMonitor' if requested.
-	if (ctxt.secmon) {
+	if (ctxt.secmon)
 		memcpy((void *)ctxt.pkg1_id->secmon_base, ctxt.secmon, ctxt.secmon_size);
-	}
 	else
 	{
-		//Else we patch it to allow for an unsigned package2.
+		//Else we patch it to allow for an unsigned package2 and patched kernel.
 		patch_t *secmon_patchset = ctxt.pkg1_id->secmon_patchset;
-		
-		if (secmon_patchset != NULL) {
-			for (u32 i = 0; secmon_patchset[i].off != 0xFFFFFFFF; i++)
-				*(vu32 *)(ctxt.pkg1_id->secmon_base + secmon_patchset[i].off) = secmon_patchset[i].val;
+		gfx_printf(&gfx_con, "%kPatching Security Monitor%k\n", 0xFF00BAFF, 0xFFCCCCCC);
+		for (u32 i = 0; secmon_patchset[i].off != 0xFFFFFFFF; i++)
+			*(vu32 *)(ctxt.pkg1_id->secmon_base + secmon_patchset[i].off) = secmon_patchset[i].val;
+	}

-			DPRINTF("loaded warmboot.bin and secmon\n");
-			
-			//Read package2.
-			if (!_read_emmc_pkg2(&ctxt))
-				return 0;
+	gfx_printf(&gfx_con, "Loaded warmboot.bin and secmon\n");

-			DPRINTF("read pkg2\n");
-			//Decrypt package2 and parse KIP1 blobs in INI1 section.
-			pkg2_hdr_t *pkg2_hdr = pkg2_decrypt(ctxt.pkg2);
+	//Read package2.
+	if (!_read_emmc_pkg2(&ctxt))
+		return 0;

-			LIST_INIT(kip1_info);
-			pkg2_parse_kips(&kip1_info, pkg2_hdr);
+	gfx_printf(&gfx_con, "Read package2\n");

-			DPRINTF("parsed ini1\n");
-			
-			//Use the kernel included in package2 in case we didn't load one already.
-			if (!ctxt.kernel)
+	//Decrypt package2 and parse KIP1 blobs in INI1 section.
+	pkg2_hdr_t *pkg2_hdr = pkg2_decrypt(ctxt.pkg2);
+
+	LIST_INIT(kip1_info);
+	pkg2_parse_kips(&kip1_info, pkg2_hdr);
+
+	gfx_printf(&gfx_con, "Parsed ini1\n");
+
+	//Use the kernel included in package2 in case we didn't load one already.
+	if (!ctxt.kernel)
+	{
+		ctxt.kernel = pkg2_hdr->data;
+		ctxt.kernel_size = pkg2_hdr->sec_size[PKG2_SEC_KERNEL];
+
+		if (ctxt.svcperm || ctxt.debugmode)
+		{
+			u32 kernel_crc32 = crc32c(ctxt.kernel, ctxt.kernel_size);
+			ctxt.pkg2_kernel_id = pkg2_identify(kernel_crc32);
+
+			//In case a kernel patch option is set; allows to disable SVC verification or/and enable debug mode.
+			patch_t *kernel_patchset = ctxt.pkg2_kernel_id->kernel_patchset;
+			if (kernel_patchset != NULL)
 			{
-				ctxt.kernel = pkg2_hdr->data;
-				ctxt.kernel_size = pkg2_hdr->sec_size[PKG2_SEC_KERNEL];
+				gfx_printf(&gfx_con, "%kPatching kernel%k\n", 0xFF00BAFF, 0xFFCCCCCC);
+				//TODO: this is a bit ugly, perhaps attach a 'key' to the patchset and pass it via ini.
+				if (ctxt.svcperm && kernel_patchset[0].off != 0xFFFFFFFF)
+					*(vu32 *)(ctxt.kernel + kernel_patchset[0].off) = kernel_patchset[0].val;
+				if (ctxt.debugmode && kernel_patchset[1].off != 0xFFFFFFFF)
+					*(vu32 *)(ctxt.kernel + kernel_patchset[1].off) = kernel_patchset[1].val;
 			}
-
-			//Merge extra KIP1s into loaded ones.
-			LIST_FOREACH_ENTRY(merge_kip_t, mki, &ctxt.kip1_list, link)
-				pkg2_merge_kip(&kip1_info, (pkg2_kip1_t *)mki->kip1);
-
-			//Rebuild and encrypt package2.
-			pkg2_build_encrypt((void *)0xA9800000, ctxt.kernel, ctxt.kernel_size, &kip1_info);
-			DPRINTF("rebuilt pkg2\n");
-		} else {
-			//Read package2.
-			if (!_read_emmc_pkg2(&ctxt))
-				return 0;
-
-			DPRINTF("read pkg2\n");
-			memcpy((void *)0xA9800000, ctxt.pkg2, ctxt.pkg2_size);
 		}
 	}

+	//Merge extra KIP1s into loaded ones.
+	gfx_printf(&gfx_con, "%kPatching kernel initial processes%k\n", 0xFF00BAFF, 0xFFCCCCCC);
+	LIST_FOREACH_ENTRY(merge_kip_t, mki, &ctxt.kip1_list, link)
+		pkg2_merge_kip(&kip1_info, (pkg2_kip1_t *)mki->kip1);
+
+	//Rebuild and encrypt package2.
+	pkg2_build_encrypt((void *)0xA9800000, ctxt.kernel, ctxt.kernel_size, &kip1_info);
+	gfx_printf(&gfx_con, "Rebuilt and loaded package2\n");
+
+	//Unmount SD card.
+	f_mount(NULL, "", 1);
+
+	gfx_printf(&gfx_con, "\n%kBooting...%k\n", 0xFF00FF96, 0xFFCCCCCC);
+
 	se_aes_key_clear(8);
 	se_aes_key_clear(11);
-	
-	switch (ctxt.pkg1_id->kb) {
-		case KB_FIRMWARE_VERSION_100_200:
-		case KB_FIRMWARE_VERSION_300:
-		case KB_FIRMWARE_VERSION_301:
-			se_key_acc_ctrl(12, 0xFF);
-			se_key_acc_ctrl(13, 0xFF);
+
+	switch (ctxt.pkg1_id->kb)
+	{
+	case KB_FIRMWARE_VERSION_100_200:
+	case KB_FIRMWARE_VERSION_300:
+	case KB_FIRMWARE_VERSION_301:
+		se_key_acc_ctrl(12, 0xFF);
+		se_key_acc_ctrl(13, 0xFF);
+		bootStateDramPkg2 = 2;
+		bootStatePkg2Continue = 3;
+		end_di = 1;
 		break;
-		default:
-		case KB_FIRMWARE_VERSION_400:
-		case KB_FIRMWARE_VERSION_500:
-			se_key_acc_ctrl(12, 0xFF);
-			se_key_acc_ctrl(15, 0xFF);
+	default:
+	case KB_FIRMWARE_VERSION_400:
+	case KB_FIRMWARE_VERSION_500:
+		se_key_acc_ctrl(12, 0xFF);
+		se_key_acc_ctrl(15, 0xFF);
+		bootStateDramPkg2 = 2;
+		bootStatePkg2Continue = 4;
 		break;
 	}

-	//Clear 'BootConfig'.
+	//TODO: Don't Clear 'BootConfig' for retail >1.0.0.
 	memset((void *)0x4003D000, 0, 0x3000);

-	//pkg2_decrypt((void *)0xA9800000);
-	//sleep(10000);
-	//btn_wait();
-	//return 0;
-
 	//Lock SE before starting 'SecureMonitor'.
 	_se_lock();

+	//< 4.0.0 Signals. 0: Nothing ready, 1: BCT ready, 2: DRAM and pkg2 ready, 3: Continue boot
+	//>=4.0.0 Signals. 0: Nothing ready, 1: BCT ready, 2: DRAM ready, 4: pkg2 ready and continue boot
 	vu32 *mb_in = (vu32 *)0x40002EF8;
+	//Non-zero: Secmon ready
 	vu32 *mb_out = (vu32 *)0x40002EFC;

-	*mb_in = 0;
+	//Start from DRAM ready signal
+	*mb_in = bootStateDramPkg2;
 	*mb_out = 0;

 	//Wait for secmon to get ready.
@@ -471,16 +526,7 @@ DPRINTF("decrypted and unpacked pkg1\n");
 	while (!*mb_out)
 		sleep(1);

-	//Signal 'BootConfig'.
-	*mb_in = 1;
-	sleep(100);
-
-	//Signal package2 available.
-	*mb_in = 2;
-	sleep(100);
-	*mb_in = 3;
-	sleep(100);
-
+	//TODO: pkg1.1 locks PMC scratches, we can do that too at some point.
 	/*PMC(0x4) = 0x7FFFF3;
 	PMC(0x2C4) = 0xFFFFFFFF;
 	PMC(0x2D8) = 0xFFAFFFFF;
@@ -490,12 +536,12 @@ DPRINTF("decrypted and unpacked pkg1\n");
 	PMC(0x5BC) = 0xFFFFFFFF;
 	PMC(0x5C0) = 0xFFAAFFFF;*/

-	//TODO: Cleanup.
-	//display_end();
+	//Disable display.
+	if (end_di)
+		display_end();

-	//Signal to continue boot.
-	*mb_in = 4;
-	sleep(100);
+	//Signal to pkg2 ready and continue boot.
+	*mb_in = bootStatePkg2Continue;

 	//Halt ourselves in waitevent state.
 	while (1)
--- a/ipl/i2c.c
+++ b/ipl/i2c.c
@@ -46,7 +46,7 @@ static int _i2c_send_pkt(u32 idx, u32 x, u8 *buf, u32 size)
 	base[0] = (2 * size - 2) | 0x2800; //Set size and send mode.
 	_i2c_wait(base); //Kick transaction.

-	base[0] = base[0] & 0xFFFFFDFF | 0x200;
+	base[0] = (base[0] & 0xFFFFFDFF) | 0x200;
 	while (base[7] & 0x100)
 		;

@@ -66,7 +66,7 @@ static int _i2c_recv_pkt(u32 idx, u8 *buf, u32 size, u32 x)
 	base[0] = (2 * size - 2) | 0x2840; //Set size and recv mode.
 	_i2c_wait(base); //Kick transaction.

-	base[0] = base[0] & 0xFFFFFDFF | 0x200;
+	base[0] = (base[0] & 0xFFFFFDFF) | 0x200;
 	while (base[7] & 0x100)
 		;

@@ -94,11 +94,11 @@ void i2c_init(u32 idx)
 			break;
 	}

-	vu32 dummy = base[0x22];
+	(vu32)base[0x22];
 	base[0x1A] = base[0x1A];
 }

-u32 i2c_send_buf_small(u32 idx, u32 x, u32 y, u8 *buf, u32 size)
+int i2c_send_buf_small(u32 idx, u32 x, u32 y, u8 *buf, u32 size)
 {
 	u8 tmp[4];

@@ -108,7 +108,7 @@ u32 i2c_send_buf_small(u32 idx, u32 x, u32 y, u8 *buf, u32 size)
 	tmp[0] = y;
 	memcpy(tmp + 1, buf, size);

-	_i2c_send_pkt(idx, x, tmp, size + 1);
+	return _i2c_send_pkt(idx, x, tmp, size + 1);
 }

 int i2c_recv_buf_small(u8 *buf, u32 size, u32 idx, u32 x, u32 y)
@@ -119,9 +119,9 @@ int i2c_recv_buf_small(u8 *buf, u32 size, u32 idx, u32 x, u32 y)
 	return res;
 }

-u32 i2c_send_byte(u32 idx, u32 x, u32 y, u8 b)
+int i2c_send_byte(u32 idx, u32 x, u32 y, u8 b)
 {
-	i2c_send_buf_small(idx, x, y, &b, 1);
+	return i2c_send_buf_small(idx, x, y, &b, 1);
 }

 u8 i2c_recv_byte(u32 idx, u32 x, u32 y)
--- a/ipl/i2c.h
+++ b/ipl/i2c.h
@@ -27,9 +27,9 @@
 #define I2C_6 5

 void i2c_init(u32 idx);
-u32 i2c_send_buf_small(u32 idx, u32 x, u32 y, u8 *buf, u32 size);
+int i2c_send_buf_small(u32 idx, u32 x, u32 y, u8 *buf, u32 size);
 int i2c_recv_buf_small(u8 *buf, u32 size, u32 idx, u32 x, u32 y);
-u32 i2c_send_byte(u32 idx, u32 x, u32 y, u8 b);
+int i2c_send_byte(u32 idx, u32 x, u32 y, u8 b);
 u8 i2c_recv_byte(u32 idx, u32 x, u32 y);

 #endif
--- a/ipl/ini.c
+++ b/ipl/ini.c
@@ -91,3 +91,51 @@ int ini_parse(link_t *dst, char *ini_path)

 	return 1;
 }
+
+void ini_free(link_t *dst)
+{
+	LIST_FOREACH_ENTRY(ini_sec_t, ini_sec, dst, link)
+	{
+		LIST_FOREACH_ENTRY(ini_kv_t, kv, &ini_sec->kvs, link)
+		{
+			free(kv->key);
+			free(kv->val);
+			free(kv);
+		}
+		free(ini_sec->name);
+		free(ini_sec);
+	}
+}
+
+ini_sec_t *ini_clone_section(ini_sec_t *cfg)
+{
+	if (cfg == NULL)
+		return NULL;
+
+	ini_sec_t *csec = (ini_sec_t *)malloc(sizeof(ini_sec_t));
+	list_init(&csec->kvs);
+
+	LIST_FOREACH_ENTRY(ini_kv_t, kv, &cfg->kvs, link)
+	{
+		ini_kv_t *kvcfg = (ini_kv_t *)malloc(sizeof(ini_kv_t));
+		kvcfg->key = _strdup(kv->key);
+		kvcfg->val = _strdup(kv->val);
+		list_append(&csec->kvs, &kvcfg->link);
+	}
+
+	return csec;
+}
+
+void ini_free_section(ini_sec_t *cfg)
+{
+	if (cfg == NULL)
+		return;
+
+	LIST_FOREACH_ENTRY(ini_kv_t, kv, &cfg->kvs, link)
+	{
+		free(kv->key);
+		free(kv->val);
+		free(kv);
+	}
+	free(cfg);
+}
--- a/ipl/ini.h
+++ b/ipl/ini.h
@@ -35,5 +35,8 @@ typedef struct _ini_sec_t
 } ini_sec_t;

 int ini_parse(link_t *dst, char *ini_path);
+void ini_free(link_t *dst);
+ini_sec_t *ini_clone_section(ini_sec_t *cfg);
+void ini_free_section(ini_sec_t *cfg);

 #endif
--- a/ipl/main.c
+++ b/ipl/main.c
@@ -69,7 +69,7 @@ int sd_mount()

 	if (!sdmmc_storage_init_sd(&sd_storage, &sd_sdmmc, SDMMC_1, SDMMC_BUS_WIDTH_4, 11))
 	{
-		EPRINTF("Failed to init SD card (make sure that it is inserted).");
+		EPRINTF("Failed to init SD card.\nMake sure that it is inserted.");
 	}
 	else
 	{
@@ -82,7 +82,7 @@ int sd_mount()
 		}
 		else
 		{
-			EPRINTFARGS("Failed to mount SD card (FatFS Error %d).\n(make sure that a FAT32/exFAT partition exists)", res);
+			EPRINTFARGS("Failed to mount SD card (FatFS Error %d).\n(make sure that a FAT type partition exists)", res);
 		}
 	}

@@ -93,9 +93,9 @@ void sd_unmount()
 {
 	if (sd_mounted)
 	{
-		gfx_puts(&gfx_con, "Unmounting SD card...\n");
 		f_mount(NULL, "", 1);
 		sdmmc_storage_end(&sd_storage);
+		sd_mounted = 0;
 	}
 }

@@ -156,17 +156,17 @@ void panic(u32 val)

 void config_oscillators()
 {
-	CLOCK(CLK_RST_CONTROLLER_SPARE_REG0) = CLOCK(CLK_RST_CONTROLLER_SPARE_REG0) & 0xFFFFFFF3 | 4;
+	CLOCK(CLK_RST_CONTROLLER_SPARE_REG0) = (CLOCK(CLK_RST_CONTROLLER_SPARE_REG0) & 0xFFFFFFF3) | 4;
 	SYSCTR0(SYSCTR0_CNTFID0) = 19200000;
 	TMR(0x14) = 0x45F;
 	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;
+	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;
-	PMC(APBDEV_PMC_TSC_MULT) = PMC(APBDEV_PMC_TSC_MULT) & 0xFFFF0000 | 0x249F; //0x249F = 19200000 * (16 / 32.768 kHz)
+	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;
@@ -247,9 +247,9 @@ void mbist_workaround()
 	CLOCK(0x554) = 0;
 	CLOCK(0xD0) &= 0x1F7FFFFF;
 	CLOCK(0x410) &= 0xFFFF3FFF;
-	CLOCK(0x148) = CLOCK(0x148) & 0x1FFFFFFF | 0x80000000;
-	CLOCK(0x180) = CLOCK(0x180) & 0x1FFFFFFF | 0x80000000;
-	CLOCK(0x6A0) = CLOCK(0x6A0) & 0x1FFFFFFF | 0x80000000;
+	CLOCK(0x148) = (CLOCK(0x148) & 0x1FFFFFFF) | 0x80000000;
+	CLOCK(0x180) = (CLOCK(0x180) & 0x1FFFFFFF) | 0x80000000;
+	CLOCK(0x6A0) = (CLOCK(0x6A0) & 0x1FFFFFFF) | 0x80000000;
 }

 void config_se_brom()
@@ -268,7 +268,6 @@ void config_se_brom()
 	// Clear the boot reason to avoid problems later
 	PMC(APBDEV_PMC_SCRATCH200) = 0x0;
 	PMC(APBDEV_PMC_RST_STATUS_0) = 0x0;
-	PMC(APBDEV_PMC_SCRATCH49_0) = 0x0;
 }

 void config_hw()
@@ -325,7 +324,7 @@ void config_hw()

 	config_pmc_scratch();

-	CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) & 0xFFFF8888 | 0x3333;
+	CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) = (CLOCK(CLK_RST_CONTROLLER_SCLK_BURST_POLICY) & 0xFFFF8888) | 0x3333;

 	mc_config_carveout();

@@ -336,10 +335,10 @@ void config_hw()

 void print_fuseinfo()
 {
-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

-	gfx_printf(&gfx_con, "%k(Unlocked) fuse cache:\n\n%k", 0xFFFF9955, 0xFFCCCCCC);
+	gfx_printf(&gfx_con, "%k(Unlocked) fuse cache:\n\n%k", 0xFFFFDD00, 0xFFCCCCCC);
 	gfx_hexdump(&gfx_con, 0x7000F900, (u8 *)0x7000F900, 0x2FC);

 	gfx_puts(&gfx_con, "\nPress POWER to dump them to SD Card.\nPress VOL to go to the menu.\n");
@@ -365,10 +364,10 @@ void print_fuseinfo()

 void print_kfuseinfo()
 {
-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

-	gfx_printf(&gfx_con, "%kKFuse contents:\n\n%k", 0xFFFF9955, 0xFFCCCCCC);
+	gfx_printf(&gfx_con, "%kKFuse contents:\n\n%k", 0xFFFFDD00, 0xFFCCCCCC);
 	u32 buf[KFUSE_NUM_WORDS];
 	if (!kfuse_read(buf))
 		EPRINTF("CRC fail.");
@@ -398,7 +397,7 @@ void print_kfuseinfo()

 void print_mmc_info()
 {
-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

 	static const u32 SECTORS_TO_MIB_COEFF  = 11;
@@ -516,7 +515,7 @@ void print_mmc_info()
 				boot_size / 1024, boot_size / 1024 / 512);
 			gfx_printf(&gfx_con, " 3: %kRPMB       %kSize: %5d KiB (LBA Sectors: 0x%07X)\n", 0xFF00FF96, 0xFFCCCCCC,
 				rpmb_size / 1024, rpmb_size / 1024 / 512);
-			gfx_printf(&gfx_con, " 0: %kGPP (USER) %kSize: %05d MiB (LBA Sectors: 0x%07X)\n\n", 0xFF00FF96, 0xFFCCCCCC,
+			gfx_printf(&gfx_con, " 0: %kGPP (USER) %kSize: %5d MiB (LBA Sectors: 0x%07X)\n\n", 0xFF00FF96, 0xFFCCCCCC,
 				storage.sec_cnt >> SECTORS_TO_MIB_COEFF, storage.sec_cnt);
 			gfx_printf(&gfx_con, "%kGPP (eMMC USER) partition table:%k\n", 0xFFFFDD00, 0xFFCCCCCC);

@@ -541,7 +540,7 @@ out:

 void print_sdcard_info()
 {
-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

 	static const u32 SECTORS_TO_MIB_COEFF  = 11;
@@ -592,7 +591,7 @@ void print_sdcard_info()

 void print_tsec_key()
 {
-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

 	sdmmc_storage_t storage;
@@ -617,7 +616,7 @@ void print_tsec_key()
 		u8 key[0x10];
 		int res = tsec_query(key, i, pkg1 + pkg1_id->tsec_off);

-		gfx_printf(&gfx_con, "%kTSEC key %d: %k", 0xFFFF9955, i, 0xFFCCCCCC);
+		gfx_printf(&gfx_con, "%kTSEC key %d: %k", 0xFFFFDD00, i, 0xFFCCCCCC);
 		if (res >= 0)
 		{
 			for (u32 i = 0; i < 0x10; i++)
@@ -658,6 +657,81 @@ void power_off()
 	i2c_send_byte(I2C_5, 0x3C, MAX77620_REG_ONOFFCNFG1, MAX77620_ONOFFCNFG1_PWR_OFF);
 }

+int dump_emmc_verify(sdmmc_storage_t *storage, u32 lba_curr, char* outFilename, u32 NUM_SECTORS_PER_ITER, emmc_part_t *part)
+{
+	FIL fp;
+	u32 prevPct = 200;
+
+	if (f_open(&fp, outFilename, FA_READ) == FR_OK)
+	{
+		u8 *bufEm = (u8 *)malloc(NX_EMMC_BLOCKSIZE * NUM_SECTORS_PER_ITER);
+		u8 *bufSd = (u8 *)malloc(NX_EMMC_BLOCKSIZE * NUM_SECTORS_PER_ITER);
+
+		u32 totalSectorsVer = (u32)(f_size(&fp) >> 9);
+		u32 lbaCurrVer = lba_curr - totalSectorsVer;
+
+		u32 pct = (u64)((u64)(lbaCurrVer - part->lba_start) * 100u) / (u64)(part->lba_end - part->lba_start);
+		tui_pbar(&gfx_con, 0, gfx_con.y, pct, 0xFF00FF96, 0xFF005515);
+
+		while (totalSectorsVer > 0)
+		{
+			u32 num = MIN(totalSectorsVer, NUM_SECTORS_PER_ITER);
+
+			if(!sdmmc_storage_read(storage, lbaCurrVer, num, bufEm))
+			{
+				EPRINTFARGS("\nFailed to read %d blocks @ LBA %08X\nfrom eMMC. Aborting..\n",
+				num, lbaCurrVer);
+
+				free(bufEm);
+				free(bufSd);
+				f_close(&fp);
+				return 1;
+			}
+			if (!(f_read(&fp, bufSd, num, NULL) == FR_OK))
+			{
+				EPRINTFARGS("\nFailed to read %d blocks from sd card.\nVerification failed..\n", num);
+
+				free(bufEm);
+				free(bufSd);
+				f_close(&fp);
+				return 1;
+			}
+
+			if(!memcmp(bufEm, bufSd, num << 9))
+			{
+				EPRINTFARGS("\nVerification failed.\nVerification failed..\n", num);
+
+				free(bufEm);
+				free(bufSd);
+				f_close(&fp);
+				return 1;
+			}
+
+			pct = (u64)((u64)(lbaCurrVer - part->lba_start) * 100u) / (u64)(part->lba_end - part->lba_start);
+			if (pct != prevPct)
+			{
+				tui_pbar(&gfx_con, 0, gfx_con.y, pct, 0xFF00FF96, 0xFF005515);
+				prevPct = pct;
+			}
+
+			lbaCurrVer += num;
+			totalSectorsVer -= num;
+		}
+		free(bufEm);
+		free(bufSd);
+		f_close(&fp);
+
+		tui_pbar(&gfx_con, 0, gfx_con.y, pct, 0xFFCCCCCC, 0xFF555555);
+
+		return 0;
+	}
+	else
+	{
+		EPRINTF("\nFile not found or could not be loaded.\nVerification failed..\n");
+		return 1;
+	}
+}
+
 int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 {
 	static const u32 FAT32_FILESIZE_LIMIT = 0xFFFFFFFF;
@@ -679,7 +753,7 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 	memcpy(partialIdxFilename, "partial.idx", 11);
 	partialIdxFilename[11] = 0;

-	gfx_printf(&gfx_con, "SD Card free space: %d MiB, Total dump size %d MiB\n",
+	gfx_printf(&gfx_con, "\nSD Card free space: %d MiB, Total backup size %d MiB\n\n",
 		sd_fs.free_clst * sd_fs.csize >> SECTORS_TO_MIB_COEFF,
 		totalSectors >> SECTORS_TO_MIB_COEFF);

@@ -694,19 +768,19 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 	{
 		isSmallSdCard = 1;

-		gfx_printf(&gfx_con, "%k\nSD card free space is smaller than dump total size.%k\n", 0xFF00BAFF, 0xFFCCCCCC);
+		gfx_printf(&gfx_con, "%k\nSD card free space is smaller than total backup size.%k\n", 0xFF00BAFF, 0xFFCCCCCC);

 		if (!maxSplitParts)
 		{
-			EPRINTF("Not enough free space for partial dumping.");
+			EPRINTF("Not enough free space for Partial Backup.");

 			return 0;
 		}
 	}
-	// Check if we continuing a previous raw eMMC dump in progress.
+	// Check if we continuing a previous raw eMMC backup in progress.
 	if (f_open(&partialIdxFp, partialIdxFilename, FA_READ) == FR_OK)
 	{
-		gfx_printf(&gfx_con, "%kFound partial dump in progress. Continuing...%k\n\n", 0xFF14FDAE, 0xFFCCCCCC);
+		gfx_printf(&gfx_con, "%kFound Partial Backup in progress. Continuing...%k\n\n", 0xFF14FDAE, 0xFFCCCCCC);

 		partialDumpInProgress = 1;
 		// Force partial dumping, even if the card is larger.
@@ -717,18 +791,18 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)

 		if (!maxSplitParts)
 		{
-			EPRINTF("Not enough free space for partial dumping.");
+			EPRINTF("Not enough free space for Partial Backup.");

 			return 0;
 		}

-		// Increase maxSplitParts to accommodate previously dumped parts
+		// Increase maxSplitParts to accommodate previously backed up parts
 		maxSplitParts += currPartIdx;
 	}
 	else if (isSmallSdCard)
-		gfx_printf(&gfx_con, "%kPartial dumping enabled (with %d MiB parts)...%k\n\n", 0xFF00BAFF, multipartSplitSize >> 20, 0xFFCCCCCC);
+		gfx_printf(&gfx_con, "%kPartial Backup enabled (with %d MiB parts)...%k\n\n", 0xFF00BAFF, multipartSplitSize >> 20, 0xFFCCCCCC);

-	// Check if filesystem is FAT32 or the free space is smaller and dump in parts
+	// Check if filesystem is FAT32 or the free space is smaller and backup in parts
 	if (((sd_fs.fs_type != FS_EXFAT) && totalSectors > (FAT32_FILESIZE_LIMIT / NX_EMMC_BLOCKSIZE)) | isSmallSdCard)
 	{
 		u32 multipartSplitSectors = multipartSplitSize / NX_EMMC_BLOCKSIZE;
@@ -749,7 +823,7 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 			else
 				outFilename[sdPathLen + 1] = 0;
 		}
-		// Continue from where we left, if partial dump in progress.
+		// Continue from where we left, if Partial Backup in progress.
 		else
 		{
 			if (numSplitParts >= 10 && currPartIdx < 10)
@@ -765,7 +839,7 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 	FIL fp;
 	if (f_open(&fp, outFilename, FA_CREATE_ALWAYS | FA_WRITE) != FR_OK)
 	{
-		EPRINTFARGS("Error creating file %s.", outFilename);
+		EPRINTFARGS("Error creating file %s.\n", outFilename);

 		return 0;
 	}
@@ -778,7 +852,7 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 	u32 prevPct = 200;
 	int retryCount = 0;

-	// Continue from where we left, if partial dump in progress.
+	// Continue from where we left, if Partial Backup in progress.
 	if (partialDumpInProgress)
 	{
 		lba_curr += currPartIdx * (multipartSplitSize / NX_EMMC_BLOCKSIZE);
@@ -793,6 +867,15 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 			memset(&fp, 0, sizeof(fp));
 			currPartIdx++;

+			// Verify part
+			if (dump_emmc_verify(storage, lba_curr, outFilename, NUM_SECTORS_PER_ITER, part))
+			{
+				EPRINTF("\nPress any key and try again...\n");
+
+				free(buf);
+				return 0;
+			}
+
 			if (numSplitParts >= 10 && currPartIdx < 10)
 			{
 				outFilename[sdPathLen] = '0';
@@ -804,7 +887,7 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 			// Always create partial.idx before next part, in case a fatal error occurs.
 			if (isSmallSdCard)
 			{
-				// Create partial dump index file
+				// Create partial backup index file
 				if (f_open(&partialIdxFp, partialIdxFilename, FA_CREATE_ALWAYS | FA_WRITE) == FR_OK)
 				{
 					f_write(&partialIdxFp, &currPartIdx, 4, NULL);
@@ -812,20 +895,20 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 				}
 				else
 				{
-					EPRINTF("\nError creating partial.idx file.");
+					EPRINTF("\nError creating partial.idx file.\n");

 					free(buf);
 					return 0;
 				}

-				// More parts to dump that do not currently fit the sd card free space or fatal error
+				// More parts to backup that do not currently fit the sd card free space or fatal error
 				if (currPartIdx >= maxSplitParts)
 				{
 					gfx_puts(&gfx_con, "\n\n1. Press any key and Power off Switch from the main menu.\n\
 						2. Move the files from SD card to free space.\n\
 						   Don\'t move the partial.idx file!\n\
 						3. Unplug and re-plug USB while pressing Vol+.\n\
-						4. Run hekate - ipl again and press Dump RAW eMMC or eMMC USER to continue\n");
+						4. Run hekate again and press Backup eMMC RAW GPP (or eMMC USER) to continue.\n");

 					free(buf);
 					return 1;
@@ -835,7 +918,7 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 			// Create next part
 			if (f_open(&fp, outFilename, FA_CREATE_ALWAYS | FA_WRITE) != FR_OK)
 			{
-				EPRINTFARGS("Error creating file %s.", outFilename);
+				EPRINTFARGS("Error creating file %s.\n", outFilename);

 				free(buf);
 				return 0;
@@ -850,11 +933,12 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 			EPRINTFARGS("Error reading %d blocks @ LBA %08X from eMMC (try %d), retrying...",
 				num, lba_curr, ++retryCount);

-			sleep(500000);
-			if (retryCount >= 10)
+			sleep(150000);
+			if (retryCount >= 3)
 			{
-				EPRINTFARGS("\nFailed to read %d blocks @ LBA %08X from eMMC. Aborting..",
+				EPRINTFARGS("\nFailed to read %d blocks @ LBA %08X from eMMC. Aborting..\n",
 				num, lba_curr);
+				EPRINTF("\nPress any key and try again...\n");

 				free(buf);
 				f_close(&fp);
@@ -865,7 +949,7 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 		if (res)
 		{
 			EPRINTFARGS("\nFatal error (%d) when writing to SD Card", res);
-			EPRINTF("\nPress any key and try again.");
+			EPRINTF("\nPress any key and try again...\n");

 			free(buf);
 			f_close(&fp);
@@ -874,7 +958,7 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 		u32 pct = (u64)((u64)(lba_curr - part->lba_start) * 100u) / (u64)(part->lba_end - part->lba_start);
 		if (pct != prevPct)
 		{
-			tui_pbar(&gfx_con, 0, gfx_con.y, pct);
+			tui_pbar(&gfx_con, 0, gfx_con.y, pct, 0xFFCCCCCC, 0xFF555555);
 			prevPct = pct;
 		}

@@ -889,18 +973,30 @@ int dump_emmc_part(char *sd_path, sdmmc_storage_t *storage, emmc_part_t *part)
 			bytesWritten = 0;
 		}
 	}
-	tui_pbar(&gfx_con, 0, gfx_con.y, 100);
+	tui_pbar(&gfx_con, 0, gfx_con.y, 100, 0xFFCCCCCC, 0xFF555555);

-out:;
+	// Backup operation ended successfully
 	free(buf);
 	f_close(&fp);
-	// Remove partial dump index file if no fatal errors occurred.
+
+	// Verify last part or single file backup
+	if (dump_emmc_verify(storage, lba_curr, outFilename, NUM_SECTORS_PER_ITER, part))
+	{
+		EPRINTF("\nPress any key and try again...\n");
+
+		free(buf);
+		return 0;
+	}
+	else
+		tui_pbar(&gfx_con, 0, gfx_con.y, 100, 0xFF00FF96, 0xFF005515);
+
+	// Remove partial backup index file if no fatal errors occurred.
 	if(isSmallSdCard)
 	{
 		f_unlink(partialIdxFilename);
-		gfx_printf(&gfx_con, "\n\nYou can now join the files and get the complete raw eMMC dump.");
+		gfx_printf(&gfx_con, "%k\n\nYou can now join the files\nand get the complete eMMC RAW GPP backup.", 0xFFCCCCCC);
 	}
-	gfx_putc(&gfx_con, '\n');
+	gfx_puts(&gfx_con, "\n\n");

 	return 1;
 }
@@ -917,14 +1013,14 @@ static void dump_emmc_selected(dumpType_t dumpType)
 {
 	int res = 0;
 	u32 timer = 0;
-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

 	if (!sd_mount())
 		goto out;

 	gfx_puts(&gfx_con, "Checking for available free space...\n\n");
-	// Get SD Card free space for partial dumping
+	// Get SD Card free space for Partial Backup
 	f_getfree("", &sd_fs.free_clst, NULL);

 	sdmmc_storage_t storage;
@@ -1000,10 +1096,10 @@ static void dump_emmc_selected(dumpType_t dumpType)
 	}

 	gfx_putc(&gfx_con, '\n');
-	gfx_printf(&gfx_con, "%kTime taken: %d seconds.%k\n", 0xFF00FF96, (get_tmr() - timer) / 1000000, 0xFFCCCCCC);
+	gfx_printf(&gfx_con, "Time taken: %d seconds.\n", (get_tmr() - timer) / 1000000);
 	sdmmc_storage_end(&storage);
 	if (res)
-		gfx_puts(&gfx_con, "\nDone. Press any key.\n");
+		gfx_printf(&gfx_con, "\n%kFinished and verified!%k\nPress any key...\n",0xFF00FF96, 0xFFCCCCCC);

 out:;
 	btn_wait();
@@ -1016,11 +1112,12 @@ void dump_emmc_rawnand() { dump_emmc_selected(DUMP_RAW); }

 void dump_package1()
 {
-	u8 * pkg1 = (u8 *)malloc(0x40000);
-	u8 * warmboot = (u8 *)malloc(0x40000);
-	u8 * secmon = (u8 *)malloc(0x40000);
+	u8 *pkg1 = (u8 *)calloc(1, 0x40000);
+	u8 *warmboot = (u8 *)calloc(1, 0x40000);
+	u8 *secmon = (u8 *)calloc(1, 0x40000);
+	u8 *loader = (u8 *)calloc(1, 0x40000);

-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

 	if (!sd_mount())
@@ -1035,7 +1132,7 @@ void dump_package1()
 	}
 	sdmmc_storage_set_mmc_partition(&storage, 1);

-	//Read package1.
+	// Read package1.
 	sdmmc_storage_read(&storage, 0x100000 / NX_EMMC_BLOCKSIZE, 0x40000 / NX_EMMC_BLOCKSIZE, pkg1);
 	const pkg1_id_t *pkg1_id = pkg1_identify(pkg1);
 	const pk11_hdr_t *hdr = (pk11_hdr_t *)(pkg1 + pkg1_id->pkg11_off + 0x20);
@@ -1045,38 +1142,51 @@ void dump_package1()
 		goto out;
 	}

-
 	// Read keyblob
 	u8 * keyblob = (u8 *)malloc(NX_EMMC_BLOCKSIZE);
 	sdmmc_storage_read(&storage, 0x180000 / NX_EMMC_BLOCKSIZE + pkg1_id->kb, 1, keyblob);

-	// decrypt
+	// Decrypt
 	keygen(keyblob, pkg1_id->kb, (u8 *)pkg1 + pkg1_id->tsec_off);
 	pkg1_decrypt(pkg1_id, pkg1);

-	pkg1_unpack(warmboot, secmon, pkg1_id, pkg1);
+	pkg1_unpack(warmboot, secmon, loader, pkg1_id, pkg1);

-	// display info
-	gfx_printf(&gfx_con, "%kSecure monitor addr: %k%08X\n", 0xFF46EAC7, 0xFFCCCCCC, pkg1_id->secmon_base);
-	gfx_printf(&gfx_con, "%kSecure monitor size: %k%08X\n", 0xFF46EAC7, 0xFFCCCCCC, hdr->sm_size);
-	gfx_printf(&gfx_con, "%kSecure monitor ofst: %k%08X\n", 0xFF46EAC7, 0xFFCCCCCC, hdr->sm_off);
+	// Display info
+	gfx_printf(&gfx_con, "%kNX Bootloader size:  %k0x%05X\n", 0xFF46EAC7, 0xFFCCCCCC, hdr->ldr_size);
+	gfx_printf(&gfx_con, "%kNX Bootloader ofst:  %k0x%05X\n\n", 0xFF46EAC7, 0xFFCCCCCC, hdr->ldr_off);

-	// dump package1
-	if (sd_save_to_file(pkg1, 0x40000, "pkg_decr.bin")) {
-		EPRINTF("\nFailed to create pkg_decr.bin");
+	gfx_printf(&gfx_con, "%kSecure monitor addr: %k0x%05X\n", 0xFF46EAC7, 0xFFCCCCCC, pkg1_id->secmon_base);
+	gfx_printf(&gfx_con, "%kSecure monitor size: %k0x%05X\n\n", 0xFF46EAC7, 0xFFCCCCCC, hdr->sm_size);
+	gfx_printf(&gfx_con, "%kSecure monitor ofst: %k0x%05X\n\n", 0xFF46EAC7, 0xFFCCCCCC, hdr->sm_off);
+
+	gfx_printf(&gfx_con, "%kWarmboot addr:       %k0x%05X\n", 0xFF46EAC7, 0xFFCCCCCC, pkg1_id->warmboot_base);
+	gfx_printf(&gfx_con, "%kWarmboot size:       %k0x%05X\n\n", 0xFF46EAC7, 0xFFCCCCCC, hdr->wb_size);
+	gfx_printf(&gfx_con, "%kWarmboot ofst:       %k0x%05X\n\n", 0xFF46EAC7, 0xFFCCCCCC, hdr->wb_off);
+
+	// Dump package1
+	if (sd_save_to_file(pkg1, 0x40000, "pkg1_decr.bin")) {
+		EPRINTF("\nFailed to create pkg1_decr.bin");
 		goto out;
 	}
-	gfx_puts(&gfx_con, "\npackage1 dumped to pkg_decr.bin\n");
+	gfx_puts(&gfx_con, "\npackage1 dumped to pkg1_decr.bin\n");

-	// dump sm
-	if (sd_save_to_file(secmon, 0x40000, "sm.bin")) {
-		EPRINTF("\nFailed to create sm.bin");
+	// Dump nxbootloader
+	if (sd_save_to_file(loader, hdr->ldr_size, "nxloader.bin")) {
+		EPRINTF("\nFailed to create nxloader.bin");
 		goto out;
 	}
-	gfx_puts(&gfx_con, "Secure Monitor dumped to sm.bin\n");
+	gfx_puts(&gfx_con, "NX Bootloader dumped to nxloader.bin\n");

-	// dump warmboot
-	if (sd_save_to_file(warmboot, 0x40000, "warmboot.bin")) {
+	// Dump secmon
+	if (sd_save_to_file(secmon, hdr->sm_size, "secmon.bin")) {
+		EPRINTF("\nFailed to create secmon.bin");
+		goto out;
+	}
+	gfx_puts(&gfx_con, "Secure Monitor dumped to secmon.bin\n");
+
+	// Dump warmboot
+	if (sd_save_to_file(warmboot, hdr->wb_size, "warmboot.bin")) {
 		EPRINTF("\nFailed to create warmboot.bin");
 		goto out;
 	}
@@ -1084,30 +1194,33 @@ void dump_package1()


 	sdmmc_storage_end(&storage);
-	gfx_puts(&gfx_con, "\nDone. Press any key.\n");
+	gfx_puts(&gfx_con, "\nDone. Press any key...\n");

 out:;
 	free(pkg1);
 	free(secmon);
 	free(warmboot);
+	free(loader);

 	btn_wait();
 }

 void launch_firmware()
 {
+	u8 max_entries = 16;
+
 	ini_sec_t *cfg_sec = NULL;
 	LIST_INIT(ini_sections);

-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

 	if (sd_mount())
 	{
 		if (ini_parse(&ini_sections, "hekate_ipl.ini"))
 		{
-			//Build configuration menu.
-			ment_t *ments = (ment_t *)malloc(sizeof(ment_t) * 16);
+			// Build configuration menu.
+			ment_t *ments = (ment_t *)malloc(sizeof(ment_t) * max_entries);
 			ments[0].type = MENT_BACK;
 			ments[0].caption = "Back";
 			u32 i = 1;
@@ -1119,6 +1232,9 @@ void launch_firmware()
 				ments[i].caption = ini_sec->name;
 				ments[i].data = ini_sec;
 				i++;
+
+				if (i > max_entries)
+					break;
 			}
 			if (i > 1)
 			{
@@ -1126,36 +1242,44 @@ void launch_firmware()
 				menu_t menu = {
 					ments, "Launch configurations", 0, 0
 				};
-				cfg_sec = (ini_sec_t *)tui_do_menu(&gfx_con, &menu);
+				cfg_sec = ini_clone_section((ini_sec_t *)tui_do_menu(&gfx_con, &menu));
 				if (!cfg_sec)
+				{
+					free(ments);
+					ini_free(&ini_sections);
 					return;
+				}
 			}
 			else
 				EPRINTF("No launch configurations found.");
 			free(ments);
+			ini_free(&ini_sections);
 		}
 		else
-			EPRINTF("Could not find or open 'hekate_ipl.ini' from SD Card!\nMake sure it exists.");
+			EPRINTF("Could not find or open 'hekate_ipl.ini'.\nMake sure it exists in SD Card!.");
 	}

 	if (!cfg_sec)
 	{
+		gfx_printf(&gfx_con, "\nUsing default launch configuration...\n");
 		sleep(3000000);
-		gfx_printf(&gfx_con, "Using default launch configuration...\n");
 	}

 	if (!hos_launch(cfg_sec))
 		EPRINTF("Failed to launch firmware.");

-	//TODO: free ini.
+	ini_free_section(cfg_sec);

-out:;
 	btn_wait();
 }

 void toggle_autorcm(){
 	sdmmc_storage_t storage;
 	sdmmc_t sdmmc;
+
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
+	gfx_con_setpos(&gfx_con, 0, 0);
+
 	if(!sdmmc_storage_init_mmc(&storage, &sdmmc, SDMMC_4, SDMMC_BUS_WIDTH_8, 4))
 	{
 		EPRINTF("Failed to init eMMC.");
@@ -1170,28 +1294,102 @@ void toggle_autorcm(){
 	{
 		sect = (0x200 + (0x4000 * i)) / NX_EMMC_BLOCKSIZE;
 		sdmmc_storage_read(&storage, sect, 1, tempbuf);
-		tempbuf[0x10] ^= 0x77; //xor by arbitrary number to corrupt
+		tempbuf[0x10] ^= 0x77; // !IMPORTANT: DO NOT CHANGE! XOR by arbitrary number to corrupt
 		sdmmc_storage_write(&storage, sect, 1, tempbuf);
 	}
 	
 	free(tempbuf);
 	sdmmc_storage_end(&storage);
 	
-	gfx_printf(&gfx_con, "%kAutoRCM mode toggled!%k\n", 0xFF00EE2C, 0xFFCCCCCC);
+	gfx_printf(&gfx_con, "%kAutoRCM mode toggled!%k\n\nPress any key...\n", 0xFF00FF96, 0xFFCCCCCC);

 out:;
 	btn_wait();
 }

+int fix_attributes(char *path, u32 *total)
+{
+	FRESULT res;
+	DIR dir;
+	u32 i = 0;
+	u32 k = 0;
+	static FILINFO fno;
+
+	// Open directory
+	res = f_opendir(&dir, path);
+	if (res == FR_OK)
+	{
+		for (;;)
+		{
+			// Read a directory item.
+			res = f_readdir(&dir, &fno);
+			// Break on error or end of dir.
+			if (res != FR_OK || fno.fname[0] == 0)
+				break;
+
+			// Set new directory
+			i = strlen(path);
+			memcpy(&path[i], "/", 1);
+			for (k = 0; k < 256; k++)
+			{
+				if (fno.fname[k] == 0)
+					break;
+			}
+			memcpy(&path[i+1], fno.fname, k + 1);
+			path[i + k + 2] = 0;
+
+			// Check if archive bit is set
+			if (fno.fattrib & AM_ARC)
+			{
+				*(u32 *)total = *(u32 *)total + 1;
+				f_chmod(path, 0, AM_ARC);
+			}
+
+			// Is it a directory?
+			if (fno.fattrib & AM_DIR)
+			{
+				// Enter the directory.
+				res = fix_attributes(path, total);
+				if (res != FR_OK)
+					break;
+				
+			}
+			// Clear file or folder path.
+			path[i] = 0;
+		}
+		f_closedir(&dir);
+	}
+
+	return res;
+}
+
+void fix_sd_attr(){
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
+	gfx_con_setpos(&gfx_con, 0, 0);
+
+	char buff[256];
+
+	u32 total = 0;
+	if (sd_mount())
+	{
+		gfx_printf(&gfx_con, "Traversing all sd card files!\nThis may take some time, please wait...\n");
+		buff[0] = '/';
+		buff[1] = 0;
+		fix_attributes(buff, &total);
+		gfx_printf(&gfx_con, "\n%kTotal archive bits cleared: %d!%k\n\nDone! Press any key...", 0xFF00FF96, total, 0xFFCCCCCC);
+	}
+	btn_wait();
+}
+
 void about()
 {
 	static const char octopus[] =
-	"hekate (c) 2018 naehrwert, st4rk\n\n"
+	"hekate (C) 2018 naehrwert, st4rk\n\n"
 	"Thanks to: %kderrek, nedwill, plutoo, shuffle2, smea, thexyz, yellows8%k\n\n"
 	"Greetings to: fincs, hexkyz, SciresM, Shiny Quagsire, WinterMute\n\n"
 	"Open source and free packages used:\n"
-	" - FatFs R0.13a (Copyright (C) 2017, ChaN)\n"
-	" - bcl-1.2.0 (Copyright (c) 2003-2006 Marcus Geelnard)\n\n"
+	" - FatFs R0.13a, (Copyright (C) 2017, ChaN)\n"
+	" - bcl-1.2.0,    (Copyright (C) 2003-2006, Marcus Geelnard)\n\n"
 	"                         %k___\n"
 	"                      .-'   `'.\n"
 	"                     /         \\\n"
@@ -1211,7 +1409,7 @@ void about()
 	"             (/`    ( (`          ) )  '-;   %k[switchbrew]%k\n"
 	"              `      '-;         (-'%k";

-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_setpos(&gfx_con, 0, 0);

 	gfx_printf(&gfx_con, octopus, 0xFFFFCC00, 0xFFCCCCCC,
@@ -1261,15 +1459,16 @@ ment_t ment_tools[] = {
 	MDEF_BACK(),
 	MDEF_CHGLINE(),
 	MDEF_CAPTION("------ Full --------", 0xFFE6B90A),
-	MDEF_HANDLER("Dump RAW eMMC", dump_emmc_rawnand),
-	MDEF_HANDLER("Dump eMMC BOOT", dump_emmc_boot),
+	MDEF_HANDLER("Backup eMMC RAW GPP", dump_emmc_rawnand),
+	MDEF_HANDLER("Backup eMMC BOOT0/1", dump_emmc_boot),
 	MDEF_CHGLINE(),
-	MDEF_CAPTION("-- GP Partitions --", 0xFFE6B90A),
-	MDEF_HANDLER("Dump eMMC SYS", dump_emmc_system),
-	MDEF_HANDLER("Dump eMMC USER", dump_emmc_user),
+	MDEF_CAPTION("-- GPP Partitions --", 0xFFE6B90A),
+	MDEF_HANDLER("Backup eMMC SYS", dump_emmc_system),
+	MDEF_HANDLER("Backup eMMC USER", dump_emmc_user),
 	MDEF_CHGLINE(),
 	MDEF_CAPTION("------ Misc -------", 0xFFE6B90A),
 	MDEF_HANDLER("Dump package1", dump_package1),
+	MDEF_HANDLER("Fix SD files attributes", fix_sd_attr),
 	MDEF_CHGLINE(),
 	MDEF_CAPTION("---- Dangerous ----", 0xFF0000FF),
 	MDEF_MENU("AutoRCM", &menu_autorcm),
@@ -1296,7 +1495,7 @@ ment_t ment_top[] = {
 };
 menu_t menu_top = {
 	ment_top,
-	"hekate - ipl (CTCaer mod v1.6)", 0, 0
+	"hekate - ipl", 0, 0
 };

 extern void pivot_stack(u32 stack_top);
@@ -1318,9 +1517,12 @@ void ipl_main()
 	//display_color_screen(0xAABBCCDD);
 	u32 *fb = display_init_framebuffer();
 	gfx_init_ctxt(&gfx_ctxt, fb, 720, 1280, 768);
-	gfx_clear(&gfx_ctxt, 0xFF1B1B1B);
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
 	gfx_con_init(&gfx_con, &gfx_ctxt);

+	// Enable backlight after initializing gfx
+	display_backlight(1);
+
 	while (1)
 		tui_do_menu(&gfx_con, &menu_top);

--- a/ipl/max7762x.c
+++ b/ipl/max7762x.c
@@ -73,8 +73,8 @@ int max77620_regulator_get_status(u32 id)
 	const max77620_regulator_t *reg = &_pmic_regulators[id];

 	if (reg->type == REGULATOR_SD)
-		return i2c_recv_byte(I2C_5, 0x3C, MAX77620_REG_STATSD) & reg->status_mask ? 0 : 1;
-	return i2c_recv_byte(I2C_5, 0x3C, reg->cfg_addr) & 8 ? 1 : 0;
+		return (i2c_recv_byte(I2C_5, 0x3C, MAX77620_REG_STATSD) & reg->status_mask) ? 0 : 1;
+	return (i2c_recv_byte(I2C_5, 0x3C, reg->cfg_addr) & 8) ? 1 : 0;
 }

 int max77620_regulator_config_fps(u32 id)
--- a/ipl/mc.c
+++ b/ipl/mc.c
@@ -102,7 +102,7 @@ void mc_config_carveout()

 void mc_enable_ahb_redirect()
 {
-	CLOCK(0x3A4) = CLOCK(0x3A4) & 0xFFF7FFFF | 0x80000;
+	CLOCK(0x3A4) = (CLOCK(0x3A4) & 0xFFF7FFFF) | 0x80000;
 	//MC(MC_IRAM_REG_CTRL) &= 0xFFFFFFFE;
 	MC(MC_IRAM_BOM) = 0x40000000;
 	MC(MC_IRAM_TOM) = 0x4003F000;
@@ -119,13 +119,13 @@ void mc_disable_ahb_redirect()

 void mc_enable()
 {
-	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) = CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) & 0x1FFFFFFF | 0x40000000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) = (CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) & 0x1FFFFFFF) | 0x40000000;
 	//Enable MIPI CAL clock.
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) & 0xFDFFFFFF | 0x2000000;
+	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;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = (CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) & 0xFFFFFFFE) | 1;
 	//Enable EMC DLL clock.
-	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) = CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) & 0xFFFFBFFF | 0x4000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) = (CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) & 0xFFFFBFFF) | 0x4000;
 	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_SET) = 0x2000001; //Clear EMC and MC reset.
 	sleep(5);

--- a/ipl/nx_emmc.c
+++ b/ipl/nx_emmc.c
@@ -37,7 +37,7 @@ void nx_emmc_gpt_parse(link_t *gpt, sdmmc_storage_t *storage)
 		//HACK
 		for (u32 i = 0; i < 36; i++)
 			part->name[i] = ent->name[i];
-		part->name[37] = 0;
+		part->name[36] = 0;

 		list_append(gpt, &part->link);
 	}
--- a/ipl/nx_emmc.h
+++ b/ipl/nx_emmc.h
@@ -59,7 +59,7 @@ typedef struct _emmc_part_t
 	u32 lba_start;
 	u32 lba_end;
 	u64 attrs;
-	u8 name[37];
+	s8 name[37];
 	link_t link;
 } emmc_part_t;

--- a/ipl/pinmux.h
+++ b/ipl/pinmux.h
@@ -41,6 +41,11 @@
 #define PINMUX_AUX_DMIC3_CLK 0xB4
 #define PINMUX_AUX_UART2_TX 0xF4
 #define PINMUX_AUX_UART3_TX 0x104
+#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
--- a/ipl/pkg1.c
+++ b/ipl/pkg1.c
@@ -16,17 +16,13 @@

 #include <string.h>
 #include "pkg1.h"
+#include "arm64.h"
 #include "se.h"

-#define _ADRP(r, o) 0x90000000 | ((((o) >> 12) & 0x3) << 29) | ((((o) >> 12) & 0x1FFFFC) << 3) | ((r) & 0x1F)
-#define _MOVZX(r, i, s) 0xD2800000 | (((s) & 0x30) << 17) | (((i) & 0xFFFF) << 5) | ((r) & 0x1F)
-#define _MOVKX(r, i, s) 0xF2800000 | (((s) & 0x30) << 17) | (((i) & 0xFFFF) << 5) | ((r) & 0x1F)
-#define _BL(a, o) 0x94000000 | (((o) - (a)) >> 2) & 0x3FFFFFF
-#define _NOP() 0xD503201F
-
+#define SM_100_ADR 0x4002B020
 PATCHSET_DEF(_secmon_1_patchset,
-	//Patch the relocator to be able to run from 0x4002D000.
-	//{ 0x1E0, _ADRP(0, 0x7C013000 - 0x4002D000) }
+	//Patch the relocator to be able to run from SM_100_ADR.
+	{ 0x1E0, _ADRP(0, 0x7C013000 - _PAGEOFF(SM_100_ADR)) },
 	//Patch package2 decryption and signature/hash checks.
 	{ 0x9F0 + 0xADC, _NOP() }, //Header signature.
 	{ 0x9F0 + 0xB8C, _NOP() }, //Version.
@@ -42,27 +38,26 @@ PATCHSET_DEF(_secmon_2_patchset,

 PATCHSET_DEF(_secmon_3_patchset,
 	//Patch package2 decryption and signature/hash checks.
-	{ 0xAC8 + 0xAB4, _NOP() },
 	{ 0xAC8 + 0xA30, _NOP() }, //Header signature.
+	{ 0xAC8 + 0xAB4, _NOP() }, //package2 structure.
 	{ 0xAC8 + 0xAC0, _NOP() }, //Version.
 	{ 0xAC8 + 0xADC, _NOP() }  //Sections SHA2.
 );

-PATCHSET_DEF(_secmon_5_patchset,
+PATCHSET_DEF(_secmon_4_patchset,
 	//Patch package2 decryption and signature/hash checks.
-	{ 0x1218 + 0x6E68, _NOP() }, //Header signature.
-	{ 0x1218 + 0x6E74, _NOP() }, //Version.
-	{ 0x1218 + 0x6FE4, _NOP() }, //Sections SHA2.
-	{ 0x1218 + 0x2DC, _NOP() }   //Unknown.
+	{ 0x2300 + 0x5D80, _NOP() }, //package2 structure.
+	{ 0x2300 + 0x5D8C, _NOP() }, //Version.
+	{ 0x2300 + 0x5EFC, _NOP() }, //Header signature.
+	{ 0xAC8 + 0xA2C, _NOP() }    //Sections SHA2.
 );

-PATCHSET_DEF(_secmon_6_patchset,
+PATCHSET_DEF(_secmon_5_patchset,
 	//Patch package2 decryption and signature/hash checks.
-	{ 0x12b0 + 0x4d0, _NOP() },
-	{ 0x12b0 + 0x4dc, _NOP() },
-	{ 0x12b0 + 0x794, _NOP() },
-	{ 0x12b0 + 0xb30, _NOP() }//,
-	//{ 0x12b0 + 0xa18 , _NOP() } // BootConfig Retail Check
+	{ 0xDA8 + 0x9D8 , _NOP() }, //package2 structure.
+	{ 0xDA8 + 0x9E4 , _NOP() }, //Version.
+	{ 0xDA8 + 0xC9C , _NOP() }, //Header signature.
+	{ 0xDA8 + 0x1038 , _NOP() } //Sections SHA2.
 );

 /*
@@ -77,13 +72,13 @@ PATCHSET_DEF(_secmon_6_patchset,
 */

 static const pkg1_id_t _pkg1_ids[] = {
-	{ "20161121183008", 0, 0x1900, 0x3FE0, { 2, 1, 0 }, 0x40014020, _secmon_1_patchset }, //1.0.0
-	{ "20170210155124", 0, 0x1900, 0x3FE0, { 0, 1, 2 }, 0x4002D000, _secmon_2_patchset }, //2.0.0
-	{ "20170519101410", 1, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, _secmon_3_patchset }, //3.0.0
-	{ "20170710161758", 2, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, NULL }, //3.0.1
-	{ "20170921172629", 3, 0x1800, 0x3FE0, { 1, 2, 0 }, 0x4002B000, _secmon_5_patchset }, //4.0.0
-	{ "20180220163747", 4, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, _secmon_6_patchset }, //5.0.0
-	{ NULL, 0, 0, 0, 0 } //End.
+	{ "20161121183008", 0, 0x1900, 0x3FE0, { 2, 1, 0 }, SM_100_ADR, 0x8000D000, 1, _secmon_1_patchset }, //1.0.0 (Patched relocator)
+	{ "20170210155124", 0, 0x1900, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, 1, _secmon_2_patchset }, //2.0.0 - 2.3.0
+	{ "20170519101410", 1, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, 1, _secmon_3_patchset }, //3.0.0
+	{ "20170710161758", 2, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, 0x8000D000, 1, _secmon_3_patchset }, //3.0.1 - 3.0.2
+	{ "20170921172629", 3, 0x1800, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003B000, 0, _secmon_4_patchset }, //4.0.0 - 4.1.0
+	{ "20180220163747", 4, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, 0x4003B000, 0, _secmon_5_patchset }, //5.0.0 - 5.1.0
+	{ NULL } //End.
 };


@@ -103,7 +98,7 @@ void pkg1_decrypt(const pkg1_id_t *id, u8 *pkg1)
 	se_aes_crypt_ctr(11, pkg11 + 0x20, pkg11_size, pkg11 + 0x20, pkg11_size, pkg11 + 0x10);
 }

-void pkg1_unpack(void *warmboot_dst, void *secmon_dst, const pkg1_id_t *id, u8 *pkg1)
+void pkg1_unpack(void *warmboot_dst, void *secmon_dst, void *ldr_dst, const pkg1_id_t *id, u8 *pkg1)
 {
 	pk11_hdr_t *hdr = (pk11_hdr_t *)(pkg1 + id->pkg11_off + 0x20);

@@ -115,6 +110,8 @@ void pkg1_unpack(void *warmboot_dst, void *secmon_dst, const pkg1_id_t *id, u8 *
 	{
 		if (id->sec_map[i] == 0 && warmboot_dst)
 			memcpy(warmboot_dst, pdata, sec_size[id->sec_map[i]]);
+		else if (id->sec_map[i] == 1 && ldr_dst)
+			memcpy(ldr_dst, pdata, sec_size[id->sec_map[i]]);
 		else if (id->sec_map[i] == 2 && secmon_dst)
 			memcpy(secmon_dst, pdata, sec_size[id->sec_map[i]]);
 		pdata += sec_size[id->sec_map[i]];
--- a/ipl/pkg1.h
+++ b/ipl/pkg1.h
@@ -25,12 +25,6 @@
 		{ 0xFFFFFFFF, 0xFFFFFFFF } \
 	}

-typedef struct _patch_t
-{
-	u32 off;
-	u32 val;
-} patch_t;
-
 typedef struct _pkg1_id_t
 {
 	const char *id;
@@ -39,6 +33,8 @@ typedef struct _pkg1_id_t
 	u32 pkg11_off;
 	u32 sec_map[3];
 	u32 secmon_base;
+	u32 warmboot_base;
+	int set_warmboot;
 	patch_t *secmon_patchset;
 } pkg1_id_t;

@@ -56,6 +52,6 @@ typedef struct _pk11_hdr_t

 const pkg1_id_t *pkg1_identify(u8 *pkg1);
 void pkg1_decrypt(const pkg1_id_t *id, u8 *pkg1);
-void pkg1_unpack(void *warmboot_dst, void *secmon_dst, const pkg1_id_t *id, u8 *pkg1);
+void pkg1_unpack(void *warmboot_dst, void *secmon_dst, void *ldr_dst, const pkg1_id_t *id, u8 *pkg1);

 #endif
--- a/ipl/pkg2.c
+++ b/ipl/pkg2.c
@@ -16,6 +16,7 @@

 #include <string.h>
 #include "pkg2.h"
+#include "arm64.h"
 #include "heap.h"
 #include "se.h"

@@ -25,6 +26,55 @@ extern gfx_con_t gfx_con;
 #define DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)*/
 #define DPRINTF(...)

+// Include kernel patches here, so we can utilize pkg1 id
+PATCHSET_DEF(_kernel_1_patchset,
+	{ 0x3764C, _NOP() },         // Disable SVC verifications
+	{ 0x44074, _MOVZX(8, 1, 0) } // Enable Debug Patch
+);
+
+PATCHSET_DEF(_kernel_2_patchset,
+	{ 0x54834, _NOP() },         // Disable SVC verifications
+	{ 0x6086C, _MOVZX(8, 1, 0) } // Enable Debug Patch
+);
+
+PATCHSET_DEF(_kernel_3_patchset,
+	{ 0x3BD24, _NOP() },         // Disable SVC verifications
+	{ 0x483FC, _MOVZX(8, 1, 0) } // Enable Debug Patch
+);
+
+PATCHSET_DEF(_kernel_302_patchset,
+	{ 0x3BD24, _NOP() },         // Disable SVC verifications
+	{ 0x48414, _MOVZX(8, 1, 0) } // Enable Debug Patch
+);
+
+PATCHSET_DEF(_kernel_4_patchset,
+	{ 0x41EB4, _NOP() },         // Disable SVC verifications
+	{ 0x4EBFC, _MOVZX(8, 1, 0) } // Enable Debug Patch
+);
+
+PATCHSET_DEF(_kernel_5_patchset,
+	{ 0x45E6C, _NOP() },         // Disable SVC verifications
+	{ 0x5513C, _MOVZX(8, 1, 0) } // Enable Debug Patch
+);
+
+static const pkg2_kernel_id_t _pkg2_kernel_ids[] = {
+	{ 0x427f2647, _kernel_1_patchset },   //1.0.0
+	{ 0xae19cf1b, _kernel_2_patchset },   //2.0.0 - 2.3.0
+	{ 0x73c9e274, _kernel_3_patchset },   //3.0.0 - 3.0.1
+	{ 0xe0e8cdc4, _kernel_302_patchset }, //3.0.2
+	{ 0x485d0157, _kernel_4_patchset },   //4.0.0 - 4.1.0
+	{ 0xf3c363f2, _kernel_5_patchset },   //5.0.0 - 5.1.0
+	{ 0, 0 } //End.
+};
+
+const pkg2_kernel_id_t *pkg2_identify(u32 id)
+{
+	for (u32 i = 0; _pkg2_kernel_ids[i].crc32c_id; i++)
+		if (id == _pkg2_kernel_ids[i].crc32c_id)
+			return &_pkg2_kernel_ids[i];
+	return NULL;
+}
+
 static u32 _pkg2_calc_kip1_size(pkg2_kip1_t *kip1)
 {
 	u32 size = sizeof(pkg2_kip1_t);
--- a/ipl/pkg2.h
+++ b/ipl/pkg2.h
@@ -27,6 +27,12 @@

 #define INI1_MAGIC 0x31494E49

+#define PATCHSET_DEF(name, ...) \
+	patch_t name[] = { \
+		__VA_ARGS__, \
+		{ 0xFFFFFFFF, 0xFFFFFFFF } \
+	}
+
 typedef struct _pkg2_hdr_t
 {
 	u8 ctr[0x10];
@@ -82,12 +88,19 @@ typedef struct _pkg2_kip1_info_t
 	link_t link;
 } pkg2_kip1_info_t;

+typedef struct _pkg2_kernel_id_t
+{
+	u32 crc32c_id;
+	patch_t *kernel_patchset;
+} pkg2_kernel_id_t;
+
 void pkg2_parse_kips(link_t *info, pkg2_hdr_t *pkg2);
 int pkg2_has_kip(link_t *info, u64 tid);
 void pkg2_replace_kip(link_t *info, u64 tid, pkg2_kip1_t *kip1);
 void pkg2_add_kip(link_t *info, pkg2_kip1_t *kip1);
 void pkg2_merge_kip(link_t *info, pkg2_kip1_t *kip1);

+const pkg2_kernel_id_t *pkg2_identify(u32 id);
 pkg2_hdr_t *pkg2_decrypt(void *data);
 void pkg2_build_encrypt(void *dst, void *kernel, u32 kernel_size, link_t *kips_info);

--- a/ipl/sd.h
+++ b/ipl/sd.h
@@ -86,6 +86,7 @@
 #define UHS_SDR50_BUS_SPEED		2
 #define UHS_SDR104_BUS_SPEED	3
 #define UHS_DDR50_BUS_SPEED		4
+#define HS400_BUS_SPEED 		5

 /*
 * SD_SWITCH mode
@@ -102,6 +103,6 @@
 * SD_SWITCH access modes
 */
 #define SD_SWITCH_ACCESS_DEF	0
-#define SD_SWITCH_ACCESS_HS	1
+#define SD_SWITCH_ACCESS_HS		1

 #endif /* LINUX_MMC_SD_H */
--- a/ipl/sdmmc.c
+++ b/ipl/sdmmc.c
@@ -165,15 +165,26 @@ static int _sdmmc_storage_readwrite(sdmmc_storage_t *storage, u32 sector, u32 nu
 	while (num_sectors)
 	{
 		u32 blkcnt = 0;
-		//Retry once on error.
-		if (!_sdmmc_storage_readwrite_ex(storage, &blkcnt, sector, MIN(num_sectors, 0xFFFF), bbuf, is_write))
-			if (!_sdmmc_storage_readwrite_ex(storage, &blkcnt, sector, MIN(num_sectors, 0xFFFF), bbuf, is_write))
-				return 0;
+		//Retry 9 times on error.
+		u32 retries = 10;
+		do
+		{
+			if (_sdmmc_storage_readwrite_ex(storage, &blkcnt, sector, MIN(num_sectors, 0xFFFF), bbuf, is_write))
+				goto out;
+			else
+				retries--;
+
+			sleep(100000);
+		} while (retries);
+		return 0;
+
+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)
@@ -296,7 +307,7 @@ static void _mmc_storage_parse_csd(sdmmc_storage_t *storage)
 	storage->csd.capacity = (1 + unstuff_bits(raw_csd, 62, 12)) << (unstuff_bits(raw_csd, 47, 3) + 2);
 }

-static int _mmc_storage_parse_ext_csd(sdmmc_storage_t *storage, u8 *buf)
+static void _mmc_storage_parse_ext_csd(sdmmc_storage_t *storage, u8 *buf)
 {
 	storage->ext_csd.rev = buf[EXT_CSD_REV];
 	storage->ext_csd.ext_struct = buf[EXT_CSD_STRUCTURE];
@@ -422,9 +433,9 @@ static int _mmc_storage_enable_highspeed(sdmmc_storage_t *storage, u32 card_type
 		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
+		(sdmmc_get_bus_width(storage->sdmmc) == SDMMC_BUS_WIDTH_4
 		&& card_type & EXT_CSD_CARD_TYPE_HS200_1_8V
-		&& (type == 4 || type == 3))
+		&& (type == 4 || type == 3)))
 		return _mmc_storage_enable_HS200(storage);

 out:;
@@ -756,12 +767,12 @@ int _sd_storage_enable_highspeed(sdmmc_storage_t *storage, u32 hs_type, u8 *buf)
 	if (type_out != hs_type)
 		return 0;

-	if (((u16)buf[0] << 8) | buf[1] < 0x320)
+	if ((((u16)buf[0] << 8) | buf[1]) < 0x320)
 	{
 		if (!_sd_storage_switch(storage, buf, 1, hs_type))
 			return 0;

-		if (type_out != buf[16] & 0xF)
+		if (type_out != (buf[16] & 0xF))
 			return 0;
 	}

@@ -838,7 +849,7 @@ static void _sd_storage_parse_ssr(sdmmc_storage_t *storage)
 	raw_ssr2[1] = *(u32 *)&storage->raw_ssr[20];
 	raw_ssr2[0] = *(u32 *)&storage->raw_ssr[16];

-	storage->ssr.bus_width = unstuff_bits(raw_ssr1, 510 - 384, 2) & SD_BUS_WIDTH_4 ? 4 : 1;
+	storage->ssr.bus_width = (unstuff_bits(raw_ssr1, 510 - 384, 2) & SD_BUS_WIDTH_4) ? 4 : 1;
 	switch(unstuff_bits(raw_ssr1, 440 - 384, 8))
 	{
 	case 0:
--- a/ipl/sdmmc_driver.c
+++ b/ipl/sdmmc_driver.c
@@ -129,7 +129,7 @@ static int _sdmmc_config_ven_ceata_clk(sdmmc_t *sdmmc, u32 id)
 		static const u32 tap_values[] = { 4, 0, 3, 0 };
 		tap_val = tap_values[sdmmc->id];
 	}
-	sdmmc->regs->venclkctl = sdmmc->regs->venclkctl & 0xFF00FFFF | (tap_val << 16);
+	sdmmc->regs->venclkctl = (sdmmc->regs->venclkctl & 0xFF00FFFF) | (tap_val << 16);

 	return 1;
 }
@@ -204,32 +204,34 @@ int sdmmc_setup_clock(sdmmc_t *sdmmc, u32 type)
 	case 1:
 	case 5:
 	case 6:
-		sdmmc->regs->hostctl &= 0xFB;
-		sdmmc->regs->hostctl2 &= 0xFFF7;
+		sdmmc->regs->hostctl  &= 0xFB; //Should this be 0xFFFB (~4) ?
+		sdmmc->regs->hostctl2 &= SDHCI_CTRL_VDD_330;
 		break;
 	case 2:
 	case 7:
-		sdmmc->regs->hostctl |= 4;
-		sdmmc->regs->hostctl2 &= 0xFFF7;
+		sdmmc->regs->hostctl  |= 4;
+		sdmmc->regs->hostctl2 &= SDHCI_CTRL_VDD_330;
 		break;
 	case 3:
 	case 11:
 	case 13:
 	case 14:
-		sdmmc->regs->hostctl2 = (sdmmc->regs->hostctl2 & 0xFFF8) | 3;
-		sdmmc->regs->hostctl2 |= 8;
+		sdmmc->regs->hostctl2  = (sdmmc->regs->hostctl2 & SDHCI_CTRL_UHS_MASK) | UHS_SDR104_BUS_SPEED;
+		sdmmc->regs->hostctl2 |= SDHCI_CTRL_VDD_180;
 		break;
 	case 4:
-		sdmmc->regs->hostctl2 = (sdmmc->regs->hostctl2 & 0xFFF8) | 5;
-		sdmmc->regs->hostctl2 |= 8;
+		 //Non standard
+		sdmmc->regs->hostctl2  = (sdmmc->regs->hostctl2 & SDHCI_CTRL_UHS_MASK) | HS400_BUS_SPEED;
+		sdmmc->regs->hostctl2 |= SDHCI_CTRL_VDD_180;
 		break;
 	case 8:
-		sdmmc->regs->hostctl2 = sdmmc->regs->hostctl2 & 0xFFF8;
-		sdmmc->regs->hostctl2 |= 8;
+		sdmmc->regs->hostctl2  = (sdmmc->regs->hostctl2 & SDHCI_CTRL_UHS_MASK) | UHS_SDR12_BUS_SPEED;
+		sdmmc->regs->hostctl2 |= SDHCI_CTRL_VDD_180;
 		break;
 	case 10:
-		sdmmc->regs->hostctl2 = (sdmmc->regs->hostctl2 & 0xFFF8) | 2;
-		sdmmc->regs->hostctl2 |= 8;
+		//T210 Errata for SDR50, the host must be set to SDR104.
+		sdmmc->regs->hostctl2  = (sdmmc->regs->hostctl2 & SDHCI_CTRL_UHS_MASK) | UHS_SDR104_BUS_SPEED;
+		sdmmc->regs->hostctl2 |= SDHCI_CTRL_VDD_180;
 		break;
 	}

@@ -551,16 +553,16 @@ int sdmmc_config_tuning(sdmmc_t *sdmmc, u32 type, u32 cmd)
 	sdmmc->regs->field_1C0 = (sdmmc->regs->field_1C0 & 0xFFFF1FFF) | flag;
 	sdmmc->regs->field_1C0 = (sdmmc->regs->field_1C0 & 0xFFFFE03F) | 0x40;
 	sdmmc->regs->field_1C0 |= 0x20000;
-	sdmmc->regs->hostctl2 |= 0x40;
+	sdmmc->regs->hostctl2  |= SDHCI_CTRL_EXEC_TUNING;

 	for (u32 i = 0; i < max; i++)
 	{
 		_sdmmc_config_tuning_once(sdmmc, cmd);
-		if (!(sdmmc->regs->hostctl2 & 0x40))
+		if (!(sdmmc->regs->hostctl2 & SDHCI_CTRL_EXEC_TUNING))
 			break;
 	}

-	if (sdmmc->regs->hostctl2 & 0x80)
+	if (sdmmc->regs->hostctl2 & SDHCI_CTRL_TUNED_CLK)
 		return 1;
 	return 0;
 }
@@ -577,14 +579,14 @@ static int _sdmmc_enable_internal_clock(sdmmc_t *sdmmc)
 			return 0;
 	}

-	sdmmc->regs->hostctl2 &= 0x7FFF;
-	sdmmc->regs->clkcon &= ~TEGRA_MMC_CLKCON_CLKGEN_SELECT;
-	sdmmc->regs->hostctl2 |= 0x1000;
+	sdmmc->regs->hostctl2 &= ~SDHCI_CTRL_PRESET_VAL_EN;
+	sdmmc->regs->clkcon   &= ~TEGRA_MMC_CLKCON_CLKGEN_SELECT;
+	sdmmc->regs->hostctl2 |= SDHCI_HOST_VERSION_4_EN;

 	if (!(sdmmc->regs->capareg & 0x10000000))
 		return 0;

-	sdmmc->regs->hostctl2 |= 0x2000;
+	sdmmc->regs->hostctl2 |= SDHCI_ADDRESSING_64BIT_EN;
 	sdmmc->regs->hostctl &= 0xE7;
 	sdmmc->regs->timeoutcon = (sdmmc->regs->timeoutcon & 0xF0) | 0xE;

@@ -622,7 +624,7 @@ static int _sdmmc_autocal_config_offset(sdmmc_t *sdmmc, u32 power)
 		break;
 	}

-	sdmmc->regs->autocalcfg = ((sdmmc->regs->autocalcfg & 0xFFFF80FF | (off_pd << 8)) >> 7 << 7) | off_pu;
+	sdmmc->regs->autocalcfg = (((sdmmc->regs->autocalcfg & 0xFFFF80FF) | (off_pd << 8)) >> 7 << 7) | off_pu;
 	return 1;
 }

@@ -1076,7 +1078,7 @@ int sdmmc_enable_low_voltage(sdmmc_t *sdmmc)
 	_sdmmc_get_clkcon(sdmmc);
 	sleep(5000);
 	
-	if (sdmmc->regs->hostctl2 & 8)
+	if (sdmmc->regs->hostctl2 & SDHCI_CTRL_VDD_180)
 	{
 		sdmmc->regs->clkcon |= TEGRA_MMC_CLKCON_SD_CLOCK_ENABLE;
 		_sdmmc_get_clkcon(sdmmc);
--- a/ipl/sdmmc_driver.h
+++ b/ipl/sdmmc_driver.h
@@ -45,9 +45,28 @@
 #define SDMMC_RSP_TYPE_5 5

 /*! SDMMC mask interrupt status. */
-#define SDMMC_MASKINT_MASKED 0
+#define SDMMC_MASKINT_MASKED   0
 #define SDMMC_MASKINT_NOERROR -1
-#define SDMMC_MASKINT_ERROR -2
+#define SDMMC_MASKINT_ERROR   -2
+
+/*! SDMMC host control 2 */
+#define SDHCI_CTRL_UHS_MASK			0xFFF8
+#define SDHCI_CTRL_VDD_330			0xFFF7
+#define SDHCI_CTRL_VDD_180			8
+#define SDHCI_CTRL_EXEC_TUNING		0x40
+#define SDHCI_CTRL_TUNED_CLK		0x80
+#define SDHCI_HOST_VERSION_4_EN		0x1000
+#define SDHCI_ADDRESSING_64BIT_EN	0x2000
+#define SDHCI_CTRL_PRESET_VAL_EN	0x8000
+
+/*! SD bus speeds. */
+#define UHS_SDR12_BUS_SPEED		0
+#define HIGH_SPEED_BUS_SPEED	1
+#define UHS_SDR25_BUS_SPEED		1
+#define UHS_SDR50_BUS_SPEED		2
+#define UHS_SDR104_BUS_SPEED	3
+#define UHS_DDR50_BUS_SPEED		4
+#define HS400_BUS_SPEED 		5

 /*! Helper for SWITCH command argument. */
 #define SDMMC_SWITCH(mode, index, value) (((mode) << 24) | ((index) << 16) | ((value) << 8))
--- a/ipl/sdram.c
+++ b/ipl/sdram.c
@@ -64,7 +64,7 @@ static void _sdram_config(const sdram_params_t *params)
 	sleep(10);
 break_nosleep:

-	CLOCK(0x19C) = (params->mc_emem_arb_misc0 >> 11) & 0x10000 | params->emc_clock_source & 0xFFFEFFFF;
+	CLOCK(0x19C) = ((params->mc_emem_arb_misc0 >> 11) & 0x10000) | (params->emc_clock_source & 0xFFFEFFFF);
 	if (params->emc_clock_source_dll)
 		CLOCK(0x664) = params->emc_clock_source_dll;
 	if (params->clear_clock2_mc1)
@@ -97,7 +97,7 @@ break_nosleep:
 	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;
+	EMC(EMC_PMACRO_BRICK_CTRL_RFU1) = (params->emc_pmacro_brick_ctrl_rfu1 & 0x1120112) | 0x1EED1EED;
 	EMC(EMC_CONFIG_SAMPLE_DELAY) = params->emc_config_sample_delay;
 	EMC(EMC_FBIO_CFG8) = params->emc_fbio_cfg8;
 	EMC(EMC_SWIZZLE_RANK0_BYTE0) = params->emc_swizzle_rank0_byte0;
@@ -142,7 +142,7 @@ 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_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;
@@ -232,7 +232,7 @@ 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;
-	EMC(EMC_PMACRO_COMMON_PAD_TX_CTRL) = params->emc_pmacro_common_pad_tx_ctrl & 1 | 0xE;
+	EMC(EMC_PMACRO_COMMON_PAD_TX_CTRL) = (params->emc_pmacro_common_pad_tx_ctrl & 1) | 0xE;
 	if (params->emc_bct_spare4)
 		*(vu32 *)params->emc_bct_spare4 = params->emc_bct_spare5;
 	EMC(EMC_TIMING_CONTROL) = 1;
@@ -386,7 +386,7 @@ break_nosleep:
 	EMC(EMC_PMC_SCRATCH3) = params->emc_pmc_scratch3;
 	EMC(EMC_ACPD_CONTROL) = params->emc_acpd_control;
 	EMC(EMC_TXDSRVTTGEN) = params->emc_txdsrvttgen;
-	EMC(EMC_CFG) = params->emc_cfg & 0xE | 0x3C00000;
+	EMC(EMC_CFG) = (params->emc_cfg & 0xE) | 0x3C00000;
 	if (params->boot_rom_patch_control & 0x80000000)
 	{
 		*(vu32 *)(4 * (params->boot_rom_patch_control + 0x1C000000)) = params->boot_rom_patch_data;
@@ -481,7 +481,7 @@ break_nosleep:
 	EMC(EMC_TIMING_CONTROL) = 1;
 	EMC(EMC_CFG_PIPE_CLK) = params->emc_cfg_pipe_clk;
 	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);
+	SYSREG(AHB_ARBITRATION_XBAR_CTRL) = (SYSREG(AHB_ARBITRATION_XBAR_CTRL) & 0xFFFEFFFF) | ((params->ahb_arbitration_xbar_ctrl_meminit_done & 0xFFFF) << 16);
 	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;
--- a/ipl/sdram_lp0.c
+++ b/ipl/sdram_lp0.c
@@ -23,7 +23,7 @@
 */
 void sdram_lp0_save_params(const void *params)
 {
-	const struct sdram_params *sdram = (const struct sdram_params *)params;
+	struct sdram_params *sdram = (struct sdram_params *)params;
 	struct tegra_pmc_regs *pmc = (struct tegra_pmc_regs *)PMC_BASE;

 #define pack(src, src_bits, dst, dst_bits) { \
@@ -44,6 +44,100 @@ void sdram_lp0_save_params(const void *params)
 /* 32 bits version c macro */
 #define c32(value, pmcreg) pmc->pmcreg = value

+	//TODO: pkg1.1 (1.X - 3.X) reads them from MC.
+	//Patch carveout parameters.
+	/*sdram->McGeneralizedCarveout1Bom = 0;
+	sdram->McGeneralizedCarveout1BomHi = 0;
+	sdram->McGeneralizedCarveout1Size128kb = 0;
+	sdram->McGeneralizedCarveout1Access0 = 0;
+	sdram->McGeneralizedCarveout1Access1 = 0;
+	sdram->McGeneralizedCarveout1Access2 = 0;
+	sdram->McGeneralizedCarveout1Access3 = 0;
+	sdram->McGeneralizedCarveout1Access4 = 0;
+	sdram->McGeneralizedCarveout1ForceInternalAccess0 = 0;
+	sdram->McGeneralizedCarveout1ForceInternalAccess1 = 0;
+	sdram->McGeneralizedCarveout1ForceInternalAccess2 = 0;
+	sdram->McGeneralizedCarveout1ForceInternalAccess3 = 0;
+	sdram->McGeneralizedCarveout1ForceInternalAccess4 = 0;
+	sdram->McGeneralizedCarveout1Cfg0 = 0;
+	sdram->McGeneralizedCarveout2Bom = 0x80020000;
+	sdram->McGeneralizedCarveout2BomHi = 0;
+	sdram->McGeneralizedCarveout2Size128kb = 2;
+	sdram->McGeneralizedCarveout2Access0 = 0;
+	sdram->McGeneralizedCarveout2Access1 = 0;
+	sdram->McGeneralizedCarveout2Access2 = 0x3000000;
+	sdram->McGeneralizedCarveout2Access3 = 0;
+	sdram->McGeneralizedCarveout2Access4 = 0x300;
+	sdram->McGeneralizedCarveout2ForceInternalAccess0 = 0;
+	sdram->McGeneralizedCarveout2ForceInternalAccess1 = 0;
+	sdram->McGeneralizedCarveout2ForceInternalAccess2 = 0;
+	sdram->McGeneralizedCarveout2ForceInternalAccess3 = 0;
+	sdram->McGeneralizedCarveout2ForceInternalAccess4 = 0;
+	sdram->McGeneralizedCarveout2Cfg0 = 0x440167E;
+	sdram->McGeneralizedCarveout3Bom = 0;
+	sdram->McGeneralizedCarveout3BomHi = 0;
+	sdram->McGeneralizedCarveout3Size128kb = 0;
+	sdram->McGeneralizedCarveout3Access0 = 0;
+	sdram->McGeneralizedCarveout3Access1 = 0;
+	sdram->McGeneralizedCarveout3Access2 = 0x3000000;
+	sdram->McGeneralizedCarveout3Access3 = 0;
+	sdram->McGeneralizedCarveout3Access4 = 0x300;
+	sdram->McGeneralizedCarveout3ForceInternalAccess0 = 0;
+	sdram->McGeneralizedCarveout3ForceInternalAccess1 = 0;
+	sdram->McGeneralizedCarveout3ForceInternalAccess2 = 0;
+	sdram->McGeneralizedCarveout3ForceInternalAccess3 = 0;
+	sdram->McGeneralizedCarveout3ForceInternalAccess4 = 0;
+	sdram->McGeneralizedCarveout3Cfg0 = 0x4401E7E;
+	sdram->McGeneralizedCarveout4Bom = 0;
+	sdram->McGeneralizedCarveout4BomHi = 0;
+	sdram->McGeneralizedCarveout4Size128kb = 0;
+	sdram->McGeneralizedCarveout4Access0 = 0;
+	sdram->McGeneralizedCarveout4Access1 = 0;
+	sdram->McGeneralizedCarveout4Access2 = 0;
+	sdram->McGeneralizedCarveout4Access3 = 0;
+	sdram->McGeneralizedCarveout4Access4 = 0;
+	sdram->McGeneralizedCarveout4ForceInternalAccess0 = 0;
+	sdram->McGeneralizedCarveout4ForceInternalAccess1 = 0;
+	sdram->McGeneralizedCarveout4ForceInternalAccess2 = 0;
+	sdram->McGeneralizedCarveout4ForceInternalAccess3 = 0;
+	sdram->McGeneralizedCarveout4ForceInternalAccess4 = 0;
+	sdram->McGeneralizedCarveout4Cfg0 = 0x8F;
+	sdram->McGeneralizedCarveout5Bom = 0;
+	sdram->McGeneralizedCarveout5BomHi = 0;
+	sdram->McGeneralizedCarveout5Size128kb = 0;
+	sdram->McGeneralizedCarveout5Access0 = 0;
+	sdram->McGeneralizedCarveout5Access1 = 0;
+	sdram->McGeneralizedCarveout5Access2 = 0;
+	sdram->McGeneralizedCarveout5Access3 = 0;
+	sdram->McGeneralizedCarveout5Access4 = 0;
+	sdram->McGeneralizedCarveout5ForceInternalAccess0 = 0;
+	sdram->McGeneralizedCarveout5ForceInternalAccess1 = 0;
+	sdram->McGeneralizedCarveout5ForceInternalAccess2 = 0;
+	sdram->McGeneralizedCarveout5ForceInternalAccess3 = 0;
+	sdram->McGeneralizedCarveout5ForceInternalAccess4 = 0;
+	sdram->McGeneralizedCarveout5Cfg0 = 0x8F;*/
+
+	//TODO: this is 4.X+ behaviour which seems to work fine for < 4.X.
+	//Patch carveout parameters.
+	sdram->McGeneralizedCarveout1Cfg0 = 0;
+	sdram->McGeneralizedCarveout2Cfg0 = 0;
+	sdram->McGeneralizedCarveout3Cfg0 = 0;
+	sdram->McGeneralizedCarveout4Cfg0 = 0;
+	sdram->McGeneralizedCarveout5Cfg0 = 0;
+
+	//Patch SDRAM parameters.
+	u32 t0 = sdram->EmcSwizzleRank0Byte0 << 5 >> 29 > sdram->EmcSwizzleRank0Byte0 << 1 >> 29;
+	u32 t1 = (t0 & 0xFFFFFFEF) | ((sdram->EmcSwizzleRank1Byte0 << 5 >> 29 > sdram->EmcSwizzleRank1Byte0 << 1 >> 29) << 4);
+	u32 t2 = (t1 & 0xFFFFFFFD) | ((sdram->EmcSwizzleRank0Byte1 << 5 >> 29 > sdram->EmcSwizzleRank0Byte1 << 1 >> 29) << 1);
+	u32 t3 = (t2 & 0xFFFFFFDF) | ((sdram->EmcSwizzleRank1Byte1 << 5 >> 29 > sdram->EmcSwizzleRank1Byte1 << 1 >> 29) << 5);
+	u32 t4 = (t3 & 0xFFFFFFFB) | ((sdram->EmcSwizzleRank0Byte2 << 5 >> 29 > sdram->EmcSwizzleRank0Byte2 << 1 >> 29) << 2);
+	u32 t5 = (t4 & 0xFFFFFFBF) | ((sdram->EmcSwizzleRank1Byte2 << 5 >> 29 > sdram->EmcSwizzleRank1Byte2 << 1 >> 29) << 6);
+	u32 t6 = (t5 & 0xFFFFFFF7) | ((sdram->EmcSwizzleRank0Byte3 << 5 >> 29 > sdram->EmcSwizzleRank0Byte3 << 1 >> 29) << 3);
+	u32 t7 = (t6 & 0xFFFFFF7F) | ((sdram->EmcSwizzleRank1Byte3 << 5 >> 29 > sdram->EmcSwizzleRank1Byte3 << 1 >> 29) << 7);
+	sdram->SwizzleRankByteEncode = t7;
+	sdram->EmcBctSpare2 = 0x40000DD8;
+	sdram->EmcBctSpare3 = t7;
+
 	s(EmcClockSource, 7:0, scratch6, 15:8);
 	s(EmcClockSourceDll, 7:0, scratch6, 23:16);
 	s(EmcClockSource, 31:29, scratch6, 26:24);
@@ -714,7 +808,7 @@ void sdram_lp0_save_params(const void *params)
 	s32(EmcBctSpare6, scratch40);
 	s32(EmcBctSpare5, scratch42);
 	s32(EmcBctSpare4, scratch44);
-	s32(SwizzleRankByteEncode, scratch45);
+	s32(EmcBctSpare3, scratch45);
 	s32(EmcBctSpare2, scratch46);
 	s32(EmcBctSpare1, scratch47);
 	s32(EmcBctSpare0, scratch48);
@@ -1004,11 +1098,6 @@ void sdram_lp0_save_params(const void *params)
 	s32(McGeneralizedCarveout5ForceInternalAccess2, secure_scratch106);
 	s32(McGeneralizedCarveout5ForceInternalAccess3, secure_scratch107);

-	/* Locking PMC secure scratch register (8 ~ 15) for writing */
-	//c(0x5555, sec_disable2, 15:0);
-	/* Locking PMC secure scratch register (4~ 7) for both reading and writing */
-	//c(0xff, sec_disable, 19:12);
-
 	c32(0, scratch2);
 	s(PllMInputDivider, 7:0, scratch2, 7:0);
 	s(PllMFeedbackDivider, 7:0, scratch2, 15:8);
--- a/ipl/sdram_param_t210_lp0.h
+++ b/ipl/sdram_param_t210_lp0.h
@@ -262,7 +262,7 @@ struct sdram_params {
 	/* Specifies the value for EMC_PUTERM_WIDTH */
 	u32 EmcPutermWidth;
 	/* Specifies the value for EMC_PUTERM_ADJ */
-	u32 EmcPutermAdj;
+	////u32 EmcPutermAdj;

 	/* Specifies the value for EMC_QRST */
 	u32 EmcQRst;
@@ -647,7 +647,7 @@ struct sdram_params {
 	u32 EmcAcpdControl;

 	/* Specifies the value for EMC_SWIZZLE_RANK0_BYTE_CFG */
-	u32 EmcSwizzleRank0ByteCfg;
+	////u32 EmcSwizzleRank0ByteCfg;
 	/* Specifies the value for EMC_SWIZZLE_RANK0_BYTE0 */
 	u32 EmcSwizzleRank0Byte0;
 	/* Specifies the value for EMC_SWIZZLE_RANK0_BYTE1 */
@@ -657,7 +657,7 @@ struct sdram_params {
 	/* Specifies the value for EMC_SWIZZLE_RANK0_BYTE3 */
 	u32 EmcSwizzleRank0Byte3;
 	/* Specifies the value for EMC_SWIZZLE_RANK1_BYTE_CFG */
-	u32 EmcSwizzleRank1ByteCfg;
+	////u32 EmcSwizzleRank1ByteCfg;
 	/* Specifies the value for EMC_SWIZZLE_RANK1_BYTE0 */
 	u32 EmcSwizzleRank1Byte0;
 	/* Specifies the value for EMC_SWIZZLE_RANK1_BYTE1 */
--- a/ipl/se.c
+++ b/ipl/se.c
@@ -124,7 +124,7 @@ 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;
+		SE(SE_RSA_KEYTABLE_ACCESS_REG_OFFSET + 4 * rs) = (((flags >> 4) & 4) | (flags & 3)) ^ 7;
 	if (flags & 0x80)
 		SE(SE_RSA_KEYTABLE_ACCESS_LOCK_OFFSET) &= ~(1 << rs);
 }
--- a/ipl/tui.c
+++ b/ipl/tui.c
@@ -16,9 +16,8 @@

 #include "tui.h"
 #include "btn.h"
-#include "ctc_logo2.h"

-void tui_pbar(gfx_con_t *con, int x, int y, u32 val)
+void tui_pbar(gfx_con_t *con, int x, int y, u32 val, u32 fgcol, u32 bgcol)
 {
 	u32 cx, cy;

@@ -26,14 +25,14 @@ void tui_pbar(gfx_con_t *con, int x, int y, u32 val)

 	gfx_con_setpos(con, x, y);

-	gfx_printf(con, "[%3d%%]", val);
+	gfx_printf(con, "%k[%3d%%]%k", fgcol, val, 0xFFCCCCCC);

 	x += 7 * 8;
 	
 	for (int i = 0; i < 6; i++)
 	{
-		gfx_line(con->gfx_ctxt, x, y + i + 1, x + 3 * val, y + i + 1, 0xFFFFFFFF);
-		gfx_line(con->gfx_ctxt, x + 3 * val, y + i + 1, x + 3 * 100, y + i + 1, 0xFF888888);
+		gfx_line(con->gfx_ctxt, x, y + i + 1, x + 3 * val, y + i + 1, fgcol);
+		gfx_line(con->gfx_ctxt, x + 3 * val, y + i + 1, x + 3 * 100, y + i + 1, bgcol);
 	}

 	gfx_con_setpos(con, cx, cy);
@@ -41,11 +40,9 @@ void tui_pbar(gfx_con_t *con, int x, int y, u32 val)

 void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 {
-	int idx = 0, cnt;
-	int prev_idx = 0;
+	int idx = 0, prev_idx = 0, cnt = 0x7FFFFFFF;

-	gfx_clear(con->gfx_ctxt, 0xFF1B1B1B);
-	gfx_set_logo(con->gfx_ctxt, Kc_HEKATE_LOGO);
+	gfx_clear_grey(con->gfx_ctxt, 0x1B);

 	while (1)
 	{
@@ -53,12 +50,12 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 		gfx_con_setpos(con, menu->x, menu->y);
 		gfx_printf(con, "[%s]\n\n", menu->caption);

-		// Skip caption or seperator lines selection
+		// 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))	
-			{				
+			if (prev_idx <= idx || (!idx && prev_idx == cnt - 1))
+			{
 				idx++;
 				if (idx > (cnt - 1))
 				{
@@ -78,16 +75,16 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 		}
 		prev_idx = idx;

-		// Draw the menu
+		//Draw the menu.
 		for (cnt = 0; menu->ents[cnt].type != MENT_END; cnt++)
 		{
 			if (cnt == idx)
 				gfx_con_setcol(con, 0xFF1B1B1B, 1, 0xFFCCCCCC);
 			else
 				gfx_con_setcol(con, 0xFFCCCCCC, 1, 0xFF1B1B1B);
-			if (cnt != idx && menu->ents[cnt].type == MENT_CAPTION)
+			if (menu->ents[cnt].type == MENT_CAPTION)
 				gfx_printf(con, "%k %s", menu->ents[cnt].color, menu->ents[cnt].caption);
-			else
+			else if (menu->ents[cnt].type != MENT_CHGLINE)
 				gfx_printf(con, " %s", menu->ents[cnt].caption);
 			if(menu->ents[cnt].type == MENT_MENU)
 				gfx_printf(con, "%k...", 0xFFEE9900);
@@ -126,8 +123,7 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 			default:
 				break;
 			}
-			gfx_clear(con->gfx_ctxt, 0xFF1B1B1B);
-			gfx_set_logo(con->gfx_ctxt, Kc_HEKATE_LOGO);
+			gfx_clear_grey(con->gfx_ctxt, 0x1B);
 		}
 	}

--- a/ipl/tui.h
+++ b/ipl/tui.h
@@ -57,7 +57,7 @@ typedef struct _menu_t
 #define MDEF_CAPTION(caption, color) { MENT_CAPTION, caption, color }
 #define MDEF_CHGLINE() {MENT_CHGLINE}

-void tui_pbar(gfx_con_t *con, int x, int y, u32 val);
+void tui_pbar(gfx_con_t *con, int x, int y, u32 val, u32 fgcol, u32 bgcol);
 void *tui_do_menu(gfx_con_t *con, menu_t *menu);

 #endif
--- a/ipl/types.h
+++ b/ipl/types.h
@@ -26,6 +26,10 @@
 #define OFFSET_OF(t, m) ((u32)&((t *)NULL)->m)
 #define CONTAINER_OF(mp, t, mn) ((t *)((u32)mp - OFFSET_OF(t, mn)))

+typedef char s8;
+typedef short s16;
+typedef int s32;
+typedef long long int s64;
 typedef unsigned char u8;
 typedef unsigned short u16;
 typedef unsigned int u32;
@@ -34,4 +38,10 @@ typedef volatile unsigned char vu8;
 typedef volatile unsigned short vu16;
 typedef volatile unsigned int vu32;

+typedef struct _patch_t
+{
+	u32 off;
+	u32 val;
+} patch_t;
+
 #endif
--- a/ipl/util.c
+++ b/ipl/util.c
@@ -35,3 +35,16 @@ void exec_cfg(u32 *base, const cfg_op_t *ops, u32 num_ops)
 		base[ops[i].off] = ops[i].val;
 }

+#define CRC32C_POLY 0x82F63B78
+u32 crc32c(const void *buf, u32 len)
+{
+	const u8 *cbuf = (const u8 *)buf;
+	u32 crc = 0xFFFFFFFF;
+	while (len--)
+	{
+		crc ^= *cbuf++;
+		for (int i = 0; i < 8; i++)
+			crc = crc & 1 ? (crc >> 1) ^ CRC32C_POLY : crc >> 1;
+	}
+	return ~crc;
+}
--- a/ipl/util.h
+++ b/ipl/util.h
@@ -28,5 +28,6 @@ typedef struct _cfg_op_t
 u32 get_tmr();
 void sleep(u32 ticks);
 void exec_cfg(u32 *base, const cfg_op_t *ops, u32 num_ops);
+u32 crc32c(const void *buf, u32 len);

 #endif
Author	SHA1	Message	Date
nwert	694a8b5395	Fixed screen brightness problems (tested on 1.0.0 and 2.0.0). Some refactoring.	2018-06-15 23:38:09 +12:00
Kostas Missos	b2dd070d10	[INI] Fix memory leaks + other fixes (#38 ) * [INI] Fix memory leaks + other fixes * Add ini_free * check and return if NULL pointer in free() * Add button check with timeout * Fix a bug in menu rendering that tries to parse a string without existing * Change initial cnt value to max (fixes menus with first entry not a selectable entry) * Some & ? clarifications * [INI] Add get selected boot cfg - This allows to free ini as we launch the firmware and have the lowest mem leak * [INI] Rename ini section clone/free functions	2018-06-13 11:34:32 +12:00
nwert	62a8618606	Buildfix.	2018-06-12 10:30:40 +12:00
Kostas Missos	0f23f9baec	[Tools] Fix SD files archive bit tool Switch's filesystem sys module has a bug which rejects files if the archive bit is set. This tool, clears this attribute and will help users using Windows to edit/copy/create files in SD card.	2018-06-11 18:28:04 +03:00
nwert	486a121cbf	Normalise patchsets. Simplify LP0 config (needs testing).	2018-06-10 00:05:29 +12:00
nwert	24e172b5fb	Refactored the firmware loader and LP0 config.	2018-06-08 23:07:25 +12:00
nwert	120e8f5870	Fix all warning.	2018-06-08 21:42:24 +12:00
Kostas Missos	16ac26f583	[DP/GFX/SDMMC] Bugfixes - Sanitize framebuffer. Original idea from @StevenMattera - Change clear framebuffer to memset (for grey colors), for performance (we don't use alpha blending, so it does not matter to set it to 0xFF). - Enable screen backlight after gfx init - Reduce wait time for emmc/sd read/write retries to 100ms.	2018-06-06 13:29:38 +03:00
Kostas Missos	b299cb40c7	[PKG2] Switch kernel patching to crc32c based kernel id This provides support for the 3.0.2 kernel, where pkg1 has the same id with 3.0.1. Thanks @roblabla for noticing and providing the Debug mode kernel patch.	2018-06-05 02:04:08 +03:00
Kostas Missos	fb9e91ed0c	Better dump of pkg1 + Wording	2018-06-05 01:55:32 +03:00
nwert	b8b7889e68	Fixed sleep mode [test on 1.0.0, 2.0.0 and 5.1.0].	2018-06-04 14:44:40 +12:00
Kostas Missos	c43b6f8a5d	SD errata, bugfixes, replace hardcoded values	2018-06-03 22:27:15 +12:00
Robin Lambertz	c9f5a2516f	Add SVC verification disable on 5.x	2018-06-03 22:27:15 +12:00
Kostas Missos	e0cb0e867b	[HOS] Add sd unmount and proper secmon signaling	2018-06-03 22:27:15 +12:00
Kostas Missos	4b806f1e7d	Oups. Remove unmerged functions	2018-06-03 22:27:15 +12:00
Kostas Missos	1f501e08ff	[PKG1] Move 1.0.0 secmon to 0x4002B020 So, it's time to move 1.0.0 secmon. If we want to continue having a full featured bootloader, this change is a must. It will move the secmon to 0x4002B000 + 0x20 (to keep the values addresses aligned). It fixes relocator patch. So now the payload size limit is 160KB - .bss (~158KB).	2018-06-03 22:27:15 +12:00
Kostas Missos	e2a8b36750	[PKG1] Add kernel patching Add Disable Svc Verification and Debug mode. Can be enabled via the hekate .ini https://gist.github.com/roblabla/440f3ceaa0b2d3ca530c2a43fe258420	2018-06-03 22:27:15 +12:00
Kostas Missos	dac8cd83ec	[LP0] Inform user that console halted :P Actually this corrects sdram cfg parsing in LP0. The cfg is missing 3 variables, which ultimately are unused in T210. Remove them and fix LP0 configuration saving. The console sleep, still does not work. But now it tries to enter/leave sleep and halts with backlight on. At least, this reminds the user to power off the console to not deplete the battery completely.	2018-06-03 22:27:15 +12:00
Kostas Missos	54a0a353bc	Bug fixes, wording, etc	2018-06-03 22:27:15 +12:00
Kostas Missos	d8251ab28b	[Backup] Add dump verification	2018-06-03 22:27:15 +12:00
Kostas Missos	381b3fa7f0	[GFX] Fix hedump print last 1-16 bytes	2018-06-03 22:27:15 +12:00
Kostas Missos	7a22dddb01	[DC/DSI] Replace hardcoded values with named values To better understand the procedure.	2018-05-30 14:03:10 -07:00
Kostas Missos	7a771f4855	[sdmmc] Change internal read/write retries This can fix busy errors when writing to sd cards. Additionally it now returns properly on success	2018-05-28 18:27:02 -07:00
Kostas Missos	746a557deb	Add 3.X support through SE patches This includes @jsaowji commits and also adds support for 3.0.1 and 3.0.2	2018-05-28 18:27:02 -07:00
Kostas Missos	b2762d737d	Small cosmetic fixes	2018-05-28 18:27:02 -07:00
Kostas Missos	e268222dae	[Main] Small fixes	2018-05-25 01:28:03 -07:00
Kostas Missos	81eb1d0972	[Main] Filter POWER button down And remove uneeded double press protection through sleep().	2018-05-25 01:28:03 -07:00
Kostas Missos	997e250c43	[Menu] Add captions and seperators Additionally, skip selection of them to retain flow and maintain auto scroll.	2018-05-25 01:28:03 -07:00
Kostas Missos	03729bddd5	[Main] Back/text color to subtle dark/light grey	2018-05-25 01:28:03 -07:00