[HACK] Support raw nand dump

Lets you dump the raw USER, the BOOT0 and BOOT1 partitions. The USER dumping is done in 4 8GB parts, splitted in 4GB files.
2018-05-01 15:28:48 +03:00
20 changed files with 601 additions and 1482 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,3 @@
 .vs
 .vscode
 build_ipl/*
-/ipl.bin
--- a/11
+++ b/11
@@ -44,19 +44,19 @@ OBJS = $(addprefix $(BUILD)/, \
 OBJS += $(addprefix $(BUILD)/, diskio.o ff.o ffunicode.o)

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

 .PHONY: all clean

-all: $(TARGET).bin
+all: $(BUILD)/$(TARGET)

 clean:
 	@rm -rf $(OBJS)
-	@rm -rf $(BUILD)
-	@rm -rf $(TARGET).bin
+	@rm -rf $(BUILD)/$(TARGET).elf
+	@rm -rf $(BUILD)/$(TARGET)

-$(TARGET).bin: $(BUILD)/$(TARGET).elf
+$(BUILD)/$(TARGET): $(BUILD)/$(TARGET).elf
 	$(OBJCOPY) -S -O binary $< $@

 $(BUILD)/$(TARGET).elf: $(OBJS)
@@ -66,5 +66,4 @@ $(BUILD)/%.o: $(SOURCEDIR)/%.c
 	$(CC) $(CFLAGS) -c $< -o $@

 $(BUILD)/%.o: $(SOURCEDIR)/%.S
-	@mkdir -p "$(BUILD)"
 	$(CC) $(CFLAGS) -c $< -o $@
--- a/ipl/ff.c
+++ b/ipl/ff.c
@@ -3,7 +3,6 @@
 /-----------------------------------------------------------------------------/
 /
 / 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
@@ -23,11 +22,6 @@
 #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(...)

 /*--------------------------------------------------------------------------

@@ -3259,7 +3253,6 @@ 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 */
@@ -3273,11 +3266,9 @@ 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) */
@@ -3300,14 +3291,8 @@ 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) {
-		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 */
-	}
+	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 */

 	/* An FAT volume is found (bsect). Following code initializes the filesystem object */

@@ -3318,58 +3303,36 @@ 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) {
-			EFSPRINTF("Error: exFAT - Version check failed!");
-			return FR_NO_FILESYSTEM;	/* Check exFAT version (must be version 1.0) */
-		}
+		if (ld_word(fs->win + BPB_FSVerEx) != 0x100) 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) {
-			EFSPRINTF("Error: exFAT - Cannot handle volume LBA with 32-bit LBA!");
-			return FR_NO_FILESYSTEM;	/* (It cannot be handled in 32-bit LBA) */
-		}
+		if (maxlba >= 0x100000000) 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) {
-			EFSPRINTF("Error: exFAT - Multiple or no file allocation tables found!");
-			return FR_NO_FILESYSTEM;	/* (Supports only 1 FAT) */
-		}
+		if (fs->n_fats != 1) return FR_NO_FILESYSTEM;	/* (Supports only 1 FAT) */

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

 		nclst = ld_dword(fs->win + BPB_NumClusEx);		/* Number of clusters */
-		if (nclst > MAX_EXFAT) {
-			EFSPRINTF("Error: exFAT - Total clusters exceed allowed!");
-			return FR_NO_FILESYSTEM;	/* (Too many clusters) */
-		}
+		if (nclst > MAX_EXFAT) 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) {
-			EFSPRINTF("Error: exFAT - Volume size is lower than required!");
-			return FR_NO_FILESYSTEM;	/* (Volume size must not be smaller than the size required) */
-		}
+		if (maxlba < (QWORD)fs->database + nclst * fs->csize) return FR_NO_FILESYSTEM;	/* (Volume size must not be smaller than the size requiered) */
 		fs->dirbase = ld_dword(fs->win + BPB_RootClusEx);

 		/* Check if bitmap location is in assumption (at the first cluster) */
-		if (move_window(fs, clst2sect(fs, fs->dirbase)) != FR_OK) {
-			EFSPRINTF("Error: exFAT - Bitmap location not at first cluster!");
-			return FR_DISK_ERR;
-		}
+		if (move_window(fs, clst2sect(fs, fs->dirbase)) != FR_OK) 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? */
 		}
@@ -3381,62 +3344,38 @@ 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)) {
-			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) */
-		}
+		if (ld_word(fs->win + BPB_BytsPerSec) != SS(fs)) 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) {
-			EFSPRINTF("Error: FAT - No or more than 2 file allocation tables found!");
-			return FR_NO_FILESYSTEM;	/* (Must be 1 or 2) */
-		}
+		if (fs->n_fats != 1 && fs->n_fats != 2) return FR_NO_FILESYSTEM;	/* (Must be 1 or 2) */
 		fasize *= fs->n_fats;							/* Number of sectors for FAT area */

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

 		fs->n_rootdir = ld_word(fs->win + BPB_RootEntCnt);	/* Number of root directory entries */
-		if (fs->n_rootdir % (SS(fs) / SZDIRE)) {
-			EFSPRINTF("Error: FAT - Root directory entries are not sector aligned!");
-			return FR_NO_FILESYSTEM;	/* (Must be sector aligned) */
-		}
+		if (fs->n_rootdir % (SS(fs) / SZDIRE)) 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) {
-			EFSPRINTF("Error: FAT - Zero reserved sectors!");
-			return FR_NO_FILESYSTEM;			/* (Must not be 0) */
-		}
+		if (nrsv == 0) return FR_NO_FILESYSTEM;			/* (Must not be 0) */

 		/* Determine the FAT sub type */
 		sysect = nrsv + fasize + fs->n_rootdir / (SS(fs) / SZDIRE);	/* RSV + FAT + DIR */
-		if (tsect < sysect) {
-			EFSPRINTF("Error: FAT - Invalid volume size!");
-			return FR_NO_FILESYSTEM;	/* (Invalid volume size) */
-		}
+		if (tsect < sysect) return FR_NO_FILESYSTEM;	/* (Invalid volume size) */
 		nclst = (tsect - sysect) / fs->csize;			/* Number of clusters */
-		if (nclst == 0) {
-			EFSPRINTF("Error: FAT - Invalid volume size!");
-			return FR_NO_FILESYSTEM;		/* (Invalid volume size) */
-		}
+		if (nclst == 0) 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) {
-			EFSPRINTF("Error: FAT - Not compatible FAT12/16/32 filesystem!");
-			return FR_NO_FILESYSTEM;
-		}
+		if (fmt == 0) return FR_NO_FILESYSTEM;

 		/* Boundaries and Limits */
 		fs->n_fatent = nclst + 2;						/* Number of FAT entries */
@@ -3444,29 +3383,17 @@ 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) {
-				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) */
-			}
+			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) */
 			fs->dirbase = ld_dword(fs->win + BPB_RootClus32);	/* Root directory start cluster */
 			szbfat = fs->n_fatent * 4;					/* (Needed FAT size) */
 		} else {
-			if (fs->n_rootdir == 0)	{
-				EFSPRINTF("Error: FAT - Root entry sector is 0!");
-				return FR_NO_FILESYSTEM;	/* (BPB_RootEntCnt must not be 0) */
-			}
+			if (fs->n_rootdir == 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)) {
-			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 (fs->fsize < (szbfat + (SS(fs) - 1)) / SS(fs)) return FR_NO_FILESYSTEM;	/* (BPB_FATSz must not be less than the size needed) */

 #if !FF_FS_READONLY
 		/* Get FSInfo if available */
@@ -3499,7 +3426,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 scratch-pad buffer */
+	fs->dirbuf = DirBuf;	/* Static directory block scratchpad buuffer */
 #endif
 #endif
 #if FF_FS_RPATH != 0
@@ -3577,10 +3504,7 @@ FRESULT f_mount (

 	/* Get logical drive number */
 	vol = get_ldnumber(&rp);
-	if (vol < 0) {
-		EFSPRINTF("Error: Invalid drive!");
-		return FR_INVALID_DRIVE;
-	}
+	if (vol < 0) return FR_INVALID_DRIVE;
 	cfs = FatFs[vol];					/* Pointer to fs object */

 	if (cfs) {
@@ -3821,14 +3745,8 @@ FRESULT f_read (

 	*br = 0;	/* Clear read byte counter */
 	res = validate(&fp->obj, &fs);				/* Check validity of the file object */
-	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) {
-		EFSPRINTF("Error: File object Validation!");
-		LEAVE_FF(fs, res);	/* Check validity */
-	}
-	if (!(fp->flag & FA_READ)) {
-		EFSPRINTF("Error: Access denied!");
-		LEAVE_FF(fs, FR_DENIED);	/* Check access mode */
-	}
+	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 */
 	remain = fp->obj.objsize - fp->fptr;
 	if (btr > remain) btr = (UINT)remain;		/* Truncate btr by remaining bytes */

@@ -3849,31 +3767,19 @@ FRESULT f_read (
 						clst = get_fat(&fp->obj, fp->clust);	/* Follow cluster chain on the FAT */
 					}
 				}
-				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);
-				}
+				if (clst < 2) ABORT(fs, FR_INT_ERR);
+				if (clst == 0xFFFFFFFF) ABORT(fs, FR_DISK_ERR);
 				fp->clust = clst;				/* Update current cluster */
 			}
 			sect = clst2sect(fs, fp->clust);	/* Get current sector */
-			if (sect == 0) {
-				EFSPRINTF("Error: Get current sector error!");
-				ABORT(fs, FR_INT_ERR);
-			}
+			if (sect == 0) 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) {
-					EFSPRINTF("Error: Read - Low level disk I/O!");
-					ABORT(fs, FR_DISK_ERR);
-				}
+				if (disk_read(fs->pdrv, rbuff, sect, cc) != RES_OK) 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) {
@@ -3892,17 +3798,11 @@ 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) {
-						EFSPRINTF("Error: Write-back dirty sector cache!");
-						ABORT(fs, FR_DISK_ERR);
-					}
+					if (disk_write(fs->pdrv, fp->buf, fp->sect, 1) != RES_OK) ABORT(fs, FR_DISK_ERR);
 					fp->flag &= (BYTE)~FA_DIRTY;
 				}
 #endif
-				if (disk_read(fs->pdrv, fp->buf, sect, 1) != RES_OK) {
-					EFSPRINTF("Error: Read - Low level disk I/O!\n(fill sector cache)");
-					ABORT(fs, FR_DISK_ERR);	/* Fill sector cache */
-				}
+				if (disk_read(fs->pdrv, fp->buf, sect, 1) != RES_OK)	ABORT(fs, FR_DISK_ERR);	/* Fill sector cache */
 			}
 #endif
 			fp->sect = sect;
@@ -3944,14 +3844,8 @@ FRESULT f_write (

 	*bw = 0;	/* Clear write byte counter */
 	res = validate(&fp->obj, &fs);			/* Check validity of the file object */
-	if (res != FR_OK || (res = (FRESULT)fp->err) != FR_OK) {
-		EFSPRINTF("Error: File object Validation!");
-		LEAVE_FF(fs, res);	/* Check validity */
-	}
-	if (!(fp->flag & FA_WRITE)) {
-		EFSPRINTF("Error: Access denied!");
-		LEAVE_FF(fs, FR_DENIED);	/* Check access mode */
-	}
+	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 */

 	/* 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) {
@@ -3978,18 +3872,9 @@ FRESULT f_write (
 						clst = create_chain(&fp->obj, fp->clust);	/* Follow or stretch cluster chain on the FAT */
 					}
 				}
-				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);
-				}
+				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);
 				fp->clust = clst;			/* Update current cluster */
 				if (fp->obj.sclust == 0) fp->obj.sclust = clst;	/* Set start cluster if the first write */
 			}
@@ -3997,28 +3882,19 @@ 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) {
-					EFSPRINTF("Error: Write-back sector cache!");
-					ABORT(fs, FR_DISK_ERR);
-				}
+				if (disk_write(fs->pdrv, fp->buf, fp->sect, 1) != RES_OK) ABORT(fs, FR_DISK_ERR);
 				fp->flag &= (BYTE)~FA_DIRTY;
 			}
 #endif
 			sect = clst2sect(fs, fp->clust);	/* Get current sector */
-			if (sect == 0) {
-				EFSPRINTF("Error: Get current sector error!");
-				ABORT(fs, FR_INT_ERR);
-			}
+			if (sect == 0) 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) {
-					EFSPRINTF("Error: Write - Low level disk I/O!");
-					ABORT(fs, FR_DISK_ERR);
-				}
+				if (disk_write(fs->pdrv, wbuff, sect, cc) != RES_OK) 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 */
@@ -4044,7 +3920,6 @@ 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
--- a/ipl/ffconf.h
+++ b/ipl/ffconf.h
@@ -200,7 +200,7 @@
 /  disk_ioctl() function. */


-#define FF_FS_NOFSINFO	1
+#define FF_FS_NOFSINFO	0
 /* 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.
@@ -224,7 +224,7 @@
 /  buffer in the filesystem object (FATFS) is used for the file data transfer. */


-#define FF_FS_EXFAT		1
+#define FF_FS_EXFAT		0
 /* This option switches support for exFAT filesystem. (0:Disable or 1:Enable)
 /  When enable exFAT, also LFN needs to be enabled.
 /  Note that enabling exFAT discards ANSI C (C89) compatibility. */
--- a/ipl/gfx.c
+++ b/ipl/gfx.c
@@ -65,7 +65,6 @@ 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)
@@ -113,7 +112,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 <= 126)
+	if (c >= 32 && c < 128)
 	{
 		u8 *cbuf = (u8 *)&_gfx_font[8 * (c - 32)];
 		u32 *fb = con->gfx_ctxt->fb + con->x + con->y * con->gfx_ctxt->stride;
--- a/ipl/hos.c
+++ b/ipl/hos.c
@@ -32,20 +32,11 @@
 #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(...)
-
-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 DPRINTF(...) gfx_printf(&gfx_con, __VA_ARGS__)*/
+#define DPRINTF(...)

 #define NUM_KEYBLOB_KEYS 5
 static const u8 keyblob_keyseeds[NUM_KEYBLOB_KEYS][0x10] = {
@@ -59,22 +50,15 @@ 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 master_keyseed_retail[0x10] =
+static const u8 mkey_keyseed_retail[0x10] =
 	{ 0xD8, 0xA2, 0x41, 0x0A, 0xC6, 0xC5, 0x90, 0x01, 0xC6, 0x1D, 0x6A, 0x26, 0x7C, 0x51, 0x3F, 0x3C };

-static const u8 console_keyseed[0x10] =
+static const u8 ckey_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[0x10] = 
-	{ 0x2D, 0xC1, 0xF4, 0x8D, 0xF3, 0x5B, 0x69, 0x33, 0x42, 0x10, 0xAC, 0x65, 0xDA, 0x90, 0x46, 0x66 };
-
-static const u8 console_keyseed_4xx[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++)
@@ -102,28 +86,24 @@ static void _se_lock()
 	gfx_hexdump(&gfx_con, SE_BASE, (void *)SE_BASE, 0x400);*/
 }

-// <-- key derivation algorithm
-int keygen(u8 *keyblob, u32 kb, void *tsec_fw)
+//Key derivation for < 4.0.0
+static int _keygen_1(u8 *keyblob, u32 kb, void *tsec_fw)
 {
-	u8 tmp[0x10];
+	u8 *tmp = (u8 *)malloc(0x10);

-	se_key_acc_ctrl(0x0D, 0x15);
-	se_key_acc_ctrl(0x0E, 0x15);
+	se_key_acc_ctrl(12, 0x15);
+	se_key_acc_ctrl(13, 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);
+	//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);

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

-	se_aes_crypt_block_ecb(0x0D, 0, tmp, cmac_keyseed);
-	se_aes_unwrap_key(0x0B, 0x0D, cmac_keyseed);
-
 	//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);

-	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;
+	//Generate retail master key.
+	memcpy(tmp, mkey_keyseed_retail, 0x10);
+	se_aes_unwrap_key(12, 12, tmp);

-		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;
+	memcpy(tmp, key8_keyseed, 0x10);
+	se_key_acc_ctrl(8, 0x15);
+	se_aes_unwrap_key(8, 12, tmp);

-		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;
-	}
+	//Generate console specific key.
+	memcpy(tmp, ckey_keyseed, 0x10);
+	se_aes_unwrap_key(13, 13, tmp);

-	// Package2 key 
-	se_key_acc_ctrl(0x08, 0x15);
-	se_aes_unwrap_key(0x08, 0x0C, key8_keyseed);
+	se_key_acc_ctrl(12, 0xFF);
+	se_key_acc_ctrl(13, 0xFF);
+
+	free(tmp);
 }

-
 typedef struct _launch_ctxt_t
 {
 	void *keyblob;
@@ -358,12 +325,12 @@ int hos_launch(ini_sec_t *cfg)
 		return 0;

 	//XXX: remove this once we support 3+.
-	//if (ctxt.pkg1_id->kb > 0)
-	//	return 0;
+	if (ctxt.pkg1_id->kb > 0)
+		return 0;

 DPRINTF("loaded pkg1 and keyblob\n");
 	//Generate keys.
-	keygen(ctxt.keyblob, ctxt.pkg1_id->kb, (u8 *)ctxt.pkg1 + ctxt.pkg1_id->tsec_off);
+	_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.
 	if (!ctxt.warmboot || !ctxt.secmon)
@@ -380,66 +347,41 @@ DPRINTF("decrypted and unpacked pkg1\n");
 	//Set warmboot address in PMC.
 	PMC(APBDEV_PMC_SCRATCH1) = 0x8000D000;
 	//Replace 'SecureMonitor' if requested.
-	if (ctxt.secmon) {
+	if (ctxt.secmon)
 		memcpy((void *)ctxt.pkg1_id->secmon_base, ctxt.secmon, ctxt.secmon_size);
-	}
 	else
 	{
 		//Else we patch it to allow for an unsigned package2.
 		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;
+		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");

-			DPRINTF("loaded warmboot.bin and secmon\n");
-			
-			//Read package2.
-			if (!_read_emmc_pkg2(&ctxt))
-				return 0;
+	//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);
-
-			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);
-
-			//Rebuild and encrypt package2.
-			pkg2_build_encrypt((void *)0xA9800000, ctxt.kernel, ctxt.kernel_size, &kip1_info);
-			DPRINTF("rebuilt pkg2\n");
-		} else {
-			//Read package2.
-			if (!_read_emmc_pkg2(&ctxt))
-				return 0;
-
-			DPRINTF("read pkg2\n");
-			memcpy((void *)0xA9800000, ctxt.pkg2, ctxt.pkg2_size);
-		}
+	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];
 	}

-	se_aes_key_clear(8);
-	se_aes_key_clear(11);
-	//se_aes_key_clear(13);
-	//se_key_acc_ctrl(10, 0xFF);
-	se_key_acc_ctrl(12, 0xFF);
-	//se_key_acc_ctrl(13, 0xFF);
-	//se_key_acc_ctrl(14, 0xFF);
-	se_key_acc_ctrl(15, 0xFF);
+	//Merge extra KIP1s into loaded ones.
+	LIST_FOREACH_ENTRY(merge_kip_t, mki, &ctxt.kip1_list, link)
+		pkg2_merge_kip(&kip1_info, (pkg2_kip1_t *)mki->kip1);

+	//Rebuild and encrypt package2.
+	pkg2_build_encrypt((void *)0xA9800000, ctxt.kernel, ctxt.kernel_size, &kip1_info);
+DPRINTF("rebuilt pkg2\n");
 	//Clear 'BootConfig'.
 	memset((void *)0x4003D000, 0, 0x3000);

@@ -469,8 +411,6 @@ 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;
@@ -485,7 +425,7 @@ DPRINTF("decrypted and unpacked pkg1\n");
 	//display_end();

 	//Signal to continue boot.
-	*mb_in = 4;
+	*mb_in = 3;
 	sleep(100);

 	//Halt ourselves in waitevent state.
--- a/ipl/hos.h
+++ b/ipl/hos.h
@@ -21,6 +21,5 @@
 #include "ini.h"

 int hos_launch(ini_sec_t *cfg);
-int keygen(u8 *keyblob, u32 kb, void *tsec_fw);

 #endif
--- a/ipl/main.c
+++ b/ipl/main.c
--- a/ipl/mmc.h
+++ b/ipl/mmc.h
@@ -289,7 +289,6 @@ 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 */
--- a/ipl/pkg1.c
+++ b/ipl/pkg1.c
@@ -30,38 +30,14 @@ 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.
-);
-
-PATCHSET_DEF(_secmon_3_patchset,
-	//Patch package2 decryption and signature/hash checks.
-	{ 0xAC8 + 0xA30, _NOP() }, //Header signature.
-	{ 0xAC8 + 0xAC0, _NOP() }, //Version.
-	{ 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
+	{ 0xAC8 + 0xB58, _NOP() } //Sections SHA2.
 );

 /*
@@ -80,11 +56,22 @@ static const pkg1_id_t _pkg1_ids[] = {
 	{ "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, 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
+	{ "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.
 };

+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)
 {
--- a/ipl/pkg1.h
+++ b/ipl/pkg1.h
@@ -42,18 +42,6 @@ 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);
--- a/ipl/pkg2.c
+++ b/ipl/pkg2.c
@@ -172,4 +172,3 @@ DPRINTF("INI1 encrypted\n");
 	memset(hdr->ctr, 0 , 0x10);
 	*(u32 *)hdr->ctr = 0x100 + sizeof(pkg2_hdr_t) + kernel_size + ini1_size;
 }
-
--- a/ipl/pmc.h
+++ b/ipl/pmc.h
@@ -41,8 +41,5 @@
 #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
--- a/ipl/sd.h
+++ b/ipl/sd.h
@@ -35,11 +35,10 @@
 #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_VDD_32_33    (1 << 20)	 /* VDD voltage 3.2 ~ 3.3 */
+#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 */

 /*
 * SD_SWITCH argument format:
@@ -65,11 +64,10 @@
 /*
 * 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
@@ -77,16 +75,6 @@
 #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
 */
--- a/ipl/sdmmc.c
+++ b/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 & MMC_CARD_BUSY)
+		if (cond & 0x80000000)
 		{
 			if (cond & 0x40000000)
 				storage->has_sector_access = 1;
@@ -242,76 +242,6 @@ 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;
@@ -330,8 +260,6 @@ 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);
 }

@@ -460,7 +388,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->raw_cid))
+	if (!_sdmmc_storage_get_cid(storage, storage->cid))
 		return 0;
 	DPRINTF("[mmc] got cid\n");

@@ -468,10 +396,9 @@ 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->raw_csd))
+	if (!_sdmmc_storage_get_csd(storage, storage->csd))
 		return 0;
 	DPRINTF("[mmc] got csd\n");
-	_mmc_storage_parse_csd(storage);

 	if (!sdmmc_setup_clock(storage->sdmmc, 1))
 		return 0;
@@ -485,7 +412,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->raw_csd;
+	u32 *csd = (u32 *)storage->csd;
 	//Check system specification version, only version 4.0 and later support below features.
 	if (unstuff_bits(csd, 122, 4) < CSD_SPEC_VER_4)
 	{
@@ -503,28 +430,23 @@ 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);

-	/* 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)
-	{
-		_mmc_storage_enable_bkops(storage);
-		DPRINTF("[mmc] BKOPS enabled\n");
-	}
-	else
-		DPRINTF("[mmc] BKOPS disabled\n");
+	storage->sec_cnt = *(u32 *)&ext_csd[EXT_CSD_SEC_CNT];

-	if (!_mmc_storage_enable_highspeed(storage, storage->ext_csd.card_type, type))
+	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))
+	{
+		free(ext_csd);
 		return 0;
-	DPRINTF("[mmc] switched to highspeed mode\n");
+	}
+	DPRINTF("[mmc] switched to possible highspeed mode\n");

 	sdmmc_sd_clock_ctrl(storage->sdmmc, 1);

+	free(ext_csd);
 	return 1;
 }

@@ -562,26 +484,21 @@ 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 1; // The SD Card is version 1.X
+		return 0;
+
+	//TODO: we may have received a timeout error in the above request, which indicates a version 1 card.

 	u32 resp = 0;
 	if (!sdmmc_get_rsp(storage->sdmmc, &resp, 4, SDMMC_RSP_TYPE_5))
-		return 2;
+		return 0;

-	return (resp & 0xFF) == 0xAA ? 0 : 2;
+	return (resp & 0xFFF) == 0x1AA ? 1 : 0;
 }

 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;
-	// 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;
+	u32 arg = (((~is_version_1 & 1) << 28) & 0xBFFFFFFF | ((~is_version_1 & 1) << 30)) & 0xFEFFFFFF | ((supports_low_voltage & ~is_version_1 & 1) << 24) | 0x100000;
 	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;
@@ -597,15 +514,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 & MMC_CARD_BUSY)
+		if (cond & 0x80000000)
 		{
-			if (cond & SD_OCR_CCS)
+			if (cond & 0x40000000)
 				storage->has_sector_access = 1;

-			if (cond & SD_ROCR_S18A && supports_low_voltage)
+			if (cond & 0x1000000 && 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))
@@ -620,7 +537,7 @@ static int _sd_storage_get_op_cond(sdmmc_storage_t *storage, int is_version_1, i
 		}
 		if (get_tmr() > timeout)
 			break;
-		sleep(10000); // Needs to be at least 10ms for some SD Cards
+		sleep(1000);
 	}

 	return 0;
@@ -656,24 +573,7 @@ static int _sd_storage_get_rca(sdmmc_storage_t *storage)
 	return 0;
 }

-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)
+int _sd_storage_get_scr(sdmmc_storage_t *storage, void *buf)
 {
 	sdmmc_cmd_t cmdbuf;
 	sdmmc_init_cmd(&cmdbuf, SD_APP_SEND_SCR, 0, SDMMC_RSP_TYPE_1, 0);
@@ -691,17 +591,6 @@ int _sd_storage_get_scr(sdmmc_storage_t *storage, u8 *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);
 }

@@ -783,7 +672,7 @@ int _sd_storage_enable_highspeed_low_volt(sdmmc_storage_t *storage, u32 type, u8
 		if (buf[13] & 8)
 		{
 			type = 11;
-			hs_type = UHS_SDR104_BUS_SPEED;
+			hs_type = 3;
 			break;
 		}
 		//Fall through.
@@ -791,7 +680,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 = UHS_SDR50_BUS_SPEED;
+		hs_type = 2;
 		break;
 	default:
 		return 0;
@@ -822,130 +711,8 @@ 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;

@@ -959,40 +726,44 @@ int sdmmc_storage_init_sd(sdmmc_storage_t *storage, sdmmc_t *sdmmc, u32 id, u32
 		return 0;
 	DPRINTF("[sd] went to idle state\n");

-	is_version_1 = _sd_storage_send_if_cond(storage);
-	if (is_version_1 == 2)
+	if (!_sd_storage_send_if_cond(storage))
 		return 0;
 	DPRINTF("[sd] after send if cond\n");

-	if (!_sd_storage_get_op_cond(storage, is_version_1, bus_width == SDMMC_BUS_WIDTH_4 && type == 11))
+	//TODO: use correct version here -----v
+	if (!_sd_storage_get_op_cond(storage, 0, bus_width == SDMMC_BUS_WIDTH_4 && (type | 1) == 11))
 		return 0;
 	DPRINTF("[sd] got op cond\n");

-	if (!_sdmmc_storage_get_cid(storage, storage->raw_cid))
+	if (!_sdmmc_storage_get_cid(storage, storage->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->raw_csd))
+	if (!_sdmmc_storage_get_csd(storage, storage->csd))
 		return 0;
 	DPRINTF("[sd] got csd\n");

 	//Parse CSD.
-	_sd_storage_parse_csd(storage);
-	switch (storage->csd.structure)
+	u32 *csd = (u32 *)storage->csd;
+	u32 csd_struct = unstuff_bits(csd, 126, 2);
+	switch (csd_struct)
 	{
 	case 0:
-		storage->sec_cnt = storage->csd.capacity;
+		storage->sec_cnt = (1 + unstuff_bits(csd, 62, 12)) << (unstuff_bits(csd, 47, 3) + 2);
 		break;
 	case 1:
-		storage->sec_cnt = storage->csd.c_size << 10;
+		storage->sec_cnt = (1 + unstuff_bits(csd, 48, 22)) << 10;
 		break;
 	default:
-		DPRINTF("[sd] Unknown CSD structure %d\n", storage->csd.structure);
+		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;
 		break;
 	}

@@ -1019,11 +790,10 @@ 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;
-	//gfx_hexdump(&gfx_con, 0, storage->raw_scr, 8);
+	memcpy(storage->scr, buf, 8);
 	DPRINTF("[sd] got scr\n");

-	// 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 (bus_width == SDMMC_BUS_WIDTH_4 && storage->scr[1] & 4)
 	{
 		if (!_sd_storage_execute_app_cmd_type1(storage, &tmp, SD_APP_SET_BUS_WIDTH, SD_BUS_WIDTH_4, 0, R1_STATE_TRAN))
 		{
@@ -1045,7 +815,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.sda_vsn & 0xF) != 0)
+	else if (type != 6 && storage->scr[0] & 0xF != 0)
 	{
 		if (!_sd_storage_enable_highspeed_high_volt(storage, buf))
 		{
@@ -1057,10 +827,6 @@ 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;
 }
@@ -1077,7 +843,7 @@ int _gc_storage_custom_cmd(sdmmc_storage_t *storage, void *buf)

 	sdmmc_req_t reqbuf;
 	reqbuf.buf = buf;
-	reqbuf.blksize = 64;
+	reqbuf.blksize = 0x40;
 	reqbuf.num_sectors = 1;
 	reqbuf.is_write = 1;
 	reqbuf.is_multi_block = 0;
--- a/ipl/sdmmc.h
+++ b/ipl/sdmmc.h
@@ -20,65 +20,6 @@
 #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
 {
@@ -88,15 +29,9 @@ typedef struct _sdmmc_storage_t
 	u32 sec_cnt;
 	int is_low_voltage;
 	u32 partition;
-	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;
+	u8 cid[0x10];
+	u8 csd[0x10];
+	u8 scr[8];
 } sdmmc_storage_t;

 int sdmmc_storage_end(sdmmc_storage_t *storage);
--- a/ipl/sdmmc_driver.c
+++ b/ipl/sdmmc_driver.c
@@ -54,30 +54,24 @@ 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 = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V1_8;
-		pwr = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V1_8;
+		sdmmc->regs->pwrcon =
+			(sdmmc->regs->pwrcon & TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_MASK) |
+			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;
 	}

-	if (power != SDMMC_POWER_OFF)
-	{
-		pwr |= TEGRA_MMC_PWRCTL_SD_BUS_POWER;
-		sdmmc->regs->pwrcon = pwr;
-	}	
+	sdmmc->regs->pwrcon |= TEGRA_MMC_PWRCTL_SD_BUS_POWER;

 	return 1;
 }
@@ -304,30 +298,10 @@ static int _sdmmc_cache_rsp(sdmmc_t *sdmmc, u32 *rsp, u32 size, u32 type)
 	case SDMMC_RSP_TYPE_2:
 		if (size < 0x10)
 			return 0;
-		// 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;
-		}
+		rsp[0] = sdmmc->regs->rspreg0;
+		rsp[1] = sdmmc->regs->rspreg1;
+		rsp[2] = sdmmc->regs->rspreg2;
+		rsp[3] = sdmmc->regs->rspreg3;
 		break;
 	default:
 		return 0;
--- a/ipl/se.c
+++ b/ipl/se.c
@@ -67,7 +67,7 @@ static int _se_wait()
 	return 1;
 }

-static int _se_execute(u32 op, void *dst, u32 dst_size, const void *src, u32 src_size)
+static int _se_execute(u32 op, void *dst, u32 dst_size, 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, const void *src, u32 src
 	return res;
 }

-static int _se_execute_one_block(u32 op, void *dst, u32 dst_size, const void *src, u32 src_size)
+static int _se_execute_one_block(u32 op, void *dst, u32 dst_size, 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, const void *input)
+int se_aes_unwrap_key(u32 ks_dst, u32 ks_src, 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, const void *input)
 	return _se_execute(OP_START, NULL, 0, input, 0x10);
 }

-int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, const void *src)
+int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, void *src)
 {
 	if (enc)
 	{
@@ -180,7 +180,7 @@ int se_aes_crypt_block_ecb(u32 ks, u32 enc, void *dst, const void *src)
 	return _se_execute(OP_START, dst, 0x10, src, 0x10);
 }

-int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, const void *src, u32 src_size, void *ctr)
+int se_aes_crypt_ctr(u32 ks, void *dst, u32 dst_size, 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);
--- a/ipl/se.h
+++ b/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, 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_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);

 #endif
--- a/ipl/tui.c
+++ b/ipl/tui.c
@@ -68,14 +68,10 @@ void *tui_do_menu(gfx_con_t *con, menu_t *menu)

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