diff --git a/include/arm11/hardware/timer.h b/include/arm11/hardware/timer.h
index f6fa85f..5cc40b1 100644
--- a/include/arm11/hardware/timer.h
+++ b/include/arm11/hardware/timer.h
@@ -29,7 +29,7 @@
 #define TIMER_IRQ_ENABLE   (1u<<2)
 
 // p is the prescaler value and n the frequence
-#define TIMER_FREQ(p, n)   (TIMER_BASE_FREQ / 2 / (p) / (n))
+#define TIMER_FREQ(p, f)  (TIMER_BASE_FREQ / 2 / (p) / (f))
 
 
 
diff --git a/include/arm9/hardware/ndma.h b/include/arm9/hardware/ndma.h
index 6b66dbb..42cba51 100644
--- a/include/arm9/hardware/ndma.h
+++ b/include/arm9/hardware/ndma.h
@@ -127,22 +127,22 @@
 
 enum
 {
-	NDMA_STARTUP_TIMER0    = 0<<24,
-	NDMA_STARTUP_TIMER1    = 1<<24,
-	NDMA_STARTUP_TIMER2    = 2<<24,
-	NDMA_STARTUP_TIMER3    = 3<<24,
-	NDMA_STARTUP_CTRCARD0  = 4<<24,  // Fires with SPICARD aswell but seems to be broken.
-	NDMA_STARTUP_CTRCARD1  = 5<<24,  // Fires with SPICARD aswell but seems to be broken.
-	NDMA_STARTUP_MMC1      = 6<<24,
-	NDMA_STARTUP_MMC2      = 7<<24,  // Guess based on DSi documentation
-	NDMA_STARTUP_AES_IN    = 8<<24,  // AES write fifo
-	NDMA_STARTUP_AES_OUT   = 9<<24,  // AES read fifo
-	NDMA_STARTUP_SHA       = 10<<24,
-	NDMA_STARTUP_UNK_11    = 11<<24,
-	NDMA_STARTUP_UNK_12    = 12<<24,
-	NDMA_STARTUP_UNK_13    = 13<<24,
-	NDMA_STARTUP_UNK_14    = 14<<24,
-	NDMA_STARTUP_AES_SHA   = 15<<24  // Unconfirmed
+	NDMA_STARTUP_TIMER0       =  0u<<24,
+	NDMA_STARTUP_TIMER1       =  1u<<24,
+	NDMA_STARTUP_TIMER2       =  2u<<24,
+	NDMA_STARTUP_TIMER3       =  3u<<24,
+	NDMA_STARTUP_CTRCARD0     =  4u<<24, // Fires with SPICARD aswell but seems to be broken.
+	NDMA_STARTUP_CTRCARD1     =  5u<<24, // Fires with SPICARD aswell but seems to be broken.
+	NDMA_STARTUP_MMC1         =  6u<<24,
+	NDMA_STARTUP_MMC2         =  7u<<24, // Guess based on DSi documentation
+	NDMA_STARTUP_AES_IN       =  8u<<24, // AES write fifo
+	NDMA_STARTUP_AES_OUT      =  9u<<24, // AES read fifo
+	NDMA_STARTUP_SHA_IN       = 10u<<24,
+	NDMA_STARTUP_SHA_OUT      = 11u<<24, // For chaining
+	NDMA_STARTUP_UNK_12       = 12u<<24,
+	NDMA_STARTUP_UNK_13       = 13u<<24,
+	NDMA_STARTUP_UNK_14       = 14u<<24,
+	NDMA_STARTUP_MMC_AES_SHA  = 15u<<24  // Unconfirmed
 };
 
 
diff --git a/source/arm11/hardware/i2c.c b/source/arm11/hardware/i2c.c
index f548cb8..fb491bf 100644
--- a/source/arm11/hardware/i2c.c
+++ b/source/arm11/hardware/i2c.c
@@ -91,19 +91,19 @@
 	return base;
 }
 
-static inline void i2cWaitBusyIrq(const I2cRegs *const regs)
+static inline void i2cWaitBusyIrq(const vu8 *const I2C_CNT)
 {
 	do
 	{
 		__wfi();
-	} while(regs->I2C_CNT & I2C_ENABLE);
+	} while(*I2C_CNT & I2C_ENABLE);
 }
 
-static bool i2cCheckAck(I2cRegs *const regs)
+static bool i2cCheckAck(vu8 *const I2C_CNT)
 {
-	if(!(regs->I2C_CNT & I2C_ACK)) // If ack flag is 0 it failed.
+	if(!(*I2C_CNT & I2C_ACK)) // If ack flag is 0 it failed.
 	{
-		regs->I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_ERROR | I2C_STOP;
+		*I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_ERROR | I2C_STOP;
 		return false;
 	}
 
@@ -149,22 +149,22 @@
 		// Select device and start.
 		regs->I2C_DATA = devAddr;
 		regs->I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_DIRE_WRITE | I2C_START;
-		i2cWaitBusyIrq(regs);
-		if(!i2cCheckAck(regs)) continue;
+		i2cWaitBusyIrq(&regs->I2C_CNT);
+		if(!i2cCheckAck(&regs->I2C_CNT)) continue;
 
 		// Select register.
 		regs->I2C_DATA = regAddr;
 		regs->I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_DIRE_WRITE;
-		i2cWaitBusyIrq(regs);
-		if(!i2cCheckAck(regs)) continue;
+		i2cWaitBusyIrq(&regs->I2C_CNT);
+		if(!i2cCheckAck(&regs->I2C_CNT)) continue;
 
 		// Select device in read mode for read transfer.
 		if(read)
 		{
 			regs->I2C_DATA = devAddr | 1u; // Set bit 0 for read.
 			regs->I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_DIRE_WRITE | I2C_START;
-			i2cWaitBusyIrq(regs);
-			if(!i2cCheckAck(regs)) continue;
+			i2cWaitBusyIrq(&regs->I2C_CNT);
+			if(!i2cCheckAck(&regs->I2C_CNT)) continue;
 		}
 
 		break;
@@ -184,13 +184,13 @@
 	while(--size)
 	{
 		regs->I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_DIRE_READ | I2C_ACK;
-		i2cWaitBusyIrq(regs);
+		i2cWaitBusyIrq(&regs->I2C_CNT);
 		*out++ = regs->I2C_DATA;
 	}
 
 	// Last byte transfer.
 	regs->I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_DIRE_READ | I2C_STOP;
-	i2cWaitBusyIrq(regs);
+	i2cWaitBusyIrq(&regs->I2C_CNT);
 	*out = regs->I2C_DATA;
 
 	return true;
@@ -208,15 +208,15 @@
 	{
 		regs->I2C_DATA = *in++;
 		regs->I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_DIRE_WRITE;
-		i2cWaitBusyIrq(regs);
-		if(!i2cCheckAck(regs)) return false;
+		i2cWaitBusyIrq(&regs->I2C_CNT);
+		if(!i2cCheckAck(&regs->I2C_CNT)) return false;
 	}
 
 	// Last byte transfer.
 	regs->I2C_DATA = *in;
 	regs->I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_DIRE_WRITE | I2C_STOP;
-	i2cWaitBusyIrq(regs);
-	if(!i2cCheckAck(regs)) return false;
+	i2cWaitBusyIrq(&regs->I2C_CNT);
+	if(!i2cCheckAck(&regs->I2C_CNT)) return false;
 
 	return true;
 }
@@ -248,13 +248,13 @@
 		regs->I2C_DATA = devAddr;
 		regs->I2C_CNT = I2C_ENABLE | I2C_DIRE_WRITE | I2C_START;
 		while(regs->I2C_CNT & I2C_ENABLE);
-		if(!i2cCheckAck(regs)) continue;
+		if(!i2cCheckAck(&regs->I2C_CNT)) continue;
 
 		// Select register.
 		regs->I2C_DATA = regAddr;
 		regs->I2C_CNT = I2C_ENABLE | I2C_DIRE_WRITE;
 		while(regs->I2C_CNT & I2C_ENABLE);
-		if(!i2cCheckAck(regs)) continue;
+		if(!i2cCheckAck(&regs->I2C_CNT)) continue;
 
 		break;
 	} while(--tries > 0);
@@ -264,7 +264,7 @@
 	regs->I2C_DATA = data;
 	regs->I2C_CNT = I2C_ENABLE | I2C_DIRE_WRITE | I2C_STOP;
 	while(regs->I2C_CNT & I2C_ENABLE);
-	if(!i2cCheckAck(regs)) return false;
+	if(!i2cCheckAck(&regs->I2C_CNT)) return false;
 
 	return true;
 }
diff --git a/source/arm11/hardware/mmu.c b/source/arm11/hardware/mmu.c
index 0223832..5938972 100644
--- a/source/arm11/hardware/mmu.c
+++ b/source/arm11/hardware/mmu.c
@@ -57,6 +57,16 @@
 #define L1_TO_L2(attr)                       (((attr)>>6 | (attr)) & 0x73)
 
 
+typedef struct
+{
+	u32 l1[4096];
+	u32 l2PrivReg[256]; // L2 table for MPCore private region
+	u32 l2Axiwram[256]; // L2 table for AXIWRAM
+	u32 l2Boot11[256];  // L2 table for boot11 (high vectors)
+} MmuTables;
+static MmuTables *const mmuTables = (MmuTables*)A11_MMU_TABLES_BASE;
+
+
 
 /**
  * @brief      Maps up to 256 16 MiB sections of memory. Domain is always 0.
@@ -74,13 +84,13 @@
 	fb_assert(!(pa & ~0xFF000000));
 	fb_assert(num < 256);
 
+	u32 *const l1Table = mmuTables->l1;
 	for(u32 i = 0; i < 0x1000000 * num; i += 0x1000000)
 	{
 		const u32 l1Ss = (va + i)>>20;
 		for(u32 n = 0; n < 16; n++)
 		{
-			((u32*)A11_MMU_TABLES_BASE)[l1Ss + n] = (pa + i) | 1u<<18 | access<<10 |
-			                                        xn<<4 | attr<<2 | 0b10u;
+			l1Table[l1Ss + n] = (pa + i) | 1u<<18 | access<<10 | xn<<4 | attr<<2 | 0b10u;
 		}
 	}
 }
@@ -103,10 +113,11 @@
 	fb_assert(!(pa & ~0xFFF00000));
 	fb_assert(num < 4096);
 
+	u32 *const l1Table = mmuTables->l1;
 	for(u32 i = 0; i < 0x100000 * num; i += 0x100000)
 	{
-		((u32*)A11_MMU_TABLES_BASE)[(va + i)>>20] = (pa + i) | shared<<16 | access<<10 |
-		                                            domain<<5 | xn<<4 | attr<<2 | 0b10u;
+		l1Table[(va + i)>>20] = (pa + i) | shared<<16 | access<<10 |
+		                        domain<<5 | xn<<4 | attr<<2 | 0b10u;
 	}
 }
 
@@ -116,7 +127,7 @@
  *
  * @param[in]  va       The virtual address base. Must be aligned to 4 KiB.
  * @param[in]  pa       The physical address base. Must be aligned to 4 KiB.
- * @param[in]  num      The number of pages to map. Must be <= 128.
+ * @param[in]  num      The number of pages to map.
  * @param      l2Table  The L2 MMU table address base for this mapping.
  * @param[in]  shared   If the pages are shared memory.
  * @param[in]  access   The access permission bits.
@@ -124,19 +135,19 @@
  * @param[in]  xn       If this memory should be marked as execute never.
  * @param[in]  attr     Other attribute bits like caching.
  */
-static void mmuMapPages(u32 va, u32 pa, u32 num, u32 *l2Table, bool shared, u8 access, u8 domain, bool xn, u32 attr)
+static void mmuMapPages(u32 va, u32 pa, u32 num, u32 *const l2Table, bool shared, u8 access, u8 domain, bool xn, u32 attr)
 {
 	fb_assert(!(va & ~0xFFFFF000));
 	fb_assert(!(pa & ~0xFFFFF000));
 	fb_assert(num < 256);
 	fb_assert(!((u32)l2Table & ~0xFFFFFC00));
 
-	((u32*)A11_MMU_TABLES_BASE)[va>>20] = (u32)l2Table | domain<<5 | 0b01u;
-
 	for(u32 i = 0; i < 0x1000 * num; i += 0x1000)
 	{
 		l2Table[(va + i)>>12 & 0xFF] = ((pa + i) & 0xFFFFF000) | shared<<10 | access<<4 | attr<<2 | 0b10u | xn;
 	}
+
+	mmuTables->l1[va>>20] = (u32)l2Table | domain<<5 | 0b01u;
 }
 
 void setupMmu(void)
@@ -158,7 +169,7 @@
 	if(!__getCpuId())
 	{
 		// Clear L1 and L2 tables
-		iomemset((u32*)A11_MMU_TABLES_BASE, 0, 0x4C00);
+		iomemset((u32*)mmuTables, 0, sizeof(MmuTables));
 
 		// IO mem mapping
 		mmuMapSections(IO_MEM_ARM9_ARM11, IO_MEM_ARM9_ARM11, 4, true,
@@ -166,18 +177,18 @@
 
 		// MPCore private region mapping
 		mmuMapPages(MPCORE_PRIV_REG_BASE, MPCORE_PRIV_REG_BASE, 2,
-		            (u32*)(A11_MMU_TABLES_BASE + 0x4000), false, PERM_PRIV_RW_USR_NA,
+		            mmuTables->l2PrivReg, false, PERM_PRIV_RW_USR_NA,
 		            0, true, L1_TO_L2(ATTR_SHARED_DEVICE));
 
 		// VRAM mapping
 		mmuMapSections(VRAM_BASE, VRAM_BASE, 6, true, PERM_PRIV_RW_USR_NA, 0, true, ATTR_NORM_WRITE_TROUGH_NO_ALLOC);
 
 		// AXIWRAM core 0/1 stack mapping
-		mmuMapPages(A11_C0_STACK_START, A11_C0_STACK_START, 4, (u32*)(A11_MMU_TABLES_BASE + 0x4400),
+		mmuMapPages(A11_C0_STACK_START, A11_C0_STACK_START, 4, mmuTables->l2Axiwram,
 		            true, PERM_PRIV_RW_USR_NA, 0, true, L1_TO_L2(ATTR_NORM_WRITE_BACK_ALLOC));
 
 		// AXIWRAM MMU table mapping
-		mmuMapPages(A11_MMU_TABLES_BASE, A11_MMU_TABLES_BASE, 5, (u32*)(A11_MMU_TABLES_BASE + 0x4400), true,
+		mmuMapPages(A11_MMU_TABLES_BASE, A11_MMU_TABLES_BASE, 5, mmuTables->l2Axiwram, true,
 		            PERM_PRIV_RO_USR_NA, 0, true, L1_TO_L2(ATTR_NORM_NONCACHABLE));
 
 		extern const u32 __start__[];
@@ -189,15 +200,15 @@
 
 		// text
 		mmuMapPages((u32)__start__, (u32)__start__, (u32)__text_pages__,
-		            (u32*)(A11_MMU_TABLES_BASE + 0x4400), true, PERM_PRIV_RO_USR_NA, 0, false,
+		            mmuTables->l2Axiwram, true, PERM_PRIV_RO_USR_NA, 0, false,
 		            L1_TO_L2(ATTR_NORM_WRITE_BACK_ALLOC));
 		// rodata
 		mmuMapPages((u32)__rodata_start__, (u32)__rodata_start__, (u32)__rodata_pages__,
-		            (u32*)(A11_MMU_TABLES_BASE + 0x4400), true, PERM_PRIV_RO_USR_NA, 0, true,
+		            mmuTables->l2Axiwram, true, PERM_PRIV_RO_USR_NA, 0, true,
 		            L1_TO_L2(ATTR_NORM_WRITE_BACK_ALLOC));
 		// data, bss and heap
 		mmuMapPages((u32)__data_start__, (u32)__data_start__, dataPages,
-		            (u32*)(A11_MMU_TABLES_BASE + 0x4400u), true, PERM_PRIV_RW_USR_NA, 0, true,
+		            mmuTables->l2Axiwram, true, PERM_PRIV_RW_USR_NA, 0, true,
 		            L1_TO_L2(ATTR_NORM_WRITE_BACK_ALLOC));
 
 		// FCRAM with New 3DS extension
@@ -205,7 +216,7 @@
 		//                    ATTR_NORM_WRITE_BACK_ALLOC);
 
 		// Map fastboot executable start to boot11 mirror (exception vectors)
-		mmuMapPages(BOOT11_MIRROR2, (u32)__start__, 1, (u32*)(A11_MMU_TABLES_BASE + 0x4800u), true,
+		mmuMapPages(BOOT11_MIRROR2, (u32)__start__, 1, mmuTables->l2Boot11, true,
 		            PERM_PRIV_RO_USR_NA, 0, false, L1_TO_L2(ATTR_NORM_WRITE_BACK_ALLOC));
 
 		// Invalidate tag RAMs before enabling SMP as recommended by the MPCore doc.
diff --git a/source/arm11/hardware/spi.c b/source/arm11/hardware/spi.c
index b83c2b1..73676e5 100644
--- a/source/arm11/hardware/spi.c
+++ b/source/arm11/hardware/spi.c
@@ -85,14 +85,14 @@
 	return base;
 }
 
-static void nspiWaitBusy(const SpiRegs *const regs)
+static void nspiWaitBusy(const vu32 *const NSPI_CNT)
 {
-	while(regs->NSPI_CNT & NSPI_CNT_ENABLE);
+	while(*NSPI_CNT & NSPI_CNT_ENABLE);
 }
 
-static void nspiWaitFifoBusy(const SpiRegs *const regs)
+static void nspiWaitFifoBusy(const vu32 *const NSPI_STATUS)
 {
-	while(regs->NSPI_STATUS & NSPI_STATUS_BUSY);
+	while(*NSPI_STATUS & NSPI_STATUS_BUSY);
 }
 
 void NSPI_init(void)
@@ -151,12 +151,12 @@
 		u32 counter = 0;
 		do
 		{
-			if((counter & 31) == 0) nspiWaitFifoBusy(regs);
+			if((counter & 31) == 0) nspiWaitFifoBusy(&regs->NSPI_STATUS);
 			regs->NSPI_FIFO = *in++;
 			counter += 4;
 		} while(counter < inSize);
 
-		nspiWaitBusy(regs);
+		nspiWaitBusy(&regs->NSPI_CNT);
 	}
 	if(out)
 	{
@@ -166,12 +166,12 @@
 		u32 counter = 0;
 		do
 		{
-			if((counter & 31) == 0) nspiWaitFifoBusy(regs);
+			if((counter & 31) == 0) nspiWaitFifoBusy(&regs->NSPI_STATUS);
 			*out++ = regs->NSPI_FIFO;
 			counter += 4;
 		} while(counter < outSize);
 
-		nspiWaitBusy(regs);
+		nspiWaitBusy(&regs->NSPI_CNT);
 	}
 
 	if(done) regs->NSPI_DONE = NSPI_DONE_DONE;
diff --git a/source/arm11/hardware/timer.c b/source/arm11/hardware/timer.c
index 86967ef..9823e73 100644
--- a/source/arm11/hardware/timer.c
+++ b/source/arm11/hardware/timer.c
@@ -45,7 +45,7 @@
 	fb_assert(prescaler > 0);
 
 	REG_TIMER_LOAD = ticks;
-	REG_TIMER_CNT = (prescaler - 1)<<8 | (enableIrq ? TIMER_IRQ_ENABLE : 0) |
+	REG_TIMER_CNT = (prescaler - 1u)<<8 | (enableIrq ? TIMER_IRQ_ENABLE : 0u) |
 	                (autoReload ? TIMER_AUTO_RELOAD : TIMER_SINGLE_SHOT) | TIMER_ENABLE;
 }
 
diff --git a/source/arm9/hardware/crypto.c b/source/arm9/hardware/crypto.c
index 7a37c46..1420233 100644
--- a/source/arm9/hardware/crypto.c
+++ b/source/arm9/hardware/crypto.c
@@ -169,17 +169,13 @@
 void AES_init(void)
 {
 	REG_AESCNT = AES_MAC_SIZE(4) | AES_FLUSH_WRITE_FIFO | AES_FLUSH_READ_FIFO;
-	*((vu8*)0x10000008) |= 0xCu; // ??
+	*((vu8*)0x10000008) = 0; // ??
 
 	REG_NDMA0_DST_ADDR = REG_AESWRFIFO;
 	REG_NDMA0_INT_CNT = NDMA_INT_SYS_FREQ;
-	REG_NDMA0_CNT = NDMA_TOTAL_CNT_MODE | NDMA_STARTUP_AES_IN | NDMA_BURST_WORDS(4) |
-	                NDMA_SRC_UPDATE_INC | NDMA_DST_UPDATE_FIXED;
 
 	REG_NDMA1_SRC_ADDR = REG_AESRDFIFO;
 	REG_NDMA1_INT_CNT = NDMA_INT_SYS_FREQ;
-	REG_NDMA1_CNT = NDMA_TOTAL_CNT_MODE | NDMA_STARTUP_AES_OUT | NDMA_BURST_WORDS(4) |
-	                NDMA_SRC_UPDATE_FIXED | NDMA_DST_UPDATE_INC;
 
 	IRQ_registerHandler(IRQ_AES, NULL);
 
@@ -195,31 +191,20 @@
 	REG_AESCNT = (u32)orderEndianess<<23;
 	if(keyslot > 3)
 	{
-		REG_AESKEYCNT = 0x80u | (type > AES_KEY_NORMAL && twlScrambler ? 1u : 0u)<<6 | keyslot;
-		REG_AESKEYFIFO[type] = key[0];
-		REG_AESKEYFIFO[type] = key[1];
-		REG_AESKEYFIFO[type] = key[2];
-		REG_AESKEYFIFO[type] = key[3];
+		REG_AESKEYCNT = 1u<<7 | (type > AES_KEY_NORMAL && twlScrambler ? 1u : 0u)<<6 | keyslot;
+		for(u32 i = 0; i < 4; i++) REG_AESKEYFIFO[type] = key[i];
 	}
 	else
 	{
-		u32 lastu32;
 		vu32 *twlKeyNReg = &REG_AESKEY0[12u * keyslot + type * 4u];
 		if(orderEndianess & AES_INPUT_NORMAL)
 		{
-			twlKeyNReg[0] = key[3];
-			twlKeyNReg[1] = key[2];
-			twlKeyNReg[2] = key[1];
-			lastu32 = key[0];
+			for(u32 i = 0; i < 4; i++) twlKeyNReg[i] = key[3u - i];
 		}
 		else
 		{
-			twlKeyNReg[0] = key[0];
-			twlKeyNReg[1] = key[1];
-			twlKeyNReg[2] = key[2];
-			lastu32 = key[3];
+			for(u32 i = 0; i < 4; i++) twlKeyNReg[i] = key[i];
 		}
-		twlKeyNReg[3] = lastu32;
 	}
 }
 
@@ -238,21 +223,15 @@
 
 
 	ctx->ctrIvNonceParams = (u32)orderEndianess<<23;
-	u32 *const ctrIvNonce = ctx->ctrIvNonce;
-	u32 lastu32;
+	u32 *const ctxNonce = ctx->ctrIvNonce;
 	if(orderEndianess & AES_INPUT_NORMAL)
 	{
-		ctrIvNonce[0] = nonce[2];
-		ctrIvNonce[1] = nonce[1];
-		lastu32 = nonce[0];
+		for(u32 i = 0; i < 3; i++) ctxNonce[i] = nonce[2u - i];
 	}
 	else
 	{
-		ctrIvNonce[0] = nonce[0];
-		ctrIvNonce[1] = nonce[1];
-		lastu32 = nonce[2];
+		for(u32 i = 0; i < 3; i++) ctxNonce[i] = nonce[i];
 	}
-	ctrIvNonce[2] = lastu32;
 }
 
 void AES_setCtrIv(AES_ctx *const ctx, u8 orderEndianess, const u32 ctrIv[4])
@@ -262,38 +241,29 @@
 
 
 	ctx->ctrIvNonceParams = (u32)orderEndianess<<23;
-	u32 *const ctrIvNonce = ctx->ctrIvNonce;
-	u32 lastu32;
+	u32 *const ctxCtrIv = ctx->ctrIvNonce;
 	if(orderEndianess & AES_INPUT_NORMAL)
 	{
-		ctrIvNonce[0] = ctrIv[3];
-		ctrIvNonce[1] = ctrIv[2];
-		ctrIvNonce[2] = ctrIv[1];
-		lastu32 = ctrIv[0];
+		for(u32 i = 0; i < 4; i++) ctxCtrIv[i] = ctrIv[3u - i];
 	}
 	else
 	{
-		ctrIvNonce[0] = ctrIv[0];
-		ctrIvNonce[1] = ctrIv[1];
-		ctrIvNonce[2] = ctrIv[2];
-		lastu32 = ctrIv[3];
+		for(u32 i = 0; i < 4; i++) ctxCtrIv[i] = ctrIv[i];
 	}
-	ctrIvNonce[3] = lastu32;
 }
 
-// TODO: Handle endianess!
 NAKED void AES_addCounter(u32 ctr[4], u32 val)
 {
 	__asm__
 	(
-		"stmfd sp!, {r4, lr}\n\t"
-		"ldm r0, {r2-r4, lr}\n\t"
+		"str lr, [sp, #-4]!\n\t"
+		"ldm r0, {r2, r3, r12, lr}\n\t"
 		"adds r2, r1, lsr #4\n\t"
 		"addcss r3, r3, #1\n\t"
-		"addcss r4, r4, #1\n\t"
+		"addcss r12, r12, #1\n\t"
 		"addcs lr, lr, #1\n\t"
-		"stm r0, {r2-r4, lr}\n\t"
-		"ldmfd sp!, {r4, pc}\n\t"
+		"stm r0, {r2, r3, r12, lr}\n\t"
+		"ldr pc, [sp], #4\n\t"
 		: : "r" (ctr), "r" (val) :
 	);
 }
@@ -337,19 +307,19 @@
 
 
 	// Check block alignment
-	u32 aesFifoSize;
-	if(!(blocks & 1)) aesFifoSize = 1; // 32 bytes
-	else              aesFifoSize = 0; // 16 bytes
+	const u8 aesFifoSize = (blocks & 1u ? 0u : 1u); // 1 = 32 bytes, 0 = 16 bytes
 
 	REG_NDMA0_SRC_ADDR = (u32)in;
 	REG_NDMA0_TOTAL_CNT = blocks<<2;
 	REG_NDMA0_LOG_BLK_CNT = aesFifoSize * 4 + 4;
-	REG_NDMA0_CNT |= NDMA_ENABLE;
+	REG_NDMA0_CNT = NDMA_ENABLE | NDMA_TOTAL_CNT_MODE | NDMA_STARTUP_AES_IN |
+	                NDMA_BURST_WORDS(4) | NDMA_SRC_UPDATE_INC | NDMA_DST_UPDATE_FIXED;
 
 	REG_NDMA1_DST_ADDR = (u32)out;
 	REG_NDMA1_TOTAL_CNT = blocks<<2;
 	REG_NDMA1_LOG_BLK_CNT = aesFifoSize * 4 + 4;
-	REG_NDMA1_CNT |= NDMA_ENABLE;
+	REG_NDMA1_CNT = NDMA_ENABLE | NDMA_TOTAL_CNT_MODE | NDMA_STARTUP_AES_OUT |
+	                NDMA_BURST_WORDS(4) | NDMA_SRC_UPDATE_FIXED | NDMA_DST_UPDATE_INC;
 
 	REG_AES_BLKCNT_HIGH = blocks;
 	REG_AESCNT |= AES_ENABLE | AES_IRQ_ENABLE | aesFifoSize<<14 | (3 - aesFifoSize)<<12 |
@@ -374,10 +344,7 @@
 	while(blocks)
 	{
 		REG_AESCNT = ctrParams;
-		REG_AESCTR[0] = ctr[0];
-		REG_AESCTR[1] = ctr[1];
-		REG_AESCTR[2] = ctr[2];
-		REG_AESCTR[3] = ctr[3];
+		for(u32 i = 0; i < 4; i++) REG_AESCTR[i] = ctr[i];
 
 		REG_AESCNT = aesParams;
 		u32 blockNum = ((blocks > AES_MAX_BLOCKS) ? AES_MAX_BLOCKS : blocks);
@@ -496,9 +463,7 @@
 
 
 	REG_AESCNT = ctx->ctrIvNonceParams;
-	REG_AESCTR[0] = ctx->ctrIvNonce[0];
-	REG_AESCTR[1] = ctx->ctrIvNonce[1];
-	REG_AESCTR[2] = ctx->ctrIvNonce[2];
+	for(u32 i = 0; i < 3; i++) REG_AESCTR[i] = ctx->ctrIvNonce[i];
 
 	REG_AES_BLKCNT_LOW = 0;
 	REG_AESCNT = (enc ? AES_MODE_CCM_ENCRYPT : AES_MODE_CCM_DECRYPT) |
@@ -508,18 +473,12 @@
 	// This is broken right now with DMA due to a (AES engine?) bug.
 	if(!enc)
 	{
-		*((vu32*)REG_AESWRFIFO) = mac[0];
-		*((vu32*)REG_AESWRFIFO) = mac[1];
-		*((vu32*)REG_AESWRFIFO) = mac[2];
-		*((vu32*)REG_AESWRFIFO) = mac[3];
+		for(u32 i = 0; i < 4; i++) *((vu32*)REG_AESWRFIFO) = mac[i];
 		while(REG_AESCNT & AES_ENABLE);
 	}
 	else
 	{
-		mac[0] = *((vu32*)REG_AESRDFIFO);
-		mac[1] = *((vu32*)REG_AESRDFIFO);
-		mac[2] = *((vu32*)REG_AESRDFIFO);
-		mac[3] = *((vu32*)REG_AESRDFIFO);
+		for(u32 i = 0; i < 4; i++) mac[i] = *((vu32*)REG_AESRDFIFO);
 	}
 
 	if(enc) return true;