Bump version to v3.1

Based on tests they are not faster, even though the raw sleeps have less instuctions.

But having them call get_tmr breaks important logic.
Make both raw to avoid any future problems.

.gitignore

@@ -1,3 +1,4 @@
 .vs
 .vscode
 build_ipl/*
+/ipl.bin

Makefile

@@ -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
@@ -12,6 +10,7 @@ SOURCEDIR := ipl
 OBJS = $(addprefix $(BUILD)/, \
 	start.o \
 	main.o \
+	config.o \
 	btn.o \
 	clock.o \
 	cluster.o \
@@ -22,7 +21,9 @@ OBJS = $(addprefix $(BUILD)/, \
 	i2c.o \
 	kfuse.o \
 	lz.o \
+	bq24193.o \
 	max7762x.o \
+	max17050.o \
 	mc.o \
 	nx_emmc.o \
 	sdmmc.o \
@@ -41,22 +42,26 @@ OBJS = $(addprefix $(BUILD)/, \
 	uart.o \
 	ini.o \
 )
-OBJS += $(addprefix $(BUILD)/, diskio.o ff.o ffunicode.o)
+OBJS += $(addprefix $(BUILD)/, diskio.o ff.o ffunicode.o ffsystem.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
+CUSTOMDEFINES := -DMENU_LOGO_ENABLE
+CFLAGS = $(ARCH) -O2 -nostdlib -ffunction-sections -fdata-sections -fomit-frame-pointer -fno-inline -std=gnu11 -Wall $(CUSTOMDEFINES)
 LDFLAGS = $(ARCH) -nostartfiles -lgcc -Wl,--nmagic,--gc-sections
 
 .PHONY: all clean
 
-all: $(BUILD)/$(TARGET)
+all: $(TARGET).bin
+	@echo -n "Payload size is "
+	@wc -c < $(TARGET).bin
+	@echo "Max size is 126296 Bytes."
 
 clean:
 	@rm -rf $(OBJS)
-	@rm -rf $(BUILD)/$(TARGET).elf
-	@rm -rf $(BUILD)/$(TARGET)
+	@rm -rf $(BUILD)
+	@rm -rf $(TARGET).bin
 
-$(BUILD)/$(TARGET): $(BUILD)/$(TARGET).elf
+$(TARGET).bin: $(BUILD)/$(TARGET).elf
 	$(OBJCOPY) -S -O binary $< $@
 
 $(BUILD)/$(TARGET).elf: $(OBJS)
@@ -66,4 +71,5 @@ $(BUILD)/%.o: $(SOURCEDIR)/%.c
 	$(CC) $(CFLAGS) -c $< -o $@
 
 $(BUILD)/%.o: $(SOURCEDIR)/%.S
+	@mkdir -p "$(BUILD)"
 	$(CC) $(CFLAGS) -c $< -o $@

README.md

@@ -1,16 +1,72 @@
-# hekate
+# hekate - CTCaer mod
 
-![Image of Hekate](https://upload.wikimedia.org/wikipedia/commons/f/fc/H%C3%A9cate_-_Mallarm%C3%A9.png)
+![Image of Hekate](https://i.imgur.com/O3REoy5.png)
 
-Nintendo Switch bootloader, firmware patcher, and more.
 
-## ipl config
+Custom Nintendo Switch bootloader, firmware patcher, and more.
+
+
+## IPL configuration
 
 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:
 
- - warmboot={SD path}
- - secmon={SD path}
- - kernel={SD path}
- - kip1={SD path}
+There are four possible type of entries. "**[ ]**": Boot entry, "**{ }**": Caption, "**#**": Comment, "*newline*": .ini cosmetic newline.
+
+
+### Configuration keys/values when boot entry is **config**:
+
+| Config option      | Description                                                |
+| ------------------ | ---------------------------------------------------------- |
+| autoboot=0         | 0: Disable, #: Boot entry number to auto boot.             |
+| bootwait=3         | 0: Disable (Having VOL- pressed since injection goes to menu. It also disables bootlogo.), #: Time to wait for **VOL-** to enter menu. |
+| customlogo=0       | 0: Use default hekate bootlogo, 1: Use bootlogo.bmp.       |
+| verification=2     | 0: Disable Backup/Restore verification, 1: Sparse (block based, fast and not 100% reliable), 2: Full (sha256 based, slow and 100% reliable). |
+
+
+### Possible boot entry key/value combinations:
+
+| Config option      | Description                                                |
+| ------------------ | ---------------------------------------------------------- |
+| logopath={SD path} | If global customlogo is 1 and logopath empty, bootlogo.bmp will be used. If logopath exists, it will load the specified bitmap. |
+| 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                                         |
+
+
+You can find a template [Here](./res/hekate_ipl_template.ini)
+
+
+```
+hekate     (C) 2018 naehrwert, st4rk
+CTCaer mod (C) 2018 CTCaer.
+
+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
+ - Atmosphère (SE sha256, prc id kernel patches), Copyright (C) 2018, Atmosphère-NX
+
+                         ___
+                      .-'   `'.
+                     /         \
+                     |         ;
+                     |         |           ___.--,
+            _.._     |0) = (0) |    _.---'`__.-( (_.
+     __.--'`_.. '.__.\    '--. \_.-' ,.--'`     `""`
+    ( ,.--'`   ',__ /./;   ;, '.__.'`    __
+    _`) )  .---.__.' / |   |\   \__..--""  """--.,_
+   `---' .'.''-._.-'`_./  /\ '.  \ _.--''````'''--._`-.__.'
+         | |  .' _.-' |  |  \  \  '.               `----`
+          \ \/ .'     \  \   '. '-._)
+           \/ /        \  \    `=.__`'-.
+           / /\         `) )    / / `"".`\
+     , _.-'.'\ \        / /    ( (     / /
+      `--'`   ) )    .-'.'      '.'.  | (
+             (/`    ( (`          ) )  '-;   [switchbrew]
+```

README_BOOTLOGO.md

@@ -0,0 +1,14 @@
+# hekate - Bootlogo
+
+The bootlogo can be any size with a maximum of 720 x 1280. It is automatically centered when it's smaller than 720 x 1280.
+
+When saving a landscape bootlogo, it should be rotated 90o counterclockwise.
+
+Lastly, the supported format is 32-bit BMP. Classic 24-bit BMPs are not supported for performance reasons.
+
+
+## How to configure
+
+If the custom logo option is enabled, it will try to load /bootlogo.bmp. If this is not found, the default hekate's logo will be used.
+
+If a boot entry specifies a custom logo path, this is one will be loaded. Again if this is not found, bootlogo.bmp will be loaded and if that fails, hekate's built-in will be used.

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) 0x14000000 | ((((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

ipl/bq24193.c

@@ -0,0 +1,162 @@
+/*
+* Battery charger driver for Nintendo Switch's TI BQ24193
+*
+* Copyright (C) 2018 CTCaer
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms and conditions of the GNU General Public License,
+* version 2, as published by the Free Software Foundation.
+*
+* This program is distributed in the hope it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "bq24193.h"
+#include "i2c.h"
+#include "util.h"
+
+int bq24193_get_property(enum BQ24193_reg_prop prop, int *value)
+{
+	u8 data;
+
+	switch (prop) {
+		case BQ24193_InputVoltageLimit: // Input voltage limit (mV).
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_InputSource);
+			data = (data & BQ24193_INCONFIG_VINDPM_MASK) >> 3;
+			*value += ((data >> 0) & 1) ? 80 : 0;
+			*value += ((data >> 1) & 1) ? 160 : 0;
+			*value += ((data >> 2) & 1) ? 320 : 0;
+			*value += ((data >> 3) & 1) ? 640 : 0;
+			*value += 3880;
+			break;
+		case BQ24193_InputCurrentLimit: // Input current limit (mA).
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_InputSource);
+			data &= BQ24193_INCONFIG_INLIMIT_MASK;
+			switch (data)
+			{
+			case 0:
+				*value = 100;
+				break;
+			case 1:
+				*value = 150;
+				break;
+			case 2:
+				*value = 500;
+				break;
+			case 3:
+				*value = 900;
+				break;
+			case 4:
+				*value = 1200;
+				break;
+			case 5:
+				*value = 1500;
+				break;
+			case 6:
+				*value = 2000;
+				break;
+			case 7:
+				*value = 3000;
+				break;
+			}
+			break;
+		case BQ24193_SystemMinimumVoltage: // Minimum system voltage limit (mV).
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_PORConfig);
+			*value = (data & BQ24193_PORCONFIG_SYSMIN_MASK) >> 1;
+			*value *= 100;
+			*value += 3000;
+			break;
+		case BQ24193_FastChargeCurrentLimit: // Fast charge current limit (mA).
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_ChrgCurr);
+			data = (data & BQ24193_CHRGCURR_ICHG_MASK) >> 2;
+			*value += ((data >> 0) & 1) ? 64 : 0;
+			*value += ((data >> 1) & 1) ? 128 : 0;
+			*value += ((data >> 2) & 1) ? 256 : 0;
+			*value += ((data >> 3) & 1) ? 512 : 0;
+			*value += ((data >> 4) & 1) ? 1024 : 0;
+			*value += ((data >> 5) & 1) ? 2048 : 0;
+			*value += 512;
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_ChrgCurr);
+			data &= BQ24193_CHRGCURR_20PCT_MASK;
+			if (data)
+				*value = *value * 20 / 100; // Fast charge current limit is 20%.
+			break;
+		case BQ24193_ChargeVoltageLimit: // Charge voltage limit (mV).
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_ChrgVolt);
+			data = (data & BQ24193_CHRGVOLT_VREG) >> 2; 
+			*value += ((data >> 0) & 1) ? 16 : 0;
+			*value += ((data >> 1) & 1) ? 32 : 0;
+			*value += ((data >> 2) & 1) ? 64 : 0;
+			*value += ((data >> 3) & 1) ? 128 : 0;
+			*value += ((data >> 4) & 1) ? 256 : 0;
+			*value += ((data >> 5) & 1) ? 512 : 0;
+			*value += 3504;
+			break;
+		case BQ24193_RechargeThreshold: // Recharge voltage threshold less than voltage limit (mV).
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_ChrgVolt);
+			data &= BQ24193_IRTHERMAL_THERM_MASK;
+			if (data)
+				*value = 300;
+			else
+				*value = 100;
+			break;
+		case BQ24193_ThermalRegulation: // Thermal regulation threshold (oC).
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_IRCompThermal);
+			data &= BQ24193_IRTHERMAL_THERM_MASK;
+			switch (data)
+			{
+			case 0:
+				*value = 60;
+				break;
+			case 1:
+				*value = 80;
+				break;
+			case 2:
+				*value = 100;
+				break;
+			case 3:
+				*value = 120;
+				break;
+			}
+			break;
+		case BQ24193_ChargeStatus: // 0: Not charging, 1: Pre-charge, 2: Fast charging, 3: Charge termination done
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_Status);
+			*value = (data & BQ24193_STATUS_CHRG_MASK) >> 4;
+			break;
+		case BQ24193_TempStatus: // 0: Normal, 2: Warm, 3: Cool, 5: Cold, 6: Hot.
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_FaultReg);
+			*value = data & BQ24193_FAULT_THERM_MASK;
+			break;
+		case BQ24193_DevID: // Dev ID.
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_VendorPart);
+			*value = data & BQ24193_VENDORPART_DEV_MASK;
+			break;
+		case BQ24193_ProductNumber: // Product number.
+			data = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_VendorPart);
+			*value = (data & BQ24193_VENDORPART_PN_MASK) >> 3;
+			break;
+		default:
+			return -1;
+	}
+	return 0;
+}
+
+void bq24193_fake_battery_removal()
+{
+	u8  value;
+
+	// Disable watchdog to keep BATFET disabled.
+	value = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_ChrgTermTimer);
+	value &= ~BQ24193_CHRGTERM_WATCHDOG_MASK;
+	i2c_send_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_ChrgTermTimer, value);
+
+	// Force BATFET to disabled state. This disconnects the battery from the system.
+	value = i2c_recv_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_Misc);
+	value |= BQ24193_MISC_BATFET_DI_MASK;
+	i2c_send_byte(I2C_1, BQ24193_I2C_ADDR, BQ24193_Misc, value);
+}

ipl/bq24193.h

@@ -0,0 +1,119 @@
+/*
+* Battery charger driver for Nintendo Switch's TI BQ24193
+*
+* Copyright (C) 2018 CTCaer
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms and conditions of the GNU General Public License,
+* version 2, as published by the Free Software Foundation.
+*
+* This program is distributed in the hope it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef __BQ24193_H_
+#define __BQ24193_H_
+
+#define BQ24193_I2C_ADDR 0x6B
+
+// REG 0 masks.
+#define BQ24193_INCONFIG_INLIMIT_MASK (7<<0)
+#define BQ24193_INCONFIG_VINDPM_MASK   0x78
+#define BQ24193_INCONFIG_HIZ_EN_MASK  (1<<7)
+
+// REG 1 masks.
+#define BQ24193_PORCONFIG_BOOST_MASK       (1<<0)
+#define BQ24193_PORCONFIG_SYSMIN_MASK      (7<<1)
+#define BQ24193_PORCONFIG_CHGCONFIG_MASK   (3<<4)
+#define BQ24193_PORCONFIG_I2CWATCHDOG_MASK (1<<6)
+#define BQ24193_PORCONFIG_RESET_MASK       (1<<7)
+
+// REG 2 masks.
+#define BQ24193_CHRGCURR_20PCT_MASK (1<<0)
+#define BQ24193_CHRGCURR_ICHG_MASK   0xFC
+
+// REG 3 masks.
+#define BQ24193_PRECHRG_ITERM   0x0F
+#define BQ24193_PRECHRG_IPRECHG 0xF0
+
+// REG 4 masks.
+#define BQ24193_CHRGVOLT_VTHRES  (1<<0)
+#define BQ24193_CHRGVOLT_BATTLOW (1<<1)
+#define BQ24193_CHRGVOLT_VREG     0xFC
+
+// REG 5 masks.
+#define BQ24193_CHRGTERM_ISET_MASK     (1<<0)
+#define BQ24193_CHRGTERM_CHGTIMER_MASK (3<<1)
+#define BQ24193_CHRGTERM_ENTIMER_MASK  (1<<3)
+#define BQ24193_CHRGTERM_WATCHDOG_MASK (3<<4)
+#define BQ24193_CHRGTERM_TERM_ST_MASK  (1<<6)
+#define BQ24193_CHRGTERM_TERM_EN_MASK  (1<<7)
+
+// REG 6 masks.
+#define BQ24193_IRTHERMAL_THERM_MASK    (3<<0)
+#define BQ24193_IRTHERMAL_VCLAMP_MASK   (7<<2)
+#define BQ24193_IRTHERMAL_BATTCOMP_MASK (7<<5)
+
+// REG 7 masks.
+#define BQ24193_MISC_INT_MASK       (3<<0)
+#define BQ24193_MISC_VSET_MASK      (1<<4)
+#define BQ24193_MISC_BATFET_DI_MASK (1<<5)
+#define BQ24193_MISC_TMR2X_EN_MASK  (1<<6)
+#define BQ24193_MISC_DPDM_EN_MASK   (1<<7)
+
+// REG 8 masks.
+#define BQ24193_STATUS_VSYS_MASK  (1<<0)
+#define BQ24193_STATUS_THERM_MASK (1<<1)
+#define BQ24193_STATUS_PG_MASK    (1<<2)
+#define BQ24193_STATUS_DPM_MASK   (1<<3)
+#define BQ24193_STATUS_CHRG_MASK  (3<<4)
+#define BQ24193_STATUS_VBUS_MASK  (3<<6)
+
+// REG 9 masks.
+#define BQ24193_FAULT_THERM_MASK    (7<<0)
+#define BQ24193_FAULT_BATT_OVP_MASK (1<<3)
+#define BQ24193_FAULT_CHARGE_MASK   (3<<4)
+#define BQ24193_FAULT_BOOST_MASK    (1<<6)
+#define BQ24193_FAULT_WATCHDOG_MASK (1<<7)
+
+// REG A masks.
+#define BQ24193_VENDORPART_DEV_MASK (3<<0)
+#define BQ24193_VENDORPART_PN_MASK  (7<<3)
+
+enum BQ24193_reg {
+	BQ24193_InputSource     = 0x00,
+	BQ24193_PORConfig       = 0x01,
+	BQ24193_ChrgCurr        = 0x02,
+	BQ24193_PreChrgTerm     = 0x03,
+	BQ24193_ChrgVolt        = 0x04,
+	BQ24193_ChrgTermTimer   = 0x05,
+	BQ24193_IRCompThermal   = 0x06,
+	BQ24193_Misc            = 0x07,
+	BQ24193_Status          = 0x08,
+	BQ24193_FaultReg        = 0x09,
+	BQ24193_VendorPart      = 0x0A,
+};
+
+enum BQ24193_reg_prop {
+	BQ24193_InputVoltageLimit, // REG 0.
+	BQ24193_InputCurrentLimit,  // REG 0.
+	BQ24193_SystemMinimumVoltage, // REG 1.
+	BQ24193_FastChargeCurrentLimit, // REG 2.
+	BQ24193_ChargeVoltageLimit, // REG 4.
+	BQ24193_RechargeThreshold,  // REG 4.
+	BQ24193_ThermalRegulation, // REG 6.
+	BQ24193_ChargeStatus, // REG 8.
+	BQ24193_TempStatus, // REG 9.
+	BQ24193_DevID,         // REG A.
+	BQ24193_ProductNumber, // REG A.
+};
+
+int bq24193_get_property(enum BQ24193_reg_prop prop, int *value);
+void bq24193_fake_battery_removal();
+
+#endif /* __BQ24193_H_ */

ipl/btn.c

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -18,6 +19,7 @@
 #include "i2c.h"
 #include "gpio.h"
 #include "t210.h"
+#include "util.h"
 
 u32 btn_read()
 {
@@ -34,9 +36,38 @@ u32 btn_read()
 u32 btn_wait()
 {
 	u32 res = 0, btn = btn_read();
+	int pwr = 0;
+
+	//Power button down, raise a filter.
+	if (btn & BTN_POWER)
+	{
+		pwr = 1;
+		btn &= ~BTN_POWER;
+	}
+
 	do
 	{
 		res = btn_read();
+		//Power button up, remove filter.
+		if (!(res & BTN_POWER) && pwr)
+			pwr = 0;
+		else if (pwr) //Power button still down.
+			res &= ~BTN_POWER;
 	} while (btn == res);
+
+	return res;
+}
+
+u32 btn_wait_timeout(u32 time_ms, u32 mask)
+{
+	u32 timeout = get_tmr_ms() + time_ms;
+	u32 res = btn_read() & mask;
+
+	do
+	{
+		if (!(res & mask))
+			res = btn_read() & mask;
+	} while (get_tmr_ms() < timeout);
+
 	return res;
 }

ipl/btn.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -25,5 +26,6 @@
 
 u32 btn_read();
 u32 btn_wait();
+u32 btn_wait_timeout(u32 time_ms, u32 mask);
 
 #endif

ipl/clock.c

@@ -20,45 +20,45 @@
 #include "sdmmc.h"
 
 static const clock_t _clock_uart[] =  {
-	/* UART A */ { 4, 0x10, 0x178, 6, 0, 0 },
-	/* UART B */ { 4, 0x10, 0x17C, 7, 0, 0 },
-	/* UART C */ { 8, 0x14, 0x1A0, 0x17, 0, 0 },
+	/* UART A */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_UARTA, 6, 0, 0 },
+	/* UART B */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_UARTB, 7, 0, 0 },
+	/* UART C */ { CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_UARTC, 0x17, 0, 0 },
 	/* UART D */ { 0 },
 	/* UART E */ { 0 }
 };
 
 static const clock_t _clock_i2c[] = {
-	/* I2C1 */ { 4, 0x10, 0x124, 0xC, 6, 0 },
+	/* I2C1 */ { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_I2C1, 0xC, 6, 0 },
 	/* I2C2 */ { 0 },
 	/* I2C3 */ { 0 },
 	/* I2C4 */ { 0 },
-	/* I2C5 */ { 8, 0x14, 0x128, 0xF, 6, 0 },
+	/* I2C5 */ { CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_I2C5, 0xF, 6, 0 },
 	/* I2C6 */ { 0 }
 };
 
-static clock_t _clock_se = { 0x358, 0x360, 0x42C, 0x1F, 0, 0 };
+static clock_t _clock_se = { CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, 0x42C, 0x1F, 0, 0 };
 
-static clock_t _clock_host1x = { 4, 0x10, 0x180, 0x1C, 4, 3 };
-static clock_t _clock_tsec = { 0xC, 0x18, 0x1F4, 0x13, 0, 2 };
-static clock_t _clock_sor_safe = { 0x2A4, 0x298, 0, 0x1E, 0, 0 };
-static clock_t _clock_sor0 = { 0x28C, 0x280, 0, 0x16, 0, 0 };
-static clock_t _clock_sor1 = { 0x28C, 0x280, 0x410, 0x17, 0, 2 };
-static clock_t _clock_kfuse = { 8, 0x14, 0, 8, 0, 0 };
+static clock_t _clock_host1x = { CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X, 0x1C, 4, 3 };
+static clock_t _clock_tsec = { CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_TSEC, 0x13, 0, 2 };
+static clock_t _clock_sor_safe = { CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_RST_CONTROLLER_RST_SOURCE, 0x1E, 0, 0 };
+static clock_t _clock_sor0 = { CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_RST_CONTROLLER_RST_SOURCE, 0x16, 0, 0 };
+static clock_t _clock_sor1 = { CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_RST_CONTROLLER_CLK_SOURCE_SOR1, 0x17, 0, 2 };
+static clock_t _clock_kfuse = { CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_RST_SOURCE, 8, 0, 0 };
 
-static clock_t _clock_cl_dvfs = { 0x35C, 0x364, 0, 0x1B, 0, 0 };
-static clock_t _clock_coresight = { 0xC, 0x18, 0x1D4, 9, 0, 4};
+static clock_t _clock_cl_dvfs = { CLK_RST_CONTROLLER_RST_DEVICES_W, CLK_RST_CONTROLLER_CLK_OUT_ENB_W, CLK_RST_CONTROLLER_RST_SOURCE, 0x1B, 0, 0 };
+static clock_t _clock_coresight = { CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_CSITE, 9, 0, 4};
 
 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,12 +144,12 @@ 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;
-	sleep(10);
+	CLOCK(0x14) = (CLOCK(0x14) & 0xFFFFFEFF) | 0x100;
+	usleep(10);
 	CLOCK(0x8) &= 0xFFFFFEFF;
-	sleep(20);
+	usleep(20);
 }
 
 void clock_disable_kfuse()
@@ -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)
 	{
@@ -436,7 +436,7 @@ void clock_sdmmc_enable(u32 id, u32 val)
 	_clock_sdmmc_config_clock_source_inner(&div, id, val);
 	_clock_sdmmc_set_enable(id);
 	_clock_sdmmc_is_reset(id);
-	sleep((100000 + div - 1) / div);
+	usleep((100000 + div - 1) / div);
 	_clock_sdmmc_clear_reset(id);
 	_clock_sdmmc_is_reset(id);
 }

ipl/clock.h

@@ -20,7 +20,9 @@
 #include "types.h"
 
 /*! Clock registers. */
+#define CLK_RST_CONTROLLER_RST_SOURCE 0x0
 #define CLK_RST_CONTROLLER_RST_DEVICES_L 0x4
+#define CLK_RST_CONTROLLER_RST_DEVICES_H 0x8
 #define CLK_RST_CONTROLLER_RST_DEVICES_U 0xC
 #define CLK_RST_CONTROLLER_CLK_OUT_ENB_L 0x10
 #define CLK_RST_CONTROLLER_CLK_OUT_ENB_H 0x14
@@ -32,38 +34,79 @@
 #define CLK_RST_CONTROLLER_CLK_SYSTEM_RATE 0x30
 #define CLK_RST_CONTROLLER_MISC_CLK_ENB 0x48
 #define CLK_RST_CONTROLLER_OSC_CTRL 0x50
+#define CLK_RST_CONTROLLER_PLLC_MISC 0x88
+#define CLK_RST_CONTROLLER_PLLM_BASE 0x90
+#define CLK_RST_CONTROLLER_PLLM_MISC1 0x98
+#define CLK_RST_CONTROLLER_PLLM_MISC2 0x9C
+#define CLK_RST_CONTROLLER_PLLP_BASE 0xA0
+#define CLK_RST_CONTROLLER_PLLD_BASE 0xD0
 #define CLK_RST_CONTROLLER_PLLX_BASE 0xE0
 #define CLK_RST_CONTROLLER_PLLX_MISC 0xE4
+#define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRA 0xF8
+#define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRB 0xFC
+#define CLK_RST_CONTROLLER_CLK_SOURCE_I2C1 0x124
+#define CLK_RST_CONTROLLER_CLK_SOURCE_I2C5 0x128
+#define CLK_RST_CONTROLLER_CLK_SOURCE_VI 0x148
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC1 0x150
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC2 0x154
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC4 0x164
+#define CLK_RST_CONTROLLER_CLK_SOURCE_UARTA 0x178
+#define CLK_RST_CONTROLLER_CLK_SOURCE_UARTB 0x17C
+#define CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X 0x180
+#define CLK_RST_CONTROLLER_CLK_SOURCE_UARTC 0x1A0
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC3 0x1BC
+#define CLK_RST_CONTROLLER_CLK_SOURCE_CSITE 0x1D4
 #define CLK_RST_CONTROLLER_CLK_SOURCE_EMC 0x19C
+#define CLK_RST_CONTROLLER_CLK_SOURCE_TSEC 0x1F4
 #define CLK_RST_CONTROLLER_CLK_OUT_ENB_X 0x280
 #define CLK_RST_CONTROLLER_CLK_ENB_X_SET 0x284
+#define CLK_RST_CONTROLLER_CLK_ENB_X_CLR 0x288
+#define CLK_RST_CONTROLLER_RST_DEVICES_X 0x28C
+#define CLK_RST_CONTROLLER_RST_DEV_X_SET 0x290
+#define CLK_RST_CONTROLLER_RST_DEV_X_CLR 0x294
 #define CLK_RST_CONTROLLER_CLK_OUT_ENB_Y 0x298
 #define CLK_RST_CONTROLLER_CLK_ENB_Y_SET 0x29C
+#define CLK_RST_CONTROLLER_CLK_ENB_Y_CLR 0x2A0
+#define CLK_RST_CONTROLLER_RST_DEVICES_Y 0x2A4
+#define CLK_RST_CONTROLLER_RST_DEV_Y_SET 0x2A8
+#define CLK_RST_CONTROLLER_RST_DEV_Y_CLR 0x2AC
 #define CLK_RST_CONTROLLER_RST_DEV_L_SET 0x300
 #define CLK_RST_CONTROLLER_RST_DEV_L_CLR 0x304
 #define CLK_RST_CONTROLLER_RST_DEV_H_SET 0x308
+#define CLK_RST_CONTROLLER_RST_DEV_H_CLR 0x30C
 #define CLK_RST_CONTROLLER_RST_DEV_U_SET 0x310
 #define CLK_RST_CONTROLLER_RST_DEV_U_CLR 0x314
 #define CLK_RST_CONTROLLER_CLK_ENB_L_SET 0x320
 #define CLK_RST_CONTROLLER_CLK_ENB_L_CLR 0x324
 #define CLK_RST_CONTROLLER_CLK_ENB_H_SET 0x328
+#define CLK_RST_CONTROLLER_CLK_ENB_H_CLR 0x32C
 #define CLK_RST_CONTROLLER_CLK_ENB_U_SET 0x330
 #define CLK_RST_CONTROLLER_CLK_ENB_U_CLR 0x334
 #define CLK_RST_CONTROLLER_RST_DEVICES_V 0x358
+#define CLK_RST_CONTROLLER_RST_DEVICES_W 0x35C
 #define CLK_RST_CONTROLLER_CLK_OUT_ENB_V 0x360
 #define CLK_RST_CONTROLLER_CLK_OUT_ENB_W 0x364
 #define CLK_RST_CONTROLLER_CPU_SOFTRST_CTRL2 0x388
+#define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRC 0x3A0
+#define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRD 0x3A4
 #define CLK_RST_CONTROLLER_CLK_SOURCE_MSELECT 0x3B4
+#define CLK_RST_CONTROLLER_CLK_SOURCE_SOR1 0x410
 #define CLK_RST_CONTROLLER_CLK_ENB_V_SET 0x440
+#define CLK_RST_CONTROLLER_CLK_ENB_W_SET 0x448
+#define CLK_RST_CONTROLLER_CLK_ENB_W_CLR 0x44C
 #define CLK_RST_CONTROLLER_RST_CPUG_CMPLX_CLR 0x454
+#define CLK_RST_CONTROLLER_UTMIP_PLL_CFG2 0x488
+#define CLK_RST_CONTROLLER_AUDIO_SYNC_CLK_I2S0 0x4A0
 #define CLK_RST_CONTROLLER_PLLX_MISC_3 0x518
+#define CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRE 0x554
 #define CLK_RST_CONTROLLER_SPARE_REG0 0x55C
 #define CLK_RST_CONTROLLER_PLLMB_BASE 0x5E8
+#define CLK_RST_CONTROLLER_CLK_SOURCE_DSIA_LP 0x620
+#define CLK_RST_CONTROLLER_CLK_SOURCE_EMC_DLL 0x664
+#define CLK_RST_CONTROLLER_CLK_SOURCE_UART_FST_MIP_CAL 0x66C
 #define CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC_LEGACY_TM 0x694
+#define CLK_RST_CONTROLLER_CLK_SOURCE_NVENC 0x6A0
+#define CLK_RST_CONTROLLER_SE_SUPER_CLK_DIVIDER 0x704
 
 /*! Generic clock descriptor. */
 typedef struct _clock_t

ipl/cluster.c

@@ -44,7 +44,7 @@ int _cluster_pmc_enable_partition(u32 part, u32 toggle)
 	u32 i = 5001;
 	while (PMC(APBDEV_PMC_PWRGATE_TOGGLE) & 0x100)
 	{
-		sleep(1);
+		usleep(1);
 		i--;
 		if (i < 1)
 			return 0;
@@ -57,7 +57,7 @@ int _cluster_pmc_enable_partition(u32 part, u32 toggle)
 	{
 		if (PMC(APBDEV_PMC_PWRGATE_STATUS) & part)
 			break;
-		sleep(1);
+		usleep(1);
 		i--;
 	}
 
@@ -74,18 +74,18 @@ void cluster_boot_cpu0(u32 entry)
 	if (!(CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) & 0x40000000))
 	{
 		CLOCK(CLK_RST_CONTROLLER_PLLX_MISC_3) &= 0xFFFFFFF7;
-		sleep(2);
+		usleep(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);

ipl/cluster.h

@@ -20,6 +20,7 @@
 #include "types.h"
 
 /*! Flow controller registers. */
+#define FLOW_CTLR_HALT_COP_EVENTS 0x4
 #define FLOW_CTLR_RAM_REPAIR 0x40
 #define FLOW_CTLR_BPMP_CLUSTER_CONTROL 0x98
 

ipl/config.c

@@ -0,0 +1,437 @@
+/*
+* Copyright (c) 2018 CTCaer
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms and conditions of the GNU General Public License,
+* version 2, as published by the Free Software Foundation.
+*
+* This program is distributed in the hope it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <string.h>
+#include <stdlib.h>
+
+#include "btn.h"
+#include "config.h"
+#include "ff.h"
+#include "ini.h"
+#include "list.h"
+#include "tui.h"
+#include "util.h"
+#include "gfx.h"
+
+extern gfx_ctxt_t gfx_ctxt;
+extern gfx_con_t gfx_con;
+//TODO: Create more macros (info, header, debug, etc) with different colors and utilize them for consistency.
+#define EPRINTF(text) gfx_printf(&gfx_con, "%k"text"%k\n", 0xFFFF0000, 0xFFCCCCCC)
+#define EPRINTFARGS(text, args...) gfx_printf(&gfx_con, "%k"text"%k\n", 0xFFFF0000, args, 0xFFCCCCCC)
+
+extern hekate_config h_cfg;
+extern int sd_mount();
+extern int sd_unmount();
+
+void set_default_configuration()
+{
+	h_cfg.autoboot = 0;
+	h_cfg.bootwait = 3;
+	h_cfg.customlogo = 0;
+	h_cfg.verification = 2;
+}
+
+int create_config_entry()
+{
+	if (!sd_mount())
+		return 1;
+
+	char lbuf[16];
+	FIL fp;
+
+	LIST_INIT(ini_sections);
+
+	if (ini_parse(&ini_sections, "hekate_ipl.ini"))
+	{
+		if (f_open(&fp, "hekate_ipl.ini", FA_WRITE | FA_CREATE_ALWAYS) != FR_OK)
+			return 0;
+		// Add config entry.
+		f_puts("[config]\nautoboot=", &fp);
+		itoa(h_cfg.autoboot, lbuf, 10);
+		f_puts(lbuf, &fp);
+		f_puts("\nbootwait=", &fp);
+		itoa(h_cfg.bootwait, lbuf, 10);
+		f_puts(lbuf, &fp);
+		f_puts("\ncustomlogo=", &fp);
+		itoa(h_cfg.customlogo, lbuf, 10);
+		f_puts(lbuf, &fp);
+		f_puts("\nverification=", &fp);
+		itoa(h_cfg.verification, lbuf, 10);   
+		f_puts(lbuf, &fp);
+		f_puts("\n", &fp);
+
+		// Re-construct existing entries.
+		LIST_FOREACH_ENTRY(ini_sec_t, ini_sec, &ini_sections, link)
+		{
+			if (!strcmp(ini_sec->name, "config"))
+				continue;
+
+			switch (ini_sec->type)
+			{
+			case INI_CHOICE: // Re-construct Boot entry [ ].
+				f_puts("[", &fp);
+				f_puts(ini_sec->name, &fp);
+				f_puts("]\n", &fp);
+				// Re-construct boot entry's config.
+				LIST_FOREACH_ENTRY(ini_kv_t, kv, &ini_sec->kvs, link)
+				{
+					f_puts(kv->key, &fp);
+					f_puts("=", &fp);
+					f_puts(kv->val, &fp);
+					f_puts("\n", &fp);
+				}
+				break;
+			case INI_CAPTION: // Re-construct caption entry { }.
+				f_puts("{", &fp);
+				f_puts(ini_sec->name, &fp);
+				f_puts("}\n", &fp);
+				break;
+			case INI_NEWLINE: // Re-construct cosmetic newline \n.
+				f_puts("\n", &fp);
+				break;
+			case INI_COMMENT: // Re-construct comment entry #.
+				f_puts("#", &fp);
+				f_puts(ini_sec->name, &fp);
+				f_puts("\n", &fp);
+				break;
+			}
+		}
+
+		f_close(&fp);
+		sd_unmount();
+	}
+	else
+		return 1;
+
+	ini_free(&ini_sections);
+
+	return 0;
+}
+
+void config_autoboot()
+{
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
+	gfx_con_setpos(&gfx_con, 0, 0);
+
+	u32 *temp_autoboot = NULL;
+
+	LIST_INIT(ini_sections);
+
+	u8 max_entries = 29;
+	int ini_freed = 1;
+
+	ment_t *ments = (ment_t *)malloc(sizeof(ment_t) * (max_entries + 3));
+	u32 *boot_values = (u32 *)malloc(sizeof(u32) * max_entries);
+	char *boot_text = (char *)malloc(512 * max_entries);
+
+	for (u32 j = 0; j < max_entries; j++)
+		boot_values[j] = j;
+
+	if (sd_mount())
+	{
+		if (ini_parse(&ini_sections, "hekate_ipl.ini"))
+		{
+			ini_freed = 0;
+
+			// Build configuration menu.
+			ments[0].type = MENT_BACK;
+			ments[0].caption = "Back";
+
+			ments[1].type = MENT_CHGLINE;
+
+			ments[2].type = MENT_CHOICE;
+			if (!h_cfg.autoboot)
+				ments[2].caption = "*Disable";
+			else
+				ments[2].caption = " Disable";
+			ments[2].data = &boot_values[0];
+
+			u32 i = 3;
+			LIST_FOREACH_ENTRY(ini_sec_t, ini_sec, &ini_sections, link)
+			{
+				// Skip other ini entries for autoboot.
+				if (ini_sec->type == INI_CHOICE)
+				{
+					if (!strcmp(ini_sec->name, "config"))
+						continue;
+
+					if (strlen(ini_sec->name) > 510)
+						ments[i].caption = ini_sec->name;
+					else
+					{
+						if (h_cfg.autoboot != (i - 2))
+							boot_text[(i - 2) * 512] = ' ';
+
+						else
+							boot_text[(i - 2) * 512] = '*';
+						memcpy(boot_text + (i - 2) * 512 + 1, ini_sec->name, strlen(ini_sec->name));
+						boot_text[strlen(ini_sec->name) + (i - 2) * 512 + 1] = 0;
+						ments[i].caption = &boot_text[(i - 2) * 512];
+					}
+					ments[i].type = ini_sec->type;
+					ments[i].data = &boot_values[i - 2];
+					i++;
+
+					if (i > max_entries)
+						break;
+				}
+			}
+			if (i < 4)
+			{
+				EPRINTF("No launch configurations found.");
+				goto out;
+			}
+
+			memset(&ments[i], 0, sizeof(ment_t));
+			menu_t menu = {
+				ments, "Disable or select entry to auto boot", 0, 0
+			};
+			temp_autoboot = (u32 *)tui_do_menu(&gfx_con, &menu);
+			if (temp_autoboot != NULL)
+			{
+				gfx_clear_grey(&gfx_ctxt, 0x1B);
+				gfx_con_setpos(&gfx_con, 0, 0);
+
+				h_cfg.autoboot = *(u32 *)temp_autoboot;
+				// Save choice to ini file.
+				if (!create_config_entry())
+					gfx_puts(&gfx_con, "\nConfiguration was saved!\n");
+				else
+					EPRINTF("\nConfiguration saving failed!");
+				gfx_puts(&gfx_con, "\nPress any key...");
+			}
+			else
+				goto out2;
+		}
+		else
+		{
+			EPRINTF("Could not find or open 'hekate_ipl.ini'.\nMake sure it exists in SD Card!.");
+			goto out;
+		}
+	}
+
+out:;
+	btn_wait();
+out2:;
+	free(ments);
+	free(boot_values);
+	free(boot_text);
+	if (!ini_freed)
+		ini_free(&ini_sections);
+
+	sd_unmount();
+
+	if (temp_autoboot == NULL)
+		return;
+}
+
+void config_bootdelay()
+{
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
+	gfx_con_setpos(&gfx_con, 0, 0);
+
+	u32 delay_entries = 6;
+
+	ment_t *ments = (ment_t *)malloc(sizeof(ment_t) * (delay_entries + 3));
+	u32 *delay_values = (u32 *)malloc(sizeof(u32) * delay_entries);
+	char *delay_text = (char *)malloc(32 * delay_entries);
+
+	for (u32 j = 0; j < delay_entries; j++)
+		delay_values[j] = j;
+
+	ments[0].type = MENT_BACK;
+	ments[0].caption = "Back";
+
+	ments[1].type = MENT_CHGLINE;
+
+	ments[2].type = MENT_CHOICE;
+	if (h_cfg.bootwait)
+		ments[2].caption = " 0 seconds (Bootlogo disabled)";
+	else
+		ments[2].caption = "*0 seconds (Bootlogo disabled)";
+	ments[2].data = &delay_values[0];
+
+	u32 i = 0;
+	for (i = 1; i < delay_entries; i++)
+	{
+		if (h_cfg.bootwait != i)
+			delay_text[i * 32] = ' ';
+		else
+			delay_text[i * 32] = '*';
+		delay_text[i * 32 + 1] = i + '0';
+		memcpy(delay_text + i * 32 + 2, " seconds\0", 9);
+		
+		ments[i + 2].type = MENT_CHOICE;
+		ments[i + 2].caption = delay_text + i * 32;
+		ments[i + 2].data = &delay_values[i];
+	}
+
+	memset(&ments[i + 2], 0, sizeof(ment_t));
+	menu_t menu = {
+		ments, "Time delay for entering bootloader menu", 0, 0
+	};
+
+	u32 *temp_bootwait = (u32 *)tui_do_menu(&gfx_con, &menu);
+	if (temp_bootwait != NULL)
+	{
+		gfx_clear_grey(&gfx_ctxt, 0x1B);
+		gfx_con_setpos(&gfx_con, 0, 0);
+
+		h_cfg.bootwait = *(u32 *)temp_bootwait;
+		// Save choice to ini file.
+		if (!create_config_entry())
+			gfx_puts(&gfx_con, "\nConfiguration was saved!\n");
+		else
+			EPRINTF("\nConfiguration saving failed!");
+		gfx_puts(&gfx_con, "\nPress any key...");
+	}
+
+	free(ments);
+	free(delay_values);
+	free(delay_text);
+
+	if (temp_bootwait == NULL)
+		return;
+	btn_wait();
+}
+
+void config_customlogo()
+{
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
+	gfx_con_setpos(&gfx_con, 0, 0);
+
+	ment_t *ments = (ment_t *)malloc(sizeof(ment_t) * 5);
+	u32 *cb_values = (u32 *)malloc(sizeof(u32) * 2);
+
+	for (u32 j = 0; j < 2; j++)
+	{
+		cb_values[j] = j;
+		ments[j + 2].type = MENT_CHOICE;
+		ments[j + 2].data = &cb_values[j];
+	}
+
+	ments[0].type = MENT_BACK;
+	ments[0].caption = "Back";
+
+	ments[1].type = MENT_CHGLINE;
+
+	if (h_cfg.customlogo)
+	{
+		ments[2].caption = " Disable";
+		ments[3].caption = "*Enable";
+
+	}
+	else
+	{
+		ments[2].caption = "*Disable";
+		ments[3].caption = " Enable";
+	}
+		
+	memset(&ments[4], 0, sizeof(ment_t));
+	menu_t menu = {
+		ments, "Custom bootlogo", 0, 0
+	};
+
+	u32 *temp_customlogo = (u32 *)tui_do_menu(&gfx_con, &menu);
+	if (temp_customlogo != NULL)
+	{
+		gfx_clear_grey(&gfx_ctxt, 0x1B);
+		gfx_con_setpos(&gfx_con, 0, 0);
+
+		h_cfg.customlogo = *(u32 *)temp_customlogo;
+		// Save choice to ini file.
+		if (!create_config_entry())
+			gfx_puts(&gfx_con, "\nConfiguration was saved!\n");
+		else
+			EPRINTF("\nConfiguration saving failed!");
+		gfx_puts(&gfx_con, "\nPress any key...");
+	}
+
+	free(ments);
+	free(cb_values);
+
+	if (temp_customlogo == NULL)
+		return;
+	btn_wait();
+}
+
+void config_verification()
+{
+	gfx_clear_grey(&gfx_ctxt, 0x1B);
+	gfx_con_setpos(&gfx_con, 0, 0);
+
+	ment_t *ments = (ment_t *)malloc(sizeof(ment_t) * 6);
+	u32 *vr_values = (u32 *)malloc(sizeof(u32) * 3);
+	char *vr_text = (char *)malloc(64 * 3);
+
+	for (u32 j = 0; j < 3; j++)
+	{
+		vr_values[j] = j;
+		ments[j + 2].type = MENT_CHOICE;
+		ments[j + 2].data = &vr_values[j];
+	}
+
+	ments[0].type = MENT_BACK;
+	ments[0].caption = "Back";
+
+	ments[1].type = MENT_CHGLINE;
+
+	memcpy(vr_text,       " Disable\0", 9);
+	memcpy(vr_text + 64,  " Sparse (Fast - Not  reliable)\0", 31);
+	memcpy(vr_text + 128, " Full   (Slow - 100% reliable)\0", 31);
+
+	for (u32 i = 0; i < 3; i++)
+	{
+		if (h_cfg.verification != i)
+		{
+			vr_text[64 * i] = ' ';
+			ments[2 + i].caption = vr_text + (i * 64);
+		}
+		else
+		{
+			vr_text[64 * i] = '*';
+			ments[2 + i].caption = vr_text + (i * 64);
+		}
+	}
+		
+	memset(&ments[5], 0, sizeof(ment_t));
+	menu_t menu = {
+		ments, "Backup & Restore verification", 0, 0
+	};
+
+	u32 *temp_verification = (u32 *)tui_do_menu(&gfx_con, &menu);
+	if (temp_verification != NULL)
+	{
+		gfx_clear_grey(&gfx_ctxt, 0x1B);
+		gfx_con_setpos(&gfx_con, 0, 0);
+
+		h_cfg.verification = *(u32 *)temp_verification;
+		// Save choice to ini file.
+		if (!create_config_entry())
+			gfx_puts(&gfx_con, "\nConfiguration was saved!\n");
+		else
+			EPRINTF("\nConfiguration saving failed!");
+		gfx_puts(&gfx_con, "\nPress any key...");
+	}
+
+	free(ments);
+	free(vr_values);
+	free(vr_text);
+
+	if (temp_verification == NULL)
+		return;
+	btn_wait();
+}

ipl/config.h

@@ -0,0 +1,37 @@
+/*
+* Copyright (c) 2018 CTCaer
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms and conditions of the GNU General Public License,
+* version 2, as published by the Free Software Foundation.
+*
+* This program is distributed in the hope it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _CONFIG_H_
+#define _CONFIG_H_
+
+#include "types.h"
+
+typedef struct _hekate_config
+{
+	u32 autoboot;
+	u32 bootwait;
+	u32 customlogo;
+	u32 verification;
+}hekate_config;
+
+void set_default_configuration();
+int create_config_entry();
+void config_autoboot();
+void config_bootdelay();
+void config_customlogo();
+void config_verification();
+
+#endif /* _CONFIG_H_ */

ipl/di.c

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -14,12 +15,17 @@
 * 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 "clock.h"
 
 #include "di.inl"
 
@@ -27,10 +33,10 @@ static u32 _display_ver = 0;
 
 static void _display_dsi_wait(u32 timeout, u32 off, u32 mask)
 {
-	u32 end = TMR(0x10) + timeout;
-	while (TMR(0x10) < end && DSI(off) & mask)
+	u32 end = get_tmr_us() + timeout;
+	while (get_tmr_us() < end && DSI(off) & mask)
 		;
-	sleep(5);
+	usleep(5);
 }
 
 void display_init()
@@ -40,39 +46,39 @@ void display_init()
 	i2c_send_byte(I2C_5, 0x3C, MAX77620_REG_GPIO7, 0x09);
 
 	//Enable MIPI CAL, DSI, DISP1, HOST1X, UART_FST_MIPI_CAL, DSIA LP clocks.
-	CLOCK(0x30C) = 0x1010000;
-	CLOCK(0x328) = 0x1010000;
-	CLOCK(0x304) = 0x18000000;
-	CLOCK(0x320) = 0x18000000;
-	CLOCK(0x284) = 0x20000;
-	CLOCK(0x66C) = 0xA;
-	CLOCK(0x448) = 0x80000;
-	CLOCK(0x620) = 0xA;
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_CLR) = 0x1010000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = 0x1010000;
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_CLR) = 0x18000000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_SET) = 0x18000000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) = 0x20000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_UART_FST_MIP_CAL) = 0xA;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_W_SET) = 0x80000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_DSIA_LP) = 0xA;
 
 	//DPD idle.
 	PMC(APBDEV_PMC_IO_DPD_REQ) = 0x40000000;
 	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) &= ~PINMUX_TRISTATE;
+	PINMUX_AUX(PINMUX_AUX_NFC_INT) &= ~PINMUX_TRISTATE;
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_PWM) &= ~PINMUX_TRISTATE;
+	PINMUX_AUX(PINMUX_AUX_LCD_BL_EN) &= ~PINMUX_TRISTATE;
+	PINMUX_AUX(PINMUX_AUX_LCD_RST) &= ~PINMUX_TRISTATE;
 
-	GPIO_3(0x00) = GPIO_3(0x00) & 0xFFFFFFFC | 0x3;
-	GPIO_3(0x10) = GPIO_3(0x10) & 0xFFFFFFFC | 0x3;
-	GPIO_3(0x20) = GPIO_3(0x20) & 0xFFFFFFFE | 0x1;
+	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(10000u);
+	usleep(10000);
 
-	GPIO_3(0x20) = GPIO_3(0x20) & 0xFFFFFFFD | 0x2;
+	gpio_write(GPIO_PORT_I, GPIO_PIN_1, GPIO_HIGH); //Backlight -5V enable.
 
-	sleep(10000);
+	usleep(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_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;
@@ -81,107 +87,115 @@ void display_init()
 	exec_cfg((u32 *)DISPLAY_A_BASE, _display_config_2, 94);
 	exec_cfg((u32 *)DSI_BASE, _display_config_3, 60);
 
-	sleep(10000);
+	usleep(10000);
 
-	GPIO_6(0x24) = GPIO_6(0x24) & 0xFFFFFFFB | 0x4;
+	gpio_write(GPIO_PORT_V, GPIO_PIN_2, GPIO_HIGH); //Backlight Reset enable.
 
-	sleep(60000);
+	usleep(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);
+	usleep(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);
+	usleep(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);
+	usleep(20000);
 
 	exec_cfg((u32 *)DSI_BASE, _display_config_5, 21);
 	exec_cfg((u32 *)CLOCK_BASE, _display_config_6, 3);
 	DISPLAY_A(_DIREG(DC_DISP_DISP_CLOCK_CONTROL)) = 4;
 	exec_cfg((u32 *)DSI_BASE, _display_config_7, 10);
 
-	sleep(10000);
+	usleep(10000);
 
 	exec_cfg((u32 *)MIPI_CAL_BASE, _display_config_8, 6);
 	exec_cfg((u32 *)DSI_BASE, _display_config_9, 4);
 	exec_cfg((u32 *)MIPI_CAL_BASE, _display_config_10, 16);
 
-	sleep(10000);
+	usleep(10000);
 
 	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);
 
-	sleep(10000);
+	usleep(10000);
 
 	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);
+	usleep(50000);
 
-	GPIO_6(0x24) &= 0xFFFFFFFB;
+	//gpio_write(GPIO_PORT_V, GPIO_PIN_2, GPIO_LOW); //Backlight Reset disable.
 
-	sleep(10000);
+	//usleep(10000);
 
-	GPIO_3(0x20) &= 0xFFFFFFFD;
+	//gpio_write(GPIO_PORT_I, GPIO_PIN_1, GPIO_LOW); //Backlight -5V disable.
 
-	sleep(10000);
+	//usleep(10000);
 
-	GPIO_3(0x20) = (GPIO_3(0x20) >> 1) << 1;
+	//gpio_write(GPIO_PORT_I, GPIO_PIN_0, GPIO_LOW); //Backlight +5V disable.
 
-	sleep(10000);
+	//usleep(10000);
 
 	//Disable clocks.
-	CLOCK(0x308) = 0x1010000;
-	CLOCK(0x32C) = 0x1010000;
-	CLOCK(0x300) = 0x18000000;
-	CLOCK(0x324) = 0x18000000;
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_SET) = 0x1010000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_CLR) = 0x1010000;
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_SET) = 0x18000000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_CLR) = 0x18000000;
 
-	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) & ~PINMUX_TRISTATE) | PINMUX_TRISTATE;
+	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 +207,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);
+	usleep(35000);
 
-	GPIO_6(0x24) = GPIO_6(0x24) & 0xFFFFFFFE | 1;
+	display_backlight(1);
 }
 
-u32 *display_init_framebuffer(u32 *fb)
+u32 *display_init_framebuffer()
 {
+	//Sanitize framebuffer area.
+	memset((u32 *)0xC0000000, 0, 0x3C0000);
 	//This configures the framebuffer @ 0xC0000000 with a resolution of 1280x720 (line stride 768).
 	exec_cfg((u32 *)DISPLAY_A_BASE, cfg_display_framebuffer, 32);
 
-	sleep(35000);
+	usleep(35000);
 
-	GPIO_6(0x24) = GPIO_6(0x24) & 0xFFFFFFFE | 1;
+	//Enable backlight
+	//display_backlight(1);
 
 	return (u32 *)0xC0000000;
 }

ipl/di.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -21,44 +22,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,7 +342,10 @@ void display_end();
 /*! Show one single color on the display. */
 void display_color_screen(u32 color);
 
-/*! Init display in full 1280x720 resolution (32bpp, line stride 768, framebuffer size = 1280*768*4 bytes). */
+/*! Switches screen backlight ON/OFF. */
+void display_backlight(u8 enable);
+
+/*! Init display in full 1280x720 resolution (B8G8R8A8, line stride 768, framebuffer size = 1280*768*4 bytes). */
 u32 *display_init_framebuffer();
 
 #endif

ipl/di.inl

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -18,263 +19,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 +296,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 +324,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_B8G8R8A8}, //T_A8R8G8B8 //NX Default: T_A8B8G8R8, WIN_COLOR_DEPTH_R8G8B8A8
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_WIN_WIN_OPTIONS, 0},
+	{DC_WIN_POSITION, 0}, //(0,0)
+	{DC_WIN_H_INITIAL_DDA, 0},
+	{DC_WIN_V_INITIAL_DDA, 0},
+	{DC_WIN_PRESCALED_SIZE, V_PRESCALED_SIZE(1280) | H_PRESCALED_SIZE(2880)}, //Pre-scaled size: 1280x2880 bytes.
+	{DC_WIN_DDA_INC, V_DDA_INC(0x1000) | H_DDA_INC(0x1000)},
+	{DC_WIN_SIZE, V_SIZE(1280) | H_SIZE(720)}, //Window size: 1280 vertical lines x 720 horizontal pixels.
+	{DC_WIN_LINE_STRIDE, 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.
 };

ipl/ff.h

@@ -1,8 +1,8 @@
 /*----------------------------------------------------------------------------/
-/  FatFs - Generic FAT Filesystem module  R0.13a                              /
+/  FatFs - Generic FAT Filesystem module  R0.13b                              /
 /-----------------------------------------------------------------------------/
 /
-/ Copyright (C) 2017, ChaN, all right reserved.
+/ Copyright (C) 2018, ChaN, all right reserved.
 /
 / FatFs module is an open source software. Redistribution and use of FatFs in
 / source and binary forms, with or without modification, are permitted provided
@@ -20,7 +20,7 @@
 
 
 #ifndef FF_DEFINED
-#define FF_DEFINED	89352	/* Revision ID */
+#define FF_DEFINED	63463	/* Revision ID */
 
 #ifdef __cplusplus
 extern "C" {
@@ -45,6 +45,12 @@ typedef struct {
 extern PARTITION VolToPart[];	/* Volume - Partition resolution table */
 #endif
 
+#if FF_STR_VOLUME_ID
+#ifndef FF_VOLUME_STRS
+extern const char* VolumeStr[FF_VOLUMES];	/* User defied volume ID */
+#endif
+#endif
+
 
 
 /* Type of path name strings on FatFs API */
@@ -60,7 +66,11 @@ typedef WCHAR TCHAR;
 typedef char TCHAR;
 #define _T(x) u8 ## x
 #define _TEXT(x) u8 ## x
-#elif FF_USE_LFN && (FF_LFN_UNICODE < 0 || FF_LFN_UNICODE > 2)
+#elif FF_USE_LFN && FF_LFN_UNICODE == 3	/* Unicode in UTF-32 encoding */
+typedef DWORD TCHAR;
+#define _T(x) U ## x
+#define _TEXT(x) U ## x
+#elif FF_USE_LFN && (FF_LFN_UNICODE < 0 || FF_LFN_UNICODE > 3)
 #error Wrong FF_LFN_UNICODE setting
 #else									/* ANSI/OEM code in SBCS/DBCS */
 typedef char TCHAR;

ipl/ffconf.h

@@ -2,7 +2,7 @@
 /  FatFs - Configuration file
 /---------------------------------------------------------------------------*/
 
-#define FFCONF_DEF 89352	/* Revision ID */
+#define FFCONF_DEF 63463	/* Revision ID */
 
 /*---------------------------------------------------------------------------/
 / Function Configurations
@@ -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. */
 
@@ -97,7 +97,7 @@
 */
 
 
-#define FF_USE_LFN		1
+#define FF_USE_LFN		3
 #define FF_MAX_LFN		255
 /* The FF_USE_LFN switches the support for LFN (long file name).
 /
@@ -123,6 +123,7 @@
 /   0: ANSI/OEM in current CP (TCHAR = char)
 /   1: Unicode in UTF-16 (TCHAR = WCHAR)
 /   2: Unicode in UTF-8 (TCHAR = char)
+/   3: Unicode in UTF-32 (TCHAR = DWORD)
 /
 /  Also behavior of string I/O functions will be affected by this option.
 /  When LFN is not enabled, this option has no effect. */
@@ -168,11 +169,16 @@
 
 #define FF_STR_VOLUME_ID	0
 #define FF_VOLUME_STRS		"sd"
-/* FF_STR_VOLUME_ID switches string support for volume ID.
-/  When FF_STR_VOLUME_ID is set to 1, also pre-defined strings can be used as drive
-/  number in the path name. FF_VOLUME_STRS defines the drive ID strings for each
-/  logical drives. Number of items must be equal to FF_VOLUMES. Valid characters for
-/  the drive ID strings are: A-Z and 0-9. */
+/* FF_STR_VOLUME_ID switches support for volume ID in arbitrary strings.
+/  When FF_STR_VOLUME_ID is set to 1 or 2, arbitrary strings can be used as drive
+/  number in the path name. FF_VOLUME_STRS defines the volume ID strings for each
+/  logical drives. Number of items must not be less than FF_VOLUMES. Valid
+/  characters for the volume ID strings are A-Z, a-z and 0-9, however, they are
+/  compared in case-insensitive. If FF_STR_VOLUME_ID >= 1 and FF_VOLUME_STRS is
+/  not defined, a user defined volume string table needs to be defined as:
+/
+/  const char* VolumeStr[FF_VOLUMES] = {"ram","flash","sd","usb",...
+*/
 
 
 #define FF_MULTI_PARTITION	0
@@ -226,7 +232,7 @@
 
 #define FF_FS_EXFAT		1
 /* This option switches support for exFAT filesystem. (0:Disable or 1:Enable)
-/  When enable exFAT, also LFN needs to be enabled.
+/  To enable exFAT, also LFN needs to be enabled.
 /  Note that enabling exFAT discards ANSI C (C89) compatibility. */
 
 
@@ -234,9 +240,9 @@
 #define FF_NORTC_MON	1
 #define FF_NORTC_MDAY	1
 #define FF_NORTC_YEAR	2018
-/* The option FF_FS_NORTC switches timestamp functiton. If the system does not have
+/* The option FF_FS_NORTC switches timestamp function. If the system does not have
 /  any RTC function or valid timestamp is not needed, set FF_FS_NORTC = 1 to disable
-/  the timestamp function. All objects modified by FatFs will have a fixed timestamp
+/  the timestamp function. Every object modified by FatFs will have a fixed timestamp
 /  defined by FF_NORTC_MON, FF_NORTC_MDAY and FF_NORTC_YEAR in local time.
 /  To enable timestamp function (FF_FS_NORTC = 0), get_fattime() function need to be
 /  added to the project to read current time form real-time clock. FF_NORTC_MON,

ipl/ffsystem.c

@@ -0,0 +1,39 @@
+/*------------------------------------------------------------------------*/
+/* Sample Code of OS Dependent Functions for FatFs                        */
+/* (C) ChaN, 2017                                                         */
+/* (C) CTCaer, 2018                                                       */
+/*------------------------------------------------------------------------*/
+
+
+#include "ff.h"
+#include "heap.h"
+
+
+
+#if FF_USE_LFN == 3	/* Dynamic memory allocation */
+
+/*------------------------------------------------------------------------*/
+/* Allocate a memory block                                                */
+/*------------------------------------------------------------------------*/
+
+void* ff_memalloc (	/* Returns pointer to the allocated memory block (null on not enough core) */
+	UINT msize		/* Number of bytes to allocate */
+)
+{
+	return malloc(msize);	/* Allocate a new memory block with POSIX API */
+}
+
+
+/*------------------------------------------------------------------------*/
+/* Free a memory block                                                    */
+/*------------------------------------------------------------------------*/
+
+void ff_memfree (
+	void* mblock	/* Pointer to the memory block to free (nothing to do for null) */
+)
+{
+	free(mblock);	/* Free the memory block with POSIX API */
+}
+
+#endif
+

ipl/ffunicode.c

@@ -27,7 +27,7 @@
 
 #if FF_USE_LFN	/* This module is blanked when non-LFN configuration */
 
-#if FF_DEFINED != 89352	/* Revision ID */
+#if FF_DEFINED != 63463	/* Revision ID */
 #error Wrong include file (ff.h).
 #endif
 
@@ -40,8 +40,7 @@
 /*------------------------------------------------------------------------*/
 
 #if FF_CODE_PAGE == 437 || FF_CODE_PAGE == 0
-static
-const WCHAR uc437[] = {	/*  CP437(U.S.) to Unicode conversion table */
+static const WCHAR uc437[] = {	/*  CP437(U.S.) to Unicode conversion table */
 	0x00C7, 0x00FC, 0x00E9, 0x00E2, 0x00E4, 0x00E0, 0x00E5, 0x00E7, 0x00EA, 0x00EB, 0x00E8, 0x00EF, 0x00EE, 0x00EC, 0x00C4, 0x00C5,
 	0x00C9, 0x00E6, 0x00C6, 0x00F4, 0x00F6, 0x00F2, 0x00FB, 0x00F9, 0x00FF, 0x00D6, 0x00DC, 0x00A2, 0x00A3, 0x00A5, 0x20A7, 0x0192,
 	0x00E1, 0x00ED, 0x00F3, 0x00FA, 0x00F1, 0x00D1, 0x00AA, 0x00BA, 0x00BF, 0x2310, 0x00AC, 0x00BD, 0x00BC, 0x00A1, 0x00AB, 0x00BB,
@@ -53,8 +52,7 @@ const WCHAR uc437[] = {	/*  CP437(U.S.) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 720 || FF_CODE_PAGE == 0
-static
-const WCHAR uc720[] = {	/*  CP720(Arabic) to Unicode conversion table */
+static const WCHAR uc720[] = {	/*  CP720(Arabic) to Unicode conversion table */
 	0x0000, 0x0000, 0x00E9, 0x00E2, 0x0000, 0x00E0, 0x0000, 0x00E7, 0x00EA, 0x00EB, 0x00E8, 0x00EF, 0x00EE, 0x0000, 0x0000, 0x0000,
 	0x0000, 0x0651, 0x0652, 0x00F4, 0x00A4, 0x0640, 0x00FB, 0x00F9, 0x0621, 0x0622, 0x0623, 0x0624, 0x00A3, 0x0625, 0x0626, 0x0627,
 	0x0628, 0x0629, 0x062A, 0x062B, 0x062C, 0x062D, 0x062E, 0x062F, 0x0630, 0x0631, 0x0632, 0x0633, 0x0634, 0x0635, 0x00AB, 0x00BB,
@@ -66,8 +64,7 @@ const WCHAR uc720[] = {	/*  CP720(Arabic) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 737 || FF_CODE_PAGE == 0
-static
-const WCHAR uc737[] = {	/*  CP737(Greek) to Unicode conversion table */
+static const WCHAR uc737[] = {	/*  CP737(Greek) to Unicode conversion table */
 	0x0391, 0x0392, 0x0393, 0x0394, 0x0395, 0x0396, 0x0397, 0x0398, 0x0399, 0x039A, 0x039B, 0x039C, 0x039D, 0x039E, 0x039F, 0x03A0,
 	0x03A1, 0x03A3, 0x03A4, 0x03A5, 0x03A6, 0x03A7, 0x03A8, 0x03A9, 0x03B1, 0x03B2, 0x03B3, 0x03B4, 0x03B5, 0x03B6, 0x03B7, 0x03B8,
 	0x03B9, 0x03BA, 0x03BB, 0x03BC, 0x03BD, 0x03BE, 0x03BF, 0x03C0, 0x03C1, 0x03C3, 0x03C2, 0x03C4, 0x03C5, 0x03C6, 0x03C7, 0x03C8,
@@ -79,8 +76,7 @@ const WCHAR uc737[] = {	/*  CP737(Greek) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 771 || FF_CODE_PAGE == 0
-static
-const WCHAR uc771[] = {	/*  CP771(KBL) to Unicode conversion table */
+static const WCHAR uc771[] = {	/*  CP771(KBL) to Unicode conversion table */
 	0x0410, 0x0411, 0x0412, 0x0413, 0x0414, 0x0415, 0x0416, 0x0417, 0x0418, 0x0419, 0x041A, 0x041B, 0x041C, 0x041D, 0x041E, 0x041F,
 	0x0420, 0x0421, 0x0422, 0x0423, 0x0424, 0x0425, 0x0426, 0x0427, 0x0428, 0x0429, 0x042A, 0x042B, 0x042C, 0x042D, 0x042E, 0x042F,
 	0x0430, 0x0431, 0x0432, 0x0433, 0x0434, 0x0435, 0x0436, 0x0437, 0x0438, 0x0439, 0x043A, 0x043B, 0x043C, 0x043D, 0x043E, 0x043F,
@@ -92,8 +88,7 @@ const WCHAR uc771[] = {	/*  CP771(KBL) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 775 || FF_CODE_PAGE == 0
-static
-const WCHAR uc775[] = {	/*  CP775(Baltic) to Unicode conversion table */
+static const WCHAR uc775[] = {	/*  CP775(Baltic) to Unicode conversion table */
 	0x0106, 0x00FC, 0x00E9, 0x0101, 0x00E4, 0x0123, 0x00E5, 0x0107, 0x0142, 0x0113, 0x0156, 0x0157, 0x012B, 0x0179, 0x00C4, 0x00C5,
 	0x00C9, 0x00E6, 0x00C6, 0x014D, 0x00F6, 0x0122, 0x00A2, 0x015A, 0x015B, 0x00D6, 0x00DC, 0x00F8, 0x00A3, 0x00D8, 0x00D7, 0x00A4,
 	0x0100, 0x012A, 0x00F3, 0x017B, 0x017C, 0x017A, 0x201D, 0x00A6, 0x00A9, 0x00AE, 0x00AC, 0x00BD, 0x00BC, 0x0141, 0x00AB, 0x00BB,
@@ -105,8 +100,7 @@ const WCHAR uc775[] = {	/*  CP775(Baltic) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 850 || FF_CODE_PAGE == 0
-static
-const WCHAR uc850[] = {	/*  CP850(Latin 1) to Unicode conversion table */
+static const WCHAR uc850[] = {	/*  CP850(Latin 1) to Unicode conversion table */
 	0x00C7, 0x00FC, 0x00E9, 0x00E2, 0x00E4, 0x00E0, 0x00E5, 0x00E7, 0x00EA, 0x00EB, 0x00E8, 0x00EF, 0x00EE, 0x00EC, 0x00C4, 0x00C5,
 	0x00C9, 0x00E6, 0x00C6, 0x00F4, 0x00F6, 0x00F2, 0x00FB, 0x00F9, 0x00FF, 0x00D6, 0x00DC, 0x00F8, 0x00A3, 0x00D8, 0x00D7, 0x0192,
 	0x00E1, 0x00ED, 0x00F3, 0x00FA, 0x00F1, 0x00D1, 0x00AA, 0x00BA, 0x00BF, 0x00AE, 0x00AC, 0x00BD, 0x00BC, 0x00A1, 0x00AB, 0x00BB,
@@ -118,8 +112,7 @@ const WCHAR uc850[] = {	/*  CP850(Latin 1) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 852 || FF_CODE_PAGE == 0
-static
-const WCHAR uc852[] = {	/*  CP852(Latin 2) to Unicode conversion table */
+static const WCHAR uc852[] = {	/*  CP852(Latin 2) to Unicode conversion table */
 	0x00C7, 0x00FC, 0x00E9, 0x00E2, 0x00E4, 0x016F, 0x0107, 0x00E7, 0x0142, 0x00EB, 0x0150, 0x0151, 0x00EE, 0x0179, 0x00C4, 0x0106,
 	0x00C9, 0x0139, 0x013A, 0x00F4, 0x00F6, 0x013D, 0x013E, 0x015A, 0x015B, 0x00D6, 0x00DC, 0x0164, 0x0165, 0x0141, 0x00D7, 0x010D,
 	0x00E1, 0x00ED, 0x00F3, 0x00FA, 0x0104, 0x0105, 0x017D, 0x017E, 0x0118, 0x0119, 0x00AC, 0x017A, 0x010C, 0x015F, 0x00AB, 0x00BB,
@@ -131,8 +124,7 @@ const WCHAR uc852[] = {	/*  CP852(Latin 2) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 855 || FF_CODE_PAGE == 0
-static
-const WCHAR uc855[] = {	/*  CP855(Cyrillic) to Unicode conversion table */
+static const WCHAR uc855[] = {	/*  CP855(Cyrillic) to Unicode conversion table */
 	0x0452, 0x0402, 0x0453, 0x0403, 0x0451, 0x0401, 0x0454, 0x0404, 0x0455, 0x0405, 0x0456, 0x0406, 0x0457, 0x0407, 0x0458, 0x0408,
 	0x0459, 0x0409, 0x045A, 0x040A, 0x045B, 0x040B, 0x045C, 0x040C, 0x045E, 0x040E, 0x045F, 0x040F, 0x044E, 0x042E, 0x044A, 0x042A,
 	0x0430, 0x0410, 0x0431, 0x0411, 0x0446, 0x0426, 0x0434, 0x0414, 0x0435, 0x0415, 0x0444, 0x0424, 0x0433, 0x0413, 0x00AB, 0x00BB,
@@ -144,8 +136,7 @@ const WCHAR uc855[] = {	/*  CP855(Cyrillic) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 857 || FF_CODE_PAGE == 0
-static
-const WCHAR uc857[] = {	/*  CP857(Turkish) to Unicode conversion table */
+static const WCHAR uc857[] = {	/*  CP857(Turkish) to Unicode conversion table */
 	0x00C7, 0x00FC, 0x00E9, 0x00E2, 0x00E4, 0x00E0, 0x00E5, 0x00E7, 0x00EA, 0x00EB, 0x00E8, 0x00EF, 0x00EE, 0x0131, 0x00C4, 0x00C5,
 	0x00C9, 0x00E6, 0x00C6, 0x00F4, 0x00F6, 0x00F2, 0x00FB, 0x00F9, 0x0130, 0x00D6, 0x00DC, 0x00F8, 0x00A3, 0x00D8, 0x015E, 0x015F,
 	0x00E1, 0x00ED, 0x00F3, 0x00FA, 0x00F1, 0x00D1, 0x011E, 0x011F, 0x00BF, 0x00AE, 0x00AC, 0x00BD, 0x00BC, 0x00A1, 0x00AB, 0x00BB,
@@ -157,8 +148,7 @@ const WCHAR uc857[] = {	/*  CP857(Turkish) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 860 || FF_CODE_PAGE == 0
-static
-const WCHAR uc860[] = {	/*  CP860(Portuguese) to Unicode conversion table */
+static const WCHAR uc860[] = {	/*  CP860(Portuguese) to Unicode conversion table */
 	0x00C7, 0x00FC, 0x00E9, 0x00E2, 0x00E3, 0x00E0, 0x00C1, 0x00E7, 0x00EA, 0x00CA, 0x00E8, 0x00CD, 0x00D4, 0x00EC, 0x00C3, 0x00C2,
 	0x00C9, 0x00C0, 0x00C8, 0x00F4, 0x00F5, 0x00F2, 0x00DA, 0x00F9, 0x00CC, 0x00D5, 0x00DC, 0x00A2, 0x00A3, 0x00D9, 0x20A7, 0x00D3,
 	0x00E1, 0x00ED, 0x00F3, 0x00FA, 0x00F1, 0x00D1, 0x00AA, 0x00BA, 0x00BF, 0x00D2, 0x00AC, 0x00BD, 0x00BC, 0x00A1, 0x00AB, 0x00BB,
@@ -170,8 +160,7 @@ const WCHAR uc860[] = {	/*  CP860(Portuguese) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 861 || FF_CODE_PAGE == 0
-static
-const WCHAR uc861[] = {	/*  CP861(Icelandic) to Unicode conversion table */
+static const WCHAR uc861[] = {	/*  CP861(Icelandic) to Unicode conversion table */
 	0x00C7, 0x00FC, 0x00E9, 0x00E2, 0x00E4, 0x00E0, 0x00E6, 0x00E7, 0x00EA, 0x00EB, 0x00E8, 0x00D0, 0x00F0, 0x00DE, 0x00C4, 0x00C5,
 	0x00C9, 0x00E6, 0x00C6, 0x00F4, 0x00F6, 0x00FE, 0x00FB, 0x00DD, 0x00FD, 0x00D6, 0x00DC, 0x00F8, 0x00A3, 0x00D8, 0x20A7, 0x0192,
 	0x00E1, 0x00ED, 0x00F3, 0x00FA, 0x00C1, 0x00CD, 0x00D3, 0x00DA, 0x00BF, 0x2310, 0x00AC, 0x00BD, 0x00BC, 0x00A1, 0x00AB, 0x00BB,
@@ -183,8 +172,7 @@ const WCHAR uc861[] = {	/*  CP861(Icelandic) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 862 || FF_CODE_PAGE == 0
-static
-const WCHAR uc862[] = {	/*  CP862(Hebrew) to Unicode conversion table */
+static const WCHAR uc862[] = {	/*  CP862(Hebrew) to Unicode conversion table */
 	0x05D0, 0x05D1, 0x05D2, 0x05D3, 0x05D4, 0x05D5, 0x05D6, 0x05D7, 0x05D8, 0x05D9, 0x05DA, 0x05DB, 0x05DC, 0x05DD, 0x05DE, 0x05DF,
 	0x05E0, 0x05E1, 0x05E2, 0x05E3, 0x05E4, 0x05E5, 0x05E6, 0x05E7, 0x05E8, 0x05E9, 0x05EA, 0x00A2, 0x00A3, 0x00A5, 0x20A7, 0x0192,
 	0x00E1, 0x00ED, 0x00F3, 0x00FA, 0x00F1, 0x00D1, 0x00AA, 0x00BA, 0x00BF, 0x2310, 0x00AC, 0x00BD, 0x00BC, 0x00A1, 0x00AB, 0x00BB,
@@ -196,8 +184,7 @@ const WCHAR uc862[] = {	/*  CP862(Hebrew) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 863 || FF_CODE_PAGE == 0
-static
-const WCHAR uc863[] = {	/*  CP863(Canadian French) to Unicode conversion table */
+static const WCHAR uc863[] = {	/*  CP863(Canadian French) to Unicode conversion table */
 	0x00C7, 0x00FC, 0x00E9, 0x00E2, 0x00C2, 0x00E0, 0x00B6, 0x00E7, 0x00EA, 0x00EB, 0x00E8, 0x00EF, 0x00EE, 0x00EC, 0x2017, 0x00C0,
 	0x00C9, 0x00C8, 0x00CA, 0x00F4, 0x00CB, 0x00CF, 0x00FB, 0x00F9, 0x00A4, 0x00D4, 0x00DC, 0x00A2, 0x00A3, 0x00D9, 0x00DB, 0x0192,
 	0x00A6, 0x00B4, 0x00F3, 0x00FA, 0x00A8, 0x00BB, 0x00B3, 0x00AF, 0x00CE, 0x3210, 0x00AC, 0x00BD, 0x00BC, 0x00BE, 0x00AB, 0x00BB,
@@ -209,8 +196,7 @@ const WCHAR uc863[] = {	/*  CP863(Canadian French) to Unicode conversion table *
 };
 #endif
 #if FF_CODE_PAGE == 864 || FF_CODE_PAGE == 0
-static
-const WCHAR uc864[] = {	/*  CP864(Arabic) to Unicode conversion table */
+static const WCHAR uc864[] = {	/*  CP864(Arabic) to Unicode conversion table */
 	0x00B0, 0x00B7, 0x2219, 0x221A, 0x2592, 0x2500, 0x2502, 0x253C, 0x2524, 0x252C, 0x251C, 0x2534, 0x2510, 0x250C, 0x2514, 0x2518,
 	0x03B2, 0x221E, 0x03C6, 0x00B1, 0x00BD, 0x00BC, 0x2248, 0x00AB, 0x00BB, 0xFEF7, 0xFEF8, 0x0000, 0x0000, 0xFEFB, 0xFEFC, 0x0000,
 	0x00A0, 0x00AD, 0xFE82, 0x00A3, 0x00A4, 0xFE84, 0x0000, 0x20AC, 0xFE8E, 0xFE8F, 0xFE95, 0xFE99, 0x060C, 0xFE9D, 0xFEA1, 0xFEA5,
@@ -222,8 +208,7 @@ const WCHAR uc864[] = {	/*  CP864(Arabic) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 865 || FF_CODE_PAGE == 0
-static
-const WCHAR uc865[] = {	/*  CP865(Nordic) to Unicode conversion table */
+static const WCHAR uc865[] = {	/*  CP865(Nordic) to Unicode conversion table */
 	0x00C7, 0x00FC, 0x00E9, 0x00E2, 0x00E4, 0x00E0, 0x00E5, 0x00E7, 0x00EA, 0x00EB, 0x00E8, 0x00EF, 0x00EE, 0x00EC, 0x00C4, 0x00C5,
 	0x00C5, 0x00E6, 0x00C6, 0x00F4, 0x00F6, 0x00F2, 0x00FB, 0x00F9, 0x00FF, 0x00D6, 0x00DC, 0x00F8, 0x00A3, 0x00D8, 0x20A7, 0x0192,
 	0x00E1, 0x00ED, 0x00F3, 0x00FA, 0x00F1, 0x00D1, 0x00AA, 0x00BA, 0x00BF, 0x2310, 0x00AC, 0x00BD, 0x00BC, 0x00A1, 0x00AB, 0x00A4,
@@ -235,8 +220,7 @@ const WCHAR uc865[] = {	/*  CP865(Nordic) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 866 || FF_CODE_PAGE == 0
-static
-const WCHAR uc866[] = {	/*  CP866(Russian) to Unicode conversion table */
+static const WCHAR uc866[] = {	/*  CP866(Russian) to Unicode conversion table */
 	0x0410, 0x0411, 0x0412, 0x0413, 0x0414, 0x0415, 0x0416, 0x0417, 0x0418, 0x0419, 0x041A, 0x041B, 0x041C, 0x041D, 0x041E, 0x041F,
 	0x0420, 0x0421, 0x0422, 0x0423, 0x0424, 0x0425, 0x0426, 0x0427, 0x0428, 0x0429, 0x042A, 0x042B, 0x042C, 0x042D, 0x042E, 0x042F,
 	0x0430, 0x0431, 0x0432, 0x0433, 0x0434, 0x0435, 0x0436, 0x0437, 0x0438, 0x0439, 0x043A, 0x043B, 0x043C, 0x043D, 0x043E, 0x043F,
@@ -248,8 +232,7 @@ const WCHAR uc866[] = {	/*  CP866(Russian) to Unicode conversion table */
 };
 #endif
 #if FF_CODE_PAGE == 869 || FF_CODE_PAGE == 0
-static
-const WCHAR uc869[] = {	/*  CP869(Greek 2) to Unicode conversion table */
+static const WCHAR uc869[] = {	/*  CP869(Greek 2) to Unicode conversion table */
 	0x00B7, 0x00B7, 0x00B7, 0x00B7, 0x00B7, 0x00B7, 0x0386, 0x00B7, 0x00B7, 0x00AC, 0x00A6, 0x2018, 0x2019, 0x0388, 0x2015, 0x0389,
 	0x038A, 0x03AA, 0x038C, 0x00B7, 0x00B7, 0x038E, 0x03AB, 0x00A9, 0x038F, 0x00B2, 0x00B3, 0x03AC, 0x00A3, 0x03AD, 0x03AE, 0x03AF,
 	0x03CA, 0x0390, 0x03CC, 0x03CD, 0x0391, 0x0392, 0x0393, 0x0394, 0x0395, 0x0396, 0x0397, 0x00BD, 0x0398, 0x0399, 0x00AB, 0x00BB,
@@ -283,7 +266,7 @@ WCHAR ff_uni2oem (	/* Returns OEM code character, zero on error */
 		c = (WCHAR)uni;
 
 	} else {			/* Non-ASCII */
-		if (uni < 0x10000 && cp == FF_CODE_PAGE) {	/* Is it a valid code? */
+		if (uni < 0x10000 && cp == FF_CODE_PAGE) {	/* Is it in BMP and valid code page? */
 			for (c = 0; c < 0x80 && uni != p[c]; c++) ;
 			c = (c + 0x80) & 0xFF;
 		}
@@ -330,30 +313,28 @@ WCHAR ff_uni2oem (	/* Returns OEM code character, zero on error */
 {
 	const WCHAR *p;
 	WCHAR c = 0, uc;
-	UINT i, n, li, hi;
+	UINT i = 0, n, li, hi;
 
 
 	if (uni < 0x80) {	/* ASCII? */
 		c = (WCHAR)uni;
 
 	} else {			/* Non-ASCII */
-		if (uni < 0x10000) {	/* Is it in BMP? */
-			if (cp == FF_CODE_PAGE) {	/* Is it a valid code? */
-				uc = (WCHAR)uni;
-				p = CVTBL(uni2oem, FF_CODE_PAGE);
-				hi = sizeof CVTBL(uni2oem, FF_CODE_PAGE) / 4 - 1;
-				li = 0;
-				for (n = 16; n; n--) {
-					i = li + (hi - li) / 2;
-					if (uc == p[i * 2]) break;
-					if (uc > p[i * 2]) {
-						li = i;
-					} else {
-						hi = i;
-					}
+		if (uni < 0x10000 && cp == FF_CODE_PAGE) {	/* Is it in BMP and valid code page? */
+			uc = (WCHAR)uni;
+			p = CVTBL(uni2oem, FF_CODE_PAGE);
+			hi = sizeof CVTBL(uni2oem, FF_CODE_PAGE) / 4 - 1;
+			li = 0;
+			for (n = 16; n; n--) {
+				i = li + (hi - li) / 2;
+				if (uc == p[i * 2]) break;
+				if (uc > p[i * 2]) {
+					li = i;
+				} else {
+					hi = i;
 				}
-				if (n != 0) c = p[i * 2 + 1];
 			}
+			if (n != 0) c = p[i * 2 + 1];
 		}
 	}
 
@@ -368,14 +349,14 @@ WCHAR ff_oem2uni (	/* Returns Unicode character, zero on error */
 {
 	const WCHAR *p;
 	WCHAR c = 0;
-	UINT i, n, li, hi;
+	UINT i = 0, n, li, hi;
 
 
 	if (oem < 0x80) {	/* ASCII? */
 		c = oem;
 
 	} else {			/* Extended char */
-		if (cp == FF_CODE_PAGE) {	/* Is it a valid code page? */
+		if (cp == FF_CODE_PAGE) {	/* Is it valid code page? */
 			p = CVTBL(oem2uni, FF_CODE_PAGE);
 			hi = sizeof CVTBL(oem2uni, FF_CODE_PAGE) / 4 - 1;
 			li = 0;
@@ -405,7 +386,7 @@ WCHAR ff_oem2uni (	/* Returns Unicode character, zero on error */
 #if FF_CODE_PAGE == 0
 
 static const WORD cp_code[]          = {  437,   720,   737,   771,   775,   850,   852,   855,   857,   860,   861,   862,   863,   864,   865,   866,   869, 0};
-static const WCHAR *const cp_table[] = {uc437, uc720, uc737, uc771, uc775, uc850, uc852, uc855, uc857, uc860, uc861, uc862, uc863, uc864, uc865, uc866, uc869, 0};
+static const WCHAR* const cp_table[] = {uc437, uc720, uc737, uc771, uc775, uc850, uc852, uc855, uc857, uc860, uc861, uc862, uc863, uc864, uc865, uc866, uc869, 0};
 
 
 WCHAR ff_uni2oem (	/* Returns OEM code character, zero on error */
@@ -426,20 +407,20 @@ WCHAR ff_uni2oem (	/* Returns OEM code character, zero on error */
 			uc = (WCHAR)uni;
 			p = 0;
 			if (cp < 900) {	/* SBCS */
-				for (i = 0; cp_code[i] != 0 && cp_code[i] != cp; i++) ;		/* Get table */
+				for (i = 0; cp_code[i] != 0 && cp_code[i] != cp; i++) ;		/* Get conversion table */
 				p = cp_table[i];
-				if (p) {	/* Is it a valid CP ? */
+				if (p) {	/* Is it valid code page ? */
 					for (c = 0; c < 0x80 && uc != p[c]; c++) ;	/* Find OEM code in the table */
 					c = (c + 0x80) & 0xFF;
 				}
 			} else {	/* DBCS */
-				switch (cp) {
+				switch (cp) {	/* Get conversion table */
 				case 932 : p = uni2oem932; hi = sizeof uni2oem932 / 4 - 1; break;
 				case 936 : p = uni2oem936; hi = sizeof uni2oem936 / 4 - 1; break;
 				case 949 : p = uni2oem949; hi = sizeof uni2oem949 / 4 - 1; break;
 				case 950 : p = uni2oem950; hi = sizeof uni2oem950 / 4 - 1; break;
 				}
-				if (p) {	/* Is it a valid code page? */
+				if (p) {	/* Is it valid code page? */
 					li = 0;
 					for (n = 16; n; n--) {	/* Find OEM code */
 						i = li + (hi - li) / 2;
@@ -518,50 +499,90 @@ DWORD ff_wtoupper (	/* Returns up-converted code point */
 	DWORD uni		/* Unicode code point to be up-converted */
 )
 {
-	/* Compressed upper conversion table */
-	static const WORD cvt1[] = {	/* U+0000 - U+0FFF */
+	const WORD *p;
+	WORD uc, bc, nc, cmd;
+	static const WORD cvt1[] = {	/* Compressed up conversion table for U+0000 - U+0FFF */
 		/* Basic Latin */
 		0x0061,0x031A,
 		/* Latin-1 Supplement */
-		0x00E0,0x0317,  0x00F8,0x0307,  0x00FF,0x0001,0x0178,
+		0x00E0,0x0317,
+		0x00F8,0x0307,
+		0x00FF,0x0001,0x0178,
 		/* Latin Extended-A */
-		0x0100,0x0130,  0x0132,0x0106,  0x0139,0x0110,  0x014A,0x012E,  0x0179,0x0106,
+		0x0100,0x0130,
+		0x0132,0x0106,
+		0x0139,0x0110,
+		0x014A,0x012E,
+		0x0179,0x0106,
 		/* Latin Extended-B */
 		0x0180,0x004D,0x0243,0x0181,0x0182,0x0182,0x0184,0x0184,0x0186,0x0187,0x0187,0x0189,0x018A,0x018B,0x018B,0x018D,0x018E,0x018F,0x0190,0x0191,0x0191,0x0193,0x0194,0x01F6,0x0196,0x0197,0x0198,0x0198,0x023D,0x019B,0x019C,0x019D,0x0220,0x019F,0x01A0,0x01A0,0x01A2,0x01A2,0x01A4,0x01A4,0x01A6,0x01A7,0x01A7,0x01A9,0x01AA,0x01AB,0x01AC,0x01AC,0x01AE,0x01AF,0x01AF,0x01B1,0x01B2,0x01B3,0x01B3,0x01B5,0x01B5,0x01B7,0x01B8,0x01B8,0x01BA,0x01BB,0x01BC,0x01BC,0x01BE,0x01F7,0x01C0,0x01C1,0x01C2,0x01C3,0x01C4,0x01C5,0x01C4,0x01C7,0x01C8,0x01C7,0x01CA,0x01CB,0x01CA,
-		0x01CD,0x0110,  0x01DD,0x0001,0x018E,  0x01DE,0x0112,  0x01F3,0x0003,0x01F1,0x01F4,0x01F4,  0x01F8,0x0128,
-		0x0222,0x0112,  0x023A,0x0009,0x2C65,0x023B,0x023B,0x023D,0x2C66,0x023F,0x0240,0x0241,0x0241,  0x0246,0x010A,
+		0x01CD,0x0110,
+		0x01DD,0x0001,0x018E,
+		0x01DE,0x0112,
+		0x01F3,0x0003,0x01F1,0x01F4,0x01F4,
+		0x01F8,0x0128,
+		0x0222,0x0112,
+		0x023A,0x0009,0x2C65,0x023B,0x023B,0x023D,0x2C66,0x023F,0x0240,0x0241,0x0241,
+		0x0246,0x010A,
 		/* IPA Extensions */
 		0x0253,0x0040,0x0181,0x0186,0x0255,0x0189,0x018A,0x0258,0x018F,0x025A,0x0190,0x025C,0x025D,0x025E,0x025F,0x0193,0x0261,0x0262,0x0194,0x0264,0x0265,0x0266,0x0267,0x0197,0x0196,0x026A,0x2C62,0x026C,0x026D,0x026E,0x019C,0x0270,0x0271,0x019D,0x0273,0x0274,0x019F,0x0276,0x0277,0x0278,0x0279,0x027A,0x027B,0x027C,0x2C64,0x027E,0x027F,0x01A6,0x0281,0x0282,0x01A9,0x0284,0x0285,0x0286,0x0287,0x01AE,0x0244,0x01B1,0x01B2,0x0245,0x028D,0x028E,0x028F,0x0290,0x0291,0x01B7,
 		/* Greek, Coptic */
-		0x037B,0x0003,0x03FD,0x03FE,0x03FF,  0x03AC,0x0004,0x0386,0x0388,0x0389,0x038A,  0x03B1,0x0311,
-		0x03C2,0x0002,0x03A3,0x03A3,  0x03C4,0x0308,  0x03CC,0x0003,0x038C,0x038E,0x038F,  0x03D8,0x0118,
+		0x037B,0x0003,0x03FD,0x03FE,0x03FF,
+		0x03AC,0x0004,0x0386,0x0388,0x0389,0x038A,
+		0x03B1,0x0311,
+		0x03C2,0x0002,0x03A3,0x03A3,
+		0x03C4,0x0308,
+		0x03CC,0x0003,0x038C,0x038E,0x038F,
+		0x03D8,0x0118,
 		0x03F2,0x000A,0x03F9,0x03F3,0x03F4,0x03F5,0x03F6,0x03F7,0x03F7,0x03F9,0x03FA,0x03FA,
 		/* Cyrillic */
-		0x0430,0x0320,  0x0450,0x0710,  0x0460,0x0122,  0x048A,0x0136,  0x04C1,0x010E,  0x04CF,0x0001,0x04C0,  0x04D0,0x0144,
+		0x0430,0x0320,
+		0x0450,0x0710,
+		0x0460,0x0122,
+		0x048A,0x0136,
+		0x04C1,0x010E,
+		0x04CF,0x0001,0x04C0,
+		0x04D0,0x0144,
 		/* Armenian */
 		0x0561,0x0426,
 
-		0x0000
+		0x0000	/* EOT */
 	};
-	static const WORD cvt2[] = {	/* U+1000 - U+FFFF */
+	static const WORD cvt2[] = {	/* Compressed up conversion table for U+1000 - U+FFFF */
 		/* Phonetic Extensions */
 		0x1D7D,0x0001,0x2C63,
 		/* Latin Extended Additional */
-		0x1E00,0x0196,  0x1EA0,0x015A,
+		0x1E00,0x0196,
+		0x1EA0,0x015A,
 		/* Greek Extended */
-		0x1F00,0x0608,  0x1F10,0x0606,  0x1F20,0x0608,  0x1F30,0x0608,  0x1F40,0x0606,
-		0x1F51,0x0007,0x1F59,0x1F52,0x1F5B,0x1F54,0x1F5D,0x1F56,0x1F5F,  0x1F60,0x0608,
+		0x1F00,0x0608,
+		0x1F10,0x0606,
+		0x1F20,0x0608,
+		0x1F30,0x0608,
+		0x1F40,0x0606,
+		0x1F51,0x0007,0x1F59,0x1F52,0x1F5B,0x1F54,0x1F5D,0x1F56,0x1F5F,
+		0x1F60,0x0608,
 		0x1F70,0x000E,0x1FBA,0x1FBB,0x1FC8,0x1FC9,0x1FCA,0x1FCB,0x1FDA,0x1FDB,0x1FF8,0x1FF9,0x1FEA,0x1FEB,0x1FFA,0x1FFB,
-		0x1F80,0x0608,  0x1F90,0x0608,  0x1FA0,0x0608,  0x1FB0,0x0004,0x1FB8,0x1FB9,0x1FB2,0x1FBC,
-		0x1FCC,0x0001,0x1FC3,  0x1FD0,0x0602,  0x1FE0,0x0602,  0x1FE5,0x0001,0x1FEC,  0x1FF3,0x0001,0x1FFC,
+		0x1F80,0x0608,
+		0x1F90,0x0608,
+		0x1FA0,0x0608,
+		0x1FB0,0x0004,0x1FB8,0x1FB9,0x1FB2,0x1FBC,
+		0x1FCC,0x0001,0x1FC3,
+		0x1FD0,0x0602,
+		0x1FE0,0x0602,
+		0x1FE5,0x0001,0x1FEC,
+		0x1FF3,0x0001,0x1FFC,
 		/* Letterlike Symbols */
 		0x214E,0x0001,0x2132,
 		/* Number forms */
-		0x2170,0x0210,  0x2184,0x0001,0x2183,
+		0x2170,0x0210,
+		0x2184,0x0001,0x2183,
 		/* Enclosed Alphanumerics */
-		0x24D0,0x051A,  0x2C30,0x042F,
+		0x24D0,0x051A,
+		0x2C30,0x042F,
 		/* Latin Extended-C */
-		0x2C60,0x0102,  0x2C67,0x0106, 0x2C75,0x0102,
+		0x2C60,0x0102,
+		0x2C67,0x0106, 0x2C75,0x0102,
 		/* Coptic */
 		0x2C80,0x0164,
 		/* Georgian Supplement */
@@ -569,18 +590,16 @@ DWORD ff_wtoupper (	/* Returns up-converted code point */
 		/* Full-width */
 		0xFF41,0x031A,
 
-		0x0000
+		0x0000	/* EOT */
 	};
-	const WORD *p;
-	WORD uc, bc, nc, cmd;
 
 
 	if (uni < 0x10000) {	/* Is it in BMP? */
 		uc = (WORD)uni;
 		p = uc < 0x1000 ? cvt1 : cvt2;
 		for (;;) {
-			bc = *p++;								/* Get block base */
-			if (!bc || uc < bc) break;
+			bc = *p++;								/* Get the block base */
+			if (bc == 0 || uc < bc) break;			/* Not matched? */
 			nc = *p++; cmd = nc >> 8; nc &= 0xFF;	/* Get processing command and block size */
 			if (uc < bc + nc) {	/* In the block? */
 				switch (cmd) {
@@ -596,7 +615,7 @@ DWORD ff_wtoupper (	/* Returns up-converted code point */
 				}
 				break;
 			}
-			if (!cmd) p += nc;
+			if (cmd == 0) p += nc;	/* Skip table if needed */
 		}
 		uni = uc;
 	}

ipl/gfx.c

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -15,56 +16,105 @@
 */
 
 #include <stdarg.h>
+#include <string.h>
 #include "gfx.h"
 
 static const u8 _gfx_font[] = {
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x18, 0x18, 0x00, 0x0C, 0x00,
-	0x00, 0x22, 0x22, 0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x66, 0x66, 0xFF, 0x66, 0xFF, 0x66, 0x66,
-	0x00, 0x18, 0x7C, 0x06, 0x3C, 0x60, 0x3E, 0x18, 0x10, 0x46, 0x66, 0x30, 0x18, 0x0C, 0x66, 0x62,
-	0x00, 0x3C, 0x66, 0x3C, 0x1C, 0xE6, 0x66, 0xFC, 0x00, 0x18, 0x0C, 0x06, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x30, 0x18, 0x0C, 0x0C, 0x18, 0x30, 0x00, 0x00, 0x0C, 0x18, 0x30, 0x30, 0x18, 0x0C, 0x00,
-	0x00, 0x66, 0x3C, 0xFF, 0x3C, 0x66, 0x00, 0x00, 0x00, 0x18, 0x18, 0x7E, 0x18, 0x18, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x3E, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x40, 0x60, 0x30, 0x18, 0x0C, 0x06, 0x00,
-	0x00, 0x3C, 0x66, 0x76, 0x6E, 0x66, 0x3C, 0x00, 0x00, 0x18, 0x1C, 0x18, 0x18, 0x18, 0x7E, 0x00,
-	0x00, 0x3C, 0x62, 0x30, 0x0C, 0x06, 0x7E, 0x00, 0x00, 0x3C, 0x62, 0x38, 0x60, 0x66, 0x3C, 0x00,
-	0x00, 0x6C, 0x6C, 0x66, 0xFE, 0x60, 0x60, 0x00, 0x00, 0x7E, 0x06, 0x7E, 0x60, 0x66, 0x3C, 0x00,
-	0x00, 0x3C, 0x06, 0x3E, 0x66, 0x66, 0x3C, 0x00, 0x00, 0x7E, 0x30, 0x30, 0x18, 0x18, 0x18, 0x00,
-	0x00, 0x3C, 0x66, 0x3C, 0x66, 0x66, 0x3C, 0x00, 0x00, 0x3C, 0x66, 0x7C, 0x60, 0x66, 0x3C, 0x00,
-	0x00, 0x00, 0x18, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00, 0x00, 0x18, 0x00, 0x18, 0x18, 0x0C, 0x00,
-	0x00, 0x70, 0x1C, 0x06, 0x06, 0x1C, 0x70, 0x00, 0x00, 0x00, 0x3E, 0x00, 0x3E, 0x00, 0x00, 0x00,
-	0x00, 0x0E, 0x38, 0x60, 0x60, 0x38, 0x0E, 0x00, 0x00, 0x3C, 0x66, 0x30, 0x18, 0x00, 0x18, 0x00,
-	0x00, 0x3C, 0x66, 0x76, 0x76, 0x06, 0x46, 0x3C, 0x00, 0x3C, 0x66, 0x7E, 0x66, 0x66, 0x66, 0x00,
-	0x00, 0x3E, 0x66, 0x3E, 0x66, 0x66, 0x3E, 0x00, 0x00, 0x3C, 0x66, 0x06, 0x06, 0x66, 0x3C, 0x00,
-	0x00, 0x1E, 0x36, 0x66, 0x66, 0x36, 0x1E, 0x00, 0x00, 0x7E, 0x06, 0x1E, 0x06, 0x06, 0x7E, 0x00,
-	0x00, 0x3E, 0x06, 0x1E, 0x06, 0x06, 0x06, 0x00, 0x00, 0x3C, 0x66, 0x06, 0x76, 0x66, 0x3C, 0x00,
-	0x00, 0x66, 0x66, 0x7E, 0x66, 0x66, 0x66, 0x00, 0x00, 0x3C, 0x18, 0x18, 0x18, 0x18, 0x3C, 0x00,
-	0x00, 0x78, 0x30, 0x30, 0x30, 0x36, 0x1C, 0x00, 0x00, 0x66, 0x36, 0x1E, 0x1E, 0x36, 0x66, 0x00,
-	0x00, 0x06, 0x06, 0x06, 0x06, 0x06, 0x7E, 0x00, 0x00, 0x46, 0x6E, 0x7E, 0x56, 0x46, 0x46, 0x00,
-	0x00, 0x66, 0x6E, 0x7E, 0x76, 0x66, 0x66, 0x00, 0x00, 0x3C, 0x66, 0x66, 0x66, 0x66, 0x3C, 0x00,
-	0x00, 0x3E, 0x66, 0x3E, 0x06, 0x06, 0x06, 0x00, 0x00, 0x3C, 0x66, 0x66, 0x66, 0x3C, 0x70, 0x00,
-	0x00, 0x3E, 0x66, 0x3E, 0x1E, 0x36, 0x66, 0x00, 0x00, 0x3C, 0x66, 0x0C, 0x30, 0x66, 0x3C, 0x00,
-	0x00, 0x7E, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00, 0x00, 0x66, 0x66, 0x66, 0x66, 0x66, 0x3C, 0x00,
-	0x00, 0x66, 0x66, 0x66, 0x66, 0x3C, 0x18, 0x00, 0x00, 0x46, 0x46, 0x56, 0x7E, 0x6E, 0x46, 0x00,
-	0x00, 0x66, 0x3C, 0x18, 0x3C, 0x66, 0x66, 0x00, 0x00, 0x66, 0x66, 0x3C, 0x18, 0x18, 0x18, 0x00,
-	0x00, 0x7E, 0x30, 0x18, 0x0C, 0x06, 0x7E, 0x00, 0x00, 0x3C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x3C,
-	0x00, 0x06, 0x0C, 0x18, 0x30, 0x60, 0x40, 0x00, 0x00, 0x3C, 0x30, 0x30, 0x30, 0x30, 0x30, 0x3C,
-	0x00, 0x18, 0x3C, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7E,
-	0x00, 0x0C, 0x18, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3C, 0x60, 0x7C, 0x66, 0x7C, 0x00,
-	0x00, 0x06, 0x06, 0x3E, 0x66, 0x66, 0x3E, 0x00, 0x00, 0x00, 0x3C, 0x06, 0x06, 0x06, 0x3C, 0x00,
-	0x00, 0x60, 0x60, 0x7C, 0x66, 0x66, 0x7C, 0x00, 0x00, 0x00, 0x3C, 0x66, 0x7E, 0x06, 0x3C, 0x00,
-	0x00, 0x38, 0x0C, 0x3E, 0x0C, 0x0C, 0x0C, 0x00, 0x00, 0x00, 0x7C, 0x66, 0x7C, 0x40, 0x3C, 0x00,
-	0x00, 0x06, 0x06, 0x3E, 0x66, 0x66, 0x66, 0x00, 0x00, 0x18, 0x00, 0x1C, 0x18, 0x18, 0x3C, 0x00,
-	0x00, 0x30, 0x00, 0x30, 0x30, 0x30, 0x1E, 0x00, 0x00, 0x06, 0x06, 0x36, 0x1E, 0x36, 0x66, 0x00,
-	0x00, 0x1C, 0x18, 0x18, 0x18, 0x18, 0x3C, 0x00, 0x00, 0x00, 0x66, 0xFE, 0xFE, 0xD6, 0xC6, 0x00,
-	0x00, 0x00, 0x3E, 0x66, 0x66, 0x66, 0x66, 0x00, 0x00, 0x00, 0x3C, 0x66, 0x66, 0x66, 0x3C, 0x00,
-	0x00, 0x00, 0x3E, 0x66, 0x66, 0x3E, 0x06, 0x00, 0x00, 0x00, 0x7C, 0x66, 0x66, 0x7C, 0x60, 0x00,
-	0x00, 0x00, 0x3E, 0x66, 0x06, 0x06, 0x06, 0x00, 0x00, 0x00, 0x7C, 0x06, 0x3C, 0x60, 0x3E, 0x00,
-	0x00, 0x18, 0x7E, 0x18, 0x18, 0x18, 0x70, 0x00, 0x00, 0x00, 0x66, 0x66, 0x66, 0x66, 0x7C, 0x00,
-	0x00, 0x00, 0x66, 0x66, 0x66, 0x3C, 0x18, 0x00, 0x00, 0x00, 0xC6, 0xD6, 0xFE, 0x7C, 0x6C, 0x00,
-	0x00, 0x00, 0x66, 0x3C, 0x18, 0x3C, 0x66, 0x00, 0x00, 0x00, 0x66, 0x66, 0x7C, 0x60, 0x3C, 0x00,
-	0x00, 0x00, 0x7E, 0x30, 0x18, 0x0C, 0x7E, 0x00, 0x00, 0x00, 0x18, 0x08, 0x08, 0x04, 0x08, 0x08,
-	0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00, 0x0C, 0x08, 0x08, 0x10, 0x08, 0x08,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	// Char 032 ( )
+	0x00, 0x30, 0x30, 0x18, 0x18, 0x00, 0x0C, 0x00,	// Char 033 (!)
+	0x00, 0x22, 0x22, 0x22, 0x00, 0x00, 0x00, 0x00,	// Char 034 (")
+	0x00, 0x66, 0x66, 0xFF, 0x66, 0xFF, 0x66, 0x66,	// Char 035 (#)
+	0x00, 0x18, 0x7C, 0x06, 0x3C, 0x60, 0x3E, 0x18,	// Char 036 ($)
+	0x00, 0x46, 0x66, 0x30, 0x18, 0x0C, 0x66, 0x62,	// Char 037 (%)
+	0x00, 0x3C, 0x66, 0x3C, 0x1C, 0xE6, 0x66, 0xFC,	// Char 038 (&)
+	0x00, 0x18, 0x0C, 0x06, 0x00, 0x00, 0x00, 0x00,	// Char 039 (')
+	0x00, 0x30, 0x18, 0x0C, 0x0C, 0x18, 0x30, 0x00,	// Char 040 (()
+	0x00, 0x0C, 0x18, 0x30, 0x30, 0x18, 0x0C, 0x00,	// Char 041 ())
+	0x00, 0x66, 0x3C, 0xFF, 0x3C, 0x66, 0x00, 0x00,	// Char 042 (*)
+	0x00, 0x18, 0x18, 0x7E, 0x18, 0x18, 0x00, 0x00,	// Char 043 (+)
+	0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x0C, 0x00,	// Char 044 (,)
+	0x00, 0x00, 0x00, 0x3E, 0x00, 0x00, 0x00, 0x00,	// Char 045 (-)
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00,	// Char 046 (.)
+	0x00, 0x40, 0x60, 0x30, 0x18, 0x0C, 0x06, 0x00,	// Char 047 (/)
+	0x00, 0x3C, 0x66, 0x76, 0x6E, 0x66, 0x3C, 0x00,	// Char 048 (0)
+	0x00, 0x18, 0x1C, 0x18, 0x18, 0x18, 0x7E, 0x00,	// Char 049 (1)
+	0x00, 0x3C, 0x62, 0x30, 0x0C, 0x06, 0x7E, 0x00,	// Char 050 (2)
+	0x00, 0x3C, 0x62, 0x38, 0x60, 0x66, 0x3C, 0x00,	// Char 051 (3)
+	0x00, 0x6C, 0x6C, 0x66, 0xFE, 0x60, 0x60, 0x00,	// Char 052 (4)
+	0x00, 0x7E, 0x06, 0x7E, 0x60, 0x66, 0x3C, 0x00,	// Char 053 (5)
+	0x00, 0x3C, 0x06, 0x3E, 0x66, 0x66, 0x3C, 0x00,	// Char 054 (6)
+	0x00, 0x7E, 0x30, 0x30, 0x18, 0x18, 0x18, 0x00,	// Char 055 (7)
+	0x00, 0x3C, 0x66, 0x3C, 0x66, 0x66, 0x3C, 0x00,	// Char 056 (8)
+	0x00, 0x3C, 0x66, 0x7C, 0x60, 0x66, 0x3C, 0x00,	// Char 057 (9)
+	0x00, 0x00, 0x18, 0x00, 0x00, 0x00, 0x18, 0x00,	// Char 058 (:)
+	0x00, 0x00, 0x18, 0x00, 0x18, 0x18, 0x0C, 0x00,	// Char 059 (;)
+	0x00, 0x70, 0x1C, 0x06, 0x06, 0x1C, 0x70, 0x00,	// Char 060 (<)
+	0x00, 0x00, 0x3E, 0x00, 0x3E, 0x00, 0x00, 0x00,	// Char 061 (=)
+	0x00, 0x0E, 0x38, 0x60, 0x60, 0x38, 0x0E, 0x00,	// Char 062 (>)
+	0x00, 0x3C, 0x66, 0x30, 0x18, 0x00, 0x18, 0x00,	// Char 063 (?)
+	0x00, 0x3C, 0x66, 0x76, 0x76, 0x06, 0x46, 0x3C,	// Char 064 (@)
+	0x00, 0x3C, 0x66, 0x7E, 0x66, 0x66, 0x66, 0x00,	// Char 065 (A)
+	0x00, 0x3E, 0x66, 0x3E, 0x66, 0x66, 0x3E, 0x00,	// Char 066 (B)
+	0x00, 0x3C, 0x66, 0x06, 0x06, 0x66, 0x3C, 0x00,	// Char 067 (C)
+	0x00, 0x1E, 0x36, 0x66, 0x66, 0x36, 0x1E, 0x00,	// Char 068 (D)
+	0x00, 0x7E, 0x06, 0x1E, 0x06, 0x06, 0x7E, 0x00,	// Char 069 (E)
+	0x00, 0x3E, 0x06, 0x1E, 0x06, 0x06, 0x06, 0x00,	// Char 070 (F)
+	0x00, 0x3C, 0x66, 0x06, 0x76, 0x66, 0x3C, 0x00,	// Char 071 (G)
+	0x00, 0x66, 0x66, 0x7E, 0x66, 0x66, 0x66, 0x00,	// Char 072 (H)
+	0x00, 0x3C, 0x18, 0x18, 0x18, 0x18, 0x3C, 0x00,	// Char 073 (I)
+	0x00, 0x78, 0x30, 0x30, 0x30, 0x36, 0x1C, 0x00,	// Char 074 (J)
+	0x00, 0x66, 0x36, 0x1E, 0x1E, 0x36, 0x66, 0x00,	// Char 075 (K)
+	0x00, 0x06, 0x06, 0x06, 0x06, 0x06, 0x7E, 0x00,	// Char 076 (L)
+	0x00, 0x46, 0x6E, 0x7E, 0x56, 0x46, 0x46, 0x00,	// Char 077 (M)
+	0x00, 0x66, 0x6E, 0x7E, 0x76, 0x66, 0x66, 0x00,	// Char 078 (N)
+	0x00, 0x3C, 0x66, 0x66, 0x66, 0x66, 0x3C, 0x00,	// Char 079 (O)
+	0x00, 0x3E, 0x66, 0x3E, 0x06, 0x06, 0x06, 0x00,	// Char 080 (P)
+	0x00, 0x3C, 0x66, 0x66, 0x66, 0x3C, 0x70, 0x00,	// Char 081 (Q)
+	0x00, 0x3E, 0x66, 0x3E, 0x1E, 0x36, 0x66, 0x00,	// Char 082 (R)
+	0x00, 0x3C, 0x66, 0x0C, 0x30, 0x66, 0x3C, 0x00,	// Char 083 (S)
+	0x00, 0x7E, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00,	// Char 084 (T)
+	0x00, 0x66, 0x66, 0x66, 0x66, 0x66, 0x3C, 0x00,	// Char 085 (U)
+	0x00, 0x66, 0x66, 0x66, 0x66, 0x3C, 0x18, 0x00,	// Char 086 (V)
+	0x00, 0x46, 0x46, 0x56, 0x7E, 0x6E, 0x46, 0x00,	// Char 087 (W)
+	0x00, 0x66, 0x3C, 0x18, 0x3C, 0x66, 0x66, 0x00,	// Char 088 (X)
+	0x00, 0x66, 0x66, 0x3C, 0x18, 0x18, 0x18, 0x00,	// Char 089 (Y)
+	0x00, 0x7E, 0x30, 0x18, 0x0C, 0x06, 0x7E, 0x00,	// Char 090 (Z)
+	0x00, 0x3C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x3C,	// Char 091 ([)
+	0x00, 0x06, 0x0C, 0x18, 0x30, 0x60, 0x40, 0x00,	// Char 092 (\)
+	0x00, 0x3C, 0x30, 0x30, 0x30, 0x30, 0x30, 0x3C,	// Char 093 (])
+	0x00, 0x18, 0x3C, 0x66, 0x00, 0x00, 0x00, 0x00,	// Char 094 (^)
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF,	// Char 095 (_)
+	0x00, 0x0C, 0x18, 0x30, 0x00, 0x00, 0x00, 0x00,	// Char 096 (`)
+	0x00, 0x00, 0x3C, 0x60, 0x7C, 0x66, 0x7C, 0x00,	// Char 097 (a)
+	0x00, 0x06, 0x06, 0x3E, 0x66, 0x66, 0x3E, 0x00,	// Char 098 (b)
+	0x00, 0x00, 0x3C, 0x06, 0x06, 0x06, 0x3C, 0x00,	// Char 099 (c)
+	0x00, 0x60, 0x60, 0x7C, 0x66, 0x66, 0x7C, 0x00,	// Char 100 (d)
+	0x00, 0x00, 0x3C, 0x66, 0x7E, 0x06, 0x3C, 0x00,	// Char 101 (e)
+	0x00, 0x38, 0x0C, 0x3E, 0x0C, 0x0C, 0x0C, 0x00,	// Char 102 (f)
+	0x00, 0x00, 0x7C, 0x66, 0x7C, 0x40, 0x3C, 0x00,	// Char 103 (g)
+	0x00, 0x06, 0x06, 0x3E, 0x66, 0x66, 0x66, 0x00,	// Char 104 (h)
+	0x00, 0x18, 0x00, 0x1C, 0x18, 0x18, 0x3C, 0x00,	// Char 105 (i)
+	0x00, 0x30, 0x00, 0x30, 0x30, 0x30, 0x1E, 0x00,	// Char 106 (j)
+	0x00, 0x06, 0x06, 0x36, 0x1E, 0x36, 0x66, 0x00,	// Char 107 (k)
+	0x00, 0x1C, 0x18, 0x18, 0x18, 0x18, 0x3C, 0x00,	// Char 108 (l)
+	0x00, 0x00, 0x66, 0xFE, 0xFE, 0xD6, 0xC6, 0x00,	// Char 109 (m)
+	0x00, 0x00, 0x3E, 0x66, 0x66, 0x66, 0x66, 0x00,	// Char 110 (n)
+	0x00, 0x00, 0x3C, 0x66, 0x66, 0x66, 0x3C, 0x00,	// Char 111 (o)
+	0x00, 0x00, 0x3E, 0x66, 0x66, 0x3E, 0x06, 0x00,	// Char 112 (p)
+	0x00, 0x00, 0x7C, 0x66, 0x66, 0x7C, 0x60, 0x00,	// Char 113 (q)
+	0x00, 0x00, 0x3E, 0x66, 0x06, 0x06, 0x06, 0x00,	// Char 114 (r)
+	0x00, 0x00, 0x7C, 0x06, 0x3C, 0x60, 0x3E, 0x00,	// Char 115 (s)
+	0x00, 0x18, 0x7E, 0x18, 0x18, 0x18, 0x70, 0x00,	// Char 116 (t)
+	0x00, 0x00, 0x66, 0x66, 0x66, 0x66, 0x7C, 0x00,	// Char 117 (u)
+	0x00, 0x00, 0x66, 0x66, 0x66, 0x3C, 0x18, 0x00,	// Char 118 (v)
+	0x00, 0x00, 0xC6, 0xD6, 0xFE, 0x7C, 0x6C, 0x00,	// Char 119 (w)
+	0x00, 0x00, 0x66, 0x3C, 0x18, 0x3C, 0x66, 0x00,	// Char 120 (x)
+	0x00, 0x00, 0x66, 0x66, 0x7C, 0x60, 0x3C, 0x00,	// Char 121 (y)
+	0x00, 0x00, 0x7E, 0x30, 0x18, 0x0C, 0x7E, 0x00,	// Char 122 (z)
+	0x00, 0x18, 0x08, 0x08, 0x04, 0x08, 0x08, 0x18,	// Char 123 ({)
+	0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,	// Char 124 (|)
+	0x00, 0x0C, 0x08, 0x08, 0x10, 0x08, 0x08, 0x0C,	// Char 125 (})
+	0x00, 0x00, 0x00, 0x4C, 0x32, 0x00, 0x00, 0x00	// Char 126 (~)
 };
 
 void gfx_init_ctxt(gfx_ctxt_t *ctxt, u32 *fb, u32 width, u32 height, u32 stride)
@@ -75,7 +125,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, 0x3C0000);
+}
+
+void gfx_clear_color(gfx_ctxt_t *ctxt, u32 color)
 {
 	for (u32 i = 0; i < ctxt->height * ctxt->stride; i++)
 		ctxt->fb[i] = color;
@@ -84,11 +139,15 @@ void gfx_clear(gfx_ctxt_t *ctxt, u32 color)
 void gfx_con_init(gfx_con_t *con, gfx_ctxt_t *ctxt)
 {
 	con->gfx_ctxt = ctxt;
+	con->fntsz = 16;
 	con->x = 0;
 	con->y = 0;
-	con->fgcol = 0xFFFFFFFF;
+	con->savedx = 0;
+	con->savedy = 0;
+	con->fgcol = 0xFFCCCCCC;
 	con->fillbg = 0;
-	con->bgcol = 0xFF000000;
+	con->bgcol = 0xFF1B1B1B;
+	con->mute = 0;
 }
 
 void gfx_con_setcol(gfx_con_t *con, u32 fgcol, int fillbg, u32 bgcol)
@@ -112,38 +171,90 @@ void gfx_con_setpos(gfx_con_t *con, u32 x, u32 y)
 
 void gfx_putc(gfx_con_t *con, char c)
 {
-	if (c >= 32 && c < 128)
+	// Duplicate code for performance reasons.
+	switch (con->fntsz)
 	{
-		u8 *cbuf = (u8 *)&_gfx_font[8 * (c - 32)];
-		u32 *fb = con->gfx_ctxt->fb + con->x + con->y * con->gfx_ctxt->stride;
-		for (u32 i = 0; i < 8; i++)
+	case 16:
+		if (c >= 32 && c <= 126)
 		{
-			u8 v = *cbuf++;
-			for (u32 j = 0; j < 8; j++)
+			u8 *cbuf = (u8 *)&_gfx_font[8 * (c - 32)];
+			u32 *fb = con->gfx_ctxt->fb + con->x + con->y * con->gfx_ctxt->stride;
+
+			for (u32 i = 0; i < 16; i+=2)
 			{
-				if (v & 1)
-					*fb = con->fgcol;
-				else if (con->fillbg)
-					*fb = con->bgcol;
-				v >>= 1;
-				fb++;
+				u8 v = *cbuf++;
+				for (u32 k = 0; k < 2; k++)
+				{
+					for (u32 j = 0; j < 8; j++)
+					{
+						if (v & 1)
+						{
+							*fb = con->fgcol;
+							fb++;
+							*fb = con->fgcol;
+						}
+						else if (con->fillbg)
+						{
+							*fb = con->bgcol;
+							fb++;
+							*fb = con->bgcol;
+						}
+						else
+							fb++;
+						v >>= 1;
+						fb++;
+					}
+					fb += con->gfx_ctxt->stride - 16;
+					v = *cbuf;
+				}
 			}
-			fb += con->gfx_ctxt->stride - 8;
+			con->x += 16;
 		}
-		con->x += 8;
-	}
-	else if (c == '\n')
-	{
-		con->x = 0;
-		con->y += 8;
-		if (con->y > con->gfx_ctxt->height - 8)
-			con->y = 0;
+		else if (c == '\n')
+		{
+			con->x = 0;
+			con->y +=16;
+			if (con->y > con->gfx_ctxt->height - 16)
+				con->y = 0;
+		}
+		break;
+	case 8:
+	default:
+		if (c >= 32 && c <= 126)
+		{
+			u8 *cbuf = (u8 *)&_gfx_font[8 * (c - 32)];
+			u32 *fb = con->gfx_ctxt->fb + con->x + con->y * con->gfx_ctxt->stride;
+			for (u32 i = 0; i < 8; i++)
+			{
+				u8 v = *cbuf++;
+				for (u32 j = 0; j < 8; j++)
+				{
+					if (v & 1)
+						*fb = con->fgcol;
+					else if (con->fillbg)
+						*fb = con->bgcol;
+					v >>= 1;
+					fb++;
+				}
+				fb += con->gfx_ctxt->stride - 8;
+			}
+			con->x += 8;
+		}
+		else if (c == '\n')
+		{
+			con->x = 0;
+			con->y += 8;
+			if (con->y > con->gfx_ctxt->height - 8)
+				con->y = 0;
+		}
+		break;
 	}
+	
 }
 
 void gfx_puts(gfx_con_t *con, const char *s)
 {
-	if (!s)
+	if (!s || con->mute)
 		return;
 
 	for (; *s; s++)
@@ -181,8 +292,27 @@ static void _gfx_putn(gfx_con_t *con, u32 v, int base, char fill, int fcnt)
 	gfx_puts(con, p);
 }
 
+void gfx_put_small_sep(gfx_con_t *con)
+{
+	u8 prevFontSize = con->fntsz;
+	con->fntsz = 8;
+	gfx_putc(con, '\n');
+	con->fntsz = prevFontSize;
+}
+
+void gfx_put_big_sep(gfx_con_t *con)
+{
+	u8 prevFontSize = con->fntsz;
+	con->fntsz = 16;
+	gfx_putc(con, '\n');
+	con->fntsz = prevFontSize;
+}
+
 void gfx_printf(gfx_con_t *con, const char *fmt, ...)
 {
+	if (con->mute)
+		return;
+
 	va_list ap;
 	int fill, fcnt;
 
@@ -254,6 +384,11 @@ void gfx_printf(gfx_con_t *con, const char *fmt, ...)
 
 void gfx_hexdump(gfx_con_t *con, u32 base, const u8 *buf, u32 len)
 {
+	if (con->mute)
+		return;
+
+	u8 prevFontSize = con->fntsz;
+	con->fntsz = 8;
 	for(u32 i = 0; i < len; i++)
 	{
 		if(i % 0x10 == 0)
@@ -264,7 +399,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, '.');
@@ -274,8 +409,30 @@ 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');
+	con->fntsz = prevFontSize;
 }
 
 static int abs(int x)
@@ -292,8 +449,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)
@@ -302,7 +459,64 @@ 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; }
+		if (e2 >-dx)
+		{
+			err -= dy;
+			x0 += sx;
+		}
+		if (e2 < dy)
+		{
+			err += dx;
+			y0 += sy;
+		}
+	}
+}
+
+void gfx_set_rect_grey(gfx_ctxt_t *ctxt, const u8 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y)
+{
+	u32 pos = 0;
+	for (u32 y = pos_y; y < (pos_y + size_y); y++)
+	{
+		for (u32 x = pos_x; x < (pos_x + size_x); x++)
+		{
+			memset(&ctxt->fb[x + y*ctxt->stride], buf[pos], 4);
+			pos++;
+		}
+	}
+}
+
+
+void gfx_set_rect_rgb(gfx_ctxt_t *ctxt, const u8 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y)
+{
+	u32 pos = 0;
+	for (u32 y = pos_y; y < (pos_y + size_y); y++)
+	{
+		for (u32 x = pos_x; x < (pos_x + size_x); x++)
+		{
+			ctxt->fb[x + y*ctxt->stride] = buf[pos + 2] | (buf[pos + 1] << 8) | (buf[pos] << 16);
+			pos+=3;
+		}
+	}
+}
+
+void gfx_set_rect_argb(gfx_ctxt_t *ctxt, const u32 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y)
+{
+	u32 pos = 0;
+	for (u32 y = pos_y; y < (pos_y + size_y); y++)
+	{
+		for (u32 x = pos_x; x < (pos_x + size_x); x++)
+		{
+			ctxt->fb[x + y*ctxt->stride] = buf[pos];
+			pos+=1;
+		}
+	}
+}
+
+void gfx_render_bmp_argb(gfx_ctxt_t *ctxt, const u32 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y)
+{
+	for (u32 y = pos_y; y < (pos_y + size_y); y++)
+	{
+		for (u32 x = pos_x; x < (pos_x + size_x); x++)
+			ctxt->fb[x + y*ctxt->stride]  = buf[(size_y + pos_y - 1 - y ) * size_x + x - pos_x];
 	}
 }

ipl/gfx.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -30,15 +31,20 @@ typedef struct _gfx_ctxt_t
 typedef struct _gfx_con_t
 {
 	gfx_ctxt_t *gfx_ctxt;
+	u32 fntsz;
 	u32 x;
 	u32 y;
+	u32 savedx;
+	u32 savedy;
 	u32 fgcol;
 	int fillbg;
 	u32 bgcol;
+	int mute;
 } 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,5 +56,11 @@ 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_put_small_sep(gfx_con_t *con);
+void gfx_put_big_sep(gfx_con_t *con);
+void gfx_set_rect_grey(gfx_ctxt_t *ctxt, const u8 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y);
+void gfx_set_rect_rgb(gfx_ctxt_t *ctxt, const u8 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y);
+void gfx_set_rect_argb(gfx_ctxt_t *ctxt, const u32 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y);
+void gfx_render_bmp_argb(gfx_ctxt_t *ctxt, const u32 *buf, u32 size_x, u32 size_y, u32 pos_x, u32 pos_y);
 
 #endif

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;
 }

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);
 }

ipl/hos.c

@@ -1,5 +1,8 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (c) 2018 st4rk
+* Copyright (c) 2018 Ced2911
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -31,15 +34,57 @@
 #include "pkg1.h"
 #include "pkg2.h"
 #include "ff.h"
+#include "di.h"
 
-/*#include "gfx.h"
+#include "gfx.h"
 extern gfx_ctxt_t gfx_ctxt;
 extern gfx_con_t gfx_con;
-#define DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)*/
+extern void sd_unmount();
+//#define DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)
 #define DPRINTF(...)
 
-#define NUM_KEYBLOB_KEYS 5
-static const u8 keyblob_keyseeds[NUM_KEYBLOB_KEYS][0x10] = {
+typedef struct _launch_ctxt_t
+{
+	void *keyblob;
+
+	void *pkg1;
+	const pkg1_id_t *pkg1_id;
+	const pkg2_kernel_id_t *pkg2_kernel_id;
+
+	void *warmboot;
+	u32 warmboot_size;
+	void *secmon;
+	u32 secmon_size;
+
+	void *pkg2;
+	u32 pkg2_size;
+
+	void *kernel;
+	u32 kernel_size;
+	link_t kip1_list;
+
+	int svcperm;
+	int debugmode;
+	int atmosphere;
+} 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
+
+// Exosphère magic "XBC0"
+#define MAGIC_EXOSPHERE 0x30434258
+
+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
@@ -50,15 +95,22 @@ static const u8 keyblob_keyseeds[NUM_KEYBLOB_KEYS][0x10] = {
 static const u8 cmac_keyseed[0x10] =
 	{ 0x59, 0xC7, 0xFB, 0x6F, 0xBE, 0x9B, 0xBE, 0x87, 0x65, 0x6B, 0x15, 0xC0, 0x53, 0x73, 0x36, 0xA5 };
 
-static const u8 mkey_keyseed_retail[0x10] =
+static const u8 master_keyseed_retail[0x10] =
 	{ 0xD8, 0xA2, 0x41, 0x0A, 0xC6, 0xC5, 0x90, 0x01, 0xC6, 0x1D, 0x6A, 0x26, 0x7C, 0x51, 0x3F, 0x3C };
 
-static const u8 ckey_keyseed[0x10] =
+static const u8 console_keyseed[0x10] =
 	{ 0x4F, 0x02, 0x5F, 0x0E, 0xB6, 0x6D, 0x11, 0x0E, 0xDC, 0x32, 0x7D, 0x41, 0x86, 0xC2, 0xF4, 0x78 };
 
 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_5xx[0x10] = 
+	{ 0x2D, 0xC1, 0xF4, 0x8D, 0xF3, 0x5B, 0x69, 0x33, 0x42, 0x10, 0xAC, 0x65, 0xDA, 0x90, 0x46, 0x66 };
+
+static const u8 console_keyseed_4xx_5xx[0x10] = 
+	{ 0x0C, 0x91, 0x09, 0xDB, 0x93, 0x93, 0x07, 0x81, 0x07, 0x3C, 0xC4, 0x16, 0x22, 0x7C, 0x6C, 0x28 };
+
+
 static void _se_lock()
 {
 	for (u32 i = 0; i < 16; i++)
@@ -86,23 +138,29 @@ static void _se_lock()
 	gfx_hexdump(&gfx_con, SE_BASE, (void *)SE_BASE, 0x400);*/
 }
 
-//Key derivation for < 4.0.0
-static int _keygen_1(u8 *keyblob, u32 kb, void *tsec_fw)
+int keygen(u8 *keyblob, u32 kb, void *tsec_fw)
 {
-	u8 *tmp = (u8 *)malloc(0x10);
+	u8 tmp[0x10];
+
+	if (kb > KB_FIRMWARE_VERSION_MAX)
+		return 0;
 
-	se_key_acc_ctrl(12, 0x15);
 	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(13, tmp, 0x10);
 
-	//Derive keyblob key from TSEC+SBK.
-	memcpy(tmp, keyblob_keyseeds[kb], 0x10);
-	se_aes_crypt_block_ecb(13, 0, tmp, tmp);
+	//Derive keyblob keys from TSEC+SBK.
+	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(14);
 
 	//TODO: verify keyblob CMAC.
@@ -111,58 +169,62 @@ static int _keygen_1(u8 *keyblob, u32 kb, void *tsec_fw)
 	//if (!memcmp(keyblob, tmp, 0x10))
 	//	return 0;
 
+	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(13, keyblob + 0x20, 0x90, keyblob + 0x20, 0x90, keyblob + 0x10);
-	se_aes_key_set(11, keyblob + 0x20 + 0x80, 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);
 
-	//TODO: for some reason SE likes to hang if we don't execute an operation here.
-	memcpy(tmp, mkey_keyseed_retail, 0x10);
-	se_aes_crypt_block_ecb(12, 0, tmp, tmp);
+	se_aes_crypt_block_ecb(12, 0, tmp, master_keyseed_retail);
 
-	//Generate retail master key.
-	memcpy(tmp, mkey_keyseed_retail, 0x10);
-	se_aes_unwrap_key(12, 12, tmp);
+	switch (kb)
+	{
+		case KB_FIRMWARE_VERSION_100_200:
+		case KB_FIRMWARE_VERSION_300:
+		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(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:
+			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;
+	}
 
-	memcpy(tmp, key8_keyseed, 0x10);
+	//Package2 key.
 	se_key_acc_ctrl(8, 0x15);
-	se_aes_unwrap_key(8, 12, tmp);
+	se_aes_unwrap_key(8, 12, key8_keyseed);
 
-	//Generate console specific key.
-	memcpy(tmp, ckey_keyseed, 0x10);
-	se_aes_unwrap_key(13, 13, tmp);
-
-	se_key_acc_ctrl(12, 0xFF);
-	se_key_acc_ctrl(13, 0xFF);
-
-	free(tmp);
+	return 1;
 }
 
-typedef struct _launch_ctxt_t
+static void _copy_bootconfig(launch_ctxt_t *ctxt)
 {
-	void *keyblob;
+	sdmmc_storage_t storage;
+	sdmmc_t sdmmc;
 
-	void *pkg1;
-	const pkg1_id_t *pkg1_id;
+	sdmmc_storage_init_mmc(&storage, &sdmmc, SDMMC_4, SDMMC_BUS_WIDTH_8, 4);
 
-	void *warmboot;
-	u32 warmboot_size;
-	void *secmon;
-	u32 secmon_size;
+	//Read BCT.
+	u8 *buf = (u8 *)0x4003D000;
+	sdmmc_storage_set_mmc_partition(&storage, 1);
+	sdmmc_storage_read(&storage, 0, 0x3000 / NX_EMMC_BLOCKSIZE, buf);
 
-	void *pkg2;
-	u32 pkg2_size;
+	gfx_printf(&gfx_con, "Copied BCT to 0x4003D000\n");
 
-	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;
+	sdmmc_storage_end(&storage);
+}
 
 static int _read_emmc_pkg1(launch_ctxt_t *ctxt)
 {
@@ -179,10 +241,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,\nVersion (= '%s').%k\n", 0xFFFF0000, (char *)ctxt->pkg1 + 0x10, 0xFFCCCCCC);
 		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'),\nKeyblob version %d\n\n", (char *)(ctxt->pkg1 + 0x10), ctxt->pkg1_id->kb);
 
 	//Read the correct keyblob.
 	ctxt->keyblob = (u8 *)malloc(NX_EMMC_BLOCKSIZE);
@@ -207,7 +269,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)
@@ -220,10 +282,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, 
@@ -281,12 +343,42 @@ 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;
+}
+
+static int _config_atmosphere(launch_ctxt_t *ctxt, const char *value)
+{
+	if (*value == '1')
+	{
+		DPRINTF("Enabled atmosphere patching\n");
+		ctxt->atmosphere = 1;
+	}
+	return 1;
+}
+
 typedef struct _cfg_handler_t
 {
 	const char *key;
@@ -298,6 +390,9 @@ static const cfg_handler_t _config_handlers[] = {
 	{ "secmon", _config_secmon },
 	{ "kernel", _config_kernel },
 	{ "kip1", _config_kip1 },
+	{ "fullsvcperm", _config_svcperm },
+	{ "debugmode", _config_debugmode },
+	{ "atmosphere", _config_atmosphere },
 	{ NULL, NULL },
 };
 
@@ -313,105 +408,198 @@ static int _config(launch_ctxt_t *ctxt, ini_sec_t *cfg)
 
 int hos_launch(ini_sec_t *cfg)
 {
+	int bootStateDramPkg2 = 0;
+	int bootStatePkg2Continue = 0;
+	int exoFwNumber = 0;
+	int end_di = 0;
 	launch_ctxt_t ctxt;
+
 	memset(&ctxt, 0, sizeof(launch_ctxt_t));
 	list_init(&ctxt.kip1_list);
 
+	if (!gfx_con.mute)
+		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;
 
-	//Read package1 and the correct keyblob.
+	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_1(ctxt.keyblob, ctxt.pkg1_id->kb, (u8 *)ctxt.pkg1 + ctxt.pkg1_id->tsec_off);
-DPRINTF("generated keys\n");
-	//Decrypt and unpack package1 if we require parts of it.
+	// Generate keys.
+	keygen(ctxt.keyblob, ctxt.pkg1_id->kb, (u8 *)ctxt.pkg1 + ctxt.pkg1_id->tsec_off);
+	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.
+
+	// 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;
-	//Replace 'SecureMonitor' if requested.
+		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)
 		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;
+		gfx_printf(&gfx_con, "%kPatching Security Monitor%k\n", 0xFFFFBA00, 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.
+	gfx_printf(&gfx_con, "Loaded warmboot.bin and secmon\n");
+
+	// Read package2.
 	if (!_read_emmc_pkg2(&ctxt))
 		return 0;
-DPRINTF("read pkg2\n");
-	//Decrypt package2 and parse KIP1 blobs in INI1 section.
+
+	gfx_printf(&gfx_con, "Read package2\n");
+
+	// 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);
-DPRINTF("parsed ini1\n");
-	//Use the kernel included in package2 in case we didn't load one already.
+
+	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 || ctxt.atmosphere)
+		{
+			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.
+			kernel_patch_t *kernel_patchset = ctxt.pkg2_kernel_id->kernel_patchset;
+			if (kernel_patchset != NULL)
+			{
+				gfx_printf(&gfx_con, "%kPatching kernel%k\n", 0xFFFFBA00, 0xFFCCCCCC);
+				u32 *temp;
+				for (u32 i = 0; kernel_patchset[i].id != 0xFFFFFFFF; i++)
+				{
+					if ((ctxt.svcperm && kernel_patchset[i].id == SVC_VERIFY_DS)
+					|| (ctxt.debugmode && kernel_patchset[i].id == DEBUG_MODE_EN)
+					|| (ctxt.atmosphere && kernel_patchset[i].id == ATM_GEN_PATCH))
+						*(vu32 *)(ctxt.kernel + kernel_patchset[i].off) = kernel_patchset[i].val;
+					else if (ctxt.atmosphere && kernel_patchset[i].id == ATM_ARR_PATCH)
+					{
+						temp = (u32 *)kernel_patchset[i].ptr;
+						for (u32 j = 0; j < kernel_patchset[i].val; j++)
+							*(vu32 *)(ctxt.kernel + kernel_patchset[i].off + (j << 2)) = temp[j];
+					}
+				}
+			}
+		}
 	}
 
-	//Merge extra KIP1s into loaded ones.
+	// Merge extra KIP1s into loaded ones.
+	gfx_printf(&gfx_con, "%kPatching kernel initial processes%k\n", 0xFFFFBA00, 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.
+	// Rebuild and encrypt package2.
 	pkg2_build_encrypt((void *)0xA9800000, ctxt.kernel, ctxt.kernel_size, &kip1_info);
-DPRINTF("rebuilt pkg2\n");
-	//Clear 'BootConfig'.
+	gfx_printf(&gfx_con, "Rebuilt and loaded package2\n");
+
+	// Unmount SD card.
+	sd_unmount();
+
+	gfx_printf(&gfx_con, "\n%kBooting...%k\n", 0xFF96FF00, 0xFFCCCCCC);
+
+	se_aes_key_clear(8);
+	se_aes_key_clear(11);
+
+	// Final per firmware configuration.
+	switch (ctxt.pkg1_id->kb)
+	{
+	case KB_FIRMWARE_VERSION_100_200:
+		if (!exoFwNumber)
+		{
+			if(!strcmp(ctxt.pkg1_id->id, "20161121183008"))
+				exoFwNumber = 1;
+			else
+				exoFwNumber = 2;
+		}
+	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;
+		if (!exoFwNumber)
+			exoFwNumber = 3;
+		break;
+	default:
+	case KB_FIRMWARE_VERSION_400:
+		if (!exoFwNumber)
+			exoFwNumber = 4;
+	case KB_FIRMWARE_VERSION_500:
+		se_key_acc_ctrl(12, 0xFF);
+		se_key_acc_ctrl(15, 0xFF);
+		bootStateDramPkg2 = 2;
+		bootStatePkg2Continue = 4;
+		if (!exoFwNumber)
+			exoFwNumber = 5;
+		break;
+	}
+
+	// Copy BCT if debug mode is enabled.
 	memset((void *)0x4003D000, 0, 0x3000);
+	if(ctxt.debugmode)
+		_copy_bootconfig(&ctxt);
 
-	//pkg2_decrypt((void *)0xA9800000);
-	//sleep(10000);
-	//btn_wait();
-	//return 0;
+	// Config Exosphère if booting Atmosphère.
+	if (ctxt.atmosphere)
+	{
+		vu32 *mb_exo_magic = (vu32 *)0x40002E40;
+		vu32 *mb_exo_fw_no = (vu32 *)0x40002E44;
 
-	//Lock SE before starting 'SecureMonitor'.
+		*mb_exo_magic = MAGIC_EXOSPHERE;
+		*mb_exo_fw_no = exoFwNumber;
+	}
+
+	// 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.
 	cluster_boot_cpu0(ctxt.pkg1_id->secmon_base);
 	while (!*mb_out)
-		sleep(1);
-
-	//Signal 'BootConfig'.
-	*mb_in = 1;
-	sleep(100);
-
-	//Signal package2 available.
-	*mb_in = 2;
-	sleep(100);
+		usleep(1);
 
+	//TODO: pkg1.1 locks PMC scratches, we can do that too at some point.
 	/*PMC(0x4) = 0x7FFFF3;
 	PMC(0x2C4) = 0xFFFFFFFF;
 	PMC(0x2D8) = 0xFFAFFFFF;
@@ -421,16 +609,16 @@ DPRINTF("rebuilt pkg2\n");
 	PMC(0x5BC) = 0xFFFFFFFF;
 	PMC(0x5C0) = 0xFFAAFFFF;*/
 
-	//TODO: Cleanup.
-	//display_end();
+	//Disable display.
+	if (end_di)
+		display_end();
 
-	//Signal to continue boot.
-	*mb_in = 3;
-	sleep(100);
+	//Signal to pkg2 ready and continue boot.
+	*mb_in = bootStatePkg2Continue;
 
-	//Halt ourselves in waitevent state.
+	//Halt ourselves in waitevent state and resume if there's JTAG activity.
 	while (1)
-		FLOW_CTLR(0x4) = 0x50000000;
+		FLOW_CTLR(FLOW_CTLR_HALT_COP_EVENTS) = 0x50000000;
 
 	return 0;
 }

ipl/hos.h

@@ -21,5 +21,6 @@
 #include "ini.h"
 
 int hos_launch(ini_sec_t *cfg);
+int keygen(u8 *keyblob, u32 kb, void *tsec_fw);
 
 #endif

ipl/i2c.c

@@ -26,7 +26,7 @@ static void _i2c_wait(vu32 *base)
 	base[0x23] = 0x25;
 	for (u32 i = 0; i < 20; i++)
 	{
-		sleep(1);
+		usleep(1);
 		if (!(base[0x23] & 1))
 			break;
 	}
@@ -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)
 		;
 
@@ -89,16 +89,16 @@ void i2c_init(u32 idx)
 
 	for (u32 i = 0; i < 10; i++)
 	{
-		sleep(20000);
+		usleep(20000);
 		if (base[0x1A] & 0x800)
 			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)

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

ipl/ini.c

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -45,10 +46,6 @@ int ini_parse(link_t *dst, char *ini_path)
 		f_gets(lbuf, 512, &fp);
 		lblen = strlen(lbuf);
 
-		//Skip empty lines and comments.
-		if (lblen <= 1 || lbuf[0] == '#')
-			continue;
-
 		//Remove trailing newline.
 		if (lbuf[lblen - 1] == '\n')
 			lbuf[lblen - 1] = 0;
@@ -68,9 +65,58 @@ int ini_parse(link_t *dst, char *ini_path)
 
 			csec = (ini_sec_t *)malloc(sizeof(ini_sec_t));
 			csec->name = _strdup(&lbuf[1]);
+			csec->type = INI_CHOICE;
 			list_init(&csec->kvs);
 		}
-		else if (csec) //Extract key/value.
+		else if (lblen > 2 && lbuf[0] == '{') //Create new caption.
+		{
+			if (csec)
+			{
+				list_append(dst, &csec->link);
+				csec = NULL;
+			}
+
+			u32 i;
+			for (i = 0; i < lblen && lbuf[i] != '\n' && lbuf[i] != '}'; i++)
+				;
+			lbuf[i] = 0;
+
+			csec = (ini_sec_t *)malloc(sizeof(ini_sec_t));
+			csec->name = _strdup(&lbuf[1]);
+			csec->type = INI_CAPTION;
+			csec->color = 0xFF0AB9E6;
+		}
+		else if (lblen > 2 && lbuf[0] == '#') //Create empty lines and comments.
+		{
+			if (csec)
+			{
+				list_append(dst, &csec->link);
+				csec = NULL;
+			}
+
+			u32 i;
+			for (i = 0; i < lblen && lbuf[i] != '\n'; i++)
+				;
+			lbuf[i] = 0;
+
+			csec = (ini_sec_t *)malloc(sizeof(ini_sec_t));
+			csec->name = _strdup(&lbuf[1]);
+			csec->type = INI_COMMENT;
+				
+		}
+		else if (lblen <= 1)
+		{
+			if (csec)
+			{
+				list_append(dst, &csec->link);
+				csec = NULL;
+			}
+
+			csec = (ini_sec_t *)malloc(sizeof(ini_sec_t));
+			csec->name = NULL;
+			csec->type = INI_NEWLINE;
+		}
+		else if (csec->type == INI_CHOICE) //Extract key/value.
 		{
 			u32 i;
 			for (i = 0; i < lblen && lbuf[i] != '\n' && lbuf[i] != '='; i++)
@@ -91,3 +137,57 @@ int ini_parse(link_t *dst, char *ini_path)
 
 	return 1;
 }
+
+void ini_free(link_t *dst)
+{
+	if (dst == NULL)
+		return;
+
+	LIST_FOREACH_ENTRY(ini_sec_t, ini_sec, dst, link)
+	{
+		if (ini_sec->type == INI_CHOICE)
+		{
+			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);
+}

ipl/ini.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -20,6 +21,12 @@
 #include "types.h"
 #include "list.h"
 
+#define INI_CHOICE  3
+#define INI_CAPTION 5
+#define INI_CHGLINE 6
+#define INI_NEWLINE 0xFE
+#define INI_COMMENT 0xFF
+
 typedef struct _ini_kv_t
 {
 	char *key;
@@ -32,8 +39,13 @@ typedef struct _ini_sec_t
 	char *name;
 	link_t kvs;
 	link_t link;
+	u32 type;
+	u32 color;
 } ini_sec_t;
 
 int ini_parse(link_t *dst, char *ini_path);
+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

ipl/integer.h

@@ -8,10 +8,8 @@
 #ifdef _WIN32	/* FatFs development platform */
 
 #include <windows.h>
-#include <tchar.h>
 typedef unsigned __int64 QWORD;
 
-
 #else			/* Embedded platform */
 
 /* These types MUST be 16-bit or 32-bit */

ipl/link.ld

@@ -1,7 +1,7 @@
 ENTRY(_start)
 
 SECTIONS {
-	PROVIDE(__ipl_start = 0x40003000);
+	PROVIDE(__ipl_start = 0x40008000);
 	. = __ipl_start;
 	.text : {
 		*(.text*);

ipl/max17050.c

@@ -0,0 +1,266 @@
+/*
+ * Fuel gauge driver for Nintendo Switch's Maxim 17050
+ *
+ * Copyright (C) 2011 Samsung Electronics
+ * MyungJoo Ham <myungjoo.ham@samsung.com>
+ * Copyright (C) 2018 CTCaer
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * This driver is based on max17040_battery.c
+ */
+
+#include "max17050.h"
+#include "i2c.h"
+#include "util.h"
+
+/* Status register bits */
+#define STATUS_POR_BIT (1 << 1)
+#define STATUS_BST_BIT (1 << 3)
+#define STATUS_VMN_BIT (1 << 8)
+#define STATUS_TMN_BIT (1 << 9)
+#define STATUS_SMN_BIT (1 << 10)
+#define STATUS_BI_BIT  (1 << 11)
+#define STATUS_VMX_BIT (1 << 12)
+#define STATUS_TMX_BIT (1 << 13)
+#define STATUS_SMX_BIT (1 << 14)
+#define STATUS_BR_BIT  (1 << 15)
+
+#define VFSOC0_LOCK   0x0000
+#define VFSOC0_UNLOCK 0x0080
+
+#define MAX17050_VMAX_TOLERANCE 50 /* 50 mV */
+
+int max17050_get_property(enum MAX17050_reg reg, int *value)
+{
+	u16 data;
+
+	switch (reg)
+	{
+	case MAX17050_Age: //Age (percent). Based on 100% x (FullCAP Register/DesignCap).
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_Age);
+		*value = data >> 8; /* Show MSB. 1% increments */
+		break;
+	case MAX17050_Cycles: //Cycle count.
+		i2c_recv_buf_small((u8 *)value, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_Cycles);
+		break;
+	case MAX17050_MinVolt: //Voltage max/min
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_MinMaxVolt);
+		*value = (data & 0xff) * 20; /* Voltage MIN. Units of 20mV */
+		break;
+	case MAX17050_MaxVolt: //Voltage max/min
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_MinMaxVolt);
+		*value = (data >> 8) * 20; /* Voltage MAX. Units of LSB = 20mV */
+		break;
+	case MAX17050_V_empty: //Voltage min design.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_V_empty);
+		*value = (data >> 7) * 10; /* Units of LSB = 10mV */
+		break;
+	case MAX17050_VCELL: //Voltage now.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_VCELL);
+		*value = data * 625 / 8 / 1000;
+		break;
+	case MAX17050_AvgVCELL: //Voltage avg.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_AvgVCELL);
+		*value = data * 625 / 8 / 1000;
+		break;
+	case MAX17050_OCVInternal: //Voltage ocv.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_OCVInternal);
+		*value = data * 625 / 8 / 1000;
+		break;
+	case MAX17050_RepSOC: //Capacity %.
+		i2c_recv_buf_small((u8 *)value, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_RepSOC);
+		break;
+	case MAX17050_DesignCap: //Charge full design.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_DesignCap);
+		data = data * 5 / 10;
+		*value = data;
+		break;
+	case MAX17050_FullCAP: //Charge full.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_FullCAP);
+		data = data * 5 / 10;
+		*value = data;
+		break;
+	case MAX17050_RepCap: //Charge now.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_RepCap);
+		data = data * 5 / 10;
+		*value = data;
+		break;
+	case MAX17050_TEMP: //Temp.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_TEMP);
+		*value = (s16)data;
+		*value = *value * 10 / 256;
+		break;
+	case MAX17050_Current: //Current now.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_Current);
+		*value = (s16)data;
+		*value *= 1562500 / MAX17050_DEFAULT_SNS_RESISTOR;
+		break;
+	case MAX17050_AvgCurrent: //Current avg.
+		i2c_recv_buf_small((u8 *)&data, 2, I2C_1, MAXIM17050_I2C_ADDR, MAX17050_AvgCurrent);
+		*value = (s16)data;
+		*value *= 1562500 / MAX17050_DEFAULT_SNS_RESISTOR;
+		break;
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+static int _max17050_write_verify_reg(u8 reg, u16 value)
+{
+	int retries = 8;
+	int ret;
+	u16 read_value;
+
+	do {
+		ret = i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, reg, (u8 *)&value, 2);
+		i2c_recv_buf_small((u8 *)&read_value, 2, I2C_1, MAXIM17050_I2C_ADDR, reg);
+		if (read_value != value) {
+			ret = -1;
+			retries--;
+		}
+	} while (retries && read_value != value);
+
+	return ret;
+}
+
+static void _max17050_override_por(u8 reg, u16 value)
+{
+	if (value)
+		i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, reg, (u8 *)&value, 2);
+}
+
+static void _max17050_load_new_capacity_params()
+{
+	u16 fullcap, repSoc, dq_acc, dp_acc;
+
+	fullcap = 0x2476; // 4667mAh design capacity.
+	dq_acc = 0x10bc;  // From a healthy fuel gauge.
+	dp_acc = 0x5e09;  //          =||=
+	repSoc = 0x6400;  // 100%.
+
+
+	_max17050_write_verify_reg(MAX17050_RemCap, fullcap);
+	_max17050_write_verify_reg(MAX17050_RepCap, fullcap);
+
+	_max17050_write_verify_reg(MAX17050_dQacc, dq_acc);
+	_max17050_write_verify_reg(MAX17050_dPacc, dp_acc);
+
+	_max17050_write_verify_reg(MAX17050_FullCAP, fullcap);
+	//i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_DesignCap, (u8 *)&fullcap, 2);
+	_max17050_write_verify_reg(MAX17050_FullCAPNom, fullcap);
+	/* Update SOC register with new SOC */
+	i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_RepSOC, (u8 *)&repSoc, 2);
+}
+
+static void _max17050_reset_vfsoc0_reg()
+{
+	u16 lockVal = 0;
+	u16 vfSoc = 0x6440; // >100% for fully charged battery
+
+	lockVal = VFSOC0_UNLOCK;
+	i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_VFSOC0Enable, (u8 *)&lockVal, 2);
+
+	_max17050_write_verify_reg(MAX17050_VFSOC0, vfSoc);
+
+	lockVal = VFSOC0_LOCK;
+	i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_VFSOC0Enable, (u8 *)&lockVal, 2);
+}
+
+static void _max17050_update_capacity_regs()
+{
+	u16 value = 0x2476; //Set to 4667mAh design capacity.
+	_max17050_write_verify_reg(MAX17050_FullCAP, value);
+	_max17050_write_verify_reg(MAX17050_FullCAPNom, value);
+	//i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_DesignCap, config->design_cap, 2);
+}
+
+static void _max17050_write_config_regs()
+{
+	u16 value = 0;
+
+	value = 0x7254;
+	i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_CONFIG, (u8 *)&value, 2);
+	value = 0x2473;
+	i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_LearnCFG, (u8 *)&value, 2);
+	//i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_FilterCFG, (u8 *)&value, 2)
+	//i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_RelaxCFG, (u8 *)&value, 2)
+	//i2c_send_buf_small(I2C_1, MAXIM17050_I2C_ADDR, MAX17050_FullSOCThr, (u8 *)&value, 2)
+}
+
+/*
+ * Block write all the override values coming from platform data.
+ * This function MUST be called before the POR initialization proceedure
+ * specified by maxim.
+ */
+static void _max17050_override_por_values()
+{
+	u16 dq_acc = 0x10bc; // From a healthy fuel gauge.
+	u16 dp_acc = 0x5e09; //           =||=
+	
+	_max17050_override_por(MAX17050_dQacc, dq_acc);
+	_max17050_override_por(MAX17050_dPacc, dp_acc);
+
+	//_max17050_override_por(MAX17050_RCOMP0, config->rcomp0);  //0x58
+	//_max17050_override_por(MAX17050_TempCo, config->tcompc0); //0x1b22
+
+	//u16 k_empty0 = 0x439;
+	//_max17050_override_por(map, MAX17050_K_empty0, k_empty0); // unknown cell data
+}
+
+static void _max17050_set_por_bit(u16 value)
+{
+	_max17050_write_verify_reg(MAX17050_STATUS, value);
+}
+
+int max17050_fix_configuration()
+{
+	/* Init phase, set the POR bit */
+	_max17050_set_por_bit(STATUS_POR_BIT);
+
+	/* Override POR values */
+	_max17050_override_por_values();
+	/* After Power up, the MAX17050 requires 500ms in order
+	 * to perform signal debouncing and initial SOC reporting
+	 */
+	msleep(500);
+
+	/* Initialize configaration */
+	_max17050_write_config_regs();
+
+
+	/* update capacity params */
+	_max17050_update_capacity_regs();
+
+	/* delay must be atleast 350mS to allow VFSOC
+	 * to be calculated from the new configuration
+	 */
+	msleep(350);
+
+	/* reset vfsoc0 reg */
+	_max17050_reset_vfsoc0_reg();
+
+	/* load new capacity params */
+	_max17050_load_new_capacity_params();
+
+	/* Init complete, Clear the POR bit */
+	//_max17050_set_por_bit(0); // Should we? Or let the switch to reconfigure POR?
+
+	// Sets POR, BI, BR. 
+	_max17050_set_por_bit(0x8801);
+
+	return 0;
+}

ipl/max17050.h

@@ -0,0 +1,127 @@
+/*
+ * Fuel gauge driver for Nintendo Switch's Maxim 17050
+ *  Note that Maxim 8966 and 8997 are mfd and this is its subdevice.
+ *
+ * Copyright (C) 2011 Samsung Electronics
+ * MyungJoo Ham <myungjoo.ham@samsung.com>
+ * Copyright (C) 2018 CTCaer
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef __MAX17050_H_
+#define __MAX17050_H_
+
+#define MAX17050_STATUS_BattAbsent    (1 << 3)
+#define MAX17050_DEFAULT_SNS_RESISTOR 10000
+
+/* Consider RepCap which is less then 10 units below FullCAP full */
+#define MAX17050_FULL_THRESHOLD 10
+
+#define MAX17050_CHARACTERIZATION_DATA_SIZE 48
+
+#define MAXIM17050_I2C_ADDR 0x36
+
+enum MAX17050_reg {
+	MAX17050_STATUS		= 0x00,
+	MAX17050_VALRT_Th	= 0x01,
+	MAX17050_TALRT_Th	= 0x02,
+	MAX17050_SALRT_Th	= 0x03,
+	MAX17050_AtRate		= 0x04,
+	MAX17050_RepCap		= 0x05,
+	MAX17050_RepSOC		= 0x06,
+	MAX17050_Age		= 0x07,
+	MAX17050_TEMP		= 0x08,
+	MAX17050_VCELL		= 0x09,
+	MAX17050_Current	= 0x0A,
+	MAX17050_AvgCurrent	= 0x0B,
+
+	MAX17050_SOC		= 0x0D,
+	MAX17050_AvSOC		= 0x0E,
+	MAX17050_RemCap		= 0x0F,
+	MAX17050_FullCAP	= 0x10,
+	MAX17050_TTE		= 0x11,
+	MAX17050_QRTbl00	= 0x12,
+	MAX17050_FullSOCThr	= 0x13,
+	MAX17050_RSLOW		= 0x14,
+
+	MAX17050_AvgTA		= 0x16,
+	MAX17050_Cycles		= 0x17,
+	MAX17050_DesignCap	= 0x18,
+	MAX17050_AvgVCELL	= 0x19,
+	MAX17050_MinMaxTemp	= 0x1A,
+	MAX17050_MinMaxVolt	= 0x1B,
+	MAX17050_MinMaxCurr	= 0x1C,
+	MAX17050_CONFIG		= 0x1D,
+	MAX17050_ICHGTerm	= 0x1E,
+	MAX17050_AvCap		= 0x1F,
+	MAX17050_ManName	= 0x20,
+	MAX17050_DevName	= 0x21,
+	MAX17050_QRTbl10	= 0x22,
+	MAX17050_FullCAPNom	= 0x23,
+	MAX17050_TempNom	= 0x24,
+	MAX17050_TempLim	= 0x25,
+	MAX17050_TempHot	= 0x26,
+	MAX17050_AIN		= 0x27,
+	MAX17050_LearnCFG	= 0x28,
+	MAX17050_FilterCFG	= 0x29,
+	MAX17050_RelaxCFG	= 0x2A,
+	MAX17050_MiscCFG	= 0x2B,
+	MAX17050_TGAIN		= 0x2C,
+	MAX17050_TOFF		= 0x2D,
+	MAX17050_CGAIN		= 0x2E,
+	MAX17050_COFF		= 0x2F,
+
+	MAX17050_QRTbl20	= 0x32,
+	MAX17050_SOC_empty	= 0x33,
+	MAX17050_T_empty	= 0x34,
+	MAX17050_FullCAP0	= 0x35,
+	MAX17050_LAvg_empty	= 0x36,
+	MAX17050_FCTC		= 0x37,
+	MAX17050_RCOMP0		= 0x38,
+	MAX17050_TempCo		= 0x39,
+	MAX17050_V_empty	= 0x3A,
+	MAX17050_K_empty0	= 0x3B,
+	MAX17050_TaskPeriod	= 0x3C,
+	MAX17050_FSTAT		= 0x3D,
+
+	MAX17050_SHDNTIMER	= 0x3F,
+	MAX17050_QRTbl30	= 0x42,
+	MAX17050_dQacc		= 0x45,
+	MAX17050_dPacc		= 0x46,
+
+	MAX17050_VFSOC0		= 0x48,
+
+	MAX17050_QH			= 0x4D,
+	MAX17050_QL			= 0x4E,
+
+	MAX17050_MinVolt	= 0x50, // Custom ID. Not to be sent to i2c.
+	MAX17050_MaxVolt	= 0x51, // Custom ID. Not to be sent to i2c.
+
+	MAX17050_VFSOC0Enable	= 0x60,
+
+	MAX17050_MODELChrTbl	= 0x80,
+
+	MAX17050_OCV			= 0xEE,
+
+	MAX17050_OCVInternal	= 0xFB,
+
+	MAX17050_VFSOC			= 0xFF,
+};
+
+int max17050_get_property(enum MAX17050_reg reg, int *value);
+int max17050_fix_configuration();
+
+#endif /* __MAX17050_H_ */

ipl/max77620.h

@@ -265,7 +265,9 @@
 #define MAX77620_CNFGGLBL1_LBDAC_EN		(1 << 7)
 #define MAX77620_CNFGGLBL1_MPPLD		(1 << 6)
 #define MAX77620_CNFGGLBL1_LBHYST		((1 << 5) | (1 << 4))
+#define MAX77620_CNFGGLBL1_LBHYST_N		(1 << 4)
 #define MAX77620_CNFGGLBL1_LBDAC		0x0E
+#define MAX77620_CNFGGLBL1_LBDAC_N		(1 << 1)
 #define MAX77620_CNFGGLBL1_LBRSTEN		(1 << 0)
 
 /* CNFG BBC registers */

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)
@@ -103,7 +103,7 @@ int max77620_regulator_set_voltage(u32 id, u32 mv)
 	u8 val = i2c_recv_byte(I2C_5, 0x3C, reg->volt_addr);
 	val = (val & ~reg->volt_mask) | (mult & reg->volt_mask);
 	i2c_send_byte(I2C_5, 0x3C, reg->volt_addr, val);
-	sleep(1000);
+	usleep(1000);
 
 	return 1;
 }
@@ -122,7 +122,7 @@ int max77620_regulator_enable(u32 id, int enable)
 	else
 		val &= ~reg->enable_mask;
 	i2c_send_byte(I2C_5, 0x3C, addr, val);
-	sleep(1000);
+	usleep(1000);
 
 	return 1;
 }
@@ -139,3 +139,8 @@ void max77620_config_default()
 	}
 	i2c_send_byte(I2C_5, 0x3C, MAX77620_REG_SD_CFG2, 4);
 }
+
+void max77620_low_battery_monitor_config()
+{
+	i2c_send_byte(I2C_5, 0x3C, MAX77620_REG_CNFGGLBL1, MAX77620_CNFGGLBL1_LBDAC_EN | MAX77620_CNFGGLBL1_LBHYST_N | MAX77620_CNFGGLBL1_LBDAC_N);
+}

ipl/max7762x.h

@@ -64,5 +64,6 @@ int max77620_regulator_config_fps(u32 id);
 int max77620_regulator_set_voltage(u32 id, u32 mv);
 int max77620_regulator_enable(u32 id, int enable);
 void max77620_config_default();
+void max77620_low_battery_monitor_config();
 
 #endif

ipl/mc.c

@@ -102,7 +102,7 @@ void mc_config_carveout()
 
 void mc_enable_ahb_redirect()
 {
-	CLOCK(0x3A4) = CLOCK(0x3A4) & 0xFFF7FFFF | 0x80000;
+	CLOCK(CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRD) = (CLOCK(CLK_RST_CONTROLLER_LVL2_CLK_GATE_OVRD) & 0xFFF7FFFF) | 0x80000;
 	//MC(MC_IRAM_REG_CTRL) &= 0xFFFFFFFE;
 	MC(MC_IRAM_BOM) = 0x40000000;
 	MC(MC_IRAM_TOM) = 0x4003F000;
@@ -119,15 +119,15 @@ 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);
+	usleep(5);
 
 	//#ifdef CONFIG_ENABLE_AHB_REDIRECT
 	mc_disable_ahb_redirect();

ipl/mmc.h

@@ -289,6 +289,7 @@ c : clear by read
 #define EXT_CSD_CACHE_SIZE		249	/* RO, 4 bytes */
 #define EXT_CSD_PWR_CL_DDR_200_360	253	/* RO */
 #define EXT_CSD_FIRMWARE_VERSION	254	/* RO, 8 bytes */
+#define EXT_CSD_DEVICE_VERSION		262	/* RO, 2 bytes */
 #define EXT_CSD_PRE_EOL_INFO		267	/* RO */
 #define EXT_CSD_DEVICE_LIFE_TIME_EST_TYP_A	268	/* RO */
 #define EXT_CSD_DEVICE_LIFE_TIME_EST_TYP_B	269	/* RO */

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);
 	}

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;
 

ipl/pinmux.c

@@ -20,13 +20,13 @@
 void pinmux_config_uart(u32 idx)
 {
 	PINMUX_AUX(PINMUX_AUX_UARTX_RX(idx)) = 0;
-	PINMUX_AUX(PINMUX_AUX_UARTX_TX(idx)) = 0x48;
+	PINMUX_AUX(PINMUX_AUX_UARTX_TX(idx)) = PINMUX_INPUT_ENABLE | PINMUX_PULL_UP;
 	PINMUX_AUX(PINMUX_AUX_UARTX_RTS(idx)) = 0;
-	PINMUX_AUX(PINMUX_AUX_UARTX_CTS(idx)) = 0x44;
+	PINMUX_AUX(PINMUX_AUX_UARTX_CTS(idx)) = PINMUX_INPUT_ENABLE | PINMUX_PULL_DOWN;
 }
 
 void pinmux_config_i2c(u32 idx)
 {
-	PINMUX_AUX(PINMUX_AUX_X_I2C_SCL(idx)) = 0x40;
-	PINMUX_AUX(PINMUX_AUX_X_I2C_SDA(idx)) = 0x40;
+	PINMUX_AUX(PINMUX_AUX_X_I2C_SCL(idx)) = PINMUX_INPUT_ENABLE;
+	PINMUX_AUX(PINMUX_AUX_X_I2C_SDA(idx)) = PINMUX_INPUT_ENABLE;
 }

ipl/pinmux.h

@@ -22,37 +22,65 @@
 /*! APB MISC registers. */
 #define APB_MISC_GP_SDMMC1_CLK_LPBK_CONTROL 0x8D4
 #define APB_MISC_GP_SDMMC3_CLK_LPBK_CONTROL 0x8D8
-#define APB_MISC_GP_SDMMC1_PAD_CFGPADCTRL 0xA98
-#define APB_MISC_GP_VGPIO_GPIO_MUX_SEL 0xB74
+#define APB_MISC_GP_SDMMC1_PAD_CFGPADCTRL   0xA98
+#define APB_MISC_GP_VGPIO_GPIO_MUX_SEL      0xB74
 
 /*! Pinmux registers. */
-#define PINMUX_AUX_SDMMC1_CLK 0x00
-#define PINMUX_AUX_SDMMC1_CMD 0x04
+#define PINMUX_AUX_SDMMC1_CLK  0x00
+#define PINMUX_AUX_SDMMC1_CMD  0x04
 #define PINMUX_AUX_SDMMC1_DAT3 0x08
 #define PINMUX_AUX_SDMMC1_DAT2 0x0C
 #define PINMUX_AUX_SDMMC1_DAT1 0x10
 #define PINMUX_AUX_SDMMC1_DAT0 0x14
-#define PINMUX_AUX_SDMMC3_CLK 0x1C
-#define PINMUX_AUX_SDMMC3_CMD 0x20
+#define PINMUX_AUX_SDMMC3_CLK  0x1C
+#define PINMUX_AUX_SDMMC3_CMD  0x20
 #define PINMUX_AUX_SDMMC3_DAT0 0x24
 #define PINMUX_AUX_SDMMC3_DAT1 0x28
 #define PINMUX_AUX_SDMMC3_DAT2 0x2C
 #define PINMUX_AUX_SDMMC3_DAT3 0x30
-#define PINMUX_AUX_DMIC3_CLK 0xB4
-#define PINMUX_AUX_UART2_TX 0xF4
-#define PINMUX_AUX_UART3_TX 0x104
-#define PINMUX_AUX_GPIO_PE6 0x248
-#define PINMUX_AUX_GPIO_PH6 0x250
-#define PINMUX_AUX_GPIO_PZ1 0x280
+#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
 /*! 0:UART-A, 1:UART-B, 3:UART-C, 3:UART-D */
-#define PINMUX_AUX_UARTX_TX(x) (0xE4 + 0x10 * (x))
-#define PINMUX_AUX_UARTX_RX(x) (0xE8 + 0x10 * (x))
+#define PINMUX_AUX_UARTX_TX(x)  (0xE4 + 0x10 * (x))
+#define PINMUX_AUX_UARTX_RX(x)  (0xE8 + 0x10 * (x))
 #define PINMUX_AUX_UARTX_RTS(x) (0xEC + 0x10 * (x))
 #define PINMUX_AUX_UARTX_CTS(x) (0xF0 + 0x10 * (x))
 /*! 0:GEN1, 1:GEN2, 2:GEN3, 3:CAM, 4:PWR */
 #define PINMUX_AUX_X_I2C_SCL(x) (0xBC + 8 * (x))
 #define PINMUX_AUX_X_I2C_SDA(x) (0xC0 + 8 * (x))
 
+#define PINMUX_FUNC_MASK    (3 << 0)
+
+#define PINMUX_PULL_MASK    (3 << 2)
+#define PINMUX_PULL_NONE    (0 << 2)
+#define PINMUX_PULL_DOWN    (1 << 2)
+#define PINMUX_PULL_UP      (2 << 2)
+
+#define PINMUX_TRISTATE     (1 << 4)
+#define PINMUX_PARKED       (1 << 5)
+#define PINMUX_INPUT_ENABLE (1 << 6)
+#define PINMUX_LOCK         (1 << 7)
+#define PINMUX_LPDR         (1 << 8)
+#define PINMUX_HSM          (1 << 9)
+
+#define PINMUX_IO_HV        (1 << 10)
+#define PINMUX_OPEN_DRAIN   (1 << 11)
+#define PINMUX_SCHMT        (1 << 12)
+
+#define PINMUX_DRIVE_1X     (0 << 13) 
+#define PINMUX_DRIVE_2X     (1 << 13)
+#define PINMUX_DRIVE_3X     (2 << 13)
+#define PINMUX_DRIVE_4X     (3 << 13)
+
 void pinmux_config_uart(u32 idx);
 void pinmux_config_i2c(u32 idx);
 

ipl/pkg1.c

@@ -1,5 +1,7 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (c) 2018 st4rk
+* Copyright (c) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -16,28 +18,48 @@
 
 #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.
-	{ 0x9F0 + 0xBB0, _NOP() } //Sections SHA2.
+	{ 0x9F0 + 0xBB0, _NOP() }  //Sections SHA2.
 );
 
 PATCHSET_DEF(_secmon_2_patchset,
 	//Patch package2 decryption and signature/hash checks.
 	{ 0xAC8 + 0xAAC, _NOP() }, //Header signature.
 	{ 0xAC8 + 0xB3C, _NOP() }, //Version.
-	{ 0xAC8 + 0xB58, _NOP() } //Sections SHA2.
+	{ 0xAC8 + 0xB58, _NOP() }  //Sections SHA2.
+);
+
+PATCHSET_DEF(_secmon_3_patchset,
+	//Patch package2 decryption and signature/hash checks.
+	{ 0xAC8 + 0xA30, _NOP() }, //Header signature.
+	{ 0xAC8 + 0xAB4, _NOP() }, //package2 structure.
+	{ 0xAC8 + 0xAC0, _NOP() }, //Version.
+	{ 0xAC8 + 0xADC, _NOP() }  //Sections SHA2.
+);
+
+PATCHSET_DEF(_secmon_4_patchset,
+	//Patch package2 decryption and signature/hash checks.
+	{ 0x2300 + 0x5D80, _NOP() }, //package2 structure.
+	{ 0x2300 + 0x5D8C, _NOP() }, //Version.
+	{ 0x2300 + 0x5EFC, _NOP() }, //Header signature.
+	{ 0xAC8 + 0xA2C,   _NOP() }  //Sections SHA2.
+);
+
+PATCHSET_DEF(_secmon_5_patchset,
+	//Patch package2 decryption and signature/hash checks.
+	{ 0xDA8 + 0x9D8 , _NOP() }, //package2 structure.
+	{ 0xDA8 + 0x9E4 , _NOP() }, //Version.
+	{ 0xDA8 + 0xC9C , _NOP() }, //Header signature.
+	{ 0xDA8 + 0x1038 , _NOP() } //Sections SHA2.
 );
 
 /*
@@ -52,26 +74,15 @@ PATCHSET_DEF(_secmon_2_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, NULL }, //3.0.0
-	{ "20170710161758", 2, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, NULL }, //3.0.1
-	{ "20170921172629", 3, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, NULL }, //4.0.0
-	{ "20180220163747", 4, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, NULL }, //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.
 };
 
-typedef struct _pk11_hdr_t
-{
-	u32 magic;
-	u32 wb_size;
-	u32 wb_off;
-	u32 pad;
-	u32 ldr_size;
-	u32 ldr_off;
-	u32 sm_size;
-	u32 sm_off;
-} pk11_hdr_t;
 
 const pkg1_id_t *pkg1_identify(u8 *pkg1)
 {
@@ -89,7 +100,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);
 
@@ -101,6 +112,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]];

ipl/pkg1.h

@@ -19,18 +19,18 @@
 
 #include "types.h"
 
-#define PATCHSET_DEF(name, ...) \
-	patch_t name[] = { \
-		__VA_ARGS__, \
-		{ 0xFFFFFFFF, 0xFFFFFFFF } \
-	}
-
 typedef struct _patch_t
 {
 	u32 off;
 	u32 val;
 } patch_t;
 
+#define PATCHSET_DEF(name, ...) \
+	patch_t name[] = { \
+		__VA_ARGS__, \
+		{ 0xFFFFFFFF, 0xFFFFFFFF } \
+	}
+
 typedef struct _pkg1_id_t
 {
 	const char *id;
@@ -39,11 +39,25 @@ 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;
 
+typedef struct _pk11_hdr_t
+{
+	u32 magic;
+	u32 wb_size;
+	u32 wb_off;
+	u32 pad;
+	u32 ldr_size;
+	u32 ldr_off;
+	u32 sm_size;
+	u32 sm_off;
+} 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

ipl/pkg2.c

@@ -1,5 +1,7 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (c) 2018 CTCaer
+* Copyright (c) 2018 Atmosphère-NX
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -16,6 +18,7 @@
 
 #include <string.h>
 #include "pkg2.h"
+#include "arm64.h"
 #include "heap.h"
 #include "se.h"
 
@@ -25,6 +28,207 @@ extern gfx_con_t gfx_con;
 #define DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)*/
 #define DPRINTF(...)
 
+//TODO: Replace hardcoded AArch64 instructions with instruction macros.
+//TODO: Reduce hardcoded values without searching kernel for patterns?
+// The process ID send/receive kernel patches were taken from Atmosphère's kernel patches.
+// They should only be used when running Atmosphère.
+#define FREE_CODE_OFF_1ST_100 0x4797C
+#define FREE_CODE_OFF_1ST_200 0x6486C
+#define FREE_CODE_OFF_1ST_300 0x494A4
+#define FREE_CODE_OFF_1ST_302 0x494BC
+#define FREE_CODE_OFF_1ST_400 0x52890
+#define FREE_CODE_OFF_1ST_500 0x5C020
+
+#define ID_SND_OFF_100 0x23CC0
+#define ID_SND_OFF_200 0x3F134
+#define ID_SND_OFF_300 0x26080
+#define ID_SND_OFF_302 0x26080
+#define ID_SND_OFF_400 0x2AF64
+#define ID_SND_OFF_500 0x2AD34
+
+#define ID_RCV_OFF_100 0x219F0
+#define ID_RCV_OFF_200 0x3D1A8
+#define ID_RCV_OFF_300 0x240F0
+#define ID_RCV_OFF_302 0x240F0
+#define ID_RCV_OFF_400 0x28F6C
+#define ID_RCV_OFF_500 0x28DAC
+
+static u32 PRC_ID_SND_100[] =
+{
+	0xA9BF2FEA, 0x2A0E03EB, 0xD37EF56B, 0xF86B6B8B, 0x92FFFFE9, 0x8A090168, 0xD2FFFFE9, 0x8A09016B,
+	0xD2FFFFC9, 0xEB09017F, 0x54000040, 0xF9412948,	0xA8C12FEA
+};
+#define FREE_CODE_OFF_2ND_100 (FREE_CODE_OFF_1ST_100 + sizeof(PRC_ID_SND_100) + 4)
+static u32 PRC_ID_RCV_100[] =
+{
+	0xA9BF2FEA, 0x2A1C03EA, 0xD37EF54A, 0xF86A69AA, 0x92FFFFE9, 0x8A090148, 0xD2FFFFE9, 0x8A09014A,
+	0xD2FFFFC9, 0xEB09015F, 0x54000040, 0xF9412968, 0xA8C12FEA
+};
+
+static u32 PRC_ID_SND_200[] =
+{
+	0xA9BF2FEA, 0x2A1803EB, 0xD37EF56B, 0xF86B6B8B, 0x92FFFFE9, 0x8A090168, 0xD2FFFFE9, 0x8A09016B,
+	0xD2FFFFC9, 0xEB09017F, 0x54000040, 0xF9413148, 0xA8C12FEA
+};
+#define FREE_CODE_OFF_2ND_200 (FREE_CODE_OFF_1ST_200 + sizeof(PRC_ID_SND_200) + 4)
+static u32 PRC_ID_RCV_200[] =
+{
+	0xA9BF2FEA, 0x2A0F03EA, 0xD37EF54A, 0xF9405FEB, 0xF86A696A, 0xF9407BEB, 0x92FFFFE9, 0x8A090148,
+	0xD2FFFFE9, 0x8A09014A, 0xD2FFFFC9, 0xEB09015F, 0x54000040, 0xF9413168, 0xA8C12FEA
+};
+
+static u32 PRC_ID_SND_300[] =
+{
+	0xA9BF2FEA, 0x2A1803EB, 0xD37EF56B, 0xF86B6B8B, 0x92FFFFE9, 0x8A090168, 0xD2FFFFE9, 0x8A09016B,
+	0xD2FFFFC9, 0xEB09017F, 0x54000040, 0xF9415548, 0xA8C12FEA
+};
+#define FREE_CODE_OFF_2ND_300 (FREE_CODE_OFF_1ST_300 + sizeof(PRC_ID_SND_300) + 4)
+static u32 PRC_ID_RCV_300[] =
+{
+	0xA9BF2FEA, 0x2A0F03EA, 0xD37EF54A, 0xF9405FEB, 0xF86A696A, 0xF9407BEB, 0x92FFFFE9, 0x8A090148,
+	0xD2FFFFE9, 0x8A09014A, 0xD2FFFFC9, 0xEB09015F, 0x54000040, 0xF9415568, 0xA8C12FEA
+};
+
+static u32 PRC_ID_SND_302[] =
+{
+	0xA9BF2FEA, 0x2A1803EB, 0xD37EF56B, 0xF86B6B8B, 0x92FFFFE9, 0x8A090168, 0xD2FFFFE9, 0x8A09016B,
+	0xD2FFFFC9, 0xEB09017F, 0x54000040, 0xF9415548, 0xA8C12FEA
+};
+#define FREE_CODE_OFF_2ND_302 (FREE_CODE_OFF_1ST_302 + sizeof(PRC_ID_SND_302) + 4)
+static u32 PRC_ID_RCV_302[] =
+{
+	0xA9BF2FEA, 0x2A0F03EA, 0xD37EF54A, 0xF9405FEB, 0xF86A696A, 0xF9407BEB, 0x92FFFFE9, 0x8A090148,
+	0xD2FFFFE9, 0x8A09014A, 0xD2FFFFC9, 0xEB09015F, 0x54000040, 0xF9415568, 0xA8C12FEA
+};
+
+static u32 PRC_ID_SND_400[] =
+{
+	0x2A1703EA, 0xD37EF54A, 0xF86A6B8A, 0x92FFFFE9, 0x8A090148, 0xD2FFFFE9, 0x8A09014A, 0xD2FFFFC9,
+	0xEB09015F, 0x54000060, 0xF94053EA, 0xF9415948, 0xF94053EA
+};
+#define FREE_CODE_OFF_2ND_400 (FREE_CODE_OFF_1ST_400 + sizeof(PRC_ID_SND_400) + 4)
+static u32 PRC_ID_RCV_400[] =
+{
+	0xF9403BED, 0x2A0E03EA, 0xD37EF54A, 0xF86A69AA, 0x92FFFFE9, 0x8A090148, 0xD2FFFFE9, 0x8A09014A,
+	0xD2FFFFC9, 0xEB09015F, 0x54000040, 0xF9415B28, 0xD503201F
+};
+
+static u32 PRC_ID_SND_500[] =
+{
+	0x2A1703EA, 0xD37EF54A, 0xF86A6B6A, 0x92FFFFE9, 0x8A090148, 0xD2FFFFE9, 0x8A09014A, 0xD2FFFFC9,
+	0xEB09015F, 0x54000060, 0xF94043EA, 0xF9415948, 0xF94043EA
+};
+#define FREE_CODE_OFF_2ND_500 (FREE_CODE_OFF_1ST_500 + sizeof(PRC_ID_SND_500) + 4)
+static u32 PRC_ID_RCV_500[] =
+{
+	0xF9403BED, 0x2A1503EA, 0xD37EF54A, 0xF86A69AA, 0x92FFFFE9, 0x8A090148, 0xD2FFFFE9, 0x8A09014A,
+	0xD2FFFFC9, 0xEB09015F, 0x54000040, 0xF9415B08, 0xF9406FEA
+};
+
+// Include kernel patches here, so we can utilize pkg1 id
+KERNEL_PATCHSET_DEF(_kernel_1_patchset,
+	{ SVC_VERIFY_DS, 0x3764C, _NOP(), NULL },          // Disable SVC verifications
+	{ DEBUG_MODE_EN, 0x44074, _MOVZX(8, 1, 0), NULL }, // Enable Debug Patch
+	// Atmosphère kernel patches.
+	{ ATM_GEN_PATCH, ID_SND_OFF_100, _B(ID_SND_OFF_100, FREE_CODE_OFF_1ST_100), NULL},    // Send process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_1ST_100, sizeof(PRC_ID_SND_100) >> 2, PRC_ID_SND_100}, // Send process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_1ST_100 + sizeof(PRC_ID_SND_100),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_1ST_100 + sizeof(PRC_ID_SND_100), ID_SND_OFF_100 + 4), NULL},
+	{ ATM_GEN_PATCH, ID_RCV_OFF_100, _B(ID_RCV_OFF_100, FREE_CODE_OFF_2ND_100), NULL},    // Receive process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_2ND_100, sizeof(PRC_ID_RCV_100) >> 2, PRC_ID_RCV_100}, // Receive process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_2ND_100 + sizeof(PRC_ID_RCV_100),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_2ND_100 + sizeof(PRC_ID_RCV_100), ID_RCV_OFF_100 + 4), NULL}
+);
+
+KERNEL_PATCHSET_DEF(_kernel_2_patchset,
+	{ SVC_VERIFY_DS, 0x54834, _NOP(), NULL },          // Disable SVC verifications
+	{ DEBUG_MODE_EN, 0x6086C, _MOVZX(8, 1, 0), NULL }, // Enable Debug Patch
+	// Atmosphère kernel patches.
+	{ ATM_GEN_PATCH, ID_SND_OFF_200, _B(ID_SND_OFF_200, FREE_CODE_OFF_1ST_200), NULL},    // Send process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_1ST_200, sizeof(PRC_ID_SND_200) >> 2, PRC_ID_SND_200}, // Send process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_1ST_200 + sizeof(PRC_ID_SND_200),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_1ST_200 + sizeof(PRC_ID_SND_200), ID_SND_OFF_200 + 4), NULL},
+	{ ATM_GEN_PATCH, ID_RCV_OFF_200, _B(ID_RCV_OFF_200, FREE_CODE_OFF_2ND_200), NULL},    // Receive process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_2ND_200, sizeof(PRC_ID_RCV_200) >> 2, PRC_ID_RCV_200}, // Receive process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_2ND_200 + sizeof(PRC_ID_RCV_200),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_2ND_200 + sizeof(PRC_ID_RCV_200), ID_RCV_OFF_200 + 4), NULL}
+);
+
+KERNEL_PATCHSET_DEF(_kernel_3_patchset,
+	{ SVC_VERIFY_DS, 0x3BD24, _NOP(), NULL },          // Disable SVC verifications
+	{ DEBUG_MODE_EN, 0x483FC, _MOVZX(8, 1, 0), NULL }, // Enable Debug Patch
+	// Atmosphère kernel patches.
+	{ ATM_GEN_PATCH, ID_SND_OFF_300, _B(ID_SND_OFF_300, FREE_CODE_OFF_1ST_300), NULL},    // Send process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_1ST_300, sizeof(PRC_ID_SND_300) >> 2, PRC_ID_SND_300}, // Send process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_1ST_300 + sizeof(PRC_ID_SND_300),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_1ST_300 + sizeof(PRC_ID_SND_300), ID_SND_OFF_300 + 4), NULL},
+	{ ATM_GEN_PATCH, ID_RCV_OFF_300, _B(ID_RCV_OFF_300, FREE_CODE_OFF_2ND_300), NULL},    // Receive process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_2ND_300, sizeof(PRC_ID_RCV_300) >> 2, PRC_ID_RCV_300}, // Receive process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_2ND_300 + sizeof(PRC_ID_RCV_300),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_2ND_300 + sizeof(PRC_ID_RCV_300), ID_RCV_OFF_300 + 4), NULL}
+);
+
+KERNEL_PATCHSET_DEF(_kernel_302_patchset,
+	{ SVC_VERIFY_DS, 0x3BD24, _NOP(), NULL },          // Disable SVC verifications
+	{ DEBUG_MODE_EN, 0x48414, _MOVZX(8, 1, 0), NULL }, // Enable Debug Patch
+	// Atmosphère kernel patches.
+	{ ATM_GEN_PATCH, ID_SND_OFF_302, _B(ID_SND_OFF_302, FREE_CODE_OFF_1ST_302), NULL},    // Send process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_1ST_302, sizeof(PRC_ID_SND_302) >> 2, PRC_ID_SND_302}, // Send process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_1ST_302 + sizeof(PRC_ID_SND_302),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_1ST_302 + sizeof(PRC_ID_SND_302), ID_SND_OFF_302 + 4), NULL},
+	{ ATM_GEN_PATCH, ID_RCV_OFF_302, _B(ID_RCV_OFF_302, FREE_CODE_OFF_2ND_302), NULL},    // Receive process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_2ND_302, sizeof(PRC_ID_RCV_302) >> 2, PRC_ID_RCV_302}, // Receive process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_2ND_302 + sizeof(PRC_ID_RCV_302),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_2ND_302 + sizeof(PRC_ID_RCV_302), ID_RCV_OFF_302 + 4), NULL}
+);
+
+KERNEL_PATCHSET_DEF(_kernel_4_patchset,
+	{ SVC_VERIFY_DS, 0x41EB4, _NOP(), NULL },          // Disable SVC verifications
+	{ DEBUG_MODE_EN, 0x4EBFC, _MOVZX(8, 1, 0), NULL }, // Enable Debug Patch
+	// Atmosphère kernel patches.
+	{ ATM_GEN_PATCH, ID_SND_OFF_400, _B(ID_SND_OFF_400, FREE_CODE_OFF_1ST_400), NULL},    // Send process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_1ST_400, sizeof(PRC_ID_SND_400) >> 2, PRC_ID_SND_400}, // Send process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_1ST_400 + sizeof(PRC_ID_SND_400),                      // Branch back and skip 2 instructions.
+		_B(FREE_CODE_OFF_1ST_400 + sizeof(PRC_ID_SND_400), ID_SND_OFF_400 + 8), NULL},
+	{ ATM_GEN_PATCH, ID_RCV_OFF_400, _B(ID_RCV_OFF_400, FREE_CODE_OFF_2ND_400), NULL},    // Receive process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_2ND_400, sizeof(PRC_ID_RCV_400) >> 2, PRC_ID_RCV_400}, // Receive process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_2ND_400 + sizeof(PRC_ID_RCV_400),                      // Branch back and skip 1 instruction.
+		_B(FREE_CODE_OFF_2ND_400 + sizeof(PRC_ID_RCV_400), ID_RCV_OFF_400 + 4), NULL}
+);
+
+KERNEL_PATCHSET_DEF(_kernel_5_patchset,
+	{ SVC_VERIFY_DS, 0x45E6C, _NOP(), NULL },          // Disable SVC verifications
+	{ DEBUG_MODE_EN, 0x5513C, _MOVZX(8, 1, 0), NULL }, // Enable Debug Patch
+	// Atmosphère kernel patches.
+	{ ATM_GEN_PATCH, ID_SND_OFF_500, _B(ID_SND_OFF_500, FREE_CODE_OFF_1ST_500), NULL},    // Send process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_1ST_500, sizeof(PRC_ID_SND_500) >> 2, PRC_ID_SND_500}, // Send process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_1ST_500 + sizeof(PRC_ID_SND_500),                      // Branch back and skip 2 instructions.
+		_B(FREE_CODE_OFF_1ST_500 + sizeof(PRC_ID_SND_500), ID_SND_OFF_500 + 8), NULL},
+	{ ATM_GEN_PATCH, ID_RCV_OFF_500, _B(ID_RCV_OFF_500, FREE_CODE_OFF_2ND_500), NULL},    // Receive process id branch.
+	{ ATM_ARR_PATCH, FREE_CODE_OFF_2ND_500, sizeof(PRC_ID_RCV_500) >> 2, PRC_ID_RCV_500}, // Receive process id code.
+	{ ATM_GEN_PATCH, FREE_CODE_OFF_2ND_500 + sizeof(PRC_ID_RCV_500),                      // Branch back and skip 2 instructions.
+		_B(FREE_CODE_OFF_2ND_500 + sizeof(PRC_ID_RCV_500), ID_RCV_OFF_500 + 8), NULL}
+);
+
+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);
@@ -172,3 +376,4 @@ DPRINTF("INI1 encrypted\n");
 	memset(hdr->ctr, 0 , 0x10);
 	*(u32 *)hdr->ctr = 0x100 + sizeof(pkg2_hdr_t) + kernel_size + ini1_size;
 }
+

ipl/pkg2.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -27,6 +28,30 @@
 
 #define INI1_MAGIC 0x31494E49
 
+typedef struct _kernel_patch_t
+{
+	u32 id;
+	u32 off;
+	u32 val;
+	u32 *ptr;
+} kernel_patch_t;
+
+#define KERNEL_PATCHSET_DEF(name, ...) \
+	kernel_patch_t name[] = { \
+		__VA_ARGS__, \
+		{0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, (u32 *)0xFFFFFFFF} \
+	}
+
+enum
+{
+	// Generic instruction patches
+	SVC_VERIFY_DS = 0x10, // 0x0-0xF are RESERVED.
+	DEBUG_MODE_EN,
+	ATM_GEN_PATCH,
+	// >4 bytes patches. Value is a pointer of a u32 array.
+	ATM_ARR_PATCH,
+};
+
 typedef struct _pkg2_hdr_t
 {
 	u8 ctr[0x10];
@@ -82,12 +107,19 @@ typedef struct _pkg2_kip1_info_t
 	link_t link;
 } pkg2_kip1_info_t;
 
+typedef struct _pkg2_kernel_id_t
+{
+	u32 crc32c_id;
+	kernel_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);
 

ipl/pmc.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (c) 2018 st4rk
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -41,5 +42,8 @@
 #define APBDEV_PMC_SCRATCH188 0x810
 #define APBDEV_PMC_SCRATCH190 0x818
 #define APBDEV_PMC_SCRATCH200 0x840
+#define APBDEV_PMC_RST_STATUS_0 0x1B4
+#define APBDEV_PMC_SECURE_SCRATCH49_0 0x3A4
+#define APBDEV_PMC_SCRATCH49_0 0x244
 
 #endif

ipl/sd.h

@@ -2,6 +2,7 @@
 *  include/linux/mmc/sd.h
 *
 *  Copyright (C) 2005-2007 Pierre Ossman, All Rights Reserved.
+*  Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
@@ -35,10 +36,11 @@
 #define SD_APP_SEND_SCR          51   /* adtc                    R1  */
 
 /* OCR bit definitions */
-#define SD_OCR_S18R		(1 << 24)    /* 1.8V switching request */
-#define SD_ROCR_S18A		SD_OCR_S18R  /* 1.8V switching accepted by card */
-#define SD_OCR_XPC		(1 << 28)    /* SDXC power control */
-#define SD_OCR_CCS		(1 << 30)    /* Card Capacity Status */
+#define SD_OCR_S18R         (1 << 24)    /* 1.8V switching request */
+#define SD_ROCR_S18A        SD_OCR_S18R  /* 1.8V switching accepted by card */
+#define SD_OCR_XPC          (1 << 28)    /* SDXC power control */
+#define SD_OCR_CCS          (1 << 30)    /* Card Capacity Status */
+#define SD_OCR_VDD_32_33    (1 << 20)	 /* VDD voltage 3.2 ~ 3.3 */
 
 /*
 * SD_SWITCH argument format:
@@ -64,10 +66,11 @@
 /*
 * SCR field definitions
 */
-
 #define SCR_SPEC_VER_0		0	/* Implements system specification 1.0 - 1.01 */
 #define SCR_SPEC_VER_1		1	/* Implements system specification 1.10 */
 #define SCR_SPEC_VER_2		2	/* Implements system specification 2.00-3.0X */
+#define SD_SCR_BUS_WIDTH_1	(1<<0)
+#define SD_SCR_BUS_WIDTH_4	(1<<2)
 
 /*
 * SD bus widths
@@ -75,6 +78,32 @@
 #define SD_BUS_WIDTH_1		0
 #define SD_BUS_WIDTH_4		2
 
+/*
+* 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
+
+#define SD_MODE_HIGH_SPEED	(1 << HIGH_SPEED_BUS_SPEED)
+#define SD_MODE_UHS_SDR12	(1 << UHS_SDR12_BUS_SPEED)
+#define SD_MODE_UHS_SDR25	(1 << UHS_SDR25_BUS_SPEED)
+#define SD_MODE_UHS_SDR50	(1 << UHS_SDR50_BUS_SPEED)
+#define SD_MODE_UHS_SDR104	(1 << UHS_SDR104_BUS_SPEED)
+#define SD_MODE_UHS_DDR50	(1 << UHS_DDR50_BUS_SPEED)
+
+#define SD_DRIVER_TYPE_B	0x01
+#define SD_DRIVER_TYPE_A	0x02
+
+#define SD_SET_CURRENT_LIMIT_200	0
+#define SD_SET_CURRENT_LIMIT_400	1
+#define SD_SET_CURRENT_LIMIT_600	2
+#define SD_SET_CURRENT_LIMIT_800	3
+
 /*
 * SD_SWITCH mode
 */
@@ -90,6 +119,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 */

ipl/sdmmc.c

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -165,15 +166,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--;
+
+			msleep(100);
+		} 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)
@@ -187,7 +199,7 @@ int sdmmc_storage_write(sdmmc_storage_t *storage, u32 sector, u32 num_sectors, v
 }
 
 /*
-* MMC specific functions. 
+* MMC specific functions.
 */
 
 static int _mmc_storage_get_op_cond_inner(sdmmc_storage_t *storage, u32 *pout, u32 power)
@@ -216,22 +228,22 @@ static int _mmc_storage_get_op_cond_inner(sdmmc_storage_t *storage, u32 *pout, u
 
 static int _mmc_storage_get_op_cond(sdmmc_storage_t *storage, u32 power)
 {
-	u32 timeout = get_tmr() + 1500000;
+	u32 timeout = get_tmr_ms() + 1500;
 
 	while (1)
 	{
 		u32 cond = 0;
 		if (!_mmc_storage_get_op_cond_inner(storage, &cond, power))
 			break;
-		if (cond & 0x80000000)
+		if (cond & MMC_CARD_BUSY)
 		{
 			if (cond & 0x40000000)
 				storage->has_sector_access = 1;
 			return 1;
 		}
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 			break;
-		sleep(1000);
+		usleep(1000);
 	}
 
 	return 0;
@@ -242,6 +254,76 @@ static int _mmc_storage_set_relative_addr(sdmmc_storage_t *storage)
 	return _sdmmc_storage_execute_cmd_type1(storage, MMC_SET_RELATIVE_ADDR, storage->rca << 16, 0, 0x10);
 }
 
+static void _mmc_storage_parse_cid(sdmmc_storage_t *storage)
+{
+	u32 *raw_cid = (u32 *)&(storage->raw_cid);
+
+	switch (storage->csd.mmca_vsn)
+	{
+	case 0: /* MMC v1.0 - v1.2 */
+	case 1: /* MMC v1.4 */
+		storage->cid.prod_name[6] = unstuff_bits(raw_cid, 48, 8);
+		storage->cid.manfid = unstuff_bits(raw_cid, 104, 24);
+		storage->cid.hwrev  = unstuff_bits(raw_cid, 44, 4);
+		storage->cid.fwrev  = unstuff_bits(raw_cid, 40, 4);
+		storage->cid.serial = unstuff_bits(raw_cid, 16, 24);
+		break;
+	case 2: /* MMC v2.0 - v2.2 */
+	case 3: /* MMC v3.1 - v3.3 */
+	case 4: /* MMC v4 */
+		storage->cid.manfid   = unstuff_bits(raw_cid, 120, 8);
+		storage->cid.card_bga = unstuff_bits(raw_cid, 112, 2);
+		storage->cid.oemid    = unstuff_bits(raw_cid, 104, 8);
+		storage->cid.prv      = unstuff_bits(raw_cid, 48, 8);
+		storage->cid.serial   = unstuff_bits(raw_cid, 16, 32);
+		break;
+	default:
+		break;
+	}
+
+	storage->cid.prod_name[0] = unstuff_bits(raw_cid, 96, 8);
+	storage->cid.prod_name[1] = unstuff_bits(raw_cid, 88, 8);
+	storage->cid.prod_name[2] = unstuff_bits(raw_cid, 80, 8);
+	storage->cid.prod_name[3] = unstuff_bits(raw_cid, 72, 8);
+	storage->cid.prod_name[4] = unstuff_bits(raw_cid, 64, 8);
+	storage->cid.prod_name[5] = unstuff_bits(raw_cid, 56, 8);
+
+	storage->cid.month = unstuff_bits(raw_cid, 12, 4);
+	storage->cid.year  = unstuff_bits(raw_cid, 8, 4) + 1997;
+	if (storage->ext_csd.rev >= 5)
+	{
+		if (storage->cid.year < 2010)
+			storage->cid.year += 16;
+	}
+}
+
+static void _mmc_storage_parse_csd(sdmmc_storage_t *storage)
+{
+	u32 *raw_csd = (u32 *)&(storage->raw_csd);
+
+	storage->csd.mmca_vsn = unstuff_bits(raw_csd, 122, 4);
+	storage->csd.structure = unstuff_bits(raw_csd, 126, 2);
+	storage->csd.cmdclass = unstuff_bits(raw_csd, 84, 12);
+	storage->csd.read_blkbits = unstuff_bits(raw_csd, 80, 4);
+	storage->csd.capacity = (1 + unstuff_bits(raw_csd, 62, 12)) << (unstuff_bits(raw_csd, 47, 3) + 2);
+}
+
+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];
+	storage->ext_csd.card_type = buf[EXT_CSD_CARD_TYPE];
+	storage->ext_csd.dev_version = *(u16 *)&buf[EXT_CSD_DEVICE_VERSION];
+	storage->ext_csd.boot_mult = buf[EXT_CSD_BOOT_MULT];
+	storage->ext_csd.rpmb_mult = buf[EXT_CSD_RPMB_MULT];
+	storage->ext_csd.sectors = *(u32 *)&buf[EXT_CSD_SEC_CNT];
+	storage->ext_csd.bkops = buf[EXT_CSD_BKOPS_SUPPORT];
+	storage->ext_csd.bkops_en = buf[EXT_CSD_BKOPS_EN];
+	storage->ext_csd.bkops_status = buf[EXT_CSD_BKOPS_STATUS];
+
+	storage->sec_cnt  = *(u32 *)&buf[EXT_CSD_SEC_CNT];
+}
+
 static int _mmc_storage_get_ext_csd(sdmmc_storage_t *storage, void *buf)
 {
 	sdmmc_cmd_t cmdbuf;
@@ -260,6 +342,8 @@ static int _mmc_storage_get_ext_csd(sdmmc_storage_t *storage, void *buf)
 
 	u32 tmp = 0;
 	sdmmc_get_rsp(storage->sdmmc, &tmp, 4, SDMMC_RSP_TYPE_1);
+	_mmc_storage_parse_ext_csd(storage, buf);
+
 	return _sdmmc_storage_check_result(tmp);
 }
 
@@ -302,7 +386,8 @@ static int _mmc_storage_enable_HS(sdmmc_storage_t *storage, int check)
 		return 0;
 	if (!sdmmc_setup_clock(storage->sdmmc, 2))
 		return 0;
-	DPRINTF("[mmc] switched to HS\n");
+	DPRINTF("[MMC] switched to HS\n");
+	storage->csd.busspeed = 52;
 	if (check || _sdmmc_storage_check_status(storage))
 		return 1;
 	return 0;
@@ -316,7 +401,8 @@ static int _mmc_storage_enable_HS200(sdmmc_storage_t *storage)
 		return 0;
 	if (!sdmmc_config_tuning(storage->sdmmc, 3, MMC_SEND_TUNING_BLOCK_HS200))
 		return 0;
-	DPRINTF("[mmc] switched to HS200\n");
+	DPRINTF("[MMC] switched to HS200\n");
+	storage->csd.busspeed = 200;
 	return _sdmmc_storage_check_status(storage);
 }
 
@@ -333,7 +419,8 @@ static int _mmc_storage_enable_HS400(sdmmc_storage_t *storage)
 		return 0;
 	if (!sdmmc_setup_clock(storage->sdmmc, 4))
 		return 0;
-	DPRINTF("[mmc] switched to HS400\n");
+	DPRINTF("[MMC] switched to HS400\n");
+	storage->csd.busspeed = 400;
 	return _sdmmc_storage_check_status(storage);
 }
 
@@ -350,9 +437,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:;
@@ -376,43 +463,44 @@ int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 
 	if (!sdmmc_init(sdmmc, id, SDMMC_POWER_1_8, SDMMC_BUS_WIDTH_1, 0, 0))
 		return 0;
-	DPRINTF("[mmc] after init\n");
+	DPRINTF("[MMC] after init\n");
 
-	sleep(1000 + (74000 + sdmmc->divisor - 1) / sdmmc->divisor);
+	usleep(1000 + (74000 + sdmmc->divisor - 1) / sdmmc->divisor);
 
 	if (!_sdmmc_storage_go_idle_state(storage))
 		return 0;
-	DPRINTF("[mmc] went to idle state\n");
+	DPRINTF("[MMC] went to idle state\n");
 
 	if (!_mmc_storage_get_op_cond(storage, SDMMC_POWER_1_8))
 		return 0;
-	DPRINTF("[mmc] got op cond\n");
+	DPRINTF("[MMC] got op cond\n");
 
-	if (!_sdmmc_storage_get_cid(storage, storage->cid))
+	if (!_sdmmc_storage_get_cid(storage, storage->raw_cid))
 		return 0;
-	DPRINTF("[mmc] got cid\n");
+	DPRINTF("[MMC] got cid\n");
 
 	if (!_mmc_storage_set_relative_addr(storage))
 		return 0;
-	DPRINTF("[mmc] set relative addr\n");
+	DPRINTF("[MMC] set relative addr\n");
 
-	if (!_sdmmc_storage_get_csd(storage, storage->csd))
+	if (!_sdmmc_storage_get_csd(storage, storage->raw_csd))
 		return 0;
-	DPRINTF("[mmc] got csd\n");
+	DPRINTF("[MMC] got csd\n");
+	_mmc_storage_parse_csd(storage);
 
 	if (!sdmmc_setup_clock(storage->sdmmc, 1))
 		return 0;
-	DPRINTF("[mmc] after setup clock\n");
+	DPRINTF("[MMC] after setup clock\n");
 
 	if (!_sdmmc_storage_select_card(storage))
 		return 0;
-	DPRINTF("[mmc] card selected\n");
+	DPRINTF("[MMC] card selected\n");
 
 	if (!_sdmmc_storage_set_blocklen(storage, 512))
 		return 0;
-	DPRINTF("[mmc] set blocklen to 512\n");
+	DPRINTF("[MMC] set blocklen to 512\n");
 
-	u32 *csd = (u32 *)storage->csd;
+	u32 *csd = (u32 *)storage->raw_csd;
 	//Check system specification version, only version 4.0 and later support below features.
 	if (unstuff_bits(csd, 122, 4) < CSD_SPEC_VER_4)
 	{
@@ -422,7 +510,7 @@ int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 
 	if (!_mmc_storage_switch_buswidth(storage, bus_width))
 		return 0;
-	DPRINTF("[mmc] switched buswidth\n");
+	DPRINTF("[MMC] switched buswidth\n");
 
 	u8 *ext_csd = (u8 *)malloc(512);
 	if (!_mmc_storage_get_ext_csd(storage, ext_csd))
@@ -430,23 +518,28 @@ int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 		free(ext_csd);
 		return 0;
 	}
+	free(ext_csd);
+	DPRINTF("[MMC] got ext_csd\n");
+	_mmc_storage_parse_cid(storage); //This needs to be after csd and ext_csd
 	//gfx_hexdump(&gfx_con, 0, ext_csd, 512);
 
-	storage->sec_cnt = *(u32 *)&ext_csd[EXT_CSD_SEC_CNT];
-
-	if (storage->cid[0xE] == 0x11 && ext_csd[EXT_CSD_BKOPS_EN] & EXT_CSD_BKOPS_LEVEL_2)
-		_mmc_storage_enable_bkops(storage);
-
-	if (!_mmc_storage_enable_highspeed(storage, ext_csd[EXT_CSD_CARD_TYPE], type))
+	/* When auto BKOPS is enabled the mmc device should be powered all the time until we disable this and check status.
+	   Disable it for now until BKOPS disable added to power down sequence at sdmmc_storage_end().
+	   Additionally this works only when we put the device in idle mode which we don't after enabling it. */
+	if (storage->ext_csd.bkops & 0x1 && !(storage->ext_csd.bkops_en & EXT_CSD_BKOPS_LEVEL_2) && 0)
 	{
-		free(ext_csd);
-		return 0;
+		_mmc_storage_enable_bkops(storage);
+		DPRINTF("[MMC] BKOPS enabled\n");
 	}
-	DPRINTF("[mmc] switched to possible highspeed mode\n");
+	else
+		DPRINTF("[MMC] BKOPS disabled\n");
+
+	if (!_mmc_storage_enable_highspeed(storage, storage->ext_csd.card_type, type))
+		return 0;
+	DPRINTF("[MMC] succesfully switched to highspeed mode\n");
 
 	sdmmc_sd_clock_ctrl(storage->sdmmc, 1);
 
-	free(ext_csd);
 	return 1;
 }
 
@@ -484,21 +577,26 @@ static int _sd_storage_send_if_cond(sdmmc_storage_t *storage)
 	sdmmc_cmd_t cmdbuf;
 	sdmmc_init_cmd(&cmdbuf, SD_SEND_IF_COND, 0x1AA, SDMMC_RSP_TYPE_5, 0);
 	if (!sdmmc_execute_cmd(storage->sdmmc, &cmdbuf, 0, 0))
-		return 0;
-
-	//TODO: we may have received a timeout error in the above request, which indicates a version 1 card.
+		return 1; // The SD Card is version 1.X
 
 	u32 resp = 0;
 	if (!sdmmc_get_rsp(storage->sdmmc, &resp, 4, SDMMC_RSP_TYPE_5))
-		return 0;
+		return 2;
 
-	return (resp & 0xFFF) == 0x1AA ? 1 : 0;
+	return (resp & 0xFF) == 0xAA ? 0 : 2;
 }
 
 static int _sd_storage_get_op_cond_once(sdmmc_storage_t *storage, u32 *cond, int is_version_1, int supports_low_voltage)
 {
 	sdmmc_cmd_t cmdbuf;
-	u32 arg = (((~is_version_1 & 1) << 28) & 0xBFFFFFFF | ((~is_version_1 & 1) << 30)) & 0xFEFFFFFF | ((supports_low_voltage & ~is_version_1 & 1) << 24) | 0x100000;
+	// Support for Current > 150mA
+	u32 arg = (~is_version_1 & 1) ? SD_OCR_XPC : 0;
+	// Support for handling block-addressed SDHC cards
+	arg	|= (~is_version_1 & 1) ? SD_OCR_CCS : 0;
+	// Support for 1.8V
+	arg |= (supports_low_voltage & ~is_version_1 & 1) ? SD_OCR_S18R : 0;
+	// This is needed for most cards. Do not set bit7 even if 1.8V is supported.
+	arg |= SD_OCR_VDD_32_33;
 	sdmmc_init_cmd(&cmdbuf, SD_APP_OP_COND, arg, SDMMC_RSP_TYPE_3, 0);
 	if (!_sd_storage_execute_app_cmd(storage, 0x10, is_version_1 ? 0x400000 : 0, &cmdbuf, 0, 0))
 		return 0;
@@ -507,22 +605,22 @@ static int _sd_storage_get_op_cond_once(sdmmc_storage_t *storage, u32 *cond, int
 
 static int _sd_storage_get_op_cond(sdmmc_storage_t *storage, int is_version_1, int supports_low_voltage)
 {
-	u32 timeout = get_tmr() + 1500000;
+	u32 timeout = get_tmr_ms() + 1500;
 
 	while (1)
 	{
 		u32 cond = 0;
 		if (!_sd_storage_get_op_cond_once(storage, &cond, is_version_1, supports_low_voltage))
 			break;
-		if (cond & 0x80000000)
+		if (cond & MMC_CARD_BUSY)
 		{
-			if (cond & 0x40000000)
+			if (cond & SD_OCR_CCS)
 				storage->has_sector_access = 1;
 
-			if (cond & 0x1000000 && supports_low_voltage)
+			if (cond & SD_ROCR_S18A && supports_low_voltage)
 			{
 				//The low voltage regulator configuration is valid for SDMMC1 only.
-				if (storage->sdmmc->id == SDMMC_1 && 
+				if (storage->sdmmc->id == SDMMC_1 &&
 					_sdmmc_storage_execute_cmd_type1(storage, SD_SWITCH_VOLTAGE, 0, 0, R1_STATE_READY))
 				{
 					if (!sdmmc_enable_low_voltage(storage->sdmmc))
@@ -535,9 +633,9 @@ static int _sd_storage_get_op_cond(sdmmc_storage_t *storage, int is_version_1, i
 
 			return 1;
 		}
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 			break;
-		sleep(1000);
+		msleep(10); // Needs to be at least 10ms for some SD Cards
 	}
 
 	return 0;
@@ -548,7 +646,7 @@ static int _sd_storage_get_rca(sdmmc_storage_t *storage)
 	sdmmc_cmd_t cmdbuf;
 	sdmmc_init_cmd(&cmdbuf, SD_SEND_RELATIVE_ADDR, 0, SDMMC_RSP_TYPE_4, 0);
 
-	u32 timeout = get_tmr() + 1500000;
+	u32 timeout = get_tmr_ms() + 1500;
 
 	while (1)
 	{
@@ -565,15 +663,32 @@ static int _sd_storage_get_rca(sdmmc_storage_t *storage)
 			return 1;
 		}
 
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 			break;
-		sleep(1000);
+		usleep(1000);
 	}
 
 	return 0;
 }
 
-int _sd_storage_get_scr(sdmmc_storage_t *storage, void *buf)
+static void _sd_storage_parse_scr(sdmmc_storage_t *storage)
+{
+	// unstuff_bits can parse only 4 u32
+	u32 resp[4];
+
+	resp[3] = *(u32 *)&storage->raw_scr[4];
+	resp[2] = *(u32 *)&storage->raw_scr[0];
+
+	storage->scr.sda_vsn = unstuff_bits(resp, 56, 4);
+	storage->scr.bus_widths = unstuff_bits(resp, 48, 4);
+	if (storage->scr.sda_vsn == SCR_SPEC_VER_2)
+		/* Check if Physical Layer Spec v3.0 is supported */
+		storage->scr.sda_spec3 = unstuff_bits(resp, 47, 1);
+	if (storage->scr.sda_spec3)
+		storage->scr.cmds = unstuff_bits(resp, 32, 2);
+}
+
+int _sd_storage_get_scr(sdmmc_storage_t *storage, u8 *buf)
 {
 	sdmmc_cmd_t cmdbuf;
 	sdmmc_init_cmd(&cmdbuf, SD_APP_SEND_SCR, 0, SDMMC_RSP_TYPE_1, 0);
@@ -591,6 +706,17 @@ int _sd_storage_get_scr(sdmmc_storage_t *storage, void *buf)
 
 	u32 tmp = 0;
 	sdmmc_get_rsp(storage->sdmmc, &tmp, 4, SDMMC_RSP_TYPE_1);
+    //Prepare buffer for unstuff_bits
+	for (int i = 0; i < 8; i+=4)
+	{
+		storage->raw_scr[i + 3] = buf[i];
+		storage->raw_scr[i + 2] = buf[i + 1];
+		storage->raw_scr[i + 1] = buf[i + 2];
+		storage->raw_scr[i]     = buf[i + 3];
+	}
+	_sd_storage_parse_scr(storage);
+	//gfx_hexdump(&gfx_con, 0, storage->raw_scr, 8);
+
 	return _sdmmc_storage_check_result(tmp);
 }
 
@@ -615,10 +741,13 @@ int _sd_storage_switch_get(sdmmc_storage_t *storage, void *buf)
 	return _sdmmc_storage_check_result(tmp);
 }
 
-int _sd_storage_switch(sdmmc_storage_t *storage, void *buf, int flag, u32 arg)
+int _sd_storage_switch(sdmmc_storage_t *storage, void *buf, int mode, int group, u32 arg)
 {
 	sdmmc_cmd_t cmdbuf;
-	sdmmc_init_cmd(&cmdbuf, SD_SWITCH, arg | (flag << 31) | 0xFFFFF0, SDMMC_RSP_TYPE_1, 0);
+	u32 switchcmd = mode << 31 | 0x00FFFFFF;
+	switchcmd &= ~(0xF << (group * 4));
+	switchcmd |= arg << (group * 4);
+	sdmmc_init_cmd(&cmdbuf, SD_SWITCH, switchcmd, SDMMC_RSP_TYPE_1, 0);
 
 	sdmmc_req_t reqbuf;
 	reqbuf.buf = buf;
@@ -636,21 +765,52 @@ int _sd_storage_switch(sdmmc_storage_t *storage, void *buf, int flag, u32 arg)
 	return _sdmmc_storage_check_result(tmp);
 }
 
+void _sd_storage_set_current_limit(sdmmc_storage_t *storage, u8 *buf)
+{
+	u32 pwr = SD_SET_CURRENT_LIMIT_800;
+	_sd_storage_switch(storage, buf, SD_SWITCH_SET, 3, pwr);
+
+	while (pwr > 0)
+	{
+		pwr--;
+		_sd_storage_switch(storage, buf, SD_SWITCH_SET, 3, pwr);
+		if (((buf[15] >> 4) & 0x0F) == pwr)
+			break;
+	}
+
+	switch (pwr)
+	{
+	case SD_SET_CURRENT_LIMIT_800:
+		DPRINTF("[SD] Power limit raised to 800mA\n");
+		break;
+	case SD_SET_CURRENT_LIMIT_600:
+		DPRINTF("[SD] Power limit raised to 600mA\n");
+		break;
+	case SD_SET_CURRENT_LIMIT_400:
+		DPRINTF("[SD] Power limit raised to 800mA\n");
+		break;
+	default:
+	case SD_SET_CURRENT_LIMIT_200:
+		DPRINTF("[SD] Power limit defaulted to 200mA\n");
+		break;
+	}
+}
+
 int _sd_storage_enable_highspeed(sdmmc_storage_t *storage, u32 hs_type, u8 *buf)
 {
-	if (!_sd_storage_switch(storage, buf, 0, hs_type))
+	if (!_sd_storage_switch(storage, buf, SD_SWITCH_CHECK, 0, hs_type))
 		return 0;
 
 	u32 type_out = buf[16] & 0xF;
 	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))
+		if (!_sd_storage_switch(storage, buf, SD_SWITCH_SET, 0, hs_type))
 			return 0;
 
-		if (type_out != buf[16] & 0xF)
+		if (type_out != (buf[16] & 0xF))
 			return 0;
 	}
 
@@ -659,28 +819,45 @@ int _sd_storage_enable_highspeed(sdmmc_storage_t *storage, u32 hs_type, u8 *buf)
 
 int _sd_storage_enable_highspeed_low_volt(sdmmc_storage_t *storage, u32 type, u8 *buf)
 {
+	// Try to raise the current limit to let the card perform better.
+	_sd_storage_set_current_limit(storage, buf);
+
 	if (sdmmc_get_bus_width(storage->sdmmc) != SDMMC_BUS_WIDTH_4)
 		return 0;
 
 	if (!_sd_storage_switch_get(storage, buf))
 		return 0;
+	//gfx_hexdump(&gfx_con, 0, (u8 *)buf, 64);
 
 	u32 hs_type = 0;
 	switch (type)
 	{
 	case 11:
-		if (buf[13] & 8)
+		if (buf[13] & SD_MODE_UHS_SDR104)
 		{
 			type = 11;
-			hs_type = 3;
+			hs_type = UHS_SDR104_BUS_SPEED;
+			DPRINTF("[SD] Bus speed set to SDR104\n");
+			storage->csd.busspeed = 104;
 			break;
 		}
 		//Fall through.
 	case 10:
-		if (!(buf[13] & 4))
+		if (buf[13] & SD_MODE_UHS_SDR50)
+		{
+			type = 10;
+			hs_type = UHS_SDR50_BUS_SPEED;
+			DPRINTF("[SD] Bus speed set to SDR50\n");
+			storage->csd.busspeed = 50;
+			break;
+		}
+	case 8:
+		if (!(buf[13] & SD_MODE_UHS_SDR12))
 			return 0;
-		type = 10;
-		hs_type = 2;
+		type = 8;
+		hs_type = UHS_SDR12_BUS_SPEED;
+		DPRINTF("[SD] Bus speed set to SDR12\n");
+		storage->csd.busspeed = 12;
 		break;
 	default:
 		return 0;
@@ -700,7 +877,7 @@ int _sd_storage_enable_highspeed_high_volt(sdmmc_storage_t *storage, u8 *buf)
 {
 	if (!_sd_storage_switch_get(storage, buf))
 		return 0;
-
+	//gfx_hexdump(&gfx_con, 0, (u8 *)buf, 64);
 	if (!(buf[13] & 2))
 		return 1;
 
@@ -711,59 +888,177 @@ int _sd_storage_enable_highspeed_high_volt(sdmmc_storage_t *storage, u8 *buf)
 	return sdmmc_setup_clock(storage->sdmmc, 7);
 }
 
+static void _sd_storage_parse_ssr(sdmmc_storage_t *storage)
+{
+	// unstuff_bits supports only 4 u32 so break into 2 x 16byte groups
+	u32 raw_ssr1[4];
+	u32 raw_ssr2[4];
+
+	raw_ssr1[3] = *(u32 *)&storage->raw_ssr[12];
+	raw_ssr1[2] = *(u32 *)&storage->raw_ssr[8];
+	raw_ssr1[1] = *(u32 *)&storage->raw_ssr[4];
+	raw_ssr1[0] = *(u32 *)&storage->raw_ssr[0];
+
+	raw_ssr2[3] = *(u32 *)&storage->raw_ssr[28];
+	raw_ssr2[2] = *(u32 *)&storage->raw_ssr[24];
+	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;
+	switch(unstuff_bits(raw_ssr1, 440 - 384, 8))
+	{
+	case 0:
+		storage->ssr.speed_class = 0;
+		break;
+	case 1:
+		storage->ssr.speed_class = 2;
+		break;
+	case 2:
+		storage->ssr.speed_class = 4;
+		break;
+	case 3:
+		storage->ssr.speed_class = 6;
+		break;
+	case 4:
+		storage->ssr.speed_class = 10;
+		break;
+	default:
+		storage->ssr.speed_class = unstuff_bits(raw_ssr1, 440 - 384, 8);
+		break;
+	}
+	storage->ssr.uhs_grade = unstuff_bits(raw_ssr1, 396 - 384, 4);
+	storage->ssr.video_class = unstuff_bits(raw_ssr1, 384 - 384, 8);
+
+	storage->ssr.app_class = unstuff_bits(raw_ssr2, 336 - 256, 4);
+}
+
+static int _sd_storage_get_ssr(sdmmc_storage_t *storage, u8 *buf)
+{
+	sdmmc_cmd_t cmdbuf;
+	sdmmc_init_cmd(&cmdbuf, SD_APP_SD_STATUS, 0, SDMMC_RSP_TYPE_1, 0);
+
+	sdmmc_req_t reqbuf;
+	reqbuf.buf = buf;
+	reqbuf.blksize = 64;
+	reqbuf.num_sectors = 1;
+	reqbuf.is_write = 0;
+	reqbuf.is_multi_block = 0;
+	reqbuf.is_auto_cmd12 = 0;
+
+	if (!(storage->csd.cmdclass & CCC_APP_SPEC)) {
+		DPRINTF("[SD] ssr: Card lacks mandatory SD Status function\n");
+		return 0;
+	}
+
+	if (!_sd_storage_execute_app_cmd(storage, R1_STATE_TRAN, 0, &cmdbuf, &reqbuf, 0))
+		return 0;
+
+	u32 tmp = 0;
+	sdmmc_get_rsp(storage->sdmmc, &tmp, 4, SDMMC_RSP_TYPE_1);
+    //Prepare buffer for unstuff_bits
+	for (int i = 0; i < 64; i+=4)
+	{
+		storage->raw_ssr[i + 3] = buf[i];
+		storage->raw_ssr[i + 2] = buf[i + 1];
+		storage->raw_ssr[i + 1] = buf[i + 2];
+		storage->raw_ssr[i]     = buf[i + 3];
+	}
+	_sd_storage_parse_ssr(storage);
+	//gfx_hexdump(&gfx_con, 0, storage->raw_ssr, 64);
+
+	return _sdmmc_storage_check_result(tmp);
+}
+
+static void _sd_storage_parse_cid(sdmmc_storage_t *storage)
+{
+	u32 *raw_cid = (u32 *)&(storage->raw_cid);
+
+	storage->cid.manfid = unstuff_bits(raw_cid, 120, 8);
+	storage->cid.oemid  = unstuff_bits(raw_cid, 104, 16);
+	storage->cid.prod_name[0] = unstuff_bits(raw_cid, 96, 8);
+	storage->cid.prod_name[1] = unstuff_bits(raw_cid, 88, 8);
+	storage->cid.prod_name[2] = unstuff_bits(raw_cid, 80, 8);
+	storage->cid.prod_name[3] = unstuff_bits(raw_cid, 72, 8);
+	storage->cid.prod_name[4] = unstuff_bits(raw_cid, 64, 8);
+	storage->cid.hwrev  = unstuff_bits(raw_cid, 60, 4);
+	storage->cid.fwrev  = unstuff_bits(raw_cid, 56, 4);
+	storage->cid.serial = unstuff_bits(raw_cid, 24, 32);
+	storage->cid.month  = unstuff_bits(raw_cid, 8, 4);
+	storage->cid.year   = unstuff_bits(raw_cid, 12, 8) + 2000;
+}
+
+static void _sd_storage_parse_csd(sdmmc_storage_t *storage)
+{
+	u32 *raw_csd = (u32 *)&(storage->raw_csd);
+
+	storage->csd.structure = unstuff_bits(raw_csd, 126, 2);
+	storage->csd.cmdclass  = unstuff_bits(raw_csd, 84, 12);
+	storage->csd.read_blkbits  = unstuff_bits(raw_csd, 80, 4);
+	storage->csd.write_protect = unstuff_bits(raw_csd, 12, 2);
+	switch(storage->csd.structure)
+	{
+	case 0:
+		storage->csd.capacity = (1 + unstuff_bits(raw_csd, 62, 12)) << (unstuff_bits(raw_csd, 47, 3) + 2);
+		break;
+	case 1:
+		storage->csd.c_size = (1 + unstuff_bits(raw_csd, 48, 22));
+		storage->csd.capacity = storage->csd.c_size << 10;
+		storage->csd.read_blkbits = 9;
+		break;
+	}
+}
+
 int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32 bus_width, u32 type)
 {
+	int is_version_1 = 0;
+
 	memset(storage, 0, sizeof(sdmmc_storage_t));
 	storage->sdmmc = sdmmc;
 
 	if (!sdmmc_init(sdmmc, id, SDMMC_POWER_3_3, SDMMC_BUS_WIDTH_1, 5, 0))
 		return 0;
-	DPRINTF("[sd] after init\n");
+	DPRINTF("[SD] after init\n");
 
-	sleep(1000 + (74000 + sdmmc->divisor - 1) / sdmmc->divisor);
+	usleep(1000 + (74000 + sdmmc->divisor - 1) / sdmmc->divisor);
 
 	if (!_sdmmc_storage_go_idle_state(storage))
 		return 0;
-	DPRINTF("[sd] went to idle state\n");
+	DPRINTF("[SD] went to idle state\n");
 
-	if (!_sd_storage_send_if_cond(storage))
+	is_version_1 = _sd_storage_send_if_cond(storage);
+	if (is_version_1 == 2)
 		return 0;
-	DPRINTF("[sd] after send if cond\n");
+	DPRINTF("[SD] after send if cond\n");
 
-	//TODO: use correct version here -----v
-	if (!_sd_storage_get_op_cond(storage, 0, bus_width == SDMMC_BUS_WIDTH_4 && (type | 1) == 11))
+	if (!_sd_storage_get_op_cond(storage, is_version_1, bus_width == SDMMC_BUS_WIDTH_4 && type == 11))
 		return 0;
-	DPRINTF("[sd] got op cond\n");
+	DPRINTF("[SD] got op cond\n");
 
-	if (!_sdmmc_storage_get_cid(storage, storage->cid))
+	if (!_sdmmc_storage_get_cid(storage, storage->raw_cid))
 		return 0;
-	DPRINTF("[sd] got cid\n");
+	DPRINTF("[SD] got cid\n");
+	_sd_storage_parse_cid(storage);
 
 	if (!_sd_storage_get_rca(storage))
 		return 0;
-	DPRINTF("[sd] got rca (= %04X)\n", storage->rca);
+	DPRINTF("[SD] got rca (= %04X)\n", storage->rca);
 
-	if (!_sdmmc_storage_get_csd(storage, storage->csd))
+	if (!_sdmmc_storage_get_csd(storage, storage->raw_csd))
 		return 0;
-	DPRINTF("[sd] got csd\n");
+	DPRINTF("[SD] got csd\n");
 
 	//Parse CSD.
-	u32 *csd = (u32 *)storage->csd;
-	u32 csd_struct = unstuff_bits(csd, 126, 2);
-	switch (csd_struct)
+	_sd_storage_parse_csd(storage);
+	switch (storage->csd.structure)
 	{
 	case 0:
-		storage->sec_cnt = (1 + unstuff_bits(csd, 62, 12)) << (unstuff_bits(csd, 47, 3) + 2);
+		storage->sec_cnt = storage->csd.capacity;
 		break;
 	case 1:
-		storage->sec_cnt = (1 + unstuff_bits(csd, 48, 22)) << 10;
+		storage->sec_cnt = storage->csd.c_size << 10;
 		break;
 	default:
-		DPRINTF("[sd] Unknown CSD structure %d\n", csd_struct);
-		//TODO: I've encountered this with one of my SD cards, but
-		//      according to the spec only version 0 and 1 are
-		//      supposed to be in use (mine was version 2).
-		//return 0;
+		DPRINTF("[SD] Unknown CSD structure %d\n", storage->csd.structure);
 		break;
 	}
 
@@ -771,29 +1066,30 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 	{
 		if (!sdmmc_setup_clock(storage->sdmmc, 6))
 			return 0;
-		DPRINTF("[sd] after setup clock\n");
+		DPRINTF("[SD] after setup clock\n");
 	}
 
 	if (!_sdmmc_storage_select_card(storage))
 		return 0;
-	DPRINTF("[sd] card selected\n");
+	DPRINTF("[SD] card selected\n");
 
 	if (!_sdmmc_storage_set_blocklen(storage, 512))
 		return 0;
-	DPRINTF("[sd] set blocklen to 512\n");
+	DPRINTF("[SD] set blocklen to 512\n");
 
 	u32 tmp = 0;
 	if (!_sd_storage_execute_app_cmd_type1(storage, &tmp, SD_APP_SET_CLR_CARD_DETECT, 0, 0, R1_STATE_TRAN))
 		return 0;
-	DPRINTF("[sd] cleared card detect\n");
+	DPRINTF("[SD] cleared card detect\n");
 
 	u8 *buf = (u8 *)malloc(512);
 	if (!_sd_storage_get_scr(storage, buf))
 		return 0;
-	memcpy(storage->scr, buf, 8);
-	DPRINTF("[sd] got scr\n");
+	//gfx_hexdump(&gfx_con, 0, storage->raw_scr, 8);
+	DPRINTF("[SD] got scr\n");
 
-	if (bus_width == SDMMC_BUS_WIDTH_4 && storage->scr[1] & 4)
+	// Check if card supports a wider bus and if it's not SD Version 1.X
+	if (bus_width == SDMMC_BUS_WIDTH_4 && (storage->scr.bus_widths & 4) && (storage->scr.sda_vsn & 0xF))
 	{
 		if (!_sd_storage_execute_app_cmd_type1(storage, &tmp, SD_APP_SET_BUS_WIDTH, SD_BUS_WIDTH_4, 0, R1_STATE_TRAN))
 		{
@@ -801,10 +1097,10 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 			return 0;
 		}
 		sdmmc_set_bus_width(storage->sdmmc, SDMMC_BUS_WIDTH_4);
-		DPRINTF("[sd] switched to wide bus width\n");
+		DPRINTF("[SD] switched to wide bus width\n");
 	}
 	else
-		DPRINTF("[sd] SD does not support wide bus width\n");
+		DPRINTF("[SD] SD does not support wide bus width\n");
 
 	if (storage->is_low_voltage)
 	{
@@ -813,20 +1109,25 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 			free(buf);
 			return 0;
 		}
-		DPRINTF("[sd] enabled highspeed (low voltage)\n");
+		DPRINTF("[SD] enabled highspeed (low voltage)\n");
 	}
-	else if (type != 6 && storage->scr[0] & 0xF != 0)
+	else if (type != 6 && (storage->scr.sda_vsn & 0xF) != 0)
 	{
 		if (!_sd_storage_enable_highspeed_high_volt(storage, buf))
 		{
 			free(buf);
 			return 0;
 		}
-		DPRINTF("[sd] enabled highspeed (high voltage)\n");
+		DPRINTF("[SD] enabled highspeed (high voltage)\n");
+		storage->csd.busspeed = 25;
 	}
 
 	sdmmc_sd_clock_ctrl(sdmmc, 1);
 
+    // Parse additional card info from sd status
+	if (_sd_storage_get_ssr(storage, buf))
+		DPRINTF("[SD] got sd status\n");
+
 	free(buf);
 	return 1;
 }
@@ -843,7 +1144,7 @@ int _gc_storage_custom_cmd(sdmmc_storage_t *storage, void *buf)
 
 	sdmmc_req_t reqbuf;
 	reqbuf.buf = buf;
-	reqbuf.blksize = 0x40;
+	reqbuf.blksize = 64;
 	reqbuf.num_sectors = 1;
 	reqbuf.is_write = 1;
 	reqbuf.is_multi_block = 0;
@@ -871,7 +1172,7 @@ int sdmmc_storage_init_gc(sdmmc_storage_t *storage, sdmmc_t *sdmmc)
 		return 0;
 	DPRINTF("[gc] after init\n");
 
-	sleep(1000 + (10000 + sdmmc->divisor - 1) / sdmmc->divisor);
+	usleep(1000 + (10000 + sdmmc->divisor - 1) / sdmmc->divisor);
 
 	if (!sdmmc_config_tuning(storage->sdmmc, 14, MMC_SEND_TUNING_BLOCK_HS200))
 		return 0;

ipl/sdmmc.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -20,6 +21,66 @@
 #include "types.h"
 #include "sdmmc_driver.h"
 
+typedef struct _mmc_cid
+{
+	u32 manfid;
+	u8  prod_name[8];
+	u8  card_bga;
+	u8  prv;
+	u32 serial;
+	u16 oemid;
+	u16	year;
+	u8  hwrev;
+	u8  fwrev;
+	u8  month;
+} mmc_cid_t;
+
+typedef struct _mmc_csd
+{
+	u8  structure;
+	u8  mmca_vsn;
+	u16 cmdclass;
+	u32 c_size;
+	u32 r2w_factor;
+	u32 max_dtr;
+	u32 erase_size;		/* In sectors */
+	u32 read_blkbits;
+	u32 write_blkbits;
+	u32 capacity;
+	u8  write_protect;  
+	u16 busspeed;
+} mmc_csd_t;
+
+typedef struct _mmc_ext_csd
+{
+	u8  rev;
+	u32 sectors;
+	int bkops;        /* background support bit */
+	int bkops_en;     /* manual bkops enable bit */
+	u8  ext_struct;   /* 194 */
+	u8  card_type;    /* 196 */
+	u8  bkops_status; /* 246 */
+	u16 dev_version;
+	u8  boot_mult;
+	u8  rpmb_mult;
+} mmc_ext_csd_t;
+
+typedef struct _sd_scr
+{
+	u8 sda_vsn;
+	u8 sda_spec3;
+	u8 bus_widths;
+	u8 cmds;
+} sd_scr_t;
+
+typedef struct _sd_ssr {
+	u8 bus_width;
+	u8 speed_class;
+	u8 uhs_grade;
+	u8 video_class;
+	u8 app_class;
+} sd_ssr_t;
+
 /*! SDMMC storage context. */
 typedef struct _sdmmc_storage_t
 {
@@ -29,9 +90,15 @@ typedef struct _sdmmc_storage_t
 	u32 sec_cnt;
 	int is_low_voltage;
 	u32 partition;
-	u8 cid[0x10];
-	u8 csd[0x10];
-	u8 scr[8];
+	u8  raw_cid[0x10];
+	u8  raw_csd[0x10];
+	u8  raw_scr[8];
+	u8  raw_ssr[0x40];
+	mmc_cid_t     cid;
+	mmc_csd_t     csd;
+	mmc_ext_csd_t ext_csd;
+	sd_scr_t      scr;
+	sd_ssr_t      ssr;
 } sdmmc_storage_t;
 
 int sdmmc_storage_end(sdmmc_storage_t *storage);

ipl/sdmmc_driver.c

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -54,24 +55,30 @@ int sdmmc_get_voltage(sdmmc_t *sdmmc)
 
 static int _sdmmc_set_voltage(sdmmc_t *sdmmc, u32 power)
 {
+	u8 pwr = 0;
+
 	switch (power)
 	{
 	case SDMMC_POWER_OFF:
 		sdmmc->regs->pwrcon &= ~TEGRA_MMC_PWRCTL_SD_BUS_POWER;
 		break;
 	case SDMMC_POWER_1_8:
-		sdmmc->regs->pwrcon =
-			(sdmmc->regs->pwrcon & TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_MASK) |
-			TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V1_8;
+		sdmmc->regs->pwrcon = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V1_8;
+		pwr = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V1_8;
 		break;
 	case SDMMC_POWER_3_3:
 		sdmmc->regs->pwrcon = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V3_3;
+		pwr = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V3_3;
 		break;
 	default:
 		return 0;
 	}
 
-	sdmmc->regs->pwrcon |= TEGRA_MMC_PWRCTL_SD_BUS_POWER;
+	if (power != SDMMC_POWER_OFF)
+	{
+		pwr |= TEGRA_MMC_PWRCTL_SD_BUS_POWER;
+		sdmmc->regs->pwrcon = pwr;
+	}	
 
 	return 1;
 }
@@ -123,7 +130,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;
 }
@@ -154,20 +161,20 @@ static int _sdmmc_wait_type4(sdmmc_t *sdmmc)
 	sdmmc->regs->field_1B0 |= 0x80000000;
 	_sdmmc_get_clkcon(sdmmc);
 
-	u32 timeout = get_tmr() + 5000;
+	u32 timeout = get_tmr_ms() + 5;
 	while (sdmmc->regs->field_1B0 & 0x80000000)
 	{
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 		{
 			res = 0;
 			goto out;
 		}
 	}
 
-	timeout = get_tmr() + 10000;
+	timeout = get_tmr_ms() + 10;
 	while (sdmmc->regs->field_1BC & 0x80000000)
 	{
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 		{
 			res = 0;
 			goto out;
@@ -198,32 +205,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;
 	}
 
@@ -298,10 +307,30 @@ static int _sdmmc_cache_rsp(sdmmc_t *sdmmc, u32 *rsp, u32 size, u32 type)
 	case SDMMC_RSP_TYPE_2:
 		if (size < 0x10)
 			return 0;
-		rsp[0] = sdmmc->regs->rspreg0;
-		rsp[1] = sdmmc->regs->rspreg1;
-		rsp[2] = sdmmc->regs->rspreg2;
-		rsp[3] = sdmmc->regs->rspreg3;
+		// CRC is stripped, so shifting is needed.
+		u32 tempreg;
+		for (int i = 0; i < 4; i++)
+		{
+			switch(i)
+			{
+			case 0:
+				tempreg = sdmmc->regs->rspreg3;
+				break;
+			case 1:
+				tempreg = sdmmc->regs->rspreg2;
+				break;
+			case 2:
+				tempreg = sdmmc->regs->rspreg1;
+				break;
+			case 3:
+				tempreg = sdmmc->regs->rspreg0;
+				break;
+			}
+			rsp[i] = tempreg << 8;
+
+			if (i != 0)
+				rsp[i - 1] |= (tempreg >> 24) & 0xFF;
+		}
 		break;
 	default:
 		return 0;
@@ -347,8 +376,8 @@ static void _sdmmc_reset(sdmmc_t *sdmmc)
 	sdmmc->regs->swrst |= 
 		TEGRA_MMC_SWRST_SW_RESET_FOR_CMD_LINE | TEGRA_MMC_SWRST_SW_RESET_FOR_DAT_LINE;
 	_sdmmc_get_clkcon(sdmmc);
-	u32 timeout = get_tmr() + 2000000;
-	while (sdmmc->regs->swrst << 29 >> 30 && get_tmr() < timeout)
+	u32 timeout = get_tmr_ms() + 2000;
+	while (sdmmc->regs->swrst << 29 >> 30 && get_tmr_ms() < timeout)
 		;
 }
 
@@ -356,9 +385,9 @@ static int _sdmmc_wait_prnsts_type0(sdmmc_t *sdmmc, u32 wait_dat)
 {
 	_sdmmc_get_clkcon(sdmmc);
 
-	u32 timeout = get_tmr() + 2000000;
+	u32 timeout = get_tmr_ms() + 2000;
 	while(sdmmc->regs->prnsts & 1) //CMD inhibit.
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 		{
 			_sdmmc_reset(sdmmc);
 			return 0;
@@ -366,9 +395,9 @@ static int _sdmmc_wait_prnsts_type0(sdmmc_t *sdmmc, u32 wait_dat)
 
 	if (wait_dat)
 	{
-		timeout = get_tmr() + 2000000;
+		timeout = get_tmr_ms() + 2000;
 		while (sdmmc->regs->prnsts & 2) //DAT inhibit.
-			if (get_tmr() > timeout)
+			if (get_tmr_ms() > timeout)
 			{
 				_sdmmc_reset(sdmmc);
 				return 0;
@@ -382,9 +411,9 @@ static int _sdmmc_wait_prnsts_type1(sdmmc_t *sdmmc)
 {
 	_sdmmc_get_clkcon(sdmmc);
 
-	u32 timeout = get_tmr() + 2000000;
+	u32 timeout = get_tmr_ms() + 2000;
 	while (!(sdmmc->regs->prnsts & 0x100000)) //DAT0 line level.
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 		{
 			_sdmmc_reset(sdmmc);
 			return 0;
@@ -475,27 +504,27 @@ static int _sdmmc_config_tuning_once(sdmmc_t *sdmmc, u32 cmd)
 	sdmmc->regs->clkcon &= ~TEGRA_MMC_CLKCON_SD_CLOCK_ENABLE;
 	_sdmmc_parse_cmd_48(sdmmc, cmd);
 	_sdmmc_get_clkcon(sdmmc);
-	sleep(1);
+	usleep(1);
 	_sdmmc_reset(sdmmc);
 	sdmmc->regs->clkcon |= TEGRA_MMC_CLKCON_SD_CLOCK_ENABLE;
 	_sdmmc_get_clkcon(sdmmc);
 
-	u32 timeout = get_tmr() + 5000;
-	while (get_tmr() < timeout)
+	u32 timeout = get_tmr_us() + 5000;
+	while (get_tmr_us() < timeout)
 	{
 		if (sdmmc->regs->norintsts & 0x20)
 		{
 			sdmmc->regs->norintsts = 0x20;
 			sdmmc->regs->norintstsen &= 0xFFDF;
 			_sdmmc_get_clkcon(sdmmc);
-			sleep((1000 * 8 + sdmmc->divisor - 1) / sdmmc->divisor);
+			usleep((1000 * 8 + sdmmc->divisor - 1) / sdmmc->divisor);
 			return 1;
 		}
 	}
 	_sdmmc_reset(sdmmc);
 	sdmmc->regs->norintstsen &= 0xFFDF;
 	_sdmmc_get_clkcon(sdmmc);
-	sleep((1000 * 8 + sdmmc->divisor - 1) / sdmmc->divisor);
+	usleep((1000 * 8 + sdmmc->divisor - 1) / sdmmc->divisor);
 	return 0;
 }
 
@@ -525,16 +554,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;
 }
@@ -544,21 +573,21 @@ static int _sdmmc_enable_internal_clock(sdmmc_t *sdmmc)
 	//Enable internal clock and wait till it is stable.
 	sdmmc->regs->clkcon |= TEGRA_MMC_CLKCON_INTERNAL_CLOCK_ENABLE;
 	_sdmmc_get_clkcon(sdmmc);
-	u32 timeout = get_tmr() + 2000000;
+	u32 timeout = get_tmr_ms() + 2000;
 	while (!(sdmmc->regs->clkcon & TEGRA_MMC_CLKCON_INTERNAL_CLOCK_STABLE))
 	{
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 			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;
 
@@ -596,7 +625,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;
 }
 
@@ -613,17 +642,17 @@ static void _sdmmc_autocal_execute(sdmmc_t *sdmmc, u32 power)
 	{
 		sdmmc->regs->sdmemcmppadctl |= 0x80000000;
 		_sdmmc_get_clkcon(sdmmc);
-		sleep(1);
+		usleep(1);
 	}
 
 	sdmmc->regs->autocalcfg |= 0xA0000000;
 	_sdmmc_get_clkcon(sdmmc);
-	sleep(1);
+	usleep(1);
 
-	u32 timeout = get_tmr() + 10000;
+	u32 timeout = get_tmr_ms() + 10;
 	while (sdmmc->regs->autocalcfg & 0x80000000)
 	{
-		if (get_tmr() > timeout)
+		if (get_tmr_ms() > timeout)
 		{
 			//In case autocalibration fails, we load suggested standard values.
 			_sdmmc_pad_config_fallback(sdmmc, power);
@@ -681,13 +710,13 @@ static int _sdmmc_wait_request(sdmmc_t *sdmmc)
 {
 	_sdmmc_get_clkcon(sdmmc);
 
-	u32 timeout = get_tmr() + 2000000;
+	u32 timeout = get_tmr_ms() + 2000;
 	while (1)
 	{
 		int res = _sdmmc_check_mask_interrupt(sdmmc, 0, TEGRA_MMC_NORINTSTS_CMD_COMPLETE);
 		if (res == SDMMC_MASKINT_MASKED)
 			break;
-		if (res != SDMMC_MASKINT_NOERROR || get_tmr() > timeout)
+		if (res != SDMMC_MASKINT_NOERROR || get_tmr_ms() > timeout)
 		{
 			_sdmmc_reset(sdmmc);
 			return 0;
@@ -731,11 +760,11 @@ int sdmmc_stop_transmission(sdmmc_t *sdmmc, u32 *rsp)
 		should_disable_sd_clock = 1;
 		sdmmc->regs->clkcon |= TEGRA_MMC_CLKCON_SD_CLOCK_ENABLE;
 		_sdmmc_get_clkcon(sdmmc);
-		sleep((8000 + sdmmc->divisor - 1) / sdmmc->divisor);
+		usleep((8000 + sdmmc->divisor - 1) / sdmmc->divisor);
 	}
 
 	int res = _sdmmc_stop_transmission_inner(sdmmc, rsp);
-	sleep((8000 + sdmmc->divisor - 1) / sdmmc->divisor);
+	usleep((8000 + sdmmc->divisor - 1) / sdmmc->divisor);
 	if (should_disable_sd_clock)
 		sdmmc->regs->clkcon &= ~TEGRA_MMC_CLKCON_SD_CLOCK_ENABLE;
 
@@ -788,7 +817,7 @@ static int _sdmmc_update_dma(sdmmc_t *sdmmc)
 	do
 	{
 		blkcnt = sdmmc->regs->blkcnt;
-		u32 timeout = get_tmr() + 1500000;
+		u32 timeout = get_tmr_ms() + 1500;
 		do
 		{
 			int res = 0;
@@ -814,7 +843,7 @@ static int _sdmmc_update_dma(sdmmc_t *sdmmc)
 				_sdmmc_reset(sdmmc);
 				return 0;
 			}
-		} while (get_tmr() < timeout);
+		} while (get_tmr_ms() < timeout);
 	} while (sdmmc->regs->blkcnt != blkcnt);
 
 	_sdmmc_reset(sdmmc);
@@ -879,11 +908,11 @@ static int _sdmmc_execute_cmd_inner(sdmmc_t *sdmmc, sdmmc_cmd_t *cmd, sdmmc_req_
 static int _sdmmc_config_sdmmc1()
 {
 	//Configure SD card detect.
-	PINMUX_AUX(PINMUX_AUX_GPIO_PZ1) = 0x49; //GPIO control, pull up.
+	PINMUX_AUX(PINMUX_AUX_GPIO_PZ1) = PINMUX_INPUT_ENABLE | PINMUX_PULL_UP | 1; //GPIO control, pull up.
 	APB_MISC(APB_MISC_GP_VGPIO_GPIO_MUX_SEL) = 0;
 	gpio_config(GPIO_PORT_Z, GPIO_PIN_1, GPIO_MODE_GPIO);
 	gpio_output_enable(GPIO_PORT_Z, GPIO_PIN_1, GPIO_OUTPUT_DISABLE);
-	sleep(100);
+	usleep(100);
 	if(!!gpio_read(GPIO_PORT_Z, GPIO_PIN_1))
 		return 0;
 
@@ -898,12 +927,12 @@ static int _sdmmc_config_sdmmc1()
 
 	//Configure SDMMC1 pinmux.
 	APB_MISC(APB_MISC_GP_SDMMC1_CLK_LPBK_CONTROL) = 1;
-	PINMUX_AUX(PINMUX_AUX_SDMMC1_CLK) = 0x2060;
-	PINMUX_AUX(PINMUX_AUX_SDMMC1_CMD) = 0x2068;
-	PINMUX_AUX(PINMUX_AUX_SDMMC1_DAT3) = 0x2068;
-	PINMUX_AUX(PINMUX_AUX_SDMMC1_DAT2) = 0x2068;
-	PINMUX_AUX(PINMUX_AUX_SDMMC1_DAT1) = 0x2068;
-	PINMUX_AUX(PINMUX_AUX_SDMMC1_DAT0) = 0x2068;
+	PINMUX_AUX(PINMUX_AUX_SDMMC1_CLK)  = PINMUX_DRIVE_2X | PINMUX_INPUT_ENABLE | PINMUX_PARKED;
+	PINMUX_AUX(PINMUX_AUX_SDMMC1_CMD)  = PINMUX_DRIVE_2X | PINMUX_INPUT_ENABLE | PINMUX_PARKED | PINMUX_PULL_UP;
+	PINMUX_AUX(PINMUX_AUX_SDMMC1_DAT3) = PINMUX_DRIVE_2X | PINMUX_INPUT_ENABLE | PINMUX_PARKED | PINMUX_PULL_UP;
+	PINMUX_AUX(PINMUX_AUX_SDMMC1_DAT2) = PINMUX_DRIVE_2X | PINMUX_INPUT_ENABLE | PINMUX_PARKED | PINMUX_PULL_UP;
+	PINMUX_AUX(PINMUX_AUX_SDMMC1_DAT1) = PINMUX_DRIVE_2X | PINMUX_INPUT_ENABLE | PINMUX_PARKED | PINMUX_PULL_UP;
+	PINMUX_AUX(PINMUX_AUX_SDMMC1_DAT0) = PINMUX_DRIVE_2X | PINMUX_INPUT_ENABLE | PINMUX_PARKED | PINMUX_PULL_UP;
 
 	//Make sure the SDMMC1 controller is powered.
 	PMC(APBDEV_PMC_NO_IOPOWER) &= ~(1 << 12);
@@ -911,23 +940,23 @@ static int _sdmmc_config_sdmmc1()
 	PMC(APBDEV_PMC_PWR_DET_VAL) |= (1 << 12);
 
 	//Set enable SD card power.
-	PINMUX_AUX(PINMUX_AUX_DMIC3_CLK) = 0x45; //GPIO control, pull down.
+	PINMUX_AUX(PINMUX_AUX_DMIC3_CLK) = PINMUX_INPUT_ENABLE | PINMUX_PULL_DOWN | 1; //GPIO control, pull down.
 	gpio_config(GPIO_PORT_E, GPIO_PIN_4, GPIO_MODE_GPIO);
 	gpio_write(GPIO_PORT_E, GPIO_PIN_4, GPIO_HIGH);
 	gpio_output_enable(GPIO_PORT_E, GPIO_PIN_4, GPIO_OUTPUT_ENABLE);
 
-	sleep(1000);
+	usleep(1000);
 
 	//Enable SD card power.
 	max77620_regulator_set_voltage(REGULATOR_LDO2, 3300000);
 	max77620_regulator_enable(REGULATOR_LDO2, 1);
 
-	sleep(1000);
+	usleep(1000);
 
 	//For good measure.
 	APB_MISC(APB_MISC_GP_SDMMC1_PAD_CFGPADCTRL) = 0x10000000;
 
-	sleep(1000);
+	usleep(1000);
 
 	return 1;
 }
@@ -990,7 +1019,16 @@ void sdmmc_end(sdmmc_t *sdmmc)
 	if (!sdmmc->clock_stopped)
 	{
 		_sdmmc_sd_clock_disable(sdmmc);
+		// Disable SDMMC power. 
 		_sdmmc_set_voltage(sdmmc, SDMMC_POWER_OFF);
+
+		// Disable SD card power.
+		if (sdmmc->id == SDMMC_1)
+		{
+			gpio_output_enable(GPIO_PORT_E, GPIO_PIN_4, GPIO_OUTPUT_DISABLE);
+			msleep(1); // To power cycle min 1ms without power is needed.
+		}
+
 		_sdmmc_get_clkcon(sdmmc);
 		clock_sdmmc_disable(sdmmc->id);
 		sdmmc->clock_stopped = 1;
@@ -1020,11 +1058,11 @@ int sdmmc_execute_cmd(sdmmc_t *sdmmc, sdmmc_cmd_t *cmd, sdmmc_req_t *req, u32 *b
 		should_disable_sd_clock = 1;
 		sdmmc->regs->clkcon |= TEGRA_MMC_CLKCON_SD_CLOCK_ENABLE;
 		_sdmmc_get_clkcon(sdmmc);
-		sleep((8000 + sdmmc->divisor - 1) / sdmmc->divisor);
+		usleep((8000 + sdmmc->divisor - 1) / sdmmc->divisor);
 	}
 
 	int res = _sdmmc_execute_cmd_inner(sdmmc, cmd, req, blkcnt_out);
-	sleep((8000 + sdmmc->divisor - 1) / sdmmc->divisor);
+	usleep((8000 + sdmmc->divisor - 1) / sdmmc->divisor);
 	if (should_disable_sd_clock)
 		sdmmc->regs->clkcon &= ~TEGRA_MMC_CLKCON_SD_CLOCK_ENABLE;
 
@@ -1048,13 +1086,13 @@ int sdmmc_enable_low_voltage(sdmmc_t *sdmmc)
 	_sdmmc_autocal_execute(sdmmc, SDMMC_POWER_1_8);
 	_sdmmc_set_voltage(sdmmc, SDMMC_POWER_1_8);
 	_sdmmc_get_clkcon(sdmmc);
-	sleep(5000);
+	msleep(5);
 	
-	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);
-		sleep(1000u);
+		msleep(1);
 		if ((sdmmc->regs->prnsts & 0xF00000) == 0xF00000)
 			return 1;
 	}

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))

ipl/sdram.c

@@ -23,6 +23,7 @@
 #include "fuse.h"
 #include "max77620.h"
 #include "sdram_param_t210.h"
+#include "clock.h"
 
 #define CONFIG_SDRAM_COMPRESS_CFG
 
@@ -41,42 +42,42 @@ static u32 _get_sdram_id()
 static void _sdram_config(const sdram_params_t *params)
 {
 	PMC(0x45C) = (((4 * params->emc_pmc_scratch1 >> 2) + 0x80000000) ^ 0xFFFF) & 0xC000FFFF;
-	sleep(params->pmc_io_dpd3_req_wait);
+	usleep(params->pmc_io_dpd3_req_wait);
 
 	u32 req = (4 * params->emc_pmc_scratch2 >> 2) + 0x80000000;
 	PMC(APBDEV_PMC_IO_DPD4_REQ) = (req >> 16 << 16) ^ 0x3FFF0000;
-	sleep(params->pmc_io_dpd4_req_wait);
+	usleep(params->pmc_io_dpd4_req_wait);
 	PMC(APBDEV_PMC_IO_DPD4_REQ) = (req ^ 0xFFFF) & 0xC000FFFF;
-	sleep(params->pmc_io_dpd4_req_wait);
+	usleep(params->pmc_io_dpd4_req_wait);
 	PMC(APBDEV_PMC_WEAK_BIAS) = 0;
-	sleep(1);
+	usleep(1);
 
-	CLOCK(0x98) = params->pllm_setup_control;
-	CLOCK(0x9C) = 0;
-	CLOCK(0x90) = (params->pllm_feedback_divider << 8) | params->pllm_input_divider | 0x40000000 | ((params->pllm_post_divider & 0xFFFF) << 20);
+	CLOCK(CLK_RST_CONTROLLER_PLLM_MISC1) = params->pllm_setup_control;
+	CLOCK(CLK_RST_CONTROLLER_PLLM_MISC2) = 0;
+	CLOCK(CLK_RST_CONTROLLER_PLLM_BASE) = (params->pllm_feedback_divider << 8) | params->pllm_input_divider | 0x40000000 | ((params->pllm_post_divider & 0xFFFF) << 20);
 
-	u32 wait_end = TMR(0x10) + 300;
-	while (!(CLOCK(0x90) & 0x8000000))
+	u32 wait_end = get_tmr_us() + 300;
+	while (!(CLOCK(CLK_RST_CONTROLLER_PLLM_BASE) & 0x8000000))
 	{
-		if (TMR(0x10) >= wait_end)
+		if (get_tmr_us() >= wait_end)
 			goto break_nosleep;
 	}
-	sleep(10);
+	usleep(10);
 break_nosleep:
 
-	CLOCK(0x19C) = (params->mc_emem_arb_misc0 >> 11) & 0x10000 | params->emc_clock_source & 0xFFFEFFFF;
+	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC) = ((params->mc_emem_arb_misc0 >> 11) & 0x10000) | (params->emc_clock_source & 0xFFFEFFFF);
 	if (params->emc_clock_source_dll)
-		CLOCK(0x664) = params->emc_clock_source_dll;
+		CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_EMC_DLL) = params->emc_clock_source_dll;
 	if (params->clear_clock2_mc1)
-		CLOCK(0x44C) = 0x40000000;
-	CLOCK(0x328) = 0x2000001;
-	CLOCK(0x284) = 0x4000;
-	CLOCK(0x30C) = 0x2000001;
+		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_W_CLR) = 0x40000000;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = 0x2000001;
+	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_X_SET) = 0x4000;
+	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_CLR) = 0x2000001;
 	EMC(EMC_PMACRO_VTTGEN_CTRL_0) = params->emc_pmacro_vttgen_ctrl0;
 	EMC(EMC_PMACRO_VTTGEN_CTRL_1) = params->emc_pmacro_vttgen_ctrl1;
 	EMC(EMC_PMACRO_VTTGEN_CTRL_2) = params->emc_pmacro_vttgen_ctrl2;
 	EMC(EMC_TIMING_CONTROL) = 1;
-	sleep(1);
+	usleep(1);
 	EMC(EMC_DBG) = (params->emc_dbg_write_mux << 1) | params->emc_dbg;
 	if (params->emc_bct_spare2)
 		*(vu32 *)params->emc_bct_spare2 = params->emc_bct_spare3;
@@ -97,7 +98,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 +143,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 +233,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;
@@ -297,7 +298,7 @@ break_nosleep:
 	EMC(EMC_AUTO_CAL_VREF_SEL_1) = params->emc_auto_cal_vref_sel1;
 	EMC(EMC_AUTO_CAL_INTERVAL) = params->emc_auto_cal_interval;
 	EMC(EMC_AUTO_CAL_CONFIG) = params->emc_auto_cal_config;
-	sleep(params->emc_auto_cal_wait);
+	usleep(params->emc_auto_cal_wait);
 	if (params->emc_bct_spare8)
 		*(vu32 *)params->emc_bct_spare8 = params->emc_bct_spare9;
 	EMC(EMC_CFG_2) = params->emc_cfg2;
@@ -386,14 +387,14 @@ 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;
 		MC(MC_TIMING_CONTROL) = 1;
 	}
 	PMC(0x45C) = ((4 * params->emc_pmc_scratch1 >> 2) + 0x40000000) & 0xCFFF0000;
-	sleep(params->pmc_io_dpd3_req_wait);
+	usleep(params->pmc_io_dpd3_req_wait);
 	if (!params->emc_auto_cal_interval)
 		EMC(EMC_AUTO_CAL_CONFIG) = params->emc_auto_cal_config | 0x200;
 	EMC(EMC_PMACRO_BRICK_CTRL_RFU2) = params->emc_pmacro_brick_ctrl_rfu2;
@@ -408,29 +409,29 @@ break_nosleep:
 		}
 	}
 	EMC(EMC_TIMING_CONTROL) = 1;
-	sleep(params->emc_timing_control_wait);
+	usleep(params->emc_timing_control_wait);
 	PMC(0x4E4) &= 0xFFF8007F;
-	sleep(params->pmc_ddr_ctrl_wait);
+	usleep(params->pmc_ddr_ctrl_wait);
 	if (params->memory_type == 2)
 	{
 		EMC(EMC_PIN) = (params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12);
-		sleep(params->emc_pin_extra_wait + 200);
+		usleep(params->emc_pin_extra_wait + 200);
 		EMC(EMC_PIN) = ((params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12)) + 256;
-		sleep(params->emc_pin_extra_wait + 500);
+		usleep(params->emc_pin_extra_wait + 500);
 	}
 	if (params->memory_type == 3)
 	{
 		EMC(EMC_PIN) = (params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12);
-		sleep(params->emc_pin_extra_wait + 200);
+		usleep(params->emc_pin_extra_wait + 200);
 		EMC(EMC_PIN) = ((params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12)) + 256;
-		sleep(params->emc_pin_extra_wait + 2000);
+		usleep(params->emc_pin_extra_wait + 2000);
 	}
 	EMC(EMC_PIN) = ((params->emc_pin_gpio_enable << 16) | (params->emc_pin_gpio << 12)) + 0x101;
-	sleep(params->emc_pin_program_wait);
+	usleep(params->emc_pin_program_wait);
 	if (params->memory_type != 3)
 		EMC(EMC_NOP) = (params->emc_dev_select << 30) + 1;
 	if (params->memory_type == 1)
-		sleep(params->emc_pin_extra_wait + 200);
+		usleep(params->emc_pin_extra_wait + 200);
 	if (params->memory_type == 3)
 	{
 		if (params->emc_bct_spare10)
@@ -448,12 +449,12 @@ break_nosleep:
 		if (params->emc_zcal_warm_cold_boot_enables & 1)
 		{
 			EMC(EMC_ZQ_CAL) = params->emc_zcal_init_dev0;
-			sleep(params->emc_zcal_init_wait);
+			usleep(params->emc_zcal_init_wait);
 			EMC(EMC_ZQ_CAL) = params->emc_zcal_init_dev0 ^ 3;
 			if (!(params->emc_dev_select & 2))
 			{
 				EMC(EMC_ZQ_CAL) = params->emc_zcal_init_dev1;
-				sleep(params->emc_zcal_init_wait);
+				usleep(params->emc_zcal_init_wait);
 				EMC(EMC_ZQ_CAL) = params->emc_zcal_init_dev1 ^ 3;
 			}
 		}
@@ -481,7 +482,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;
@@ -510,7 +511,7 @@ void sdram_init()
 	i2c_send_byte(I2C_5, 0x3C, MAX77620_REG_SD1, 40); //40 = (1000 * 1100 - 600000) / 12500 -> 1.1V
 
 	PMC(APBDEV_PMC_VDDP_SEL) = params->pmc_vddp_sel;
-	sleep(params->pmc_vddp_sel_wait);
+	usleep(params->pmc_vddp_sel_wait);
 	PMC(APBDEV_PMC_DDR_PWR) = PMC(APBDEV_PMC_DDR_PWR);
 	PMC(APBDEV_PMC_NO_IOPOWER) = params->pmc_no_io_power;
 	PMC(APBDEV_PMC_REG_SHORT) = params->pmc_reg_short;

ipl/sdram_lp0.c

@@ -1,6 +1,8 @@
 /*
  * Copyright (c) 2013, NVIDIA CORPORATION.  All rights reserved.
  * Copyright 2014 Google Inc.
+ * Copyright (C) 2018 naehrwert
+ * Copyright (C) 2018 CTCaer
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -23,7 +25,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 +46,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 +810,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 +1100,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);

ipl/sdram_param_t210_lp0.h

@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2013-2015, NVIDIA CORPORATION.  All rights reserved.
  * Copyright 2014 Google Inc.
+ * Copyright (C) 2018 CTCaer
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -262,7 +263,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 +648,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 +658,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 */

ipl/se.c

@@ -1,5 +1,7 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (c) 2018 CTCaer
+* Copyright (c) 2018 Atmosphère-NX
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -19,6 +21,7 @@
 #include "heap.h"
 #include "t210.h"
 #include "se_t210.h"
+#include "util.h"
 
 typedef struct _se_ll_t
 {
@@ -67,7 +70,7 @@ static int _se_wait()
 	return 1;
 }
 
-static int _se_execute(u32 op, void *dst, u32 dst_size, void *src, u32 src_size)
+static int _se_execute(u32 op, void *dst, u32 dst_size, const void *src, u32 src_size)
 {
 	se_ll_t *ll_dst = NULL, *ll_src = NULL;
 
@@ -99,7 +102,7 @@ static int _se_execute(u32 op, void *dst, u32 dst_size, void *src, u32 src_size)
 	return res;
 }
 
-static int _se_execute_one_block(u32 op, void *dst, u32 dst_size, void *src, u32 src_size)
+static int _se_execute_one_block(u32 op, void *dst, u32 dst_size, const void *src, u32 src_size)
 {
 	u8 *block = (u8 *)malloc(0x10);
 	memset(block, 0, 0x10);
@@ -124,7 +127,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);
 }
@@ -156,7 +159,7 @@ void se_aes_key_clear(u32 ks)
 	}
 }
 
-int se_aes_unwrap_key(u32 ks_dst, u32 ks_src, void *input)
+int se_aes_unwrap_key(u32 ks_dst, u32 ks_src, const void *input)
 {
 	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_DEC_ALG(ALG_AES_DEC) | SE_CONFIG_DST(DST_KEYTAB);
 	SE(SE_CRYPTO_REG_OFFSET) = SE_CRYPTO_KEY_INDEX(ks_src) | SE_CRYPTO_CORE_SEL(CORE_DECRYPT);
@@ -164,7 +167,7 @@ int se_aes_unwrap_key(u32 ks_dst, u32 ks_src, void *input)
 	return _se_execute(OP_START, NULL, 0, input, 0x10);
 }
 
-int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, void *src)
+int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, const void *src)
 {
 	if (enc)
 	{
@@ -180,7 +183,7 @@ int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, void *src)
 	return _se_execute(OP_START, dst, 0x10, src, 0x10);
 }
 
-int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, void *src, u32 src_size, void *ctr)
+int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size, void *ctr)
 {
 	SE(SE_SPARE_0_REG_OFFSET) = 1;
 	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_ALG(ALG_AES_ENC) | SE_CONFIG_DST(DST_MEMORY);
@@ -254,3 +257,29 @@ int se_aes_xts_crypt(u32 ks1, u32 ks2, u32 enc, u64 sec, void *dst, void *src, u
 
 	return 1;
 }
+
+// se_calc_sha256() was derived from Atmosphère's se_calculate_sha256.
+int se_calc_sha256(void *dst, const void *src, u32 src_size) {
+	int res;
+	// Setup config for SHA256, size = BITS(src_size).
+	SE(SE_CONFIG_REG_OFFSET) = SE_CONFIG_ENC_MODE(MODE_SHA256) | SE_CONFIG_ENC_ALG(ALG_SHA) | SE_CONFIG_DST(DST_HASHREG);
+	SE(SE_SHA_CONFIG_REG_OFFSET) = 1;
+	SE(SE_SHA_MSG_LENGTH_REG_OFFSET) = (u32)(src_size << 3);
+	SE(0x208) = 0;
+	SE(0x20C) = 0;
+	SE(0x210) = 0;
+	SE(SE_SHA_MSG_LEFT_REG_OFFSET) = (u32)(src_size << 3);
+	SE(0x218) = 0;
+   	SE(0x21C) = 0;
+	SE(0x220) = 0;
+
+	// Trigger the operation.
+	res = _se_execute(OP_START, NULL, 0, src, src_size);
+
+	// Copy output hash.
+	u32 *dst32 = (u32 *)dst;
+	for (u32 i = 0; i < 8; i++)
+		dst32[i] = byte_swap_32(SE(SE_HASH_RESULT_REG_OFFSET + (i << 2)));
+
+	return res;
+}

ipl/se.h

@@ -23,8 +23,9 @@ void se_rsa_acc_ctrl(u32 rs, u32 flags);
 void se_key_acc_ctrl(u32 ks, u32 flags);
 void se_aes_key_set(u32 ks, void *key, u32 size);
 void se_aes_key_clear(u32 ks);
-int se_aes_unwrap_key(u32 ks_dst, u32 ks_src, void *input);
-int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, void *src);
-int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, void *src, u32 src_size, void *ctr);
+int se_aes_unwrap_key(u32 ks_dst, u32 ks_src, const void *input);
+int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, const void *src);
+int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size, void *ctr);
+int se_calc_sha256(void *dst, const void *src, u32 src_size);
 
 #endif

ipl/t210.h

@@ -43,6 +43,7 @@
 #define APB_MISC_BASE 0x70000000
 #define PINMUX_AUX_BASE 0x70003000
 #define UART_BASE 0x70006000
+#define RTC_BASE 0x7000E000
 #define PMC_BASE 0x7000E400
 #define SYSCTR0_BASE 0x7000F000
 #define FUSE_BASE 0x7000F800
@@ -78,6 +79,7 @@
 #define EXCP_VEC(off) _REG(EXCP_VEC_BASE, off)
 #define APB_MISC(off) _REG(APB_MISC_BASE, off)
 #define PINMUX_AUX(off) _REG(PINMUX_AUX_BASE, off)
+#define RTC(off) _REG(RTC_BASE, off)
 #define PMC(off) _REG(PMC_BASE, off)
 #define SYSCTR0(off) _REG(SYSCTR0_BASE, off)
 #define FUSE(off) _REG(FUSE_BASE, off)

ipl/tsec.c

@@ -19,13 +19,14 @@
 #include "clock.h"
 #include "t210.h"
 #include "heap.h"
+#include "util.h"
 
 static int _tsec_dma_wait_idle()
 {
-	u32 timeout = TMR(0x10) + 10000000;
+	u32 timeout = get_tmr_ms() + 10000;
 
 	while (!(TSEC(0x1118) & 2))
-		if (TMR(0x10) > timeout)
+		if (get_tmr_ms() > timeout)
 			return 0;
 
 	return 1;
@@ -93,9 +94,9 @@ int tsec_query(u8 *dst, u32 rev, void *fw)
 		res = -3;
 		goto out_free;
 	}
-	u32 timeout = TMR(0x10) + 2000000;
+	u32 timeout = get_tmr_ms() + 2000;
 	while (!TSEC(0x1044))
-		if (TMR(0x10) > timeout)
+		if (get_tmr_ms() > timeout)
 		{
 			res = -4;
 			goto out_free;

ipl/tui.c

@@ -1,5 +1,6 @@
-/*{
+/*
 * Copyright (c) 2018 naehrwert
+* Copyright (c) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -16,23 +17,34 @@
 
 #include "tui.h"
 #include "btn.h"
+#include "max17050.h"
 
-void tui_pbar(gfx_con_t *con, int x, int y, u32 val)
+#ifdef MENU_LOGO_ENABLE
+extern u8 *Kc_MENU_LOGO;
+#define X_MENU_LOGO       119
+#define Y_MENU_LOGO        57
+#define X_POS_MENU_LOGO   577
+#define Y_POS_MENU_LOGO  1199
+#endif //MENU_LOGO_ENABLE
+
+void tui_pbar(gfx_con_t *con, int x, int y, u32 val, u32 fgcol, u32 bgcol)
 {
 	u32 cx, cy;
+	if (val > 200)
+		val = 200;
 
 	gfx_con_getpos(con, &cx, &cy);
 
 	gfx_con_setpos(con, x, y);
 
-	gfx_printf(con, "[%3d%%]", val);
+	gfx_printf(con, "%k[%3d%%]%k", fgcol, val, 0xFFCCCCCC);
 
-	x += 7 * 8;
+	x += 7 * con->fntsz;
 	
-	for (int i = 0; i < 6; i++)
+	for (int i = 0; i < (con->fntsz >> 3) * 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);
@@ -40,38 +52,94 @@ 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 idx = 0, prev_idx = 0, cnt = 0x7FFFFFFF;
+	u32 battPercent = 0;
+	int battVoltCurr = 0;
 
-	gfx_clear(con->gfx_ctxt, 0xFF000000);
+	gfx_clear_grey(con->gfx_ctxt, 0x1B);
+#ifdef MENU_LOGO_ENABLE
+	gfx_set_rect_rgb(con->gfx_ctxt, Kc_MENU_LOGO,
+		X_MENU_LOGO, Y_MENU_LOGO, X_POS_MENU_LOGO, Y_POS_MENU_LOGO);
+#endif //MENU_LOGO_ENABLE
 
 	while (1)
 	{
-		gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF000000);
+		gfx_con_setcol(con, 0xFFCCCCCC, 1, 0xFF1B1B1B);
 		gfx_con_setpos(con, menu->x, menu->y);
 		gfx_printf(con, "[%s]\n\n", menu->caption);
 
+		// Skip caption or seperator lines selection.
+		while (menu->ents[idx].type == MENT_CAPTION ||
+			menu->ents[idx].type == MENT_CHGLINE)
+		{
+			if (prev_idx <= idx || (!idx && prev_idx == cnt - 1))
+			{
+				idx++;
+				if (idx > (cnt - 1))
+				{
+					idx = 0;
+					prev_idx = 0;
+				}
+			}
+			else
+			{
+				idx--;
+				if (idx < 0)
+				{
+					idx = cnt - 1;
+					prev_idx = cnt;
+				}
+			}
+		}
+		prev_idx = idx;
+
+		// Draw the menu.
 		for (cnt = 0; menu->ents[cnt].type != MENT_END; cnt++)
 		{
 			if (cnt == idx)
-				gfx_con_setcol(con, 0xFF000000, 1, 0xFFCCCCCC);
+				gfx_con_setcol(con, 0xFF1B1B1B, 1, 0xFFCCCCCC);
 			else
-				gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF000000);
-			con->x += 8;
-			gfx_printf(con, "%s", menu->ents[cnt].caption);
+				gfx_con_setcol(con, 0xFFCCCCCC, 1, 0xFF1B1B1B);
+			if (menu->ents[cnt].type == MENT_CAPTION)
+				gfx_printf(con, "%k %s", menu->ents[cnt].color, menu->ents[cnt].caption);
+			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);
-			gfx_putc(con, '\n');
+				gfx_printf(con, "%k...", 0xFF0099EE);
+			gfx_printf(con, " \n");
 		}
-
-		gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF000000);
+		gfx_con_setcol(con, 0xFFCCCCCC, 1, 0xFF1B1B1B);
 		gfx_putc(con, '\n');
 
+		// Print help and battery status.
+		gfx_con_getpos(con, &con->savedx,  &con->savedy);
+		gfx_con_setpos(con, 0,  74 * 16);
+		gfx_printf(con, "%k VOL: Move up/down\n PWR: Select option\n\n", 0xFF555555);
+			
+		max17050_get_property(MAX17050_RepSOC, (int *)&battPercent);
+		gfx_printf(con, " %d.%d%%", (battPercent >> 8) & 0xFF, (battPercent & 0xFF) / 26);
+		max17050_get_property(MAX17050_VCELL, &battVoltCurr);
+		gfx_printf(con, " (%d mV)         ", battVoltCurr);
+		max17050_get_property(MAX17050_AvgCurrent, &battVoltCurr);
+		if (battVoltCurr >= 0)
+			gfx_printf(con, "\n %kCharging:%k %d mA         %k\n",
+				0xFF008000, 0xFF555555, battVoltCurr / 1000, 0xFFCCCCCC);
+		else
+			gfx_printf(con, "\n %kDischarging:%k -%d mA     %k\n",
+				0xFF800000, 0xFF555555, (~battVoltCurr) / 1000, 0xFFCCCCCC);
+		gfx_con_setpos(con, con->savedx,  con->savedy);
+
+		// Wait for user command.
 		u32 btn = btn_wait();
 
-		if (btn & BTN_VOL_DOWN && idx < cnt - 1)
+		if (btn & BTN_VOL_DOWN && idx < (cnt - 1))
 			idx++;
+		else if (btn & BTN_VOL_DOWN && idx == (cnt - 1))
+			idx = 0;
 		if (btn & BTN_VOL_UP && idx > 0)
 			idx--;
+		else if (btn & BTN_VOL_UP && idx == 0)
+			idx = cnt - 1;
 		if (btn & BTN_POWER)
 		{
 			ment_t *ent = &menu->ents[idx];
@@ -89,8 +157,15 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 			case MENT_BACK:
 				return NULL;
 				break;
+			default:
+				break;
 			}
-			gfx_clear(con->gfx_ctxt, 0xFF000000);
+			con->fntsz = 16;
+			gfx_clear_grey(con->gfx_ctxt, 0x1B);
+#ifdef MENU_LOGO_ENABLE
+			gfx_set_rect_rgb(con->gfx_ctxt, Kc_MENU_LOGO,
+				X_MENU_LOGO, Y_MENU_LOGO, X_POS_MENU_LOGO, Y_POS_MENU_LOGO);
+#endif //MENU_LOGO_ENABLE
 		}
 	}
 

ipl/tui.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -20,16 +21,19 @@
 #include "types.h"
 #include "gfx.h"
 
-#define MENT_END 0
+#define MENT_END     0
 #define MENT_HANDLER 1
-#define MENT_MENU 2
-#define MENT_CHOICE 3
-#define MENT_BACK 4
+#define MENT_MENU    2
+#define MENT_CHOICE  3
+#define MENT_BACK    4
+#define MENT_CAPTION 5
+#define MENT_CHGLINE 6
 
 typedef struct _ment_t
 {
 	u32 type;
 	const char *caption;
+	u32 color;
 	void *data;
 	union
 	{
@@ -47,12 +51,14 @@ typedef struct _menu_t
 } menu_t;
 
 #define MDEF_END() {MENT_END}
-#define MDEF_HANDLER(caption, _handler) { MENT_HANDLER, caption, NULL, { .handler = _handler } }
-#define MDEF_HANDLER_EX(caption, data, _handler) { MENT_HANDLER, caption, data, { .handler = _handler } }
-#define MDEF_MENU(caption, _menu) { MENT_MENU, caption, NULL, { .menu = _menu } }
+#define MDEF_HANDLER(caption, _handler) { MENT_HANDLER, caption, 0, NULL, { .handler = _handler } }
+#define MDEF_HANDLER_EX(caption, data, _handler) { MENT_HANDLER, caption, 0, data, { .handler = _handler } }
+#define MDEF_MENU(caption, _menu) { MENT_MENU, caption, 0, NULL, { .menu = _menu } }
 #define MDEF_BACK() { MENT_BACK, "Back" }
+#define MDEF_CAPTION(caption, color) { MENT_CAPTION, caption, color }
+#define MDEF_CHGLINE() {MENT_CHGLINE}
 
-void tui_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

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;

ipl/uart.c

@@ -36,7 +36,7 @@ void uart_init(u32 idx, u32 baud)
 	uart->UART_IER_DLAB = 0;
 	uart->UART_IIR_FCR = 7; //Enable and clear TX and RX FIFOs.
 	(void)uart->UART_LSR;
-	sleep(3 * ((baud + 999999) / baud));
+	usleep(3 * ((baud + 999999) / baud));
 	uart->UART_LCR = 3; //Set word length 8.
 	uart->UART_MCR = 0;
 	uart->UART_MSR = 0;

ipl/util.c

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -17,15 +18,34 @@
 #include "util.h"
 #include "t210.h"
 
-u32 get_tmr()
+u32 get_tmr_s()
 {
-	return TMR(0x10);
+	return RTC(0x8); //RTC_SECONDS
 }
 
-void sleep(u32 ticks)
+u32 get_tmr_ms()
+{
+	// The registers must be read with the following order:
+	// -> RTC_MILLI_SECONDS (0x10) -> RTC_SHADOW_SECONDS (0x8)
+	return (RTC(0x10) | (RTC(0xC)<< 10));
+}
+
+u32 get_tmr_us()
+{
+	return TMR(0x10); //TMRUS
+}
+
+void msleep(u32 milliseconds)
+{
+	u32 start = RTC(0x10) | (RTC(0xC)<< 10);
+	while (((RTC(0x10) | (RTC(0xC)<< 10)) - start) <= milliseconds)
+		;
+}
+
+void usleep(u32 microseconds)
 {
 	u32 start = TMR(0x10);
-	while (TMR(0x10) - start <= ticks)
+	while ((TMR(0x10) - start) <= microseconds)
 		;
 }
 
@@ -35,3 +55,44 @@ 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;
+}
+
+u32 memcmp32sparse(const u32 *buf1, const u32 *buf2, u32 len)
+{
+	u32 len32 = len / 4;
+
+	if (!(len32 % 32))
+	{
+		while (len32)
+		{
+			len32 -= 32;
+			if(buf1[len32] != buf2[len32])
+				return 1;
+		}
+	}
+	else
+	{
+		while (len32)
+		{
+			len32 -= 32;
+			if(buf1[len32] != buf2[len32])
+				return 1;
+			if (len32 < 32)
+				return 0;
+		}
+	}
+
+	return 0;
+}

ipl/util.h

@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2018 naehrwert
+* Copyright (C) 2018 CTCaer
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
@@ -19,14 +20,25 @@
 
 #include "types.h"
 
+#define byte_swap_32(num) ((num>>24)&0xff) | ((num<<8)&0xff0000) | \
+						((num>>8)&0xff00) | ((num<<24)&0xff000000) \
+
 typedef struct _cfg_op_t
 {
 	u32 off;
 	u32 val;
 } cfg_op_t;
 
-u32 get_tmr();
-void sleep(u32 ticks);
+u32 get_tmr_us();
+u32 get_tmr_ms();
+u32 get_tmr_s();
+void usleep(u32 ticks);
+void msleep(u32 milliseconds);
 void exec_cfg(u32 *base, const cfg_op_t *ops, u32 num_ops);
+u32 crc32c(const void *buf, u32 len);
+
+/* This is a faster implementation of memcmp that checks two u32 values */
+/* every 128 Bytes block. Intented only for Backup and Restore          */
+u32 memcmp32sparse(const u32 *buf1, const u32 *buf2, u32 len);
 
 #endif

res/hekate_ipl_template.ini

@@ -0,0 +1,40 @@
+[config]
+autoboot=5
+bootwait=3
+customlogo=1
+verification=2
+
+{-------- Stock -------}
+[Stock]
+
+[Stock (SVC perms)]
+fullsvcperm=1
+debugmode=1
+{ }
+
+{-- Custom Firmwares --}
+[AtmoHB]
+kip1=cfw/loader.kip1
+kip1=cfw/sm.kip1
+
+[AtmoHB (SVC Perms)]
+kip1=cfw/loader.kip1
+kip1=cfw/sm.kip1
+fullsvcperm=1
+
+[Atmosphere Full]
+secmon=cfw/atmo/exosphere.bin
+kip1=cfw/atmo/loader.kip
+kip1=cfw/atmo/sm.kip
+kip1=cfw/atmo/fs_mitm.kip
+logopath=cfw/atmo/atmo.bmp
+atmosphere=1
+# kip1=cfw/atmo/pm.kip
+{ }
+
+{------- Tools -------}
+[Clear Error Logs]
+kip1=cfw/nx-dreport.kip
+
+
+# hekate - CTCaer mod v3.0 .ini template