Add v1.5 color changes and logo

Tui tweak:
Support auto scroll.

Makefile

@@ -49,14 +49,14 @@ LDFLAGS = $(ARCH) -nostartfiles -lgcc -Wl,--nmagic,--gc-sections
 
 .PHONY: all clean
 
-all: $(BUILD)/$(TARGET)
+all: $(TARGET).bin
 
 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 +66,5 @@ $(BUILD)/%.o: $(SOURCEDIR)/%.c
 	$(CC) $(CFLAGS) -c $< -o $@
 
 $(BUILD)/%.o: $(SOURCEDIR)/%.S
+	@mkdir -p "$(BUILD)"
 	$(CC) $(CFLAGS) -c $< -o $@

ipl/ff.c

@@ -3,6 +3,7 @@
 /-----------------------------------------------------------------------------/
 /
 / Copyright (C) 2017, ChaN, all right reserved.
+/ Copyright (c) 2018 naehrwert
 /
 / FatFs module is an open source software. Redistribution and use of FatFs in
 / source and binary forms, with or without modification, are permitted provided
@@ -22,6 +23,11 @@
 #include "ff.h"			/* Declarations of FatFs API */
 #include "diskio.h"		/* Declarations of device I/O functions */
 
+#include "gfx.h"
+extern gfx_ctxt_t gfx_ctxt;
+extern gfx_con_t gfx_con;
+#define EFSPRINTF(text, ...) gfx_printf(&gfx_con, "\n\n%k[FatFS] "text"%k\n", 0xFF00FFFF, 0xFFFFFFFF)
+//#define EFSPRINTF(...)
 
 /*--------------------------------------------------------------------------
 
@@ -3253,6 +3259,7 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		stat = disk_status(fs->pdrv);
 		if (!(stat & STA_NOINIT)) {		/* and the physical drive is kept initialized */
 			if (!FF_FS_READONLY && mode && (stat & STA_PROTECT)) {	/* Check write protection if needed */
+				EFSPRINTF("Error: Write protected!");
 				return FR_WRITE_PROTECTED;
 			}
 			return FR_OK;				/* The filesystem object is valid */
@@ -3266,9 +3273,11 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 	fs->pdrv = LD2PD(vol);				/* Bind the logical drive and a physical drive */
 	stat = disk_initialize(fs->pdrv);	/* Initialize the physical drive */
 	if (stat & STA_NOINIT) { 			/* Check if the initialization succeeded */
+		EFSPRINTF("Error: Medium not ready or hard error!");
 		return FR_NOT_READY;			/* Failed to initialize due to no medium or hard error */
 	}
 	if (!FF_FS_READONLY && mode && (stat & STA_PROTECT)) { /* Check disk write protection if needed */
+		EFSPRINTF("Error: Write protected!");
 		return FR_WRITE_PROTECTED;
 	}
 #if FF_MAX_SS != FF_MIN_SS				/* Get sector size (multiple sector size cfg only) */
@@ -3291,8 +3300,14 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 			fmt = bsect ? check_fs(fs, bsect) : 3;	/* Check the partition */
 		} while (LD2PT(vol) == 0 && fmt >= 2 && ++i < 4);
 	}
-	if (fmt == 4) return FR_DISK_ERR;		/* An error occured in the disk I/O layer */
-	if (fmt >= 2) return FR_NO_FILESYSTEM;	/* No FAT volume is found */
+	if (fmt == 4) {
+		EFSPRINTF("Error: Disk I/O error - Could not load boot record!");
+		return FR_DISK_ERR;		/* An error occured in the disk I/O layer */
+	}
+	if (fmt >= 2) {
+		EFSPRINTF("Error: No FAT/FAT32/exFAT filesystem found!");
+		return FR_NO_FILESYSTEM;	/* No FAT volume is found */
+	}
 
 	/* An FAT volume is found (bsect). Following code initializes the filesystem object */
 
@@ -3303,36 +3318,58 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		for (i = BPB_ZeroedEx; i < BPB_ZeroedEx + 53 && fs->win[i] == 0; i++) ;	/* Check zero filler */
 		if (i < BPB_ZeroedEx + 53) return FR_NO_FILESYSTEM;
 
-		if (ld_word(fs->win + BPB_FSVerEx) != 0x100) return FR_NO_FILESYSTEM;	/* Check exFAT version (must be version 1.0) */
+		if (ld_word(fs->win + BPB_FSVerEx) != 0x100) {
+			EFSPRINTF("Error: exFAT - Version check failed!");
+			return FR_NO_FILESYSTEM;	/* Check exFAT version (must be version 1.0) */
+		}
 
 		if (1 << fs->win[BPB_BytsPerSecEx] != SS(fs)) {	/* (BPB_BytsPerSecEx must be equal to the physical sector size) */
+			EFSPRINTF("Error: exFAT - Bytes per sector does not match physical sector size!");
 			return FR_NO_FILESYSTEM;
 		}
 
 		maxlba = ld_qword(fs->win + BPB_TotSecEx) + bsect;	/* Last LBA + 1 of the volume */
-		if (maxlba >= 0x100000000) return FR_NO_FILESYSTEM;	/* (It cannot be handled in 32-bit LBA) */
+		if (maxlba >= 0x100000000) {
+			EFSPRINTF("Error: exFAT - Cannot handle volume LBA with 32-bit LBA!");
+			return FR_NO_FILESYSTEM;	/* (It cannot be handled in 32-bit LBA) */
+		}
 
 		fs->fsize = ld_dword(fs->win + BPB_FatSzEx);	/* Number of sectors per FAT */
 
 		fs->n_fats = fs->win[BPB_NumFATsEx];			/* Number of FATs */
-		if (fs->n_fats != 1) return FR_NO_FILESYSTEM;	/* (Supports only 1 FAT) */
+		if (fs->n_fats != 1) {
+			EFSPRINTF("Error: exFAT - Multiple or no file allocation tables found!");
+			return FR_NO_FILESYSTEM;	/* (Supports only 1 FAT) */
+		}
 
 		fs->csize = 1 << fs->win[BPB_SecPerClusEx];		/* Cluster size */
-		if (fs->csize == 0)	return FR_NO_FILESYSTEM;	/* (Must be 1..32768) */
+		if (fs->csize == 0)	{
+			EFSPRINTF("Error: exFAT - Cluster size is not between 1KB - 32KB!");
+			return FR_NO_FILESYSTEM;	/* (Must be 1..32768) */
+		}
 
 		nclst = ld_dword(fs->win + BPB_NumClusEx);		/* Number of clusters */
-		if (nclst > MAX_EXFAT) return FR_NO_FILESYSTEM;	/* (Too many clusters) */
+		if (nclst > MAX_EXFAT) {
+			EFSPRINTF("Error: exFAT - Total clusters exceed allowed!");
+			return FR_NO_FILESYSTEM;	/* (Too many clusters) */
+		}
 		fs->n_fatent = nclst + 2;
 
 		/* Boundaries and Limits */
 		fs->volbase = bsect;
 		fs->database = bsect + ld_dword(fs->win + BPB_DataOfsEx);
 		fs->fatbase = bsect + ld_dword(fs->win + BPB_FatOfsEx);
-		if (maxlba < (QWORD)fs->database + nclst * fs->csize) return FR_NO_FILESYSTEM;	/* (Volume size must not be smaller than the size requiered) */
+		if (maxlba < (QWORD)fs->database + nclst * fs->csize) {
+			EFSPRINTF("Error: exFAT - Volume size is lower than required!");
+			return FR_NO_FILESYSTEM;	/* (Volume size must not be smaller than the size required) */
+		}
 		fs->dirbase = ld_dword(fs->win + BPB_RootClusEx);
 
 		/* Check if bitmap location is in assumption (at the first cluster) */
-		if (move_window(fs, clst2sect(fs, fs->dirbase)) != FR_OK) return FR_DISK_ERR;
+		if (move_window(fs, clst2sect(fs, fs->dirbase)) != FR_OK) {
+			EFSPRINTF("Error: exFAT - Bitmap location not at first cluster!");
+			return FR_DISK_ERR;
+		}
 		for (i = 0; i < SS(fs); i += SZDIRE) {
 			if (fs->win[i] == 0x81 && ld_dword(fs->win + i + 20) == 2) break;	/* 81 entry with cluster #2? */
 		}
@@ -3344,38 +3381,62 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 	} else
 #endif	/* FF_FS_EXFAT */
 	{
-		if (ld_word(fs->win + BPB_BytsPerSec) != SS(fs)) return FR_NO_FILESYSTEM;	/* (BPB_BytsPerSec must be equal to the physical sector size) */
+		if (ld_word(fs->win + BPB_BytsPerSec) != SS(fs)) {
+			EFSPRINTF("Error: FAT - Bytes per sector does not match physical sector size!");
+			return FR_NO_FILESYSTEM;	/* (BPB_BytsPerSec must be equal to the physical sector size) */
+		}
 
 		fasize = ld_word(fs->win + BPB_FATSz16);		/* Number of sectors per FAT */
 		if (fasize == 0) fasize = ld_dword(fs->win + BPB_FATSz32);
 		fs->fsize = fasize;
 
 		fs->n_fats = fs->win[BPB_NumFATs];				/* Number of FATs */
-		if (fs->n_fats != 1 && fs->n_fats != 2) return FR_NO_FILESYSTEM;	/* (Must be 1 or 2) */
+		if (fs->n_fats != 1 && fs->n_fats != 2) {
+			EFSPRINTF("Error: FAT - No or more than 2 file allocation tables found!");
+			return FR_NO_FILESYSTEM;	/* (Must be 1 or 2) */
+		}
 		fasize *= fs->n_fats;							/* Number of sectors for FAT area */
 
 		fs->csize = fs->win[BPB_SecPerClus];			/* Cluster size */
-		if (fs->csize == 0 || (fs->csize & (fs->csize - 1))) return FR_NO_FILESYSTEM;	/* (Must be power of 2) */
+		if (fs->csize == 0 || (fs->csize & (fs->csize - 1))) {
+			EFSPRINTF("Error: FAT - Cluster size is not a power of 2!");
+			return FR_NO_FILESYSTEM;	/* (Must be power of 2) */
+		}
 
 		fs->n_rootdir = ld_word(fs->win + BPB_RootEntCnt);	/* Number of root directory entries */
-		if (fs->n_rootdir % (SS(fs) / SZDIRE)) return FR_NO_FILESYSTEM;	/* (Must be sector aligned) */
+		if (fs->n_rootdir % (SS(fs) / SZDIRE)) {
+			EFSPRINTF("Error: FAT - Root directory entries are not sector aligned!");
+			return FR_NO_FILESYSTEM;	/* (Must be sector aligned) */
+		}
 
 		tsect = ld_word(fs->win + BPB_TotSec16);		/* Number of sectors on the volume */
 		if (tsect == 0) tsect = ld_dword(fs->win + BPB_TotSec32);
 
 		nrsv = ld_word(fs->win + BPB_RsvdSecCnt);		/* Number of reserved sectors */
-		if (nrsv == 0) return FR_NO_FILESYSTEM;			/* (Must not be 0) */
+		if (nrsv == 0) {
+			EFSPRINTF("Error: FAT - Zero reserved sectors!");
+			return FR_NO_FILESYSTEM;			/* (Must not be 0) */
+		}
 
 		/* Determine the FAT sub type */
 		sysect = nrsv + fasize + fs->n_rootdir / (SS(fs) / SZDIRE);	/* RSV + FAT + DIR */
-		if (tsect < sysect) return FR_NO_FILESYSTEM;	/* (Invalid volume size) */
+		if (tsect < sysect) {
+			EFSPRINTF("Error: FAT - Invalid volume size!");
+			return FR_NO_FILESYSTEM;	/* (Invalid volume size) */
+		}
 		nclst = (tsect - sysect) / fs->csize;			/* Number of clusters */
-		if (nclst == 0) return FR_NO_FILESYSTEM;		/* (Invalid volume size) */
+		if (nclst == 0) {
+			EFSPRINTF("Error: FAT - Invalid volume size!");
+			return FR_NO_FILESYSTEM;		/* (Invalid volume size) */
+		}
 		fmt = 0;
 		if (nclst <= MAX_FAT32) fmt = FS_FAT32;
 		if (nclst <= MAX_FAT16) fmt = FS_FAT16;
 		if (nclst <= MAX_FAT12) fmt = FS_FAT12;
-		if (fmt == 0) return FR_NO_FILESYSTEM;
+		if (fmt == 0) {
+			EFSPRINTF("Error: FAT - Not compatible FAT12/16/32 filesystem!");
+			return FR_NO_FILESYSTEM;
+		}
 
 		/* Boundaries and Limits */
 		fs->n_fatent = nclst + 2;						/* Number of FAT entries */
@@ -3383,17 +3444,29 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 		fs->fatbase = bsect + nrsv; 					/* FAT start sector */
 		fs->database = bsect + sysect;					/* Data start sector */
 		if (fmt == FS_FAT32) {
-			if (ld_word(fs->win + BPB_FSVer32) != 0) return FR_NO_FILESYSTEM;	/* (Must be FAT32 revision 0.0) */
-			if (fs->n_rootdir != 0) return FR_NO_FILESYSTEM;	/* (BPB_RootEntCnt must be 0) */
+			if (ld_word(fs->win + BPB_FSVer32) != 0) {
+				EFSPRINTF("Error: FAT32 - Not a 0.0 revision!");
+				return FR_NO_FILESYSTEM;	/* (Must be FAT32 revision 0.0) */
+			}
+			if (fs->n_rootdir != 0) {
+				EFSPRINTF("Error: FAT32 - Root entry sector is not 0!");
+				return FR_NO_FILESYSTEM;	/* (BPB_RootEntCnt must be 0) */
+			}
 			fs->dirbase = ld_dword(fs->win + BPB_RootClus32);	/* Root directory start cluster */
 			szbfat = fs->n_fatent * 4;					/* (Needed FAT size) */
 		} else {
-			if (fs->n_rootdir == 0)	return FR_NO_FILESYSTEM;	/* (BPB_RootEntCnt must not be 0) */
+			if (fs->n_rootdir == 0)	{
+				EFSPRINTF("Error: FAT - Root entry sector is 0!");
+				return FR_NO_FILESYSTEM;	/* (BPB_RootEntCnt must not be 0) */
+			}
 			fs->dirbase = fs->fatbase + fasize;			/* Root directory start sector */
 			szbfat = (fmt == FS_FAT16) ?				/* (Needed FAT size) */
 				fs->n_fatent * 2 : fs->n_fatent * 3 / 2 + (fs->n_fatent & 1);
 		}
-		if (fs->fsize < (szbfat + (SS(fs) - 1)) / SS(fs)) return FR_NO_FILESYSTEM;	/* (BPB_FATSz must not be less than the size needed) */
+		if (fs->fsize < (szbfat + (SS(fs) - 1)) / SS(fs)) {
+			EFSPRINTF("Error: FAT - FAT size is not the required size!");
+			return FR_NO_FILESYSTEM;	/* (BPB_FATSz must not be less than the size needed) */
+		}
 
 #if !FF_FS_READONLY
 		/* Get FSInfo if available */
@@ -3426,7 +3499,7 @@ FRESULT find_volume (	/* FR_OK(0): successful, !=0: any error occurred */
 #if FF_USE_LFN == 1
 	fs->lfnbuf = LfnBuf;	/* Static LFN working buffer */
 #if FF_FS_EXFAT
-	fs->dirbuf = DirBuf;	/* Static directory block scratchpad buuffer */
+	fs->dirbuf = DirBuf;	/* Static directory block scratch-pad buffer */
 #endif
 #endif
 #if FF_FS_RPATH != 0
@@ -3504,7 +3577,10 @@ FRESULT f_mount (
 
 	/* Get logical drive number */
 	vol = get_ldnumber(&rp);
-	if (vol < 0) return FR_INVALID_DRIVE;
+	if (vol < 0) {
+		EFSPRINTF("Error: Invalid drive!");
+		return FR_INVALID_DRIVE;
+	}
 	cfs = FatFs[vol];					/* Pointer to fs object */
 
 	if (cfs) {
@@ -3745,8 +3821,14 @@ FRESULT f_read (
 
 	*br = 0;	/* Clear read byte counter */
 	res = validate(&fp->obj, &fs);				/* Check validity of the file object */
-	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) LEAVE_FF(fs, res);	/* Check validity */
-	if (!(fp->flag & FA_READ)) LEAVE_FF(fs, FR_DENIED); /* Check access mode */
+	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) {
+		EFSPRINTF("Error: File object Validation!");
+		LEAVE_FF(fs, res);	/* Check validity */
+	}
+	if (!(fp->flag & FA_READ)) {
+		EFSPRINTF("Error: Access denied!");
+		LEAVE_FF(fs, FR_DENIED);	/* Check access mode */
+	}
 	remain = fp->obj.objsize - fp->fptr;
 	if (btr > remain) btr = (UINT)remain;		/* Truncate btr by remaining bytes */
 
@@ -3767,19 +3849,31 @@ FRESULT f_read (
 						clst = get_fat(&fp->obj, fp->clust);	/* Follow cluster chain on the FAT */
 					}
 				}
-				if (clst < 2) ABORT(fs, FR_INT_ERR);
-				if (clst == 0xFFFFFFFF) ABORT(fs, FR_DISK_ERR);
+				if (clst < 2) {
+					EFSPRINTF("Error: Cluster status check or Internal error!");
+					ABORT(fs, FR_INT_ERR);
+				}
+				if (clst == 0xFFFFFFFF) {
+					EFSPRINTF("Error: Disk error (cluster hard error)!");
+					ABORT(fs, FR_DISK_ERR);
+				}
 				fp->clust = clst;				/* Update current cluster */
 			}
 			sect = clst2sect(fs, fp->clust);	/* Get current sector */
-			if (sect == 0) ABORT(fs, FR_INT_ERR);
+			if (sect == 0) {
+				EFSPRINTF("Error: Get current sector error!");
+				ABORT(fs, FR_INT_ERR);
+			}
 			sect += csect;
 			cc = btr / SS(fs);					/* When remaining bytes >= sector size, */
 			if (cc > 0) {						/* Read maximum contiguous sectors directly */
 				if (csect + cc > fs->csize) {	/* Clip at cluster boundary */
 					cc = fs->csize - csect;
 				}
-				if (disk_read(fs->pdrv, rbuff, sect, cc) != RES_OK) ABORT(fs, FR_DISK_ERR);
+				if (disk_read(fs->pdrv, rbuff, sect, cc) != RES_OK) {
+					EFSPRINTF("Error: Read - Low level disk I/O!");
+					ABORT(fs, FR_DISK_ERR);
+				}
 #if !FF_FS_READONLY && FF_FS_MINIMIZE <= 2		/* Replace one of the read sectors with cached data if it contains a dirty sector */
 #if FF_FS_TINY
 				if (fs->wflag && fs->winsect - sect < cc) {
@@ -3798,11 +3892,17 @@ FRESULT f_read (
 			if (fp->sect != sect) {			/* Load data sector if not in cache */
 #if !FF_FS_READONLY
 				if (fp->flag & FA_DIRTY) {		/* Write-back dirty sector cache */
-					if (disk_write(fs->pdrv, fp->buf, fp->sect, 1) != RES_OK) ABORT(fs, FR_DISK_ERR);
+					if (disk_write(fs->pdrv, fp->buf, fp->sect, 1) != RES_OK) {
+						EFSPRINTF("Error: Write-back dirty sector cache!");
+						ABORT(fs, FR_DISK_ERR);
+					}
 					fp->flag &= (BYTE)~FA_DIRTY;
 				}
 #endif
-				if (disk_read(fs->pdrv, fp->buf, sect, 1) != RES_OK)	ABORT(fs, FR_DISK_ERR);	/* Fill sector cache */
+				if (disk_read(fs->pdrv, fp->buf, sect, 1) != RES_OK) {
+					EFSPRINTF("Error: Read - Low level disk I/O!\n(fill sector cache)");
+					ABORT(fs, FR_DISK_ERR);	/* Fill sector cache */
+				}
 			}
 #endif
 			fp->sect = sect;
@@ -3844,8 +3944,14 @@ FRESULT f_write (
 
 	*bw = 0;	/* Clear write byte counter */
 	res = validate(&fp->obj, &fs);			/* Check validity of the file object */
-	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) LEAVE_FF(fs, res);	/* Check validity */
-	if (!(fp->flag & FA_WRITE)) LEAVE_FF(fs, FR_DENIED);	/* Check access mode */
+	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) {
+		EFSPRINTF("Error: File object Validation!");
+		LEAVE_FF(fs, res);	/* Check validity */
+	}
+	if (!(fp->flag & FA_WRITE)) {
+		EFSPRINTF("Error: Access denied!");
+		LEAVE_FF(fs, FR_DENIED);	/* Check access mode */
+	}
 
 	/* Check fptr wrap-around (file size cannot reach 4 GiB at FAT volume) */
 	if ((!FF_FS_EXFAT || fs->fs_type != FS_EXFAT) && (DWORD)(fp->fptr + btw) < (DWORD)fp->fptr) {
@@ -3872,9 +3978,18 @@ FRESULT f_write (
 						clst = create_chain(&fp->obj, fp->clust);	/* Follow or stretch cluster chain on the FAT */
 					}
 				}
-				if (clst == 0) break;		/* Could not allocate a new cluster (disk full) */
-				if (clst == 1) ABORT(fs, FR_INT_ERR);
-				if (clst == 0xFFFFFFFF) ABORT(fs, FR_DISK_ERR);
+				if (clst == 0) {
+					EFSPRINTF("Error: Could not allocate a new cluster\n(disk full or low level disk I/O error)!");
+					break;		/* Could not allocate a new cluster (disk full) */
+				}
+				if (clst == 1) {
+					EFSPRINTF("Error: Cluster status check or Internal error!");
+					ABORT(fs, FR_INT_ERR);
+				}
+				if (clst == 0xFFFFFFFF) {
+					EFSPRINTF("Error: Disk error (cluster hard error)!");
+					ABORT(fs, FR_DISK_ERR);
+				}
 				fp->clust = clst;			/* Update current cluster */
 				if (fp->obj.sclust == 0) fp->obj.sclust = clst;	/* Set start cluster if the first write */
 			}
@@ -3882,19 +3997,28 @@ FRESULT f_write (
 			if (fs->winsect == fp->sect && sync_window(fs) != FR_OK) ABORT(fs, FR_DISK_ERR);	/* Write-back sector cache */
 #else
 			if (fp->flag & FA_DIRTY) {		/* Write-back sector cache */
-				if (disk_write(fs->pdrv, fp->buf, fp->sect, 1) != RES_OK) ABORT(fs, FR_DISK_ERR);
+				if (disk_write(fs->pdrv, fp->buf, fp->sect, 1) != RES_OK) {
+					EFSPRINTF("Error: Write-back sector cache!");
+					ABORT(fs, FR_DISK_ERR);
+				}
 				fp->flag &= (BYTE)~FA_DIRTY;
 			}
 #endif
 			sect = clst2sect(fs, fp->clust);	/* Get current sector */
-			if (sect == 0) ABORT(fs, FR_INT_ERR);
+			if (sect == 0) {
+				EFSPRINTF("Error: Get current sector error!");
+				ABORT(fs, FR_INT_ERR);
+			}
 			sect += csect;
 			cc = btw / SS(fs);				/* When remaining bytes >= sector size, */
 			if (cc > 0) {					/* Write maximum contiguous sectors directly */
 				if (csect + cc > fs->csize) {	/* Clip at cluster boundary */
 					cc = fs->csize - csect;
 				}
-				if (disk_write(fs->pdrv, wbuff, sect, cc) != RES_OK) ABORT(fs, FR_DISK_ERR);
+				if (disk_write(fs->pdrv, wbuff, sect, cc) != RES_OK) {
+					EFSPRINTF("Error: Write - Low level disk I/O!");
+					ABORT(fs, FR_DISK_ERR);
+				}
 #if FF_FS_MINIMIZE <= 2
 #if FF_FS_TINY
 				if (fs->winsect - sect < cc) {	/* Refill sector cache if it gets invalidated by the direct write */
@@ -3920,6 +4044,7 @@ FRESULT f_write (
 			if (fp->sect != sect && 		/* Fill sector cache with file data */
 				fp->fptr < fp->obj.objsize &&
 				disk_read(fs->pdrv, fp->buf, sect, 1) != RES_OK) {
+					EFSPRINTF("Error: Read - Low level disk I/O!\n(Could not fill sector cache with file data)");
 					ABORT(fs, FR_DISK_ERR);
 			}
 #endif

ipl/ffconf.h

@@ -15,7 +15,7 @@
 /  and optional writing functions as well. */
 
 
-#define FF_FS_MINIMIZE	3
+#define FF_FS_MINIMIZE	0
 /* This option defines minimization level to remove some basic API functions.
 /
 /   0: Basic functions are fully enabled.
@@ -200,7 +200,7 @@
 /  disk_ioctl() function. */
 
 
-#define FF_FS_NOFSINFO	0
+#define FF_FS_NOFSINFO	1
 /* If you need to know correct free space on the FAT32 volume, set bit 0 of this
 /  option, and f_getfree() function at first time after volume mount will force
 /  a full FAT scan. Bit 1 controls the use of last allocated cluster number.

ipl/gfx.c

@@ -65,6 +65,7 @@ static const u8 _gfx_font[] = {
 	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, 0x4c, 0x32, 0x00, 0x00, 0x00
 };
 
 void gfx_init_ctxt(gfx_ctxt_t *ctxt, u32 *fb, u32 width, u32 height, u32 stride)
@@ -88,7 +89,7 @@ void gfx_con_init(gfx_con_t *con, gfx_ctxt_t *ctxt)
 	con->y = 0;
 	con->fgcol = 0xFFFFFFFF;
 	con->fillbg = 0;
-	con->bgcol = 0xFF000000;
+	con->bgcol = 0xFF1B1B1B;
 }
 
 void gfx_con_setcol(gfx_con_t *con, u32 fgcol, int fillbg, u32 bgcol)
@@ -112,7 +113,7 @@ 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)
+	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;
@@ -306,3 +307,17 @@ void gfx_line(gfx_ctxt_t *ctxt, int x0, int y0, int x1, int y1, u32 color)
 		if (e2 < dy) { err += dx; y0 += sy; }
 	}
 }
+
+void gfx_set_logo(gfx_ctxt_t *ctxt, u16 pos_x, u16 pos_y,
+	const u16 size_x, const u16 size_y, const u8 *buf)
+{
+	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] = (0xFF << 24) | buf[pos] | (buf[pos + 1] << 8) | (buf[pos + 2] << 16);
+			pos+=3;
+		}
+	}
+}

ipl/gfx.h

@@ -50,5 +50,6 @@ void gfx_hexdump(gfx_con_t *con, u32 base, const u8 *buf, u32 len);
 
 void gfx_set_pixel(gfx_ctxt_t *ctxt, u32 x, u32 y, u32 color);
 void gfx_line(gfx_ctxt_t *ctxt, int x0, int y0, int x1, int y1, u32 color);
+void gfx_set_logo(gfx_ctxt_t *ctxt, u16 pos_x, u16 pos_y, const u16 size_x, const u16 size_y, const u8 *buf);
 
 #endif

ipl/hos.c

@@ -32,11 +32,20 @@
 #include "pkg2.h"
 #include "ff.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__)*/
-#define DPRINTF(...)
+#define DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)
+//#define DPRINTF(...)
+
+enum KB_FIRMWARE_VERSION {
+	KB_FIRMWARE_VERSION_100_200 = 0,
+	KB_FIRMWARE_VERSION_300 = 1,
+	KB_FIRMWARE_VERSION_301 = 2,
+	KB_FIRMWARE_VERSION_400 = 3,
+	KB_FIRMWARE_VERSION_500 = 4,
+	KB_FIRMWARE_VERSION_MAX
+};
 
 #define NUM_KEYBLOB_KEYS 5
 static const u8 keyblob_keyseeds[NUM_KEYBLOB_KEYS][0x10] = {
@@ -50,19 +59,19 @@ 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 new_masterkey_seed[0x10] = 
+static const u8 master_keyseed_4xx[0x10] = 
 	{ 0x2D, 0xC1, 0xF4, 0x8D, 0xF3, 0x5B, 0x69, 0x33, 0x42, 0x10, 0xAC, 0x65, 0xDA, 0x90, 0x46, 0x66 };
 
-static const u8 new_per_console_key[0x10] = 
+static const u8 console_keyseed_4xx[0x10] = 
 	{ 0x0C, 0x91, 0x09, 0xDB, 0x93, 0x93, 0x07, 0x81, 0x07, 0x3C, 0xC4, 0x16, 0x22, 0x7C, 0x6C, 0x28 };
 
 
@@ -94,18 +103,27 @@ static void _se_lock()
 }
 
 // <-- key derivation algorithm
-static int keygen(u8 *keyblob, u32 kb, void *tsec_fw)
+int keygen(u8 *keyblob, u32 kb, void *tsec_fw)
 {
-	u8 *tmp = (u8 *)malloc(0x10);
+	u8 tmp[0x10];
 
-	se_key_acc_ctrl(12, 0x15);
-	se_key_acc_ctrl(13, 0x15);
+	se_key_acc_ctrl(0x0D, 0x15);
+	se_key_acc_ctrl(0x0E, 0x15);
 
 	//Get TSEC key.
 	if (tsec_query(tmp, 1, tsec_fw) < 0)
 		return 0;
 
-	se_aes_key_set(13, tmp, 0x10);
+	se_aes_key_set(0x0D, tmp, 0x10);
+
+	//Derive keyblob keys from TSEC+SBK.
+	se_aes_crypt_block_ecb(0x0D, 0x00, tmp, keyblob_keyseeds[0]);
+	se_aes_unwrap_key(0x0F, 0x0E, tmp);
+	se_aes_crypt_block_ecb(0xD, 0x00, tmp, keyblob_keyseeds[kb]);
+	se_aes_unwrap_key(0x0D, 0x0E, tmp);
+
+	// Clear SBK
+	se_aes_key_clear(0x0E);
 
 	//TODO: verify keyblob CMAC.
 	//se_aes_unwrap_key(11, 13, cmac_keyseed);
@@ -113,131 +131,43 @@ static int keygen(u8 *keyblob, u32 kb, void *tsec_fw)
 	//if (!memcmp(keyblob, tmp, 0x10))
 	//	return 0;
 
-	switch(kb) {
-		// 1.0.0~2.0.0 FW
-		case 0: {
+	se_aes_crypt_block_ecb(0x0D, 0, tmp, cmac_keyseed);
+	se_aes_unwrap_key(0x0B, 0x0D, cmac_keyseed);
 
-			//Derive keyblob key from TSEC+SBK.
-			memcpy(tmp, keyblob_keyseeds[kb], 0x10);
-			se_aes_crypt_block_ecb(13, 0, tmp, tmp);
-			se_aes_unwrap_key(13, 14, tmp);
-			se_aes_key_clear(14);
+	//Decrypt keyblob and set keyslots.
+	se_aes_crypt_ctr(0x0D, keyblob + 0x20, 0x90, keyblob + 0x20, 0x90, keyblob + 0x10);
+	se_aes_key_set(0x0B, keyblob + 0x20 + 0x80, 0x10); // package1 key
+	se_aes_key_set(0x0C, keyblob + 0x20, 0x10);
+	se_aes_key_set(0x0D, keyblob + 0x20, 0x10);
 
-			se_aes_crypt_ctr(13, keyblob + 0x20, 0x90, keyblob + 0x20, 0x90, keyblob + 0x10);
-			se_aes_key_set(11, keyblob + 0x20 + 0x80, 0x10);
-			se_aes_key_set(12, keyblob + 0x20, 0x10);
+	se_aes_crypt_block_ecb(0x0C, 0, tmp, master_keyseed_retail);
 
-			//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);
-
-			//Generate retail master key.
-			memcpy(tmp, mkey_keyseed_retail, 0x10);
-			se_aes_unwrap_key(12, 12, tmp);
-
-			//Generate console specific key.
-			memcpy(tmp, ckey_keyseed, 0x10);
-			se_aes_unwrap_key(13, 13, tmp);
-
-			memcpy(tmp, key8_keyseed, 0x10);
-			se_key_acc_ctrl(8, 0x15);
-			se_aes_unwrap_key(8, 12, tmp);
-
-			se_key_acc_ctrl(12, 0xFF);
-			se_key_acc_ctrl(13, 0xFF);
-		}
-		break;
-		// 3.0.0~3.0.1 FW
-		case 1:
-		case 2: {
-
-			// keyslot 10
-			memcpy(tmp, keyblob_keyseeds[0], 0x10);
-			se_aes_crypt_block_ecb(13, 0, tmp, tmp);
-			se_aes_unwrap_key(10, 14, tmp);
-
-			// keyslot 13
-			memcpy(tmp, keyblob_keyseeds[kb], 0x10);
-			se_aes_crypt_block_ecb(13, 0, tmp, tmp);
-			se_aes_unwrap_key(13, 14, tmp);
-
-			se_aes_key_clear(14);
-			se_aes_key_clear(15);
-
-			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(12, 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);
-
-			//Generate retail master key.
-			memcpy(tmp, mkey_keyseed_retail, 0x10);
-			se_aes_unwrap_key(12, 12, tmp);
-	
-			//Generate console specific key.
-			memcpy(tmp, ckey_keyseed, 0x10);
-			se_aes_unwrap_key(13, 10, tmp);	
-			se_aes_key_clear(10);
-
-			memcpy(tmp, key8_keyseed, 0x10);
-			se_key_acc_ctrl(8, 0x15);
-			se_aes_unwrap_key(8, 12, tmp);
-
-			se_key_acc_ctrl(12, 0xFF);
-			se_key_acc_ctrl(13, 0xFF);
-		}
+	switch (kb) {
+		case KB_FIRMWARE_VERSION_100_200:
+		case KB_FIRMWARE_VERSION_300:
+			se_aes_unwrap_key(0x0D, 0x0F, console_keyseed);
+			se_aes_unwrap_key(0x0C, 0x0C, master_keyseed_retail);
 		break;
 
-		// 4.0.0~5.0.1 FW
-		case 3:
-		case 4: {
-			se_key_acc_ctrl(14, 0x15);
-			se_key_acc_ctrl(15, 0x15);
+		case KB_FIRMWARE_VERSION_400:
+			se_aes_unwrap_key(0x0D, 0x0F, console_keyseed_4xx);
+			se_aes_unwrap_key(0x0F, 0x0F, console_keyseed);
+			se_aes_unwrap_key(0x0E, 0x0C, master_keyseed_4xx);
+			se_aes_unwrap_key(0x0C, 0x0C, master_keyseed_retail);
+		break;
 
-			// keyslot 15
-			memcpy(tmp, keyblob_keyseeds[0], 0x10);
-			se_aes_crypt_block_ecb(13, 0, tmp, tmp);
-			se_aes_unwrap_key(15, 14, tmp);
-
-			// keyslot 13
-			memcpy(tmp, keyblob_keyseeds[kb], 0x10);
-			se_aes_crypt_block_ecb(13, 0, tmp, tmp);
-			se_aes_unwrap_key(13, 14, tmp);
-
-			se_aes_key_clear(14);
-
-			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(12, 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);
-
-			// keyslot 14
-			memcpy(tmp, new_masterkey_seed, 0x10);
-			se_aes_unwrap_key(14, 12, tmp);
-
-			// keyslot 12
-			memcpy(tmp, mkey_keyseed_retail, 0x10);
-			se_aes_unwrap_key(12, 12, tmp);
-
-			// keyslot 13
-			memcpy(tmp, new_per_console_key, 0x10);
-			se_aes_unwrap_key(13, 15, tmp);
-
-			// keyslot 15
-			memcpy(tmp, ckey_keyseed, 0x10);
-			se_aes_unwrap_key(15, 13, tmp);
-
-			se_key_acc_ctrl(12, 0xFF);
-			se_key_acc_ctrl(15, 0xFF);
-		}
+		case KB_FIRMWARE_VERSION_500:
+		default:
+			se_aes_unwrap_key(0x0A, 0x0F, console_keyseed_4xx);
+			se_aes_unwrap_key(0x0F, 0x0F, console_keyseed);
+			se_aes_unwrap_key(0x0E, 0x0C, master_keyseed_4xx);
+			se_aes_unwrap_key(0x0C, 0x0C, master_keyseed_retail);
 		break;
 	}
-	free(tmp);
+
+	// Package2 key 
+	se_key_acc_ctrl(0x08, 0x15);
+	se_aes_unwrap_key(0x08, 0x0C, key8_keyseed);
 }
 
 
@@ -457,38 +387,39 @@ DPRINTF("decrypted and unpacked pkg1\n");
 	{
 		//Else we patch it to allow for an unsigned package2.
 		patch_t *secmon_patchset = ctxt.pkg1_id->secmon_patchset;
+		
 		if (secmon_patchset != NULL) {
 			for (u32 i = 0; secmon_patchset[i].off != 0xFFFFFFFF; i++)
 				*(vu32 *)(ctxt.pkg1_id->secmon_base + secmon_patchset[i].off) = secmon_patchset[i].val;
 
-				DPRINTF("loaded warmboot.bin and secmon\n");
-				
-				//Read package2.
-				if (!_read_emmc_pkg2(&ctxt))
-					return 0;
+			DPRINTF("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.
-				pkg2_hdr_t *pkg2_hdr = pkg2_decrypt(ctxt.pkg2);
+			DPRINTF("read pkg2\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);
+			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.
-				if (!ctxt.kernel)
-				{
-					ctxt.kernel = pkg2_hdr->data;
-					ctxt.kernel_size = pkg2_hdr->sec_size[PKG2_SEC_KERNEL];
-				}
+			DPRINTF("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];
+			}
 
-				//Merge extra KIP1s into loaded ones.
-				LIST_FOREACH_ENTRY(merge_kip_t, mki, &ctxt.kip1_list, link)
-					pkg2_merge_kip(&kip1_info, (pkg2_kip1_t *)mki->kip1);
+			//Merge extra KIP1s into loaded ones.
+			LIST_FOREACH_ENTRY(merge_kip_t, mki, &ctxt.kip1_list, link)
+				pkg2_merge_kip(&kip1_info, (pkg2_kip1_t *)mki->kip1);
 
-				//Rebuild and encrypt package2.
-				pkg2_build_encrypt((void *)0xA9800000, ctxt.kernel, ctxt.kernel_size, &kip1_info);
+			//Rebuild and encrypt package2.
+			pkg2_build_encrypt((void *)0xA9800000, ctxt.kernel, ctxt.kernel_size, &kip1_info);
 			DPRINTF("rebuilt pkg2\n");
 		} else {
 			//Read package2.
@@ -500,6 +431,23 @@ DPRINTF("decrypted and unpacked pkg1\n");
 		}
 	}
 
+	se_aes_key_clear(8);
+	se_aes_key_clear(11);
+	
+	switch (ctxt.pkg1_id->kb) {
+		case KB_FIRMWARE_VERSION_100_200:
+		case KB_FIRMWARE_VERSION_300:
+		case KB_FIRMWARE_VERSION_301:
+			se_key_acc_ctrl(12, 0xFF);
+			se_key_acc_ctrl(13, 0xFF);
+		break;
+		default:
+		case KB_FIRMWARE_VERSION_400:
+		case KB_FIRMWARE_VERSION_500:
+			se_key_acc_ctrl(12, 0xFF);
+			se_key_acc_ctrl(15, 0xFF);
+		break;
+	}
 
 	//Clear 'BootConfig'.
 	memset((void *)0x4003D000, 0, 0x3000);
@@ -530,6 +478,8 @@ DPRINTF("decrypted and unpacked pkg1\n");
 	//Signal package2 available.
 	*mb_in = 2;
 	sleep(100);
+	*mb_in = 3;
+	sleep(100);
 
 	/*PMC(0x4) = 0x7FFFF3;
 	PMC(0x2C4) = 0xFFFFFFFF;
@@ -544,7 +494,7 @@ DPRINTF("decrypted and unpacked pkg1\n");
 	//display_end();
 
 	//Signal to continue boot.
-	*mb_in = 3;
+	*mb_in = 4;
 	sleep(100);
 
 	//Halt ourselves in waitevent state.

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/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/pkg1.c

@@ -30,21 +30,39 @@ PATCHSET_DEF(_secmon_1_patchset,
 	//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 + 0xAB4, _NOP() },
 	{ 0xAC8 + 0xA30, _NOP() }, //Header signature.
 	{ 0xAC8 + 0xAC0, _NOP() }, //Version.
-	{ 0xAC8 + 0xADC, _NOP() } //Sections SHA2.
+	{ 0xAC8 + 0xADC, _NOP() }  //Sections SHA2.
+);
+
+PATCHSET_DEF(_secmon_5_patchset,
+	//Patch package2 decryption and signature/hash checks.
+	{ 0x1218 + 0x6E68, _NOP() }, //Header signature.
+	{ 0x1218 + 0x6E74, _NOP() }, //Version.
+	{ 0x1218 + 0x6FE4, _NOP() }, //Sections SHA2.
+	{ 0x1218 + 0x2DC, _NOP() }   //Unknown.
+);
+
+PATCHSET_DEF(_secmon_6_patchset,
+	//Patch package2 decryption and signature/hash checks.
+	{ 0x12b0 + 0x4d0, _NOP() },
+	{ 0x12b0 + 0x4dc, _NOP() },
+	{ 0x12b0 + 0x794, _NOP() },
+	{ 0x12b0 + 0xb30, _NOP() }//,
+	//{ 0x12b0 + 0xa18 , _NOP() } // BootConfig Retail Check
 );
 
 /*
@@ -61,24 +79,13 @@ PATCHSET_DEF(_secmon_3_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
+	{ "20170519101410", 1, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, _secmon_3_patchset }, //3.0.0
 	{ "20170710161758", 2, 0x1A00, 0x3FE0, { 0, 1, 2 }, 0x4002D000, NULL }, //3.0.1
-	{ "20170921172629", 3, 0x1800, 0x3FE0, { 1, 2, 0 }, 0x4002B000, NULL }, //4.0.0
-	{ "20180220163747", 4, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, NULL }, //5.0.0
+	{ "20170921172629", 3, 0x1800, 0x3FE0, { 1, 2, 0 }, 0x4002B000, _secmon_5_patchset }, //4.0.0
+	{ "20180220163747", 4, 0x1900, 0x3FE0, { 1, 2, 0 }, 0x4002B000, _secmon_6_patchset }, //5.0.0
 	{ NULL, 0, 0, 0, 0 } //End.
 };
 
-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)
 {

ipl/pkg1.h

@@ -42,6 +42,18 @@ typedef struct _pkg1_id_t
 	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);

ipl/pkg2.c

@@ -172,3 +172,4 @@ DPRINTF("INI1 encrypted\n");
 	memset(hdr->ctr, 0 , 0x10);
 	*(u32 *)hdr->ctr = 0x100 + sizeof(pkg2_hdr_t) + kernel_size + ini1_size;
 }
+

ipl/sd.h

@@ -35,10 +35,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 +65,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 +77,16 @@
 #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
+
 /*
 * SD_SWITCH mode
 */

ipl/sdmmc.c

@@ -187,7 +187,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)
@@ -223,7 +223,7 @@ static int _mmc_storage_get_op_cond(sdmmc_storage_t *storage, u32 power)
 		u32 cond = 0;
 		if (!_mmc_storage_get_op_cond_inner(storage, &cond, power))
 			break;
-		if (cond & 0x80000000)
+		if (cond & MMC_CARD_BUSY)
 		{
 			if (cond & 0x40000000)
 				storage->has_sector_access = 1;
@@ -242,6 +242,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 int _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 +330,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);
 }
 
@@ -388,7 +460,7 @@ int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 		return 0;
 	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");
 
@@ -396,9 +468,10 @@ int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 		return 0;
 	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");
+	_mmc_storage_parse_csd(storage);
 
 	if (!sdmmc_setup_clock(storage->sdmmc, 1))
 		return 0;
@@ -412,7 +485,7 @@ int sdmmc_storage_init_mmc(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 		return 0;
 	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)
 	{
@@ -430,23 +503,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] switched to highspeed mode\n");
 
 	sdmmc_sd_clock_ctrl(storage->sdmmc, 1);
 
-	free(ext_csd);
 	return 1;
 }
 
@@ -484,21 +562,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;
@@ -514,15 +597,15 @@ static int _sd_storage_get_op_cond(sdmmc_storage_t *storage, int is_version_1, i
 		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))
@@ -537,7 +620,7 @@ static int _sd_storage_get_op_cond(sdmmc_storage_t *storage, int is_version_1, i
 		}
 		if (get_tmr() > timeout)
 			break;
-		sleep(1000);
+		sleep(10000); // Needs to be at least 10ms for some SD Cards
 	}
 
 	return 0;
@@ -573,7 +656,24 @@ static int _sd_storage_get_rca(sdmmc_storage_t *storage)
 	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 +691,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);
 }
 
@@ -672,7 +783,7 @@ int _sd_storage_enable_highspeed_low_volt(sdmmc_storage_t *storage, u32 type, u8
 		if (buf[13] & 8)
 		{
 			type = 11;
-			hs_type = 3;
+			hs_type = UHS_SDR104_BUS_SPEED;
 			break;
 		}
 		//Fall through.
@@ -680,7 +791,7 @@ int _sd_storage_enable_highspeed_low_volt(sdmmc_storage_t *storage, u32 type, u8
 		if (!(buf[13] & 4))
 			return 0;
 		type = 10;
-		hs_type = 2;
+		hs_type = UHS_SDR50_BUS_SPEED;
 		break;
 	default:
 		return 0;
@@ -711,8 +822,130 @@ 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;
 
@@ -726,44 +959,40 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 		return 0;
 	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");
 
-	//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");
 
-	if (!_sdmmc_storage_get_cid(storage, storage->cid))
+	if (!_sdmmc_storage_get_cid(storage, storage->raw_cid))
 		return 0;
 	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);
 
-	if (!_sdmmc_storage_get_csd(storage, storage->csd))
+	if (!_sdmmc_storage_get_csd(storage, storage->raw_csd))
 		return 0;
 	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;
 	}
 
@@ -790,10 +1019,11 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 	u8 *buf = (u8 *)malloc(512);
 	if (!_sd_storage_get_scr(storage, buf))
 		return 0;
-	memcpy(storage->scr, buf, 8);
+	//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))
 		{
@@ -815,7 +1045,7 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 		}
 		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))
 		{
@@ -827,6 +1057,10 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 
 	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 +1077,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;

ipl/sdmmc.h

@@ -20,6 +20,65 @@
 #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;  
+} 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 +88,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

@@ -54,24 +54,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;
 }
@@ -298,10 +304,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;

ipl/se.c

@@ -67,7 +67,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 +99,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);
@@ -156,7 +156,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 +164,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 +180,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);

ipl/se.h

@@ -23,8 +23,8 @@ 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);
 
 #endif

ipl/tui.c

@@ -16,6 +16,7 @@
 
 #include "tui.h"
 #include "btn.h"
+#include "ctc_logo.h"
 
 void tui_pbar(gfx_con_t *con, int x, int y, u32 val)
 {
@@ -42,11 +43,12 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 {
 	int idx = 0, cnt;
 
-	gfx_clear(con->gfx_ctxt, 0xFF000000);
+	gfx_clear(con->gfx_ctxt, 0xFF1B1B1B);
+	gfx_set_logo(con->gfx_ctxt, 538, 1180, LOGO_WIDTH, LOGO_HEIGHT, CTC_LOGO);
 
 	while (1)
 	{
-		gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF000000);
+		gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF1B1B1B);
 		gfx_con_setpos(con, menu->x, menu->y);
 		gfx_printf(con, "[%s]\n\n", menu->caption);
 
@@ -55,7 +57,7 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 			if (cnt == idx)
 				gfx_con_setcol(con, 0xFF000000, 1, 0xFFCCCCCC);
 			else
-				gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF000000);
+				gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF1B1B1B);
 			con->x += 8;
 			gfx_printf(con, "%s", menu->ents[cnt].caption);
 			if(menu->ents[cnt].type == MENT_MENU)
@@ -63,15 +65,19 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 			gfx_putc(con, '\n');
 		}
 
-		gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF000000);
+		gfx_con_setcol(con, 0xFFFFFFFF, 1, 0xFF1B1B1B);
 		gfx_putc(con, '\n');
 
 		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];
@@ -90,7 +96,8 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)
 				return NULL;
 				break;
 			}
-			gfx_clear(con->gfx_ctxt, 0xFF000000);
+			gfx_clear(con->gfx_ctxt, 0xFF1B1B1B);
+			gfx_set_logo(con->gfx_ctxt, 538, 1180, LOGO_WIDTH, LOGO_HEIGHT, CTC_LOGO);
 		}
 	}