diff --git a/bootloader/main.c b/bootloader/main.c
index 5f3f10c..c2fbd2b 100644
--- a/bootloader/main.c
+++ b/bootloader/main.c
@@ -1155,8 +1155,10 @@ void ipl_main()
 	//Tegra/Horizon configuration goes to 0x80000000+, package2 goes to 0xA9800000, we place our heap in between.
 	heap_init(0x90020000);
 
-	//uart_send(UART_C, (u8 *)0x40000000, 0x10000);
-	//uart_wait_idle(UART_C, UART_TX_IDLE);
+#ifdef DEBUG_UART_PORT
+	//uart_send(DEBUG_UART_PORT, (u8 *)0x40000000, 0x10000);
+	//uart_wait_idle(DEBUG_UART_PORT, UART_TX_IDLE);
+#endif
 
 	// Set bootloader's default configuration.
 	set_default_configuration();
diff --git a/bootloader/soc/hw_init.c b/bootloader/soc/hw_init.c
index 269a194..21b82a9 100644
--- a/bootloader/soc/hw_init.c
+++ b/bootloader/soc/hw_init.c
@@ -64,8 +64,12 @@ void _config_gpios()
 	PINMUX_AUX(PINMUX_AUX_GPIO_PE6) = PINMUX_INPUT_ENABLE;
 	PINMUX_AUX(PINMUX_AUX_GPIO_PH6) = PINMUX_INPUT_ENABLE;
 
+#if !defined (DEBUG_UART_PORT) || DEBUG_UART_PORT != UART_B
 	gpio_config(GPIO_PORT_G, GPIO_PIN_0, GPIO_MODE_GPIO);
+#endif
+#if !defined (DEBUG_UART_PORT) || DEBUG_UART_PORT != UART_C
 	gpio_config(GPIO_PORT_D, GPIO_PIN_1, GPIO_MODE_GPIO);
+#endif
 	gpio_config(GPIO_PORT_E, GPIO_PIN_6, GPIO_MODE_GPIO);
 	gpio_config(GPIO_PORT_H, GPIO_PIN_6, GPIO_MODE_GPIO);
 	gpio_output_enable(GPIO_PORT_G, GPIO_PIN_0, GPIO_OUTPUT_DISABLE);
@@ -187,8 +191,10 @@ void config_hw()
 	APB_MISC(APB_MISC_PP_PINMUX_GLOBAL) = 0;
 	_config_gpios();
 
-	//clock_enable_uart(UART_C);
-	//uart_init(UART_C, 115200);
+#ifdef DEBUG_UART_PORT
+	clock_enable_uart(DEBUG_UART_PORT);
+	uart_init(DEBUG_UART_PORT, 115200);
+#endif
 
 	clock_enable_cl_dvfs();
 
@@ -255,4 +261,4 @@ void reconfig_hw_workaround(bool extra_reconfig, u32 magic)
 
 		msleep(200);
 	}
-}
\ No newline at end of file
+}
diff --git a/bootloader/soc/pinmux.c b/bootloader/soc/pinmux.c
index a4f8b19..582ab29 100644
--- a/bootloader/soc/pinmux.c
+++ b/bootloader/soc/pinmux.c
@@ -19,8 +19,8 @@
 
 void pinmux_config_uart(u32 idx)
 {
-	PINMUX_AUX(PINMUX_AUX_UARTX_RX(idx)) = 0;
-	PINMUX_AUX(PINMUX_AUX_UARTX_TX(idx)) = PINMUX_INPUT_ENABLE | PINMUX_PULL_UP;
+	PINMUX_AUX(PINMUX_AUX_UARTX_TX(idx)) = 0;
+	PINMUX_AUX(PINMUX_AUX_UARTX_RX(idx)) = PINMUX_INPUT_ENABLE | PINMUX_PULL_UP;
 	PINMUX_AUX(PINMUX_AUX_UARTX_RTS(idx)) = 0;
 	PINMUX_AUX(PINMUX_AUX_UARTX_CTS(idx)) = PINMUX_INPUT_ENABLE | PINMUX_PULL_DOWN;
 }
diff --git a/bootloader/soc/uart.c b/bootloader/soc/uart.c
index 563fd2f..056f864 100644
--- a/bootloader/soc/uart.c
+++ b/bootloader/soc/uart.c
@@ -25,32 +25,37 @@ void uart_init(u32 idx, u32 baud)
 {
 	uart_t *uart = (uart_t *)(UART_BASE + uart_baseoff[idx]);
 
-	//Set baud rate.
-	u32 rate = (8 * baud + 408000000) / (16 * baud);
-	uart->UART_LCR = 0x80; //Enable DLAB.
-	uart->UART_THR_DLAB = (u8)rate; //Divisor latch LSB.
-	uart->UART_IER_DLAB = (u8)(rate >> 8); //Divisor latch MSB.
-	uart->UART_LCR = 0; //Diable DLAB.
+	// Make sure no data is being sent.
+	uart_wait_idle(idx, UART_TX_IDLE);
 
-	//Setup UART in fifo mode.
-	uart->UART_IER_DLAB = 0;
-	uart->UART_IIR_FCR = 7; //Enable and clear TX and RX FIFOs.
-	(void)uart->UART_LSR;
+	// Misc settings.
+	u32 rate = (8 * baud + 408000000) / (16 * baud);
+	uart->UART_IER_DLAB = 0; // Disable interrupts.
+	uart->UART_MCR = 0; // Disable hardware flow control.
+	uart->UART_LCR = UART_LCR_DLAB | UART_LCR_WORD_LENGTH_8; // Enable DLAB & set 8n1 mode.
+	uart->UART_THR_DLAB = (u8)rate; // Divisor latch LSB.
+	uart->UART_IER_DLAB = (u8)(rate >> 8); // Divisor latch MSB.
+	uart->UART_LCR = UART_LCR_WORD_LENGTH_8; // Diable DLAB.
+
+	// Setup and flush fifo.
+	uart->UART_IIR_FCR = UART_IIR_FCR_EN_FIFO | UART_IIR_FCR_RX_CLR | UART_IIR_FCR_TX_CLR;
 	usleep(3 * ((baud + 999999) / baud));
-	uart->UART_LCR = 3; //Set word length 8.
-	uart->UART_MCR = 0;
-	uart->UART_MSR = 0;
-	uart->UART_IRDA_CSR = 0;
-	uart->UART_RX_FIFO_CFG = 1;
-	uart->UART_MIE = 0;
-	uart->UART_ASR = 0;
+	uart_wait_idle(idx, UART_TX_IDLE | UART_RX_IDLE);
 }
 
 void uart_wait_idle(u32 idx, u32 which)
 {
 	uart_t *uart = (uart_t *)(UART_BASE + uart_baseoff[idx]);
-	while (!(uart->UART_VENDOR_STATUS & which))
-		;
+	if (UART_TX_IDLE & which)
+	{
+		while (!(uart->UART_LSR & UART_LSR_TMTY))
+			;
+	}
+	if (UART_RX_IDLE & which)
+	{
+		while (uart->UART_LSR & UART_LSR_RDR)
+			;
+	}
 }
 
 void uart_send(u32 idx, u8 *buf, u32 len)
@@ -59,7 +64,7 @@ void uart_send(u32 idx, u8 *buf, u32 len)
 
 	for (u32 i = 0; i != len; i++)
 	{
-		while (uart->UART_LSR & UART_TX_FIFO_FULL)
+		while (!(uart->UART_LSR & UART_LSR_THRE))
 			;
 		uart->UART_THR_DLAB = buf[i];
 	};
@@ -71,7 +76,7 @@ void uart_recv(u32 idx, u8 *buf, u32 len)
 
 	for (u32 i = 0; i != len; i++)
 	{
-		while (uart->UART_LSR & UART_RX_FIFO_EMPTY)
+		while (!(uart->UART_LSR & UART_LSR_RDR))
 			;
 		buf[i] = uart->UART_THR_DLAB;
 	};
diff --git a/bootloader/soc/uart.h b/bootloader/soc/uart.h
index a7338c9..1380e98 100644
--- a/bootloader/soc/uart.h
+++ b/bootloader/soc/uart.h
@@ -28,10 +28,17 @@
 
 #define BAUD_115200 115200
 
-#define UART_TX_IDLE 0x00000001
-#define UART_RX_IDLE 0x00000002
-#define UART_TX_FIFO_FULL 0x100
-#define UART_RX_FIFO_EMPTY 0x200
+#define UART_TX_IDLE 0x1
+#define UART_RX_IDLE 0x2
+
+#define UART_LCR_DLAB 0x80
+#define UART_LCR_WORD_LENGTH_8 0x3
+#define UART_LSR_RDR 0x1
+#define UART_LSR_THRE 0x20
+#define UART_LSR_TMTY 0x40
+#define UART_IIR_FCR_TX_CLR 0x4
+#define UART_IIR_FCR_RX_CLR 0x2
+#define UART_IIR_FCR_EN_FIFO 0x1
 
 typedef struct _uart_t
 {