diff --git a/thermosphere/src/car.c b/thermosphere/src/car.c
index 95daaecad..05b603364 100644
--- a/thermosphere/src/car.c
+++ b/thermosphere/src/car.c
@@ -23,6 +23,7 @@ static inline uint32_t get_clk_source_reg(CarDevice dev) {
         case CARDEVICE_UARTA: return 0x178;
         case CARDEVICE_UARTB: return 0x17C;
         case CARDEVICE_UARTC: return 0x1A0;
+        case CARDEVICE_UARTD: return 0x1C0;
         case CARDEVICE_I2C1: return 0x124;
         case CARDEVICE_I2C5: return 0x128;
         case CARDEVICE_TZRAM: return 0;
@@ -46,6 +47,7 @@ static inline uint32_t get_clk_source_val(CarDevice dev) {
         case CARDEVICE_UARTA: return 0;
         case CARDEVICE_UARTB: return 0;
         case CARDEVICE_UARTC: return 0;
+        case CARDEVICE_UARTD: return 0;
         case CARDEVICE_I2C1: return 6;
         case CARDEVICE_I2C5: return 6;
         case CARDEVICE_TZRAM: return 0;
@@ -66,9 +68,10 @@ static inline uint32_t get_clk_source_val(CarDevice dev) {
 
 static inline uint32_t get_clk_source_div(CarDevice dev) {
     switch (dev) {
-        case CARDEVICE_UARTA: return 0;
-        case CARDEVICE_UARTB: return 0;
-        case CARDEVICE_UARTC: return 0;
+        case CARDEVICE_UARTA: return 1 << 24; // enable bit
+        case CARDEVICE_UARTB: return 1 << 24; // enable bit
+        case CARDEVICE_UARTC: return 1 << 24; // enable bit
+        case CARDEVICE_UARTD: return 1 << 24; // enable bit
         case CARDEVICE_I2C1: return 0;
         case CARDEVICE_I2C5: return 0;
         case CARDEVICE_TZRAM: return 0;
diff --git a/thermosphere/src/car.h b/thermosphere/src/car.h
index 2c8d50172..72c9579de 100644
--- a/thermosphere/src/car.h
+++ b/thermosphere/src/car.h
@@ -37,6 +37,7 @@ typedef enum {
     CARDEVICE_UARTA = ((0 << 5) | 0x6),
     CARDEVICE_UARTB = ((0 << 5) | 0x7),
     CARDEVICE_UARTC = ((1 << 5) | 0x17),
+    CARDEVICE_UARTD = ((2 << 5) | 0x1),
     CARDEVICE_I2C1 = ((0 << 5) | 0xC),
     CARDEVICE_I2C5 = ((1 << 5) | 0xF),
     CARDEVICE_TZRAM = ((3 << 5) | 0x1E),
diff --git a/thermosphere/src/gpio.h b/thermosphere/src/gpio.h
index c27f38c84..38781cbc5 100644
--- a/thermosphere/src/gpio.h
+++ b/thermosphere/src/gpio.h
@@ -95,7 +95,7 @@ static inline volatile tegra_gpio_t *gpio_get_regs(void)
 
 /* Mode select */
 #define GPIO_MODE_GPIO          0
-#define GPIO_MODE_SFIO          1
+#define GPIO_MODE_SDIO          1
 
 /* Direction */
 #define GPIO_DIRECTION_INPUT    0
diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c
index 5235adfe4..14904e167 100644
--- a/thermosphere/src/main.c
+++ b/thermosphere/src/main.c
@@ -1,16 +1,9 @@
 #include "utils.h"
 #include "uart.h"
-#include "car.h"
 #include "log.h"
-#include "gpio.h"
 
 int main(void)
 {
-    // Init uart (hardcoded atm)
-    gpio_configure_mode(TEGRA_GPIO(G, 0), GPIO_MODE_GPIO);      // Leave UART-B as GPIO
-    gpio_configure_mode(TEGRA_GPIO(D, 1), GPIO_MODE_SFIO);      // Change UART-C to SPIO
-    uart_select(UART_C);                                        // Configure pinmux for UART-C
-    clkrst_reboot(CARDEVICE_UARTC);                             // Enable UART-C clock
     uart_init(UART_C, 115200, true);
 
     //uart_send(UART_C, "0123\n", 3);
diff --git a/thermosphere/src/uart.c b/thermosphere/src/uart.c
index 4f8eaf6c4..330af689d 100644
--- a/thermosphere/src/uart.c
+++ b/thermosphere/src/uart.c
@@ -18,8 +18,21 @@
 #include "timers.h"
 #include "uart.h"
 #include "misc.h"
+#include "car.h"
+#include "gpio.h"
 
-void uart_select(UartDevice dev) {
+static inline void uart_wait_cycles(u32 baud, u32 num)
+{
+    udelay((num * 1000000 + 16 * baud - 1) / (16 * baud));
+}
+
+static inline void uart_wait_syms(u32 baud, u32 num)
+{
+    udelay((num * 1000000 + baud - 1) / baud);
+}
+
+void uart_select(UartDevice dev)
+{
     unsigned int id = (unsigned int)dev;
     PINMUX_AUX_UARTn_TX_0(id) = 0; /* UART */
     PINMUX_AUX_UARTn_RX_0(id) = 0x48; /* UART, enable, pull up */
@@ -27,59 +40,119 @@ void uart_select(UartDevice dev) {
     PINMUX_AUX_UARTn_CTS_0(id) = 0x44; /* UART, enable, pull down */
 }
 
-void uart_init(UartDevice dev, u32 baud, bool txInverted) {
+void uart_set_baudrate(UartDevice dev, u32 baud)
+{
+    //Set baud rate.
     volatile uart_t *uart = get_uart_device(dev);
 
-    uart_wait_idle(dev, UART_VENDOR_STATE_TX_IDLE);
+    // Round to closest (hence the 8 * baud: +0.5)
+    u32 rate = (8 * baud + UART_CLKRATE) / (16 * baud);
 
-    /* Set baud rate. */
-    u32 rate = (8 * baud + 408000000) / (16 * baud);
-    uart->UART_LCR = UART_LCR_DLAB; /* 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. */
+    // Enable DLAB
+    uart->UART_LCR |= UART_LCR_DLAB;
 
-    /* Setup UART in fifo mode. */
-    uart->UART_IER_DLAB = 0;
-    uart->UART_IIR_FCR = UART_FCR_FCR_EN_FIFO | UART_FCR_RX_CLR | UART_FCR_TX_CLR; /* Enable and clear TX and RX FIFOs. */
+    // Set divisor
+    uart->UART_THR_DLAB = (u8)rate;
+    uart->UART_IER_DLAB = (u8)(rate >> 8);
+
+    // Disable DLAB
+    uart->UART_LCR &= ~UART_LCR_DLAB;
     uart->UART_LSR;
-    udelay(3 * ((baud + 999999) / baud));
-    uart->UART_LCR = UART_LCR_WD_LENGTH_8; /* Set word length 8. */
+
+    // Wait 2 characters worth of time
+    uart_wait_syms(baud, 2);
+}
+
+void uart_flush_fifos(UartDevice dev, u32 baud, bool reset)
+{
+    volatile uart_t *uart = get_uart_device(dev);
+
+    if (reset) {
+        uart->UART_IIR_FCR &= ~UART_FCR_FCR_EN_FIFO;
+        udelay(60); // From nvidia
+        uart->UART_IIR_FCR |= UART_FCR_TX_CLR | UART_FCR_RX_CLR;
+        uart->UART_IIR_FCR |= UART_FCR_FCR_EN_FIFO;
+    } else {
+        uart->UART_IIR_FCR |= UART_FCR_TX_CLR | UART_FCR_RX_CLR;
+    }
+
+    uart->UART_LSR;
+
+    // Erratum fix
+    uart_wait_cycles(baud, 32);
+}
+
+void uart_init(UartDevice dev, u32 baud, bool txInverted)
+{
+    volatile uart_t *uart = get_uart_device(dev);
+
+    CarDevice uartCarDevs[] = { CARDEVICE_UARTA, CARDEVICE_UARTB, CARDEVICE_UARTC, CARDEVICE_UARTD };
+    if (dev == UART_C) {
+        gpio_configure_mode(TEGRA_GPIO(G, 0), GPIO_MODE_GPIO);      // Leave UART-B as GPIO
+        gpio_configure_mode(TEGRA_GPIO(D, 1), GPIO_MODE_SDIO);      // Change UART-C to SPIO
+        // Fixme other uart?
+    }
+    uart_select(dev);
+    clkrst_reboot(uartCarDevs[dev]);
+
     uart->UART_MCR = 0;
     uart->UART_MSR = 0;
-    uart->UART_IRDA_CSR = txInverted ? 2 : 0;
-    uart->UART_RX_FIFO_CFG = 1; /* Set RX_FIFO trigger level */
+    uart->UART_RX_FIFO_CFG = 1; // Reset value
     uart->UART_MIE = 0;
     uart->UART_ASR = 0;
 
-    uart_wait_idle(dev, UART_VENDOR_STATE_TX_IDLE | UART_VENDOR_STATE_RX_IDLE);
+    // Enable FIFO, etc
+    uart->UART_IIR_FCR =  UART_FCR_FCR_EN_FIFO | UART_FCR_TX_TRIG_FIFO_COUNT_GREATER_16 | UART_FCR_RX_TRIG_FIFO_COUNT_GREATER_1;
+    uart->UART_IRDA_CSR = txInverted ? 2 : 0;
+    uart->UART_LSR;
+
+    // Erratum fix
+    uart_wait_cycles(baud, 3);
+
+    // Set baud rate, etc.
+    uart->UART_LCR = UART_LCR_WD_LENGTH_8;
+    uart_set_baudrate(dev, baud);
+
+    uart_flush_fifos(dev, baud, true);
 }
 
-/* This function blocks until the UART device (dev) is in the desired state (status). Make sure the desired state can be reached! */
-void uart_wait_idle(UartDevice dev, UartVendorStatus status) {
-    while (!(get_uart_device(dev)->UART_VENDOR_STATUS & status)) {
-        /* Wait */
-    }
+// This function blocks until the UART device (dev) is in the desired state (status). Make sure the desired state can be reached!
+void uart_wait_idle(UartDevice dev, UartVendorStatus status)
+{
+    while (!(get_uart_device(dev)->UART_VENDOR_STATUS & status));
 }
 
-void uart_send(UartDevice dev, const void *buf, size_t len) {
+void uart_send(UartDevice dev, const void *buf, size_t len)
+{
     volatile uart_t *uart = get_uart_device(dev);
+    const u8 *buf8 = (const u8 *)buf;
 
     for (size_t i = 0; i < len; i++) {
-        while (uart->UART_LSR & UART_LSR_TX_FIFO_FULL) {
-            /* Wait until the TX FIFO isn't full */
-        }
-        uart->UART_THR_DLAB = *((const u8 *)buf + i);
+        while (uart->UART_LSR & UART_LSR_TX_FIFO_FULL);
+        uart->UART_THR_DLAB = buf8[i];
     }
 }
 
-void uart_recv(UartDevice dev, void *buf, size_t len) {
+void uart_recv(UartDevice dev, void *buf, size_t len)
+{
     volatile uart_t *uart = get_uart_device(dev);
+    u8 *buf8 = (u8 *)buf;
 
     for (size_t i = 0; i < len; i++) {
-        while (uart->UART_LSR & UART_LSR_RX_FIFO_EMPTY) {
-            /* Wait until the RX FIFO isn't empty */
-        }
-        *((u8 *)buf + i) = uart->UART_THR_DLAB;
+        while (uart->UART_LSR & UART_LSR_RX_FIFO_EMPTY);
+        buf8[i] = uart->UART_THR_DLAB;
     }
 }
+
+size_t uart_recv_max(UartDevice dev, void *buf, size_t maxlen)
+{
+    volatile uart_t *uart = get_uart_device(dev);
+    u8 *buf8 = (u8 *)buf;
+    size_t i;
+
+    for (i = 0; i < maxlen && !(uart->UART_LSR & UART_LSR_RX_FIFO_EMPTY); i++) {
+        buf8[i] = uart->UART_THR_DLAB;
+    }
+
+    return i;
+}
diff --git a/thermosphere/src/uart.h b/thermosphere/src/uart.h
index 5baf2e822..156c006ee 100644
--- a/thermosphere/src/uart.h
+++ b/thermosphere/src/uart.h
@@ -18,10 +18,10 @@
 #pragma once
 #include "utils.h"
 
-#define UART_BASE 0x70006000ull
-
-#define BAUD_115200 115200
+#define UART_BASE 0x70006000
 
+#define BAUD_115200     115200
+#define UART_CLKRATE    408000000
 /* Exosphère: add the clkreset values for UART C,D,E */
 typedef enum {
     UART_A = 0,
@@ -156,10 +156,13 @@ typedef struct {
 } uart_t;
 
 void uart_select(UartDevice dev);
+void uart_set_baudrate(UartDevice dev, u32 baud);
+void uart_flush_fifos(UartDevice dev, u32 baud, bool reset);
 void uart_init(UartDevice dev, u32 baud, bool txInverted);
 void uart_wait_idle(UartDevice dev, UartVendorStatus status);
 void uart_send(UartDevice dev, const void *buf, size_t len);
 void uart_recv(UartDevice dev, void *buf, size_t len);
+size_t uart_recv_max(UartDevice dev, void *buf, size_t maxlen);
 
 static inline volatile uart_t *get_uart_device(UartDevice dev) {
     static const size_t offsets[] = {0, 0x40, 0x200, 0x300, 0x400};