diff --git a/README.md b/README.md
index 05c9481..4694130 100644
--- a/README.md
+++ b/README.md
@@ -2,7 +2,7 @@
 Unfortunately SDFormatter isn't available for Linux so i made this. It is a tool to format your SD card the way the SD Association intended but under Linux.
 
 Multiple SD cards with different capacities have been tested and this tool formats them 1:1 the same as SDFormatter with the following exceptions:
-* SDFormatter sets end head in the MBR to 254 for a card with 31074304 sectors (16 GB) which is a bug i believe. sdFormatLinux uses 255.
+* SDFormatter does not set the jmp instruction offset in the boot sector. sdFormatLinux does.
 * sdFormatLinux does not support exFAT right now.
 
 ## Examples
diff --git a/source/fat.cpp b/source/fat.cpp
index 38ad5bd..5d271b6 100644
--- a/source/fat.cpp
+++ b/source/fat.cpp
@@ -139,13 +139,14 @@ int makeFsFat(const FormatParams &params, BufferedFsWriter &dev, const std::stri
 
 	// Boot sector.
 	BootSec bs{};
-	memcpy(bs.jmpBoot, "\xEB\x00\x90", 3);      // Doesn't jump to bootCode but it's what SDFormatter uses.
+	const u8 fatBits = params.fatBits;
+	const u8 jmpBoot[3] = {0xEB, (u8)(fatBits < 32 ? 0x3C : 0x58), 0x90}; // Note: SDFormatter hardcodes 0xEB 0x00 0x90.
+	memcpy(bs.jmpBoot, jmpBoot, 3);
 	memcpy(bs.oemName, BS_DEFAULT_OEM_NAME, 8);
 
 	// BIOS Parameter Block (BPB).
 	const u32 secPerClus  = params.secPerClus;
 	const u32 rsvdSecCnt  = params.rsvdSecCnt;
-	const u8  fatBits     = params.fatBits;
 	const u32 partSectors = static_cast<u32>(params.totSec - partStart);
 	const u32 secPerFat   = params.secPerFat;
 	bs.bytesPerSec = bytesPerSec;
diff --git a/source/mbr.cpp b/source/mbr.cpp
index 158e668..7ab5bf2 100644
--- a/source/mbr.cpp
+++ b/source/mbr.cpp
@@ -11,28 +11,23 @@
 
 
 // Converts LBA to MBR CHS format.
-// TODO: SDFormatter uses end head 254 for a 16 GB card. Bug?
 static u32 lba2chs(u64 lba, const u32 heads, const u32 secPerTrk)
 {
-	// We can't encode this even with 255 heads and 63 sectors per track.
-	// Returning the maximum in this case is ok.
-	if(lba >= 16450560)
-		return 0x00FFFFFF; // Cylinder 1023, head 255, sector 63.
-
 	const u32 spc = heads * secPerTrk;
-	const u32 c = lba / spc;
+	u32 c = lba / spc;
 	lba %= spc;
-	const u32 h = lba / secPerTrk;
-	const u32 s = lba % secPerTrk + 1;
+	u32 h = lba / secPerTrk;
+	u32 s = lba % secPerTrk + 1;
 
-	// Return the maximum if we can't encode this
-	// and also set the uppermost byte to maximum to indicate an error.
-	if(c > 1023 || h > 255 || s > 63)
-		return 0xFFFFFFFF;
+	// Return the maximum if we can't encode this.
+	if(c >= 1024 || h >= heads || s > secPerTrk)
+	{
+		c = 1023;
+		h = 254;
+		s = 63;
+	}
 
-	//printf("lba2chs(%" PRIu64 ", %" PRIu32 ", %" PRIu32 "): c: %" PRIu32 ", h: %" PRIu32 ", s: %" PRIu32 "\n",
-	//       lba, heads, secPerTrk, c, h, s);
-	return (c & 0xFF)<<16 | (c & 0x300)<<6 | s<<8 | h;
+	return (c & 0xFFu)<<16 | (c & 0x300u)<<6 | s<<8 | h;
 }
 
 // TODO: Rewrite this function to use partSize to determine the fs type.
@@ -62,18 +57,18 @@ int createMbrAndPartition(const FormatParams &params, BufferedFsWriter &dev)
 	const u64 totSec   = params.totSec * (bytesPerSec>>9); // Convert back to physical sectors.
 	const u64 partSize = totSec - partStart; // TODO: Is this correct or should we align the end?
 	u8 type;
-	if(fatBits == 12)               type = 0x01; // FAT12 (16/32 MiB).
+	if(fatBits == 12)          type = 0x01; // FAT12 (16/32 MiB).
 	else if(fatBits == 16)
 	{
-		if(partSize < 65536)        type = 0x04; // FAT16 (<32 MiB). TODO: Is this actually "<" or "<="?
-		else                        type = 0x06; // FAT16 (>=32 MiB).
+		if(partSize < 65536)   type = 0x04; // FAT16 (<32 MiB).
+		else                   type = 0x06; // FAT16 (>=32 MiB).
 	}
 	else if(fatBits == 32)
 	{
-		if((totSec - 1) < 16450560) type = 0x0B; // FAT32 CHS.
-		else                        type = 0x0C; // FAT32 LBA.
+		if(totSec <= 16450560) type = 0x0B; // FAT32 CHS.
+		else                   type = 0x0C; // FAT32 LBA.
 	}
-	else                            type = 0x07; // exFAT.
+	else                       type = 0x07; // exFAT.
 	entry.type = type;
 	verbosePrintf("Partition type: 0x%02" PRIX8 "\n", type);