8053 wipps

Change-Id: If846da941451264e8cc72449d631928edb2354a9
diff --git a/kernel/arch/arm64/boot-mmu.cpp b/kernel/arch/arm64/boot-mmu.cpp
index 1f32132..f5185d5 100644
--- a/kernel/arch/arm64/boot-mmu.cpp
+++ b/kernel/arch/arm64/boot-mmu.cpp
@@ -194,7 +194,13 @@
         return reinterpret_cast<pte_t*>(pa);
     };
 
-    return _arm64_boot_map(kernel_table0, vaddr, paddr, len, flags, alloc, phys_to_virt);
+    zx_status_t status =  _arm64_boot_map(kernel_table0, vaddr, paddr, len, flags, alloc, phys_to_virt);
+
+if (status != ZX_OK) {
+    while (1) {}
+}
+
+    return status;
 }
 
 // called a bit later in the boot process once the kernel is in virtual memory to map early kernel data
@@ -222,3 +228,80 @@
 
     return _arm64_boot_map(arm64_get_kernel_ptable(), vaddr, paddr, len, flags, alloc, phys_to_virt);
 }
+
+#define UART_DM_N0_CHARS_FOR_TX             0x0040
+#define UART_DM_CR_CMD_RESET_TX_READY       (3 << 8)
+
+#define UART_DM_SR                          0x00A4
+#define UART_DM_SR_TXRDY                    (1 << 2)
+#define UART_DM_SR_TXEMT                    (1 << 3)
+
+#define UART_DM_TF                          0x0100
+
+#define UARTREG(reg) (*(volatile uint32_t*)(0x078af000 + (reg)))
+
+static void uart_pputc(uint8_t c) {
+    while (!(UARTREG(UART_DM_SR) & UART_DM_SR_TXEMT)) {
+        ;
+    }
+    UARTREG(UART_DM_N0_CHARS_FOR_TX) = UART_DM_CR_CMD_RESET_TX_READY;
+    UARTREG(UART_DM_N0_CHARS_FOR_TX) = 1;
+    __UNUSED uint32_t foo = UARTREG(UART_DM_N0_CHARS_FOR_TX);
+
+    // wait for TX ready
+    while (!(UARTREG(UART_DM_SR) & UART_DM_SR_TXRDY))
+        ;
+
+    *((volatile uint32_t*)(0x078af100)) = c;
+
+//    UARTREG(UART_DM_TF) = c;
+
+    // wait for TX ready
+    while (!(UARTREG(UART_DM_SR) & UART_DM_SR_TXRDY))
+        ;
+}
+
+/* qemu
+static volatile uint32_t* uart_fifo_dr = (uint32_t *)0x09000000;
+static volatile uint32_t* uart_fifo_fr = (uint32_t *)0x09000018;
+
+void uart_pputc(char c)
+{
+    while (*uart_fifo_fr & (1<<5))
+        ;
+    *uart_fifo_dr = c;
+}
+*/
+
+/* mediatek
+#define UART_THR                    (0x0)   // TX Buffer Register (write-only)
+#define UART_LSR                    (0x14)  // Line Status Register
+#define UART_LSR_THRE               (1 << 5)
+
+#define UARTREG(reg) (*(volatile uint32_t*)(0x11005000 + (reg)))
+
+static void uart_pputc(char c) {
+    while (!(UARTREG(UART_LSR) & UART_LSR_THRE))
+        ;
+    UARTREG(UART_THR) = c;
+}
+*/
+
+static void debug_print_digit(uint64_t x) {
+    x &= 0xf;
+
+    if (x < 10) {
+        uart_pputc((char)(x + '0'));
+    } else {
+        uart_pputc((char)(x - 10 + 'A'));
+    }
+}
+
+extern "C" void debug_print_int(uint64_t x) {
+    uart_pputc('\n');
+
+    for (int shift = 60; shift >= 0; shift -= 4) {
+        debug_print_digit(x >> shift);
+    }
+    uart_pputc('\n');
+}
diff --git a/kernel/arch/arm64/start.S b/kernel/arch/arm64/start.S
index 961a3dc..399e0c8 100644
--- a/kernel/arch/arm64/start.S
+++ b/kernel/arch/arm64/start.S
@@ -29,12 +29,17 @@
 page_table0             .req x20
 page_table1             .req x21
 kernel_vaddr            .req x22
+uart_tx                 .req x23
+uart                    .req x24
 
 // This code is purely position-independent and generates no relocations
 // that need boot-time fixup; gen-kaslr-fixup.sh ensures this (and would
 // ignore it if this code were in .text.boot, so don't put it there).
 .text
 FUNCTION(_start)
+    movlit   uart_tx, 0x078af040
+    movlit   uart, 0x078af100
+
     /* Save the Boot info for the primary CPU only */
     mrs     cpuid, mpidr_el1
     ubfx    cpuid, cpuid, #0, #15 /* mask Aff0 and Aff1 fields */
@@ -98,6 +103,14 @@
     /* make sure the boot allocator is given a chance to figure out where
      * we are loaded in physical memory. */
     bl      boot_alloc_init
+/*
+    mov     tmp, 0x30
+    str     tmp, [uart_tx]
+    mov     tmp, 0x1
+    str     tmp, [uart_tx]
+    mov     tmp, 'B'
+    str     tmp, [uart]
+*/
 
     /* save the physical address the kernel is loaded at */
     adr_global x0, __code_start
@@ -116,6 +129,13 @@
 
     /* void arm64_boot_map(pte_t* kernel_table0, vaddr_t vaddr, paddr_t paddr, size_t len, pte_t flags); */
 
+    mov x0, KERNEL_ASPACE_BASE
+    bl debug_print_int
+
+    mov x0, ARCH_PHYSMAP_SIZE
+    bl debug_print_int
+
+
     /* map a large run of physical memory at the base of the kernel's address space */
     mov     x0, page_table1
     mov     x1, KERNEL_ASPACE_BASE
@@ -124,6 +144,17 @@
     movlit  x4, MMU_PTE_KERNEL_DATA_FLAGS
     bl      arm64_boot_map
 
+    mov x0, kernel_vaddr
+    bl debug_print_int
+
+    adr_global x0, __code_start
+    bl debug_print_int
+
+    adr_global x2, __code_start
+    adr_global x3, _end
+    sub     x0, x3, x2
+    bl debug_print_int
+
     /* map the kernel to a fixed address */
     /* note: mapping the kernel here with full rwx, this will get locked down later in vm initialization; */
     mov     x0, page_table1
@@ -205,6 +236,8 @@
     /* Read SCTLR */
     mrs     tmp, sctlr_el1
 
+//hit this    b   .
+
     /* Turn on the MMU */
     orr     tmp, tmp, #0x1
 
@@ -213,27 +246,38 @@
 .Lmmu_on_pc:
     isb
 
+
+//did get here    b   .
+
     // Map our current physical PC to the virtual PC and jump there.
     // PC = next_PC - __code_start + kernel_vaddr
     adr     tmp, .Lmmu_on_vaddr
     adr     tmp2, __code_start
     sub     tmp, tmp, tmp2
     add     tmp, tmp, kernel_vaddr
+
+// got here    b   .
+
     br      tmp
 
 .Lmmu_on_vaddr:
+// didn't    b   .
 
     /* Disable trampoline page-table in ttbr0 */
     movlit  tmp, MMU_TCR_FLAGS_KERNEL
     msr     tcr_el1, tmp
     isb
 
+// didn't    b   .
+
     /* Invalidate TLB */
     tlbi    vmalle1
     isb
 
     cbnz    cpuid, .Lsecondary_boot
 
+// didn't get here    b   .
+
     // set up the boot stack for real
     adr_global tmp, boot_cpu_kstack_end
     mov     sp, tmp
@@ -251,6 +295,8 @@
     // set the per cpu pointer for cpu 0
     adr_global x18, arm64_percpu_array
 
+// didn't hit this    b   .
+
     // Choose a good (ideally random) stack-guard value as early as possible.
     bl      choose_stack_guard
     mrs     tmp, tpidr_el1
@@ -258,6 +304,8 @@
     // Don't leak the value to other code.
     mov     x0, xzr
 
+// didn't hit this    b   .
+
     bl  lk_main
     b   .
 
diff --git a/kernel/dev/uart/msm/rules.mk b/kernel/dev/uart/msm/rules.mk
new file mode 100644
index 0000000..ecfb667
--- /dev/null
+++ b/kernel/dev/uart/msm/rules.mk
@@ -0,0 +1,17 @@
+# Copyright 2018 The Fuchsia Authors
+# Use of this source code is governed by a MIT-style
+# license that can be found in the LICENSE file or at
+# https://opensource.org/licenses/MIT
+
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+    $(LOCAL_DIR)/uart.cpp
+
+MODULE_DEPS += \
+    kernel/dev/pdev \
+    kernel/dev/pdev/uart \
+
+include make/module.mk
diff --git a/kernel/dev/uart/msm/uart.cpp b/kernel/dev/uart/msm/uart.cpp
new file mode 100644
index 0000000..e2e69b8
--- /dev/null
+++ b/kernel/dev/uart/msm/uart.cpp
@@ -0,0 +1,373 @@
+// Copyright 2016 The Fuchsia Authors
+// Copyright (c) 2016 Gurjant Kalsi
+//
+// Use of this source code is governed by a MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT
+
+// TODO(gkalsi): Unify the two UART codepaths and use the port parameter to
+// select between the real uart and the miniuart.
+
+#include <arch/arm64/periphmap.h>
+#include <dev/interrupt.h>
+#include <dev/uart.h>
+#include <kernel/thread.h>
+#include <lib/cbuf.h>
+#include <lib/debuglog.h>
+#include <pdev/driver.h>
+#include <pdev/uart.h>
+#include <platform/debug.h>
+#include <reg.h>
+#include <stdio.h>
+#include <trace.h>
+#include <zircon/boot/driver-config.h>
+
+// clang-format off
+
+#define UART_MR1                            0x0000
+#define UART_MR1_RX_RDY_CTL                 (1 << 7)
+
+#define UART_MR2                            0x0004
+#define UART_DM_IPR                         0x0018
+#define UART_DM_DMRX                        0x0034
+#define UART_DM_N0_CHARS_FOR_TX             0x0040
+
+#define UART_DM_SR                          0x00A4
+#define UART_DM_SR_RXRDY                    (1 << 0)
+#define UART_DM_SR_RXFULL                   (1 << 1)
+#define UART_DM_SR_TXRDY                    (1 << 2)
+#define UART_DM_SR_TXEMT                    (1 << 3)
+#define UART_DM_SR_OVERRUN                  (1 << 4)
+#define UART_DM_SR_PAR_FRAME_ERR            (1 << 5)
+#define UART_DM_SR_RX_BREAK                 (1 << 6)
+#define UART_DM_SR_HUNT_CHAR                (1 << 7)
+
+#define UART_DM_CR                          0x00A8
+#define UART_DM_CR_RX_EN                    (1 << 0)
+#define UART_DM_CR_RX_DISABLE               (1 << 1)
+#define UART_DM_CR_TX_EN                    (1 << 2)
+#define UART_DM_CR_TX_DISABLE               (1 << 3)
+
+#define UART_DM_CR_CMD_RESET_RX             (1 << 4)
+#define UART_DM_CR_CMD_RESET_TX             (2 << 4)
+#define UART_DM_CR_CMD_RESET_ERR            (3 << 4)
+#define UART_DM_CR_CMD_RESET_BRK_CHG_INT    (4 << 4)
+#define UART_DM_CR_CMD_START_BRK            (5 << 4)
+#define UART_DM_CR_CMD_STOP_BRK             (6 << 4)
+#define UART_DM_CR_CMD_RESET_CTS_N          (7 << 4)
+#define UART_DM_CR_CMD_RESET_STALE_INT      (8 << 4)
+#define UART_DM_CR_CMD_SET_RFR              (13 << 4)
+#define UART_DM_CR_CMD_RESET_RFR            (14 << 4)
+#define UART_DM_CR_CMD_CLEAR_TX_ERROR       (16 << 4)
+#define UART_DM_CR_CMD_CLEAR_TX_DONE        (17 << 4)
+#define UART_DM_CR_CMD_RESET_BRK_START_INT  (18 << 4)
+#define UART_DM_CR_CMD_RESET_BRK_END_INT    (19 << 4)
+#define UART_DM_CR_CMD_RESET_PAR_FRAME_ERR_INT (20 << 4)
+#define UART_DM_CR_CMD_CLEAR_TX_WR_ERROR_IRQ (25 << 4)
+#define UART_DM_CR_CMD_CLEAR_RX_RD_ERROR_IRQ (26 << 4)
+#define UART_DM_CR_CMD_CLEAR_TX_COMP_IRQ    (27 << 4)
+#define UART_DM_CR_CMD_CLEAR_WWT_IRQ        (28 << 4)
+#define UART_DM_CR_CMD_CLEAR_NO_FINISH_CMD_VIO_IRQ (30 << 4)
+
+#define UART_DM_CR_CMD_RESET_TX_READY       (3 << 8)
+#define UART_DM_CR_CMD_FORCE_STALE          (4 << 8)
+#define UART_DM_CR_CMD_ENABLE_STALE_EVENT   (5 << 8)
+#define UART_DM_CR_CMD_DISABLE_STALE_EVENT  (6 << 8)
+
+#define UART_DM_RXFS                        0x0050
+#define UART_DM_RXFS_RX_BUFFER_STATE(r)     ((r >> 7) & 7)
+#define UART_DM_RXFS_FIFO_STATE(r)          ((r >> 14) | (r & 0x3F))
+
+#define UART_DM_MISR                        0x00AC
+#define UART_DM_IMR                         0x00B0
+#define UART_DM_ISR                         0x00B4
+
+#define UART_IRQ_TXLEV                      (1 << 0)
+#define UART_IRQ_RXHUNT                     (1 << 1)
+#define UART_IRQ_RXBREAK_CHANGE             (1 << 2)
+#define UART_IRQ_RXSTALE                    (1 << 3)
+#define UART_IRQ_RXLEV                      (1 << 4)
+#define UART_IRQ_DELTA_CTS                  (1 << 5)
+#define UART_IRQ_CURRENT_CTS                (1 << 6)
+#define UART_IRQ_TX_READY                   (1 << 7)
+#define UART_IRQ_TX_ERROR                   (1 << 8)
+#define UART_IRQ_TX_DONE                    (1 << 9)
+#define UART_IRQ_RXBREAK_START              (1 << 10)
+#define UART_IRQ_RXBREAK_END                (1 << 11)
+#define UART_IRQ_PAR_FRAME_ERR_IRQ          (1 << 12)
+#define UART_IRQ_TX_WR_ERROR_IRQ            (1 << 13)
+#define UART_IRQ_RX_RD_ERROR_IRQ            (1 << 14)
+#define UART_IRQ_TXCOMP_IRQ                 (1 << 15)
+#define UART_IRQ_WWT_IRQ                    (1 << 16)
+#define UART_IRQ_NO_FINISH_CMD_VIOL         (1 << 17)
+
+#define UART_DM_TF                          0x0100
+#define UART_DM_RF(n)                       (0x0140 + 4 * (n))
+
+#define RXBUF_SIZE 128
+
+// clang-format on
+
+// values read from zbi
+static uint64_t uart_base = 0;
+static uint32_t uart_irq = 0;
+
+static cbuf_t uart_rx_buf;
+//??static bool initialized = false;
+
+static bool uart_tx_irq_enabled = false;
+static event_t uart_dputc_event = EVENT_INITIAL_VALUE(uart_dputc_event,
+                                                      true,
+                                                      EVENT_FLAG_AUTOUNSIGNAL);
+
+static spin_lock_t uart_spinlock = SPIN_LOCK_INITIAL_VALUE;
+
+static inline uint32_t uart_read(int offset) {
+    return readl(uart_base + offset);
+}
+
+static inline void uart_write(uint32_t val, int offset) {
+    writel(val, uart_base + offset);
+}
+
+static inline void yield(void)
+{
+    __asm__ volatile("yield" ::: "memory");
+}
+
+/* panic-time getc/putc */
+static int msm_pputc(char c)
+{
+    if (!uart_base) {
+        return -1;
+    }
+
+    // spin while fifo is full
+    while (!(uart_read(UART_DM_SR) & UART_DM_SR_TXEMT)) {
+        yield();
+    }
+    uart_write(UART_DM_CR_CMD_RESET_TX_READY, UART_DM_N0_CHARS_FOR_TX);
+    uart_write(1, UART_DM_N0_CHARS_FOR_TX);
+    uart_read(UART_DM_N0_CHARS_FOR_TX);
+
+    // wait for TX ready
+    while (!(uart_read(UART_DM_SR) & UART_DM_SR_TXRDY))
+        yield();
+
+    uart_write(c, UART_DM_TF);
+
+    return 1;
+}
+
+static int msm_pgetc(void)
+{
+    cbuf_t* rxbuf = &uart_rx_buf;
+
+    char c;
+    int count = 0;
+    uint32_t val, rxfs, sr;
+    char* bytes;
+    int i;
+
+    // see if we have chars left from previous read
+    if (cbuf_read_char(rxbuf, &c, false) == 1) {
+        return c;
+    }
+
+    if ((uart_read(UART_DM_SR) & UART_DM_SR_OVERRUN)) {
+        uart_write(UART_DM_CR_CMD_RESET_ERR, UART_DM_CR);
+    }
+
+    do {
+        rxfs = uart_read(UART_DM_RXFS);
+        sr = uart_read(UART_DM_SR);
+        count = UART_DM_RXFS_RX_BUFFER_STATE(rxfs);
+        if (!(sr & UART_DM_SR_RXRDY) && !count) {
+            return -1;
+        }
+    } while (count == 0);
+
+    uart_write(UART_DM_CR_CMD_FORCE_STALE, UART_DM_CR);
+    val = uart_read(UART_DM_RF(0));
+    uart_read(UART_DM_RF(1));
+
+    uart_write(UART_DM_CR_CMD_RESET_STALE_INT, UART_DM_CR);
+    uart_write(0xffffff, UART_DM_DMRX);
+
+    bytes = (char*)&val;
+    c = bytes[0];
+
+    // save remaining chars for next call
+    for (i = 1; i < count; i++) {
+        cbuf_write_char(rxbuf, bytes[i]);
+    }
+
+    return c;
+}
+
+static interrupt_eoi uart_irq_handler(void* arg) {
+    uint32_t misr = uart_read(UART_DM_MISR);
+
+    while (uart_read(UART_DM_SR) & UART_DM_SR_RXRDY) {
+        uint32_t rxfs = uart_read(UART_DM_RXFS);
+        // count is number of words in RX fifo that have data
+        int count = UART_DM_RXFS_FIFO_STATE(rxfs);
+
+        for (int i = 0; i < count; i++) {
+            uint32_t val = uart_read(UART_DM_RF(0));
+            char* bytes = (char *)&val;
+
+            for (int j = 0; j < 4; j++) {
+                // Unfortunately there is no documented way to get number of bytes in each word
+                // so we just need to ignore zero bytes here.
+                // Apparently this problem doesn't exist in DMA mode.
+                char ch = bytes[j];
+                if (ch) {
+                    cbuf_write_char(&uart_rx_buf, ch);
+                } else {
+                    break;
+                }
+            }
+        }
+    }
+
+    if (misr & UART_IRQ_RXSTALE) {
+        uart_write(UART_DM_CR_CMD_RESET_STALE_INT, UART_DM_CR);
+    }
+
+    // ask to receive more
+    uart_write(0xFFFFFF, UART_DM_DMRX);
+    uart_write(UART_DM_CR_CMD_ENABLE_STALE_EVENT, UART_DM_CR);
+
+    return IRQ_EOI_ISSUE;
+}
+
+/*
+static void uart_irq_handler(void* arg) {
+    // read interrupt status and mask
+    while (UARTREG(UART_LSR) & UART_LSR_DR) {
+        if (cbuf_space_avail(&uart_rx_buf) == 0) {
+            break;
+        }
+        char c = UARTREG(UART_RBR) & 0xFF;
+        cbuf_write_char(&uart_rx_buf, c);
+    }
+
+    // Signal if anyone is waiting to TX
+    if (UARTREG(UART_LSR) & UART_LSR_THRE) {
+        UARTREG(UART_IER) &= ~UART_IER_ETBEI;  // Disable TX interrupt
+        spin_lock(&uart_spinlock);
+        // TODO(andresoportus): Revisit all UART drivers usage of events, from event.h:
+        // 1. The reschedule flag is not supposed to be true in interrupt context.
+        // 2. FLAG_AUTOUNSIGNAL only wakes up one thread.
+        event_signal(&uart_dputc_event, true);
+        spin_unlock(&uart_spinlock);
+    }
+}
+*/
+
+static void msm_uart_init(const void* driver_data, uint32_t length) {
+    uint32_t temp;
+
+    // disable interrupts
+    uart_write(0, UART_DM_IMR);
+
+    uart_write(UART_DM_CR_TX_EN | UART_DM_CR_RX_EN, UART_DM_CR);
+    uart_write(UART_DM_CR_CMD_RESET_TX, UART_DM_CR);
+    uart_write(UART_DM_CR_CMD_RESET_RX, UART_DM_CR);
+    uart_write(UART_DM_CR_CMD_RESET_ERR, UART_DM_CR);
+    uart_write(UART_DM_CR_CMD_RESET_BRK_CHG_INT, UART_DM_CR);
+    uart_write(UART_DM_CR_CMD_RESET_CTS_N, UART_DM_CR);
+    uart_write(UART_DM_CR_CMD_SET_RFR, UART_DM_CR);
+    uart_write(UART_DM_CR_CMD_CLEAR_TX_DONE, UART_DM_CR);
+
+    uart_write(0xFFFFFF, UART_DM_DMRX);
+    uart_write(UART_DM_CR_CMD_ENABLE_STALE_EVENT, UART_DM_CR);
+
+    temp = uart_read(UART_MR1);
+    temp |= UART_MR1_RX_RDY_CTL;
+    uart_write(temp, UART_MR1);
+
+    cbuf_initialize(&uart_rx_buf, RXBUF_SIZE);
+
+    // enable RX interrupt
+    uart_write(UART_IRQ_RXSTALE, UART_DM_IMR);
+
+    register_int_handler(uart_irq, &uart_irq_handler, nullptr);
+    unmask_interrupt(uart_irq);
+
+//??    initialized = true;
+
+    // Start up tx driven output.
+    printf("UART: starting IRQ driven TX\n");
+//??    uart_tx_irq_enabled = true;
+}
+
+static int msm_getc(bool wait) {
+    char ch;
+    size_t count = cbuf_read_char(&uart_rx_buf, &ch, wait);
+    return (count == 1 ? ch : -1);
+}
+
+static void msm_start_panic(void) {
+    uart_tx_irq_enabled = false;
+}
+
+static void msm_dputs(const char* str, size_t len,
+                         bool block, bool map_NL) {
+    spin_lock_saved_state_t state;
+    bool copied_CR = false;
+
+    if (!uart_base) {
+        return;
+    }
+    if (!uart_tx_irq_enabled) {
+        block = false;
+    }
+    spin_lock_irqsave(&uart_spinlock, state);
+
+    while (len > 0) {
+        // is FIFO full?
+        while (!(uart_read(UART_DM_SR) & UART_DM_SR_TXEMT)) {
+            spin_unlock_irqrestore(&uart_spinlock, state);
+            if (block) {
+//??                UARTREG(UART_IER) |= UART_IER_ETBEI; // Enable TX interrupt.
+                event_wait(&uart_dputc_event);
+            } else {
+                arch_spinloop_pause();
+            }
+            spin_lock_irqsave(&uart_spinlock, state);
+        }
+        if (*str == '\n' && map_NL && !copied_CR) {
+            copied_CR = true;
+            msm_pputc('\r');
+        } else {
+            copied_CR = false;
+            msm_pputc(*str++);
+            len--;
+        }
+    }
+    spin_unlock_irqrestore(&uart_spinlock, state);
+}
+
+static const struct pdev_uart_ops uart_ops = {
+    .getc = msm_getc,
+    .pputc = msm_pputc,
+    .pgetc = msm_pgetc,
+    .start_panic = msm_start_panic,
+    .dputs = msm_dputs,
+};
+
+static void msm_uart_init_early(const void* driver_data, uint32_t length) {
+    ASSERT(length >= sizeof(dcfg_simple_t));
+    auto driver = static_cast<const dcfg_simple_t*>(driver_data);
+    uart_base = periph_paddr_to_vaddr(driver->mmio_phys);
+    uart_irq = driver->irq;
+    ASSERT(uart_base);
+    ASSERT(uart_irq);
+
+    pdev_register_uart(&uart_ops);
+}
+
+LK_PDEV_INIT(msm_uart_init_early, KDRV_MSM_UART, msm_uart_init_early, LK_INIT_LEVEL_PLATFORM_EARLY);
+LK_PDEV_INIT(msm_uart_init, KDRV_MSM_UART, msm_uart_init, LK_INIT_LEVEL_PLATFORM);
diff --git a/kernel/platform/generic-arm/rules.mk b/kernel/platform/generic-arm/rules.mk
index 6f8494f..95c4540 100644
--- a/kernel/platform/generic-arm/rules.mk
+++ b/kernel/platform/generic-arm/rules.mk
@@ -32,6 +32,7 @@
 	kernel/dev/power/hisi \
 	kernel/dev/psci \
 	kernel/dev/uart/amlogic_s905 \
+	kernel/dev/uart/msm \
 	kernel/dev/uart/mt8167 \
 	kernel/dev/uart/nxp-imx \
 	kernel/dev/uart/pl011 \
diff --git a/kernel/target/arm64/board/msm8x53-som/boot-shim-config.h b/kernel/target/arm64/board/msm8x53-som/boot-shim-config.h
new file mode 100644
index 0000000..28e02f1
--- /dev/null
+++ b/kernel/target/arm64/board/msm8x53-som/boot-shim-config.h
@@ -0,0 +1,208 @@
+// Copyright 2018 The Fuchsia Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style license that can be
+// found in the LICENSE file.
+
+#define HAS_DEVICE_TREE 1
+
+static const zbi_cpu_config_t cpu_config = {
+    .cluster_count = 2,
+    .clusters = {
+        {
+            .cpu_count = 4,
+        },
+        {
+            .cpu_count = 4,
+        },
+    },
+};
+
+static const zbi_mem_range_t mem_config[] = {
+    {
+        .type = ZBI_MEM_RANGE_RAM,
+        .paddr = 0x80000000,
+        .length = 0x80000000, // 2GB
+    },
+    {
+        .type = ZBI_MEM_RANGE_PERIPHERAL,
+        // This is not the entire peripheral range, but enough to cover what we use in the kernel.
+        .paddr = 0x00000000,
+        .length = 0x10000000,
+    },
+    {
+        // other_ext_mem: other_ext_region@0
+        .type = ZBI_MEM_RANGE_RESERVED,
+        .paddr = 0x85b00000,
+        .length = 0xd00000,
+    },
+    {
+        // modem_mem: modem_region@0
+        .type = ZBI_MEM_RANGE_RESERVED,
+        .paddr = 0x86c00000,
+        .length = 0x6a00000,
+    },
+    {
+        // adsp_fw_mem: adsp_fw_region@0
+        .type = ZBI_MEM_RANGE_RESERVED,
+        .paddr = 0x8d600000,
+        .length = 0x1100000,
+    },
+    {
+        // wcnss_fw_mem: wcnss_fw_region@0
+        .type = ZBI_MEM_RANGE_RESERVED,
+        .paddr = 0x8e700000,
+        .length = 0x700000,
+    },
+    {
+        // dfps_data_mem: dfps_data_mem@0
+        .type = ZBI_MEM_RANGE_RESERVED,
+        .paddr = 0x90000000,
+        .length = 0x1000,
+    },
+    {
+        // cont_splash_mem: splash_region@0
+        .type = ZBI_MEM_RANGE_RESERVED,
+        .paddr = 0x90001000,
+        .length = 0x13ff000,
+    },
+
+
+/*
+
+		other_ext_mem: other_ext_region@0 {
+			compatible = "removed-dma-pool";
+			no-map;
+			reg = <0x0 0x85b00000 0x0 0xd00000>;
+		};
+
+		modem_mem: modem_region@0 {
+			compatible = "removed-dma-pool";
+			no-map;
+			reg = <0x0 0x86c00000 0x0 0x6a00000>;
+		};
+
+		adsp_fw_mem: adsp_fw_region@0 {
+			compatible = "removed-dma-pool";
+			no-map;
+			reg = <0x0 0x8d600000 0x0 0x1100000>;
+		};
+
+		wcnss_fw_mem: wcnss_fw_region@0 {
+			compatible = "removed-dma-pool";
+			no-map;
+			reg = <0x0 0x8e700000 0x0 0x700000>;
+		};
+
+		venus_mem: venus_region@0 {
+			compatible = "shared-dma-pool";
+			reusable;
+			alloc-ranges = <0x0 0x80000000 0x0 0x10000000>;
+			alignment = <0 0x400000>;
+			size = <0 0x0800000>;
+		};
+
+		secure_mem: secure_region@0 {
+			compatible = "shared-dma-pool";
+			reusable;
+			alignment = <0 0x400000>;
+			size = <0 0x0b400000>;
+		};
+
+		qseecom_mem: qseecom_region@0 {
+			compatible = "shared-dma-pool";
+			reusable;
+			alignment = <0 0x400000>;
+			size = <0 0x1000000>;
+		};
+
+		qseecom_ta_mem: qseecom_ta_region {
+			compatible = "shared-dma-pool";
+			alloc-ranges = <0 0x00000000 0 0xffffffff>;
+			reusable;
+			alignment = <0 0x400000>;
+			size = <0 0x400000>;
+		};
+
+		adsp_mem: adsp_region@0 {
+			compatible = "shared-dma-pool";
+			reusable;
+			size = <0 0x400000>;
+		};
+
+		dfps_data_mem: dfps_data_mem@90000000 {
+			reg = <0 0x90000000 0 0x1000>;
+			label = "dfps_data_mem";
+			status = "disabled";
+		};
+
+		cont_splash_mem: splash_region@0x90001000 {
+			reg = <0x0 0x90001000 0x0 0x13ff000>;
+			label = "cont_splash_mem";
+		};
+
+		gpu_mem: gpu_region@0 {
+			compatible = "shared-dma-pool";
+			reusable;
+			alloc-ranges = <0x0 0x80000000 0x0 0x10000000>;
+			alignment = <0 0x400000>;
+			size = <0 0x800000>;
+		};
+
+		dump_mem: mem_dump_region {
+			compatible = "shared-dma-pool";
+			reusable;
+			size = <0x400000>;
+		};
+
+*/
+};
+
+static const dcfg_simple_t uart_driver = {
+    .mmio_phys = 0x078af000,
+    .irq = 107 + 32,
+};
+
+static const dcfg_arm_gicv2_driver_t gicv2_driver = {
+    .mmio_phys = 0x0b000000,
+    .gicd_offset = 0x00000,
+    .gicc_offset = 0x1000,
+    .ipi_base = 5,
+};
+
+static const dcfg_arm_psci_driver_t psci_driver = {
+    .use_hvc = false,
+};
+
+static const dcfg_arm_generic_timer_driver_t timer_driver = {
+    .irq_phys = 16 + 14,    // PHYS_NONSECURE_PPI: GIC_PPI 14
+    .irq_virt = 16 + 11,    // VIRT_PPI: GIC_PPI 11
+};
+
+static const zbi_platform_id_t platform_id = {
+    .vid = PDEV_VID_QUALCOMM,
+    .pid = PDEV_PID_QUALCOMM_MSM8X53,
+    .board_name = "msm8x53-som",
+};
+
+static void append_board_boot_item(zbi_header_t* bootdata) {
+    // add CPU configuration
+    append_boot_item(bootdata, ZBI_TYPE_CPU_CONFIG, 0, &cpu_config,
+                    sizeof(zbi_cpu_config_t) +
+                    sizeof(zbi_cpu_cluster_t) * cpu_config.cluster_count);
+
+    // add memory configuration
+    append_boot_item(bootdata, ZBI_TYPE_MEM_CONFIG, 0, &mem_config,
+                    sizeof(zbi_mem_range_t) * countof(mem_config));
+
+    // add kernel drivers
+    append_boot_item(bootdata, ZBI_TYPE_KERNEL_DRIVER, KDRV_MSM_UART, &uart_driver,
+                    sizeof(uart_driver));
+    append_boot_item(bootdata, ZBI_TYPE_KERNEL_DRIVER, KDRV_ARM_GIC_V2, &gicv2_driver,
+                    sizeof(gicv2_driver));
+    append_boot_item(bootdata, ZBI_TYPE_KERNEL_DRIVER, KDRV_ARM_PSCI, &psci_driver,
+                    sizeof(psci_driver));
+    append_boot_item(bootdata, ZBI_TYPE_KERNEL_DRIVER, KDRV_ARM_GENERIC_TIMER, &timer_driver,
+                    sizeof(timer_driver));
+
+    // add platform ID
+    append_boot_item(bootdata, ZBI_TYPE_PLATFORM_ID, 0, &platform_id, sizeof(platform_id));
+}
diff --git a/kernel/target/arm64/board/msm8x53-som/device-tree.dtb b/kernel/target/arm64/board/msm8x53-som/device-tree.dtb
new file mode 100644
index 0000000..32c65a4
--- /dev/null
+++ b/kernel/target/arm64/board/msm8x53-som/device-tree.dtb
Binary files differ
diff --git a/kernel/target/arm64/board/msm8x53-som/device-tree.dts b/kernel/target/arm64/board/msm8x53-som/device-tree.dts
new file mode 100644
index 0000000..46a674d
--- /dev/null
+++ b/kernel/target/arm64/board/msm8x53-som/device-tree.dts
@@ -0,0 +1,19 @@
+/dts-v1/;
+
+/ {
+	model = "Qualcomm Technologies, Inc. APQ 8953 Lite";
+	qcom,msm-id = <304 0x0>;
+    qcom,pmic-id = <0x10016 0x20011 0x0 0x0>;
+    qcom,board-id = <0x01010020 0>;
+
+    chosen {
+        // need a dummy bootargs for bootloader to append to
+        bootargs = "";
+    };
+
+	memory {
+		device_type = "memory";
+	};
+};
+
+// [730] Best match DTB tags 304/01010020/0x00000001/0/10016/20011/0/0/a12f1f36/3c2e4
diff --git a/kernel/target/arm64/board/msm8x53-som/foo b/kernel/target/arm64/board/msm8x53-som/foo
new file mode 100644
index 0000000..290f058
--- /dev/null
+++ b/kernel/target/arm64/board/msm8x53-som/foo
@@ -0,0 +1,16 @@
+/dts-v1/;
+
+/ {
+	model = "Qualcomm Technologies, Inc. APQ 8953 Lite";
+	qcom,msm-id = <0x130 0x0>;
+	qcom,pmic-id = <0x10016 0x20011 0x0 0x0>;
+	qcom,board-id = <0x1010020 0x0>;
+
+	chosen {
+		bootargs = [00];
+	};
+
+	memory {
+		device_type = "memory";
+	};
+};
diff --git a/kernel/target/arm64/board/msm8x53-som/package-image.sh b/kernel/target/arm64/board/msm8x53-som/package-image.sh
new file mode 100755
index 0000000..f1384ae
--- /dev/null
+++ b/kernel/target/arm64/board/msm8x53-som/package-image.sh
@@ -0,0 +1,17 @@
+#!/usr/bin/env bash
+
+# Copyright 2018 The Fuchsia Authors
+#
+# Use of this source code is governed by a MIT-style
+# license that can be found in the LICENSE file or at
+# https://opensource.org/licenses/MIT
+
+set -eo pipefail
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+ZIRCON_DIR=${DIR}/../../../../..
+SCRIPTS_DIR=${ZIRCON_DIR}/scripts
+
+${SCRIPTS_DIR}/package-image.sh -a -b msm8x53-som \
+    -d $ZIRCON_DIR/kernel/target/arm64/board/msm8x53-som/device-tree.dtb -D append \
+    -g -m -M 0x00000000 $@
diff --git a/kernel/target/arm64/board/msm8x53-som/rules.mk b/kernel/target/arm64/board/msm8x53-som/rules.mk
new file mode 100644
index 0000000..8609d29
--- /dev/null
+++ b/kernel/target/arm64/board/msm8x53-som/rules.mk
@@ -0,0 +1,9 @@
+# Copyright 2018 The Fuchsia Authors
+#
+# Use of this source code is governed by a MIT-style
+# license that can be found in the LICENSE file or at
+# https://opensource.org/licenses/MIT
+
+PLATFORM_BOARD_NAME := msm8x53-som
+
+include kernel/target/arm64/boot-shim/rules.mk
diff --git a/kernel/target/arm64/board/msm8x53-som/update-device-tree.sh b/kernel/target/arm64/board/msm8x53-som/update-device-tree.sh
new file mode 100755
index 0000000..050cd90
--- /dev/null
+++ b/kernel/target/arm64/board/msm8x53-som/update-device-tree.sh
@@ -0,0 +1 @@
+dtc -I dts -o device-tree.dtb -O dtb device-tree.dts
diff --git a/kernel/target/arm64/boot-shim/boot-shim.S b/kernel/target/arm64/boot-shim/boot-shim.S
index fdeb533..24f602a 100644
--- a/kernel/target/arm64/boot-shim/boot-shim.S
+++ b/kernel/target/arm64/boot-shim/boot-shim.S
@@ -74,6 +74,56 @@
     br      x1
 END_FUNCTION(_start)
 
+/* void arch_invalidate_cache_all()
+ *      should only be used early in boot, prior to enabling mmu/cache
+ */
+FUNCTION(arch_invalidate_cache_all)
+    mrs     x0, clidr_el1
+    and     w3, w0, #0x07000000
+    lsr     w3, w3, #23
+    cbz     w3, finished
+    mov     w10, #0
+    mov     w8, #1
+loop1:
+    add     w2, w10, w10, lsr #1
+    lsr     w1, w0, w2
+    and     w1, w1, #0x7
+    cmp     w1, #2
+    b.lt    skip
+    msr     csselr_el1, x10
+    isb
+    mrs     x1, ccsidr_el1
+    and     w2, w1, #7
+    add     w2, w2, #4
+    ubfx    w4, w1, #3, #10
+    clz     w5, w4
+    lsl     w9, w4, w5
+
+    lsl     w16, w8, w5
+
+loop2:
+    ubfx    w7, w1, #13, #15
+    lsl     w7, w7, w2
+    lsl     w17, w8, w2
+loop3:
+    orr     w11, w10, w9
+    orr     w11, w11, w7
+    dc      isw, x11
+    subs    w7, w7, w17
+    b.ge    loop3
+
+    subs    x9, x9, x16
+    b.ge    loop2
+skip:
+    add     w10, w10, #2
+    cmp     w3, w10
+    dsb     sy
+    b.gt    loop1
+finished:
+    ic      iallu
+    ret
+END_FUNCTION(arch_invalidate_cache_all)
+
 .bss
 .balign 16
 LOCAL_DATA(stack)
diff --git a/kernel/target/arm64/boot-shim/boot-shim.c b/kernel/target/arm64/boot-shim/boot-shim.c
index e55edbe..c09aa79 100644
--- a/kernel/target/arm64/boot-shim/boot-shim.c
+++ b/kernel/target/arm64/boot-shim/boot-shim.c
@@ -399,5 +399,8 @@
     uart_puts(" with ZBI ");
     uart_print_hex((uintptr_t)result.zbi);
     uart_puts("\n");
+
+arch_invalidate_cache_all();
+
     return result;
 }
diff --git a/kernel/target/arm64/boot-shim/boot-shim.h b/kernel/target/arm64/boot-shim/boot-shim.h
index 03eb0a3..adf35da 100644
--- a/kernel/target/arm64/boot-shim/boot-shim.h
+++ b/kernel/target/arm64/boot-shim/boot-shim.h
@@ -16,3 +16,5 @@
 } boot_shim_return_t;
 
 boot_shim_return_t boot_shim(void* device_tree);
+
+void arch_invalidate_cache_all(void);
diff --git a/kernel/target/arm64/boot-shim/debug.h b/kernel/target/arm64/boot-shim/debug.h
index 3d9db66..5c268e6 100644
--- a/kernel/target/arm64/boot-shim/debug.h
+++ b/kernel/target/arm64/boot-shim/debug.h
@@ -7,7 +7,7 @@
 #include <stdint.h>
 
 // Uncomment to enable debug UART.
-// #define DEBUG_UART 1
+#define DEBUG_UART 1
 
 // Board specific.
 void uart_pputc(char c);
diff --git a/kernel/target/arm64/boot-shim/msm8x53-som-uart.c b/kernel/target/arm64/boot-shim/msm8x53-som-uart.c
new file mode 100644
index 0000000..59eef91
--- /dev/null
+++ b/kernel/target/arm64/boot-shim/msm8x53-som-uart.c
@@ -0,0 +1,39 @@
+// Copyright 2018 The Fuchsia Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style license that can be
+// found in the LICENSE file.
+
+#include <zircon/compiler.h>
+#include <stdint.h>
+#include "debug.h"
+
+#define UART_DM_N0_CHARS_FOR_TX             0x0040
+#define UART_DM_CR_CMD_RESET_TX_READY       (3 << 8)
+
+#define UART_DM_SR                          0x00A4
+#define UART_DM_SR_TXRDY                    (1 << 2)
+#define UART_DM_SR_TXEMT                    (1 << 3)
+
+#define UART_DM_TF                          0x0100
+
+#define UARTREG(reg) (*(volatile uint32_t*)(0x078af000 + (reg)))
+
+void uart_pputc(char c) {
+    while (!(UARTREG(UART_DM_SR) & UART_DM_SR_TXEMT)) {
+        ;
+    }
+    UARTREG(UART_DM_N0_CHARS_FOR_TX) = UART_DM_CR_CMD_RESET_TX_READY;
+    UARTREG(UART_DM_N0_CHARS_FOR_TX) = 1;
+    __UNUSED uint32_t foo = UARTREG(UART_DM_N0_CHARS_FOR_TX);
+
+    // wait for TX ready
+    while (!(UARTREG(UART_DM_SR) & UART_DM_SR_TXRDY))
+        ;
+
+    *((volatile uint32_t*)(0x078af100)) = c;
+
+//    UARTREG(UART_DM_TF) = c;
+
+    // wait for TX ready
+    while (!(UARTREG(UART_DM_SR) & UART_DM_SR_TXRDY))
+        ;
+}
diff --git a/kernel/top/main.cpp b/kernel/top/main.cpp
index 1ce156c..f7c3fe2 100644
--- a/kernel/top/main.cpp
+++ b/kernel/top/main.cpp
@@ -26,6 +26,8 @@
 #include <vm/vm.h>
 #include <zircon/compiler.h>
 
+#include <arch/arm64/periphmap.h>
+
 extern void (*const __init_array_start[])();
 extern void (*const __init_array_end[])();
 
@@ -40,6 +42,16 @@
 
 // called from arch code
 void lk_main() {
+
+/*
+add_periph_range(0, 0x10000000);
+uint32_t* uart_tx = (uint32_t*)periph_paddr_to_vaddr(0x078af040);
+uint32_t* uart = (uint32_t*)periph_paddr_to_vaddr(0x078af100);
+*uart_tx = 0x30;
+*uart_tx = 1;
+*uart = 'V';
+*/
+
     // serial prints to console based on compile time switch
     dlog_bypass_init_early();
 
diff --git a/scripts/flash-msm8x53-som b/scripts/flash-msm8x53-som
new file mode 100755
index 0000000..c605a9d
--- /dev/null
+++ b/scripts/flash-msm8x53-som
@@ -0,0 +1,16 @@
+#!/usr/bin/env bash
+
+# Copyright 2018 The Fuchsia Authors
+#
+# Use of this source code is governed by a MIT-style
+# license that can be found in the LICENSE file or at
+# https://opensource.org/licenses/MIT
+
+set -eo pipefail
+
+SCRIPTS_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+ZIRCON_DIR=${SCRIPTS_DIR}/..
+
+${ZIRCON_DIR}/kernel/target/arm64/board/msm8x53-som/package-image.sh -B ${ZIRCON_DIR}/build-arm64
+
+${SCRIPTS_DIR}/flash-avb -b msm8x53-som
diff --git a/system/public/zircon/boot/driver-config.h b/system/public/zircon/boot/driver-config.h
index 4913ae1..d017f20 100644
--- a/system/public/zircon/boot/driver-config.h
+++ b/system/public/zircon/boot/driver-config.h
@@ -18,6 +18,7 @@
 #define KDRV_MT8167_UART        0x5538544D  // 'MT8U'
 #define KDRV_HISILICON_POWER    0x4F505348  // 'HSPO'
 #define KDRV_AMLOGIC_HDCP       0x484C4D41  // 'AMLH'
+#define KDRV_MSM_UART           0x554D534D  // 'MSMU'
 
 // kernel driver struct that can be used for simple drivers
 // used by KDRV_PL011_UART, KDRV_AMLOGIC_UART and KDRV_NXP_IMX_UART
diff --git a/system/ulib/ddk/include/ddk/platform-defs.h b/system/ulib/ddk/include/ddk/platform-defs.h
index 1647596..bab9fcb 100644
--- a/system/ulib/ddk/include/ddk/platform-defs.h
+++ b/system/ulib/ddk/include/ddk/platform-defs.h
@@ -153,4 +153,8 @@
 #define PDEV_VID_HISILICON          15
 #define PDEV_PID_CORNEL             1
 
+// Qualcomm
+#define PDEV_VID_QUALCOMM           16
+#define PDEV_PID_QUALCOMM_MSM8X53    1
+
 __END_CDECLS;