holding the home button now tries to load a payload from a different …

…location and misc cleanup

payload_stage1/Makefile

@@ -20,7 +20,7 @@ TARGET 		:=  $(notdir $(CURDIR))
 BUILD		:=	build
 SOURCES		:=	source
 DATA		:=	data
-INCLUDES	:=	include source source/fatfs
+INCLUDES	:=	source 
 
 #---------------------------------------------------------------------------------
 # Setup some defines
@@ -32,11 +32,11 @@ INCLUDES	:=	include source source/fatfs
 ARCH	:=	-mthumb -mthumb-interwork
 
 CFLAGS	:=	-g -Wall -O2\
-			-march=armv5te -mtune=arm946e-s -fomit-frame-pointer\
+			-flto -march=armv5te -mtune=arm946e-s -fomit-frame-pointer\
 			-ffast-math -std=c99\
 			$(ARCH)
 
-CFLAGS	+=	$(INCLUDE)
+CFLAGS	+=	$(INCLUDE) -pedantic -Wall -Wextra -Wcast-align -Wcast-qual -Wdisabled-optimization -Wformat=2 -Winit-self -Wlogical-op -Wmissing-declarations -Wmissing-include-dirs -Wredundant-decls -Wshadow -Wsign-conversion -Wstrict-overflow=5 -Wswitch-default -Wundef -Wno-unused
 
 CXXFLAGS	:= $(CFLAGS) -fno-rtti -fno-exceptions
 

payload_stage1/source/i2c.c

@@ -0,0 +1,98 @@
+#include "i2c.h"
+
+//-----------------------------------------------------------------------------
+
+static const struct { u8 bus_id, reg_addr; } dev_data[] = {
+    {0, 0x4A}, {0, 0x7A}, {0, 0x78},
+    {1, 0x4A}, {1, 0x78}, {1, 0x2C},
+    {1, 0x2E}, {1, 0x40}, {1, 0x44},
+    {2, 0xD6}, {2, 0xD0}, {2, 0xD2},
+    {2, 0xA4}, {2, 0x9A}, {2, 0xA0},
+};
+
+static inline u8 i2cGetDeviceBusId(u8 device_id) {
+    return dev_data[device_id].bus_id;
+}
+
+static inline u8 i2cGetDeviceRegAddr(u8 device_id) {
+    return dev_data[device_id].reg_addr;
+}
+
+//-----------------------------------------------------------------------------
+
+static vu8* reg_data_addrs[] = {
+    (vu8*)(I2C1_REG_OFF + I2C_REG_DATA),
+    (vu8*)(I2C2_REG_OFF + I2C_REG_DATA),
+    (vu8*)(I2C3_REG_OFF + I2C_REG_DATA),
+};
+
+static inline vu8* i2cGetDataReg(u8 bus_id) {
+    return reg_data_addrs[bus_id];
+}
+
+//-----------------------------------------------------------------------------
+
+static vu8* reg_cnt_addrs[] = {
+    (vu8*)(I2C1_REG_OFF + I2C_REG_CNT),
+    (vu8*)(I2C2_REG_OFF + I2C_REG_CNT),
+    (vu8*)(I2C3_REG_OFF + I2C_REG_CNT),
+};
+
+static inline vu8* i2cGetCntReg(u8 bus_id) {
+    return reg_cnt_addrs[bus_id];
+}
+
+//-----------------------------------------------------------------------------
+
+static inline void i2cWaitBusy(u8 bus_id) {
+    while (*i2cGetCntReg(bus_id) & 0x80);
+}
+
+static inline bool i2cGetResult(u8 bus_id) {
+    i2cWaitBusy(bus_id);
+    return (*i2cGetCntReg(bus_id) >> 4) & 1;
+}
+
+static void i2cStop(u8 bus_id, u8 arg0) {
+    *i2cGetCntReg(bus_id) = (arg0 << 5) | 0xC0;
+    i2cWaitBusy(bus_id);
+    *i2cGetCntReg(bus_id) = 0xC5;
+}
+
+//-----------------------------------------------------------------------------
+
+static bool i2cSelectDevice(u8 bus_id, u8 dev_reg) {
+    i2cWaitBusy(bus_id);
+    *i2cGetDataReg(bus_id) = dev_reg;
+    *i2cGetCntReg(bus_id) = 0xC2;
+    return i2cGetResult(bus_id);
+}
+
+static bool i2cSelectRegister(u8 bus_id, u8 reg) {
+    i2cWaitBusy(bus_id);
+    *i2cGetDataReg(bus_id) = reg;
+    *i2cGetCntReg(bus_id) = 0xC0;
+    return i2cGetResult(bus_id);
+}
+
+//-----------------------------------------------------------------------------
+
+u8 i2cReadRegister(u8 dev_id, u8 reg) {
+    u8 bus_id = i2cGetDeviceBusId(dev_id);
+    u8 dev_addr = i2cGetDeviceRegAddr(dev_id);
+
+    for (size_t i = 0; i < 8; i++) {
+        if (i2cSelectDevice(bus_id, dev_addr) && i2cSelectRegister(bus_id, reg)) {
+            if (i2cSelectDevice(bus_id, dev_addr | 1)) {
+                i2cWaitBusy(bus_id);
+                i2cStop(bus_id, 1);
+                i2cWaitBusy(bus_id);
+                return *i2cGetDataReg(bus_id);
+            }
+        }
+        *i2cGetCntReg(bus_id) = 0xC5;
+        i2cWaitBusy(bus_id);
+    }
+    return 0xff;
+}
+

payload_stage1/source/i2c.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "common.h"
+
+#define I2C1_REG_OFF 0x10161000
+#define I2C2_REG_OFF 0x10144000
+#define I2C3_REG_OFF 0x10148000
+
+#define I2C_REG_DATA  0
+#define I2C_REG_CNT   1
+#define I2C_REG_CNTEX 2
+#define I2C_REG_SCL   4
+
+#define I2C_DEV_MCU  3
+#define I2C_DEV_GYRO 10
+#define I2C_DEV_IR   13
+
+u8 i2cReadRegister(u8 dev_id, u8 reg);
+

payload_stage1/source/main.c

@@ -1,15 +1,27 @@
 #include "common.h"
 #include "sdmmc.h"
+#include "i2c.h"
 
 int main()
 {
     // Initialize sdcard and nand
     *(vu32*)0x10000020 = 0;
     *(vu32*)0x10000020 = 0x340;
     sdmmc_sdcard_init();
-    sdmmc_nand_readsectors(0x5C000, 0x20, (vu8*)0x08006000);
+
+    if(i2cReadRegister(I2C_DEV_MCU, 0x10) & 0x4) //if home button is pressed
+    {
+        //Jump to alternate stage2
+        sdmmc_nand_readsectors(0x5A000, 0x20, (u8*)0x08006000);
+    }
+    else
+    {
+        //jump to normal stage2
+        sdmmc_nand_readsectors(0x5C000, 0x20, (u8*)0x08006000);
+    }
 
     // Jump to secondary payload
     ((void (*)())0x08006000)();
     return 0;
 }
+