From f3312005064c2a4a898a4d83f435bd2b6bdb7414 Mon Sep 17 00:00:00 2001 From: d0k3 Date: Thu, 3 Mar 2016 13:27:51 +0100 Subject: [PATCH] Cleaned up & improved low-level disko functions Thanks Normatt for the NAND CID function! --- Makefile | 4 +- source/fatfs/diskio.c | 187 +++++---------------------------- source/fs.c | 2 +- source/{fatfs => nand}/aes.c | 0 source/{fatfs => nand}/aes.h | 0 source/nand/nand.c | 106 +++++++++++++++++++ source/nand/nand.h | 13 +++ source/{fatfs => nand}/sdmmc.c | 42 ++++++++ source/{fatfs => nand}/sdmmc.h | 2 + source/{fatfs => nand}/sha.c | 0 source/{fatfs => nand}/sha.h | 0 11 files changed, 194 insertions(+), 162 deletions(-) rename source/{fatfs => nand}/aes.c (100%) rename source/{fatfs => nand}/aes.h (100%) create mode 100644 source/nand/nand.c create mode 100644 source/nand/nand.h rename source/{fatfs => nand}/sdmmc.c (96%) rename source/{fatfs => nand}/sdmmc.h (99%) rename source/{fatfs => nand}/sha.c (100%) rename source/{fatfs => nand}/sha.h (100%) diff --git a/Makefile b/Makefile index 5496190..ec3a227 100644 --- a/Makefile +++ b/Makefile @@ -18,9 +18,9 @@ include $(DEVKITARM)/ds_rules #--------------------------------------------------------------------------------- export TARGET := GodMode9 BUILD := build -SOURCES := source source/fatfs source/decryptor source/abstraction +SOURCES := source source/fatfs source/nand source/abstraction DATA := data -INCLUDES := include source source/fatfs source/decryptor +INCLUDES := include source source/fatfs source/nand #--------------------------------------------------------------------------------- # options for code generation diff --git a/source/fatfs/diskio.c b/source/fatfs/diskio.c index e195eb1..7f5b999 100644 --- a/source/fatfs/diskio.c +++ b/source/fatfs/diskio.c @@ -8,21 +8,22 @@ /*-----------------------------------------------------------------------*/ #include "diskio.h" /* FatFs lower layer API */ -#include "aes.h" -#include "sha.h" #include "platform.h" +#include "nand.h" #include "sdmmc.h" #define TYPE_SDCARD 0 #define TYPE_SYSNAND 1 #define TYPE_EMUNAND 2 -#define SUBTYPE_CTRN_O 0 +#define SUBTYPE_CTRN 0 #define SUBTYPE_CTRN_N 1 #define SUBTYPE_TWLN 2 #define SUBTYPE_TWLP 3 #define SUBTYPE_NONE 4 +#define SUBTYPE(pd) ((mode_n3ds && (DriveInfo[pd].subtype == SUBTYPE_CTRN)) ? SUBTYPE_CTRN_N : DriveInfo[pd].subtype) + typedef struct { BYTE type; BYTE subtype; @@ -31,120 +32,27 @@ typedef struct { typedef struct { DWORD offset; DWORD size; - DWORD mode; BYTE keyslot; } SubtypeDesc; -FATpartition DriveInfo[13] = { - { TYPE_SDCARD, SUBTYPE_NONE }, // 0 - SDCARD - { TYPE_SYSNAND, SUBTYPE_CTRN_O }, // 1 - SYSNAND O3DS CTRNAND - { TYPE_SYSNAND, SUBTYPE_TWLN }, // 2 - SYSNAND O3DS TWLN - { TYPE_SYSNAND, SUBTYPE_TWLP }, // 3 - SYSNAND O3DS TWLP - { TYPE_EMUNAND, SUBTYPE_CTRN_O }, // 4 - EMUNAND O3DS CTRNAND - { TYPE_EMUNAND, SUBTYPE_TWLN }, // 5 - EMUNAND O3DS TWLN - { TYPE_EMUNAND, SUBTYPE_TWLP }, // 6 - EMUNAND O3DS TWLP - { TYPE_SYSNAND, SUBTYPE_CTRN_N }, // *1 - SYSNAND N3DS CTRNAND - { TYPE_SYSNAND, SUBTYPE_TWLN }, // *2 - SYSNAND N3DS TWLN - { TYPE_SYSNAND, SUBTYPE_TWLP }, // *3 - SYSNAND N3DS TWLP - { TYPE_EMUNAND, SUBTYPE_CTRN_N }, // *4 - EMUNAND N3DS CTRNAND - { TYPE_EMUNAND, SUBTYPE_TWLN }, // *5 - EMUNAND N3DS TWLN - { TYPE_EMUNAND, SUBTYPE_TWLP }, // *6 - EMUNAND N3DS TWLP +FATpartition DriveInfo[7] = { + { TYPE_SDCARD, SUBTYPE_NONE }, // 0 - SDCARD + { TYPE_SYSNAND, SUBTYPE_CTRN }, // 1 - SYSNAND CTRNAND + { TYPE_SYSNAND, SUBTYPE_TWLN }, // 2 - SYSNAND TWLN + { TYPE_SYSNAND, SUBTYPE_TWLP }, // 3 - SYSNAND TWLP + { TYPE_EMUNAND, SUBTYPE_CTRN }, // 4 - EMUNAND CTRNAND + { TYPE_EMUNAND, SUBTYPE_TWLN }, // 5 - EMUNAND TWLN + { TYPE_EMUNAND, SUBTYPE_TWLP }, // 6 - EMUNAND TWLP }; SubtypeDesc SubTypes[4] = { - { 0x05CAE5, 0x179F1B, AES_CNT_CTRNAND_MODE, 0x4 }, // O3DS CTRNAND - { 0x05CAD7, 0x20E969, AES_CNT_CTRNAND_MODE, 0x5 }, // N3DS CTRNAND - { 0x000097, 0x047DA9, AES_CNT_TWLNAND_MODE, 0x3 }, // TWLN - { 0x04808D, 0x0105B3, AES_CNT_TWLNAND_MODE, 0x3 } // TWLP + { 0x05CAE5, 0x179F1B, 0x4 }, // O3DS CTRNAND + { 0x05CAD7, 0x20E969, 0x5 }, // N3DS CTRNAND + { 0x000097, 0x047DA9, 0x3 }, // TWLN + { 0x04808D, 0x0105B3, 0x3 } // TWLP }; static bool mode_n3ds = false; -static u32 emunand_base_sector = 0x000000; - - -/*-----------------------------------------------------------------------*/ -/* Get counter for NAND AES decryption */ -/*-----------------------------------------------------------------------*/ - -/*u32 GetNandCtr(u8* ctr, u32 sector) -{ - static const u8* version_ctrs[] = { - (u8*)0x080D7CAC, - (u8*)0x080D858C, - (u8*)0x080D748C, - (u8*)0x080D740C, - (u8*)0x080D74CC, - (u8*)0x080D794C - }; - static const u32 version_ctrs_len = sizeof(version_ctrs) / sizeof(u32); - static u8* ctr_start = NULL; - - if (ctr_start == NULL) { - for (u32 i = 0; i < version_ctrs_len; i++) { - if (*(u32*)version_ctrs[i] == 0x5C980) { - ctr_start = (u8*) version_ctrs[i] + 0x30; - } - } - - // If value not in previous list start memory scanning (test range) - if (ctr_start == NULL) { - for (u8* c = (u8*) 0x080D8FFF; c > (u8*) 0x08000000; c--) { - if (*(u32*)c == 0x5C980 && *(u32*)(c + 1) == 0x800005C9) { - ctr_start = c + 0x30; - break; - } - } - } - - if (ctr_start == NULL) { - return 1; - } - } - - // the ctr is stored backwards in memory - if (sector >= (0x0B100000 / 0x200)) { // CTRNAND/AGBSAVE region - for (u32 i = 0; i < 16; i++) - ctr[i] = *(ctr_start + (0xF - i)); - } else { // TWL region - for (u32 i = 0; i < 16; i++) - ctr[i] = *(ctr_start + 0x88 + (0xF - i)); - } - - // increment counter - add_ctr(ctr, sector * (0x200/0x10)); - - return 0; -}*/ - -u32 GetNandCtr(u8* ctr, u32 sector) -{ - static u8* NandCid = NULL; - static u8 CtrNandCtr[16]; - static u8 TwlNandCtr[16]; - - if (!NandCid) { - NandCid = (u8*) 0x01FFCD84; - u8 shasum[32]; - sha_init(SHA256_MODE); - sha_update(NandCid, 16); - sha_get(shasum); - memcpy(CtrNandCtr, shasum, 16); - sha_init(SHA1_MODE); - sha_update(NandCid, 16); - sha_get(shasum); - for(u32 i = 0; i < 16; i++) // little endian and reversed order - TwlNandCtr[i] = shasum[15-i]; - } - - // copy NAND CTR over - memcpy(ctr, (sector >= (0x0B100000 / 0x200)) ? CtrNandCtr : TwlNandCtr, 16); - - // increment counter - add_ctr(ctr, sector * (0x200/0x10)); - - return 0; -} - /*-----------------------------------------------------------------------*/ @@ -171,7 +79,8 @@ DSTATUS disk_initialize ( ) { mode_n3ds = (GetUnitPlatform() == PLATFORM_N3DS); - sdmmc_sdcard_init(); + sdmmc_sdcard_init(); // multiple inits should not be required (also, below) + InitNandCrypto(); return RES_OK; } @@ -189,9 +98,6 @@ DRESULT disk_read ( UINT count /* Number of sectors to read */ ) { - if ((pdrv > 0) && mode_n3ds) - pdrv += 6; - BYTE type = DriveInfo[pdrv].type; if (type == TYPE_SDCARD) { @@ -199,28 +105,12 @@ DRESULT disk_read ( return RES_PARERR; } } else { - BYTE subtype = DriveInfo[pdrv].subtype; + BYTE subtype = SUBTYPE(pdrv); + BYTE keyslot = SubTypes[subtype].keyslot; DWORD isector = SubTypes[subtype].offset + sector; - DWORD mode = SubTypes[subtype].mode; - BYTE ctr[16] __attribute__((aligned(32))); - if (type == TYPE_SYSNAND) { - if (sdmmc_nand_readsectors(isector, count, buff)) - return RES_PARERR; - } else if (sdmmc_sdcard_readsectors(emunand_base_sector + isector, count, buff)) { + if (ReadNandSectors(buff, isector, count, keyslot, type == TYPE_EMUNAND)) return RES_PARERR; - } - - if (GetNandCtr(ctr, isector) != 0) - return RES_PARERR; - use_aeskey(SubTypes[subtype].keyslot); - for (UINT s = 0; s < count; s++) { - for (UINT b = 0x0; b < 0x200; b += 0x10, buff += 0x10) { - set_ctr(ctr); - aes_decrypt((void*) buff, (void*) buff, 1, mode); - add_ctr(ctr, 0x1); - } - } } return RES_OK; @@ -241,37 +131,19 @@ DRESULT disk_write ( UINT count /* Number of sectors to write */ ) { - if ((pdrv > 0) && mode_n3ds) - pdrv += 6; + BYTE type = DriveInfo[pdrv].type; - if (DriveInfo[pdrv].type == TYPE_SDCARD) { + if (type == TYPE_SDCARD) { if (sdmmc_sdcard_writesectors(sector, count, (BYTE *)buff)) { return RES_PARERR; } } else { - BYTE subtype = DriveInfo[pdrv].subtype; + /*BYTE subtype = SUBTYPE(pdrv); + BYTE keyslot = SubTypes[subtype].keyslot; DWORD isector = SubTypes[subtype].offset + sector; - DWORD mode = SubTypes[subtype].mode; - BYTE ctr[16] __attribute__((aligned(32))); - if (GetNandCtr(ctr, isector) != 0) - return RES_PARERR; - use_aeskey(SubTypes[subtype].keyslot); - for (UINT s = 0; s < count; s++) { - for (UINT b = 0x0; b < 0x200; b += 0x10, buff += 0x10) { - set_ctr(ctr); - aes_decrypt((void*) buff, (void*) buff, 1, mode); - add_ctr(ctr, 0x1); - } - } - - if (type == TYPE_SYSNAND) { - if (sdmmc_nand_writesectors(isector, count, buff)) - return RES_PARERR; - } else if (sdmmc_sdcard_writesectors(emunand_base_sector + isector, count, buff)) { - return RES_PARERR; - } - // stubbed, better be safe! + if (WriteNandSectors(buff, isector, count, keyslot, type == TYPE_EMUNAND)) + return RES_PARERR;*/ // stubbed! } return RES_OK; @@ -294,9 +166,6 @@ DRESULT disk_ioctl ( void *buff /* Buffer to send/receive control data */ ) { - if ((pdrv > 0) && mode_n3ds) - pdrv += 6; - switch (cmd) { case GET_SECTOR_SIZE: *((DWORD*) buff) = 0x200; @@ -305,7 +174,7 @@ DRESULT disk_ioctl ( if (DriveInfo[pdrv].type == TYPE_SDCARD) { *((DWORD*) buff) = getMMCDevice(1)->total_size; } else { - *((DWORD*) buff) = SubTypes[DriveInfo[pdrv].subtype].size; + *((DWORD*) buff) = SubTypes[SUBTYPE(pdrv)].size; } return RES_OK; case GET_BLOCK_SIZE: diff --git a/source/fs.c b/source/fs.c index 87d51b9..69104fe 100644 --- a/source/fs.c +++ b/source/fs.c @@ -6,7 +6,7 @@ static FATFS* fs = (FATFS*)0x20316000; // this is the main buffer -static u8* main_buffer = (u8*)0x21100000; +static u8* main_buffer = (u8*)0x21200000; // this is the main buffer size static size_t main_buffer_size = 1 * 1024 * 1024; diff --git a/source/fatfs/aes.c b/source/nand/aes.c similarity index 100% rename from source/fatfs/aes.c rename to source/nand/aes.c diff --git a/source/fatfs/aes.h b/source/nand/aes.h similarity index 100% rename from source/fatfs/aes.h rename to source/nand/aes.h diff --git a/source/nand/nand.c b/source/nand/nand.c new file mode 100644 index 0000000..b4ddb10 --- /dev/null +++ b/source/nand/nand.c @@ -0,0 +1,106 @@ +#include "fs.h" +#include "draw.h" +#include "hid.h" +#include "platform.h" +#include "aes.h" +#include "sha.h" +#include "sdmmc.h" +#include "nand.h" + +#define NAND_BUFFER ((u8*)0x21100000) +#define NAND_BUFFER_SIZE (0x100000) // must be multiple of 0x200 + +static u8 CtrNandCtr[16]; +static u8 TwlNandCtr[16]; + +static u32 emunand_base_sector = 0x000000; + + +bool InitNandCrypto(void) +{ + // STEP #1: Get NAND CID, set up TWL/CTR counter + u8 NandCid[16]; + u8 shasum[32]; + + sdmmc_get_cid( 1, (uint32_t*) NandCid); + sha_init(SHA256_MODE); + sha_update(NandCid, 16); + sha_get(shasum); + memcpy(CtrNandCtr, shasum, 16); + sha_init(SHA1_MODE); + sha_update(NandCid, 16); + sha_get(shasum); + for(u32 i = 0; i < 16; i++) // little endian and reversed order + TwlNandCtr[i] = shasum[15-i]; + + // STEP #2: Calculate slot 0x3 key, set it up to slot 0x11 + + + return true; +} + +void CryptNand(u8* buffer, u32 sector, u32 count, u32 keyslot) +{ + u32 mode = (sector >= (0x0B100000 / 0x200)) ? AES_CNT_CTRNAND_MODE : AES_CNT_TWLNAND_MODE; + u8 ctr[16] __attribute__((aligned(32))); + + // copy NAND CTR and increment it + memcpy(ctr, (sector >= (0x0B100000 / 0x200)) ? CtrNandCtr : TwlNandCtr, 16); + add_ctr(ctr, sector * (0x200/0x10)); + + // decrypt the data + use_aeskey(keyslot); + for (u32 s = 0; s < count; s++) { + for (u32 b = 0x0; b < 0x200; b += 0x10, buffer += 0x10) { + set_ctr(ctr); + aes_decrypt((void*) buffer, (void*) buffer, 1, mode); + add_ctr(ctr, 0x1); + } + } +} + +int ReadNandSectors(u8* buffer, u32 sector, u32 count, u32 keyslot, bool read_emunand) +{ + if (read_emunand) { + int errorcode = sdmmc_sdcard_readsectors(emunand_base_sector + sector, count, buffer); + if (errorcode) return errorcode; + } else { + int errorcode = sdmmc_nand_readsectors(sector, count, buffer); + if (errorcode) return errorcode; + } + CryptNand(buffer, sector, count, keyslot); + + return 0; +} + +int WriteNandSectors(const u8* buffer, u32 sector, u32 count, u32 keyslot, bool write_emunand) +{ + // buffer must not be changed, so this is a little complicated + for (u32 s = 0; s < count; s += (NAND_BUFFER_SIZE / 0x200)) { + u32 pcount = min((NAND_BUFFER_SIZE/0x200), (count - s)); + memcpy(NAND_BUFFER, buffer + (s*0x200), pcount * 0x200); + CryptNand(NAND_BUFFER, sector + s, pcount, keyslot); + if (write_emunand) { + int errorcode = sdmmc_sdcard_writesectors(emunand_base_sector + sector + s, pcount, NAND_BUFFER); + if (errorcode) return errorcode; + } else { + int errorcode = sdmmc_nand_writesectors(sector + s, pcount, NAND_BUFFER); + if (errorcode) return errorcode; + } + } + + return 0; +} + +u32 GetEmuNandBase(void) +{ + return emunand_base_sector; +} + +u32 SwitchEmuNandBase(int start_sector) +{ + // switching code goes here + return emunand_base_sector; +} + + diff --git a/source/nand/nand.h b/source/nand/nand.h new file mode 100644 index 0000000..b315792 --- /dev/null +++ b/source/nand/nand.h @@ -0,0 +1,13 @@ +#pragma once + +#include "common.h" + +bool InitNandCrypto(void); +void CryptNand(u8* buffer, u32 sector, u32 count, u32 keyslot); + +int ReadNandSectors(u8* buffer, u32 sector, u32 count, u32 keyslot, bool read_emunand); +int WriteNandSectors(const u8* buffer, u32 sector, u32 count, u32 keyslot, bool write_emunand); + +u32 GetEmuNandBase(void); +u32 SwitchEmuNandBase(int start_sector); + diff --git a/source/fatfs/sdmmc.c b/source/nand/sdmmc.c similarity index 96% rename from source/fatfs/sdmmc.c rename to source/nand/sdmmc.c index 97e9940..9d69965 100644 --- a/source/fatfs/sdmmc.c +++ b/source/nand/sdmmc.c @@ -618,3 +618,45 @@ void sdmmc_sdcard_init() SD_Init(); DEBUGPRINT(topScreen, "sd_res ", sd_res, 10, 20 + 4*8, RGB(40, 40, 40), RGB(208, 208, 208)); } + +int sdmmc_get_cid( int isNand, uint32_t *info) +{ + struct mmcdevice *device; + if(isNand) + device = &handelNAND; + else + device = &handelSD; + + inittarget(device); + // use cmd7 to put sd card in standby mode + // CMD7 + { + sdmmc_send_command(device,0x10507,0); + //if((device->error & 0x4)) return -1; + } + + // get sd card info + // use cmd10 to read CID + { + sdmmc_send_command(device,0x1060A,device->initarg << 0x10); + //if((device->error & 0x4)) return -2; + + for( int i = 0; i < 4; ++i ) { + info[i] = device->ret[i]; + } + } + + // put sd card back to transfer mode + // CMD7 + { + sdmmc_send_command(device,0x10507,device->initarg << 0x10); + //if((device->error & 0x4)) return -3; + } + + if(isNand) + { + inittarget(&handelSD); + } + + return 0; +} diff --git a/source/fatfs/sdmmc.h b/source/nand/sdmmc.h similarity index 99% rename from source/fatfs/sdmmc.h rename to source/nand/sdmmc.h index ceafb41..ee612b5 100644 --- a/source/fatfs/sdmmc.h +++ b/source/nand/sdmmc.h @@ -130,6 +130,8 @@ extern "C" { int sdmmc_nand_readsectors(uint32_t sector_no, uint32_t numsectors, uint8_t *out); int sdmmc_nand_writesectors(uint32_t sector_no, uint32_t numsectors, uint8_t *in); + + int sdmmc_get_cid( int isNand, uint32_t *info); mmcdevice *getMMCDevice(int drive); diff --git a/source/fatfs/sha.c b/source/nand/sha.c similarity index 100% rename from source/fatfs/sha.c rename to source/nand/sha.c diff --git a/source/fatfs/sha.h b/source/nand/sha.h similarity index 100% rename from source/fatfs/sha.h rename to source/nand/sha.h