diff --git a/firmware/Application/ButtonManager.cpp b/firmware/Application/ButtonManager.cpp
index 7636d81..d0866c3 100644
--- a/firmware/Application/ButtonManager.cpp
+++ b/firmware/Application/ButtonManager.cpp
@@ -23,7 +23,8 @@
 #include "Bsp/SystemTime.h"
 #include "Bsp/macros.h"
 
-#include "stm32l0xx.h"
+
+#include "Mcu.h"
 
 namespace BSP {
 
@@ -36,20 +37,33 @@ ReturnCode ButtonManager::init()
 {
     for (auto &btn: m_buttons) {
         /** Enable pin for input (pulled up) */
-        CLR(GPIOA->MODER, 3u << (2 * btn.m_gpio_idx));
-        CLR(GPIOA->OTYPER, 1u << btn.m_gpio_idx);
-        SET_TO(GPIOA->PUPDR, 3u << (2 * btn.m_gpio_idx), 1u << (2 * btn.m_gpio_idx));
+        uint32_t index = btn.m_pin.get_index();
 
+#if defined(STM32L0XX)
         // Unmask this interrupt
-        SET(EXTI->IMR, 1u << btn.m_gpio_idx);
+        SET(EXTI->IMR, 1u << index);
         // Enable this interrupt
-        SET(EXTI->EMR, 1u << btn.m_gpio_idx);
+        SET(EXTI->EMR, 1u << index);
         // Enable interrupt for rising edge
-        SET(EXTI->RTSR, 1u << btn.m_gpio_idx);
+        SET(EXTI->RTSR, 1u << index);
         // Enable interrupt for falling edge
-        SET(EXTI->FTSR, 1u << btn.m_gpio_idx);
+        SET(EXTI->FTSR, 1u << index);
+#elif defined(STM32L4XX)
+        // Unmask this interrupt
+        SET(EXTI->IMR1, 1u << index);
+        // Enable this interrupt
+        SET(EXTI->EMR1, 1u << index);
+        // Enable interrupt for rising edge
+        SET(EXTI->RTSR1, 1u << index);
+        // Enable interrupt for falling edge
+        SET(EXTI->FTSR1, 1u << index);
+
+#else
+#error "Unsupported device type"
+#endif
     }
 
+#if defined (STM32L0XX)
     CLR(SYSCFG->EXTICR[0],
         SYSCFG_EXTICR1_EXTI3 | SYSCFG_EXTICR1_EXTI2 |
         SYSCFG_EXTICR1_EXTI1 | SYSCFG_EXTICR1_EXTI0);
@@ -61,6 +75,31 @@ ReturnCode ButtonManager::init()
     NVIC_SetPriority(EXTI0_1_IRQn, 1);
     NVIC_SetPriority(EXTI2_3_IRQn, 1);
     NVIC_SetPriority(EXTI4_15_IRQn, 1);
+#elif defined(STM32L4XX)
+    CLR(SYSCFG->EXTICR[0],
+        SYSCFG_EXTICR1_EXTI3 | SYSCFG_EXTICR1_EXTI2 |
+        SYSCFG_EXTICR1_EXTI1 | SYSCFG_EXTICR1_EXTI0);
+
+    NVIC_EnableIRQ(EXTI0_IRQn);
+    NVIC_EnableIRQ(EXTI1_IRQn);
+    NVIC_EnableIRQ(EXTI2_IRQn);
+    NVIC_EnableIRQ(EXTI3_IRQn);
+    NVIC_EnableIRQ(EXTI4_IRQn);
+    NVIC_EnableIRQ(EXTI9_5_IRQn);
+    NVIC_EnableIRQ(EXTI15_10_IRQn);
+
+    NVIC_SetPriority(EXTI0_IRQn, 1);
+    NVIC_SetPriority(EXTI1_IRQn, 1);
+    NVIC_SetPriority(EXTI2_IRQn, 1);
+    NVIC_SetPriority(EXTI3_IRQn, 1);
+    NVIC_SetPriority(EXTI4_IRQn, 1);
+    NVIC_SetPriority(EXTI9_5_IRQn, 1);
+    NVIC_SetPriority(EXTI15_10_IRQn, 1);
+
+#else
+#error "Unsupported device type"
+#endif
+
 
     return ReturnCode::OK;
 }
@@ -71,7 +110,6 @@ NextTime ButtonManager::execute()
     BSP::time_t endtime = 0;
     BSP::SystemTimer::get_time(systime);
 
-
     for (auto &btn: m_buttons) {
         // Has the state changed?
         if (btn.m_prev_call_state != btn.m_state) {
@@ -120,30 +158,36 @@ void ButtonManager::remove_callback(Button btn)
 
 void ButtonManager::irq()
 {
-    uint32_t idr = GPIOA->IDR;
     static BSP::time_t systime;
 
     // TODO: is this call too expensive for an interrupt handler?
-    BSP::SystemTimer::get_time(systime);
+    //BSP::SystemTimer::get_time(systime);
 
     if (!m_instance) {
         return;
     }
 
     for (auto &btn: m_instance->m_buttons) {
-        bool is_pressed = !(idr & (1 << btn.m_gpio_idx));
+        // Buttons are pulled up. Maybe this will be configurable one day.
+        bool is_pressed = !btn.m_pin.read();
 
         // Check if the button state has changed, and timestamp it
         if (is_pressed && (btn.m_state == ButtonState::NOT_PRESSED)) {
-            btn.m_state = ButtonState::PRESSED;
             btn.m_state_change_ts = systime;
         } else if (!is_pressed && (btn.m_state == ButtonState::PRESSED)) {
-            btn.m_state = ButtonState::NOT_PRESSED;
             btn.m_state_change_ts = systime;
         }
 
+        btn.m_state = is_pressed ? ButtonState::PRESSED : ButtonState::NOT_PRESSED;
+
         // Clear the event
-        SET(EXTI->PR, 1u << btn.m_gpio_idx);
+#if defined(STM32L0XX)
+        SET(EXTI->PR, 1u << btn.m_pin.get_index());
+#elif defined (STM32L4XX)
+        SET(EXTI->PR1, 1u << btn.m_pin.get_index());
+#else
+        #error "Unsupported device type"
+#endif
     }
 
     if (m_instance != nullptr) {
diff --git a/firmware/Application/ButtonManager.h b/firmware/Application/ButtonManager.h
index ae5c4e9..80ab409 100644
--- a/firmware/Application/ButtonManager.h
+++ b/firmware/Application/ButtonManager.h
@@ -26,22 +26,23 @@
 #include "Bsp/TaskScheduler.h"
 #include "Bsp/Task.h"
 #include "Bsp/ReturnCode.h"
+#include "Bsp/Drivers/GpioDriver.h"
 
 namespace BSP {
 
 class ButtonManager : public BSP::Schedule::Task {
 public:
     ButtonManager(BSP::Schedule::TaskScheduler &scheduler,
-                  uint8_t up_gpio_idx,
-                  uint8_t mid_gpio_idx,
-                  uint8_t down_gpio_idx,
+                  GpioPin &btn_down,
+                  GpioPin &btn_mid,
+                  GpioPin &btn_up,
                   BSP::time_t debounce_time)
         : m_scheduler(scheduler)
         , m_buttons
         {
-         button_state(up_gpio_idx, debounce_time, ButtonManager::nop_callback),
-         button_state(mid_gpio_idx, debounce_time, ButtonManager::nop_callback),
-         button_state(down_gpio_idx, debounce_time, ButtonManager::nop_callback),
+         button_state(btn_down, debounce_time, ButtonManager::nop_callback),
+         button_state(btn_mid, debounce_time, ButtonManager::nop_callback),
+         button_state(btn_up, debounce_time, ButtonManager::nop_callback),
         }
     {
         m_instance = this;
@@ -78,16 +79,16 @@ private:
 
     struct button_state {
 
-        button_state(uint8_t gpio_index,
+        button_state(GpioPin &pin,
                      BSP::time_t debounce_time,
                      ChangeCallback callback)
-            : m_gpio_idx(gpio_index)
+            : m_pin(pin)
             , m_debounce_time(debounce_time)
             , m_state(ButtonState::NOT_PRESSED)
             , m_state_change_ts(0)
             , m_callback(callback)
         {}
-        uint8_t const m_gpio_idx;
+        GpioPin &m_pin;
         BSP::time_t const m_debounce_time;
 
         ButtonState m_prev_call_state; /*<! The last state the button was in when it the callback was called */
diff --git a/firmware/Bsp/Drivers/GpioDriver.cpp b/firmware/Bsp/Drivers/GpioDriver.cpp
new file mode 100644
index 0000000..a71695e
--- /dev/null
+++ b/firmware/Bsp/Drivers/GpioDriver.cpp
@@ -0,0 +1,215 @@
+
+#include "Bsp/Drivers/GpioDriver.h"
+#include "Bsp/macros.h"
+
+namespace BSP {
+
+    /*********************************************
+     *
+     * GpioPin
+     *
+     *********************************************/
+
+    GpioDriver::GpioDriver(GPIO_TypeDef *gpio)
+        : m_gpio(gpio)
+    {}
+
+    void GpioDriver::init() {
+        return;
+    }
+
+    void GpioDriver::enable() {
+#if defined(STM32L0XX)
+        if (m_gpio == GPIOA) {
+            SET(RCC->IOPENR, RCC_IOPENR_IOPAEN);
+        } else if (m_gpio == GPIOB) {
+            SET(RCC->IOPENR, RCC_IOPENR_IOPBEN);
+        } else if (m_gpio == GPIOC) {
+            SET(RCC->IOPENR, RCC_IOPENR_IOPCEN);
+        } else if (m_gpio == GPIOH) {
+            SET(RCC->IOPENR, RCC_IOPENR_IOPHEN);
+        }
+#elif defined(STM32L4XX)
+        if (m_gpio == GPIOA) {
+            SET(RCC->AHB2ENR, RCC_AHB2ENR_GPIOAEN);
+        } else if (m_gpio == GPIOB) {
+            SET(RCC->AHB2ENR, RCC_AHB2ENR_GPIOBEN);
+        } else if (m_gpio == GPIOC) {
+            SET(RCC->AHB2ENR, RCC_AHB2ENR_GPIOCEN);
+        } else if (m_gpio == GPIOH) {
+            SET(RCC->AHB2ENR, RCC_AHB2ENR_GPIOHEN);
+        }
+#else
+#error "Unknown device family"
+#endif
+    }
+
+    void GpioDriver::disable() {
+#if defined(STM32L0XX)
+        if (m_gpio == GPIOA) {
+            CLR(RCC->IOPENR, RCC_IOPENR_IOPAEN);
+        } else if (m_gpio == GPIOB) {
+            CLR(RCC->IOPENR, RCC_IOPENR_IOPBEN);
+        } else if (m_gpio == GPIOC) {
+            CLR(RCC->IOPENR, RCC_IOPENR_IOPCEN);
+        } else if (m_gpio == GPIOH) {
+            CLR(RCC->IOPENR, RCC_IOPENR_IOPHEN);
+        }
+#elif defined(STM32L4XX)
+        if (m_gpio == GPIOA) {
+            CLR(RCC->AHB2ENR, RCC_AHB2ENR_GPIOAEN);
+        } else if (m_gpio == GPIOB) {
+            CLR(RCC->AHB2ENR, RCC_AHB2ENR_GPIOBEN);
+        } else if (m_gpio == GPIOC) {
+            CLR(RCC->AHB2ENR, RCC_AHB2ENR_GPIOCEN);
+        } else if (m_gpio == GPIOH) {
+            CLR(RCC->AHB2ENR, RCC_AHB2ENR_GPIOHEN);
+        }
+#else
+#error "Unknown device family"
+#endif
+    }
+
+    void GpioDriver::set_pin_moder(uint32_t index, moder_t mode) {
+        uint32_t moder_value = 0;
+        switch (mode) {
+        case moder_t::INPUT:
+            moder_value = 0;
+            break;
+        case moder_t::OUTPUT:
+            moder_value = 1;
+            break;
+        case moder_t::ALTERNATE_FUNCTION:
+            moder_value = 2;
+            break;
+        }
+        SET_STRIDE_TO(m_gpio->MODER, 2, index,  moder_value);
+    }
+    
+    void GpioDriver::set_pin_pupdr(uint32_t index, input_pull_t pull_mode) {
+        uint32_t pupdr_value = 0;
+        switch (pull_mode) {
+        case input_pull_t::FLOATING:
+            pupdr_value = 0;
+            break;
+        case input_pull_t::PULL_UP:
+            pupdr_value = 1;
+            break;
+        case input_pull_t::PULL_DOWN:
+            pupdr_value = 2;
+            break;
+        }
+        SET_STRIDE_TO(m_gpio->PUPDR, 2, index, pupdr_value);
+    }
+
+    void GpioDriver::set_pin_otyper(uint32_t index, output_mode_t mode) {
+        uint32_t mask = 1 << index;
+        if (mode == output_mode_t::OPEN_DRAIN) {
+            SET(m_gpio->OTYPER, mask);
+        } else {
+            SET(m_gpio->OTYPER, mask);
+        }
+    }
+
+    void GpioDriver::set_pin_ospeedr(uint32_t index, output_speed_t speed) {
+        uint32_t speed_val = 0;
+
+        if (speed == output_speed_t::LOW) {
+            speed_val = 0;
+        } else if (speed == output_speed_t::MEDIUM) {
+            speed_val = 1;
+        } else if (speed == output_speed_t::HIGH) {
+            speed_val = 2;
+        } else if (speed == output_speed_t::VERY_HIGH) {
+            speed_val = 3;
+        }
+
+        SET_STRIDE_TO(m_gpio->OSPEEDR, 2, index, speed_val);
+    }
+
+    void GpioDriver::configure_pin_input(uint32_t index, input_pull_t pull_mode) {
+        if (index > 15) {
+            return;
+        }
+
+        set_pin_pupdr(index, pull_mode);
+        set_pin_moder(index, moder_t::INPUT);
+    }
+
+    void GpioDriver::configure_pin_output(uint32_t index, output_mode_t mode, output_speed_t speed) {
+        if (index > 15) {
+            return;
+        }
+
+        set_pin_moder(index, moder_t::INPUT); // Avoid glitches during configuration
+        set_pin_otyper(index, mode);
+        set_pin_ospeedr(index, speed);
+        set_pin_moder(index, moder_t::OUTPUT);
+    }
+
+    void GpioDriver::configure_pin_alternate_function(uint32_t index, uint32_t function) {
+        if (index > 15) {
+            return;
+        }
+        if (function > 7) {
+            return;
+        }
+
+        set_pin_moder(index, moder_t::INPUT); // Avoid glitches during configuration
+        if (index < 8) {
+            SET_STRIDE_TO(m_gpio->AFR[0], 4, index, function);
+        } else {
+            SET_STRIDE_TO(m_gpio->AFR[1], 4, index - 8, function);
+        }
+        set_pin_moder(index, moder_t::ALTERNATE_FUNCTION);
+    }
+
+    bool GpioDriver::read_pin(uint32_t index) {
+        return (m_gpio->IDR >> index) & 1;
+    }
+
+    void GpioDriver::write_pin(uint32_t index, bool value) {
+        SET_STRIDE_TO(m_gpio->ODR, 1, index, value);
+    }
+
+    /*********************************************
+     *
+     * GpioPin
+     *
+     *********************************************/
+
+    GpioPin::GpioPin(GpioDriver &driver, uint32_t index)
+        : m_gpio(driver)
+        , m_index(index)
+    {}
+
+    void GpioPin::configure_input(GpioDriver::input_pull_t pull_mode) {
+        m_gpio.configure_pin_input(m_index, pull_mode);
+    }
+
+    void GpioPin::configure_output(GpioDriver::output_mode_t mode, GpioDriver::output_speed_t speed) {
+        m_gpio.configure_pin_output(m_index, mode, speed);
+    }
+
+    void GpioPin::configure_alternate_function(uint32_t function) {
+        m_gpio.configure_pin_alternate_function(m_index, function);
+    }
+
+    bool GpioPin::read() {
+        return m_gpio.read_pin(m_index);
+    }
+
+    void GpioPin::write(bool val) {
+        return m_gpio.write_pin(m_index, val);
+    }
+
+    uint32_t GpioPin::get_index() {
+        return m_index;
+    }
+
+    GpioDriver& GpioPin::get_driver() {
+        return m_gpio;
+    }
+
+
+}
diff --git a/firmware/Bsp/Drivers/GpioDriver.h b/firmware/Bsp/Drivers/GpioDriver.h
new file mode 100644
index 0000000..f0baae2
--- /dev/null
+++ b/firmware/Bsp/Drivers/GpioDriver.h
@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) 2019  Max Regan
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include "Bsp/ReturnCode.h"
+#include "Bsp/TaskScheduler.h"
+
+#include "Mcu.h"
+
+namespace BSP {
+
+class GpioDriver {
+public:
+    GpioDriver(GPIO_TypeDef *gpio);
+
+    void init();
+    void enable();
+    void disable();
+
+    enum class output_mode_t {
+        PUSH_PULL,
+        OPEN_DRAIN,
+    };
+
+    enum class output_speed_t {
+        LOW,
+        MEDIUM,
+        HIGH,
+        VERY_HIGH,
+    };
+
+    enum class input_pull_t {
+        FLOATING,
+        PULL_UP,
+        PULL_DOWN,
+    };
+
+    friend class GpioPin;
+
+private:
+
+    enum class moder_t {
+        INPUT,
+        OUTPUT,
+        ALTERNATE_FUNCTION,
+    };
+
+    void set_pin_moder(uint32_t index, moder_t mode);
+    void set_pin_pupdr(uint32_t index, input_pull_t pull_mode);
+    void set_pin_otyper(uint32_t index, output_mode_t mode);
+    void set_pin_ospeedr(uint32_t index, output_speed_t speed);
+
+    void configure_pin_input(uint32_t index, input_pull_t pull_mode);
+    void configure_pin_output(uint32_t index, output_mode_t mode, output_speed_t speed);
+    void configure_pin_alternate_function(uint32_t index, uint32_t function);
+
+    void write_pin(uint32_t index, bool value);
+    bool read_pin(uint32_t index);
+
+    GPIO_TypeDef * const m_gpio;
+};
+
+class GpioPin {
+public:
+    GpioPin(GpioDriver &m_driver, uint32_t index);
+
+    void configure_input(GpioDriver::input_pull_t pull_mode);
+    void configure_output(GpioDriver::output_mode_t mode, GpioDriver::output_speed_t speed);
+    void configure_alternate_function(uint32_t function);
+
+    void write(bool value);
+    bool read();
+
+    GpioDriver &get_driver();
+    uint32_t get_index();
+
+private:
+    GpioDriver &m_gpio;
+    uint32_t const m_index;
+};
+
+}
diff --git a/firmware/Bsp/Drivers/LowPower.cpp b/firmware/Bsp/Drivers/LowPower.cpp
index 37bdfdf..e01933b 100644
--- a/firmware/Bsp/Drivers/LowPower.cpp
+++ b/firmware/Bsp/Drivers/LowPower.cpp
@@ -23,7 +23,7 @@
 #include "Bsp/Drivers/LowPower.h"
 #include "Bsp/macros.h"
 
-#include "stm32l0xx.h"
+#include "Mcu.h"
 
 uint32_t wakeups = 0;
 
@@ -56,7 +56,12 @@ ReturnCode LowPower::disable_debug()
     CLR(DBGMCU->CR, DBGMCU_CR_DBG_SLEEP);
     CLR(DBGMCU->CR, DBGMCU_CR_DBG_STANDBY);
 
+#if defined(STM32L0XX)
     CLR(RCC->APB2SMENR, RCC_APB2SMENR_DBGSMEN);
+#elif defined(STM32L4XX)
+#else
+    #error "Unsupported device type"
+#endif
 
     return ReturnCode::OK;
 }
@@ -70,12 +75,24 @@ ReturnCode LowPower::sleep()
 
 ReturnCode LowPower::stop()
 {
+#if defined(STM32L0XX)
     SET(PWR->CR, PWR_CR_CWUF); // clear wakeup flag
     while(PWR->CSR & PWR_CSR_WUF) {};
 
     CLR(PWR->CR, PWR_CR_PDDS); // Enter stop mode when the CPU enters deepsleep
     CLR(RCC->CFGR, RCC_CFGR_STOPWUCK); // MSI oscillator is wake-up from stop clock
     SET(SCB->SCR, SCB_SCR_SLEEPDEEP_Msk); // low-power mode = stop mode
+#elif defined(STM32L4XX)
+    SET(PWR->SCR, PWR_SCR_CWUF1); // clear wakeup flag
+
+    while(PWR->SR1 & PWR_SR1_WUF1) {};
+    SET_TO(PWR->CR1, PWR_CR1_LPMS, 1 << PWR_CR1_LPMS_Pos); // Enter stop mode 1 when the CPU enters deepsleep
+    CLR(RCC->CFGR, RCC_CFGR_STOPWUCK); // MSI oscillator is wake-up from stop clock
+    SET(SCB->SCR, SCB_SCR_SLEEPDEEP_Msk); // low-power mode = stop mode
+
+#else
+    #error "Unsupported device type"
+#endif
     __WFI(); // enter low-power mode (Wake from interrupt)
 
     wakeups++;
diff --git a/firmware/Bsp/Drivers/LptimPwm.cpp b/firmware/Bsp/Drivers/LptimPwm.cpp
index 91ee0f7..faa2d78 100644
--- a/firmware/Bsp/Drivers/LptimPwm.cpp
+++ b/firmware/Bsp/Drivers/LptimPwm.cpp
@@ -34,9 +34,15 @@ LptimPwm::LptimPwm(LPTIM_TypeDef *lptim)
 void LptimPwm::init_lptim()
 {
     /* Enable LPTIM in APB1 */
+#if defined(STM32L0XX)
     SET(RCC->APB1ENR,
         RCC_APB1ENR_LPTIM1EN);
-
+#elif defined(STM32L4XX)
+    SET(RCC->APB1ENR1,
+        RCC_APB1ENR1_LPTIM1EN);
+#else
+#error "Unsupported family"
+#endif
     // Enable low-speed internal
     RCC->CSR |= RCC_CSR_LSION;
     while (!(RCC->CSR & RCC_CSR_LSIRDY)) {};
@@ -90,21 +96,6 @@ ReturnCode LptimPwm::init()
 {
     init_lptim();
 
-    /* Enable GPIO port A */
-    SET(RCC->IOPENR, RCC_IOPENR_IOPAEN);
-
-    /* Assign LPTIM1_OUT to PA7 */
-    SET_TO(GPIOA->AFR[0],
-           GPIO_AFRL_AFRL7,
-           1u << GPIO_AFRL_AFRL7_Pos);
-
-    SET_TO(GPIOA->MODER,
-           GPIO_MODER_MODE7,
-           2u << GPIO_MODER_MODE7_Pos);
-
-    CLR(GPIOA->OTYPER, GPIO_OTYPER_OT_7);
-    CLR(GPIOA->PUPDR, GPIO_PUPDR_PUPD7);
-
     return ReturnCode::OK;
 }
 
diff --git a/firmware/Bsp/Drivers/LptimPwm.h b/firmware/Bsp/Drivers/LptimPwm.h
index b66ec91..044a3d8 100644
--- a/firmware/Bsp/Drivers/LptimPwm.h
+++ b/firmware/Bsp/Drivers/LptimPwm.h
@@ -22,7 +22,7 @@
 #pragma once
 
 #include "Bsp/ReturnCode.h"
-#include "stm32l0xx.h"
+#include "Mcu.h"
 
 namespace BSP {
 
diff --git a/firmware/Bsp/Drivers/RtcDriver.cpp b/firmware/Bsp/Drivers/RtcDriver.cpp
index 2c64389..e42b027 100644
--- a/firmware/Bsp/Drivers/RtcDriver.cpp
+++ b/firmware/Bsp/Drivers/RtcDriver.cpp
@@ -58,19 +58,27 @@ void RtcDriver::enable_periodic_alarm()
 
     // Only calculate alarms when second rolls over
     SET_TO(RTC->ALRMASSR, RTC_ALRMASSR_MASKSS, RTC_ALRMASSR_MASKSS);
-
+#if defined(STM32L0XX)
     SET(RTC->CR, RTC_CR_ALRAE | RTC_CR_ALRAIE);
     SET(EXTI->IMR, EXTI_IMR_IM17);
     SET(EXTI->EMR, EXTI_EMR_EM17);
     SET(EXTI->RTSR, EXTI_RTSR_RT17);
-
+#elif defined(STM32L4XX)
+    SET(RTC->CR, RTC_CR_ALRAE | RTC_CR_ALRAIE);
+    SET(EXTI->IMR1, EXTI_IMR1_IM18);
+    SET(EXTI->EMR1, EXTI_EMR1_EM18);
+    SET(EXTI->RTSR1, EXTI_RTSR1_RT18);
+#else
+#error "Unsupported family"
+#endif
 }
 
 ReturnCode RtcDriver::init_hw()
 {
+    //SET(RCC->CSR, RCC_CSR_RTCRST);
+#if defined(STM32L0XX)
     uint32_t temp = RCC->CSR;
 
-    SET(RCC->CSR, RCC_CSR_RTCRST);
     SET(RCC->APB1ENR, RCC_APB1ENR_PWREN);
     SET(PWR->CR, PWR_CR_DBP);
 
@@ -91,6 +99,32 @@ ReturnCode RtcDriver::init_hw()
     RTC->ISR = RTC_ISR_INIT;
     while (!(RTC->ISR & RTC_ISR_INITF)) {}
 
+#elif defined(STM32L4XX)
+    uint32_t temp = RCC->CSR;
+
+    SET(RCC->APB1ENR1, RCC_APB1ENR1_PWREN);
+    SET(PWR->CR1, PWR_CR1_DBP);
+
+    /*<! Set RTC input clock to the LSE (low-speed external 32.768kHz) clock */
+    if (!(RCC->BDCR & RCC_BDCR_LSERDY)) {
+        // TODO: Does this help?
+        SET(temp, RCC_BDCR_LSEON);
+    }
+
+    SET_TO(temp, RCC_BDCR_RTCSEL, RCC_BDCR_RTCSEL_0);
+    SET(temp, RCC_BDCR_RTCEN);
+    RCC->BDCR = temp;
+
+    while (!(RCC->BDCR & RCC_BDCR_LSERDY)) {}
+
+    enable_rtc_write();
+
+    RTC->ICSR = RTC_ICSR_INIT;
+    while (!(RTC->ICSR & RTC_ICSR_INITF)) {}
+
+#else
+#error "Unsupported device type"
+#endif
     /*<! Set the Clock Prescalers (32.768kHz / 1 / 32768 = 1Hz */
     /*<! Set the Async prescaler to the Maximum (divide the clock by 128) */
     // XXX reset to 0, this is to enable easier debugging
@@ -113,6 +147,7 @@ ReturnCode RtcDriver::init_hw()
     SET_TO(time, RTC_TR_SU, 6 << RTC_TR_SU_Pos);
     RTC->TR = time;
 
+#if defined(STM32L0XX)
     CLR(RTC->ISR, RTC_ISR_INIT);
 
     SET(EXTI->IMR, EXTI_IMR_IM20);
@@ -124,6 +159,22 @@ ReturnCode RtcDriver::init_hw()
     NVIC_EnableIRQ(RTC_IRQn);
     NVIC_SetPriority(RTC_IRQn, 0);
 
+#elif defined(STM32L4XX)
+    CLR(RTC->ICSR, RTC_ICSR_INIT);
+
+    SET(EXTI->IMR1, EXTI_IMR1_IM20);
+    SET(EXTI->EMR1, EXTI_EMR1_EM20);
+    SET(EXTI->RTSR1, EXTI_RTSR1_RT20);
+
+    // Enable Wakeup irq, we may/will use them later
+    SET(RTC->CR, RTC_CR_WUTIE);
+    NVIC_EnableIRQ(RTC_WKUP_IRQn);
+    NVIC_SetPriority(RTC_WKUP_IRQn, 0);
+
+
+#else
+#error "Unsupported device type"
+#endif
     enable_periodic_alarm();
 
     disable_rtc_write();
@@ -164,8 +215,16 @@ ReturnCode RtcDriver::set_time(const BSP::WallClockTime &wall_time)
 {
     enable_rtc_write();
 
+#if defined(STM32L0XX)
     RTC->ISR = RTC_ISR_INIT;
     while (!(RTC->ISR & RTC_ISR_INITF)) {}
+#elif defined(STM32L4XX)
+    RTC->ICSR = RTC_ICSR_INIT;
+    while (!(RTC->ICSR & RTC_ICSR_INITF)) {}
+#else
+#error "Unsupported device type"
+#endif
+
 
     /*<! Load initial date and time */
 
@@ -182,9 +241,17 @@ ReturnCode RtcDriver::set_time(const BSP::WallClockTime &wall_time)
     SET_TO(time, RTC_TR_SU,  wall_time.get_seconds_ones()  << RTC_TR_SU_Pos);
     RTC->TR = time;
 
+#if defined(STM32L0XX)
     CLR(RTC->ISR, RTC_ISR_INIT);
     while ((RTC->ISR & RTC_ISR_INITF)) {}
     while (!(RTC->ISR & RTC_ISR_RSF)) {}
+#elif defined(STM32L4XX)
+    CLR(RTC->ICSR, RTC_ICSR_INIT);
+    while ((RTC->ICSR & RTC_ICSR_INITF)) {}
+    while (!(RTC->ICSR & RTC_ICSR_RSF)) {}
+#else
+#error "Unsupported device type"
+#endif
 
     disable_rtc_write();
 
@@ -204,7 +271,15 @@ ReturnCode RtcDriver::set_wakeup_in(BSP::time_t wakeup_delay)
     /*<! If there is an ongoing wakeup, disable it */
     if (RTC->CR & RTC_CR_WUTE) {
         CLR(RTC->CR, RTC_CR_WUTE);
+
+#if defined(STM32L0XX)
         while (!(RTC->ISR & RTC_ISR_WUTWF)) {}
+#elif defined(STM32L4XX)
+        while (!(RTC->ICSR & RTC_ICSR_WUTWF)) {}
+#else
+#error "Unsupported device type"
+#endif
+
     }
 
     uint32_t wucksel = 0;
@@ -278,7 +353,9 @@ static uint32_t wakeup_alarms = 0;
 
 extern "C" void RTC_IRQHandler()
 {
+
     // Clear the wakeup and alarm irq in the EXTI
+#if defined(STM32L0XX)
     SET(EXTI->PR, EXTI_PR_PIF20 | EXTI_PR_PIF17);
 
     if (RTC->ISR & RTC_ISR_ALRAF) {
@@ -295,6 +372,28 @@ extern "C" void RTC_IRQHandler()
         CLR(RTC->CR, RTC_CR_WUTE);
     }
 
+#elif defined(STM32L4XX)
+    SET(EXTI->PR1, EXTI_PR1_PIF20 | EXTI_PR1_PIF18);
+
+    if (RTC->SR & RTC_SR_ALRAF) {
+        RtcDriver::increment_seconds();
+        CLR(RTC->SR, RTC_SR_ALRAF);
+    }
+
+    if (RTC->SR & RTC_SR_WUTF) {
+        wakeup_alarms++;
+        // Clear the interrupt in the RTC
+        SET(RTC->SCR, RTC_SCR_CWUTF);
+        // Disable the Wakeup timer (its periodic, but we use it as a
+        // one-shot timer
+        CLR(RTC->CR, RTC_CR_WUTE);
+    }
+
+#else
+#error "Unsupported device type"
+#endif
+
+
 }
 
 }
diff --git a/firmware/Bsp/Drivers/RtcDriver.h b/firmware/Bsp/Drivers/RtcDriver.h
index 9676820..4714e1f 100644
--- a/firmware/Bsp/Drivers/RtcDriver.h
+++ b/firmware/Bsp/Drivers/RtcDriver.h
@@ -27,7 +27,7 @@
 #include "Bsp/ReturnCode.h"
 #include "Bsp/Time.h"
 
-#include "stm32l0xx.h"
+#include "Mcu.h"
 
 namespace BSP {
 
diff --git a/firmware/Bsp/Drivers/SpiDriver.cpp b/firmware/Bsp/Drivers/SpiDriver.cpp
index 40353bb..aef8c92 100644
--- a/firmware/Bsp/Drivers/SpiDriver.cpp
+++ b/firmware/Bsp/Drivers/SpiDriver.cpp
@@ -27,51 +27,19 @@ namespace BSP {
 using RC = BSP::ReturnCode;
 using BSP::Schedule::TaskScheduler;
 using BSP::Schedule::NextTime;
+using BSP::GpioPin;
 using BSP::Time;
 
-SpiDriver::SpiDriver(TaskScheduler &scheduler)
+SpiDriver::SpiDriver(TaskScheduler &scheduler, GpioPin &nss)
     : m_scheduler(scheduler)
     , m_spi(SPI1)
+    , m_nss(nss)
 {}
 
 void SpiDriver::init()
 {
-    SET(RCC->IOPENR, RCC_IOPENR_IOPAEN);
-    SET(RCC->IOPENR, RCC_IOPENR_IOPBEN);
     RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
 
-    /* Assign SPI_MOSI to PB1, since PA7 is taken by LPTIM_OUT */
-    GPIOB->AFR[0] &= ~GPIO_AFRH_AFRH1;
-    GPIOB->AFR[0] |= 1u << GPIO_AFRH_AFRH1_Pos;
-
-    SET_TO(GPIOB->MODER, GPIO_MODER_MODE1, 2u << GPIO_MODER_MODE1_Pos);
-
-    GPIOB->OTYPER &= ~GPIO_OTYPER_OT_1;
-    GPIOB->PUPDR &= ~GPIO_PUPDR_PUPD12;
-
-    // SPI1 NSS (PA4)
-
-    SET_TO(GPIOA->MODER, GPIO_MODER_MODE4, 1u << GPIO_MODER_MODE4_Pos);
-
-    GPIOA->OTYPER &= ~GPIO_OTYPER_OT_4;
-    GPIOA->PUPDR &= ~GPIO_PUPDR_PUPD4;
-
-    // SPI1 SCK (PA5)
-    GPIOA->AFR[0] &= ~GPIO_AFRL_AFRL5;
-
-    SET_TO(GPIOA->MODER, GPIO_MODER_MODE5, 2u << GPIO_MODER_MODE5_Pos);
-
-    GPIOA->OTYPER &= ~GPIO_OTYPER_OT_5;
-    GPIOA->PUPDR &= ~GPIO_PUPDR_PUPD5;
-
-    // SPI1 MISO (PA6)
-    GPIOA->AFR[0] &= ~GPIO_AFRL_AFRL6;
-
-    SET_TO(GPIOA->MODER, GPIO_MODER_MODE6, 2u << GPIO_MODER_MODE6_Pos);
-
-    GPIOA->OTYPER &= ~GPIO_OTYPER_OT_6;
-    GPIOA->PUPDR &= ~GPIO_PUPDR_PUPD6;
-
     // Enable Master mode and half the baud rate, so it's set to ~1MHz
     m_spi->CR1 |= SPI_CR1_MSTR | SPI_CR1_LSBFIRST | SPI_CR1_SSM;
     //m_spi->CR1 |= 1u << SPI_CR1_BR_Pos;
@@ -91,17 +59,14 @@ RC SpiDriver::tx_blocking(const uint8_t *data, size_t len)
 
     m_spi->CR1 |= SPI_CR1_SPE;
 
-    //FLIP(GPIOB->ODR, GPIO_ODR_OD3);
     CLR(m_spi->CR1, SPI_CR1_SSI);
-    SET(GPIOA->ODR, GPIO_ODR_OD4);
+    m_nss.write(1);
 
     for (size_t i = 0; i < len; i++) {
         while (!(m_spi->SR & SPI_SR_TXE)) {}
         m_spi->DR = data[i];
     }
 
-   //FLIP(GPIOB->ODR, GPIO_ODR_OD3);
-
     while (!(m_spi->SR & SPI_SR_TXE)) {}
 
     // Ensure that NSS is held for long enough to meet the display's thSCS
@@ -109,6 +74,7 @@ RC SpiDriver::tx_blocking(const uint8_t *data, size_t len)
 
     m_spi->CR1 &= ~SPI_CR1_SPE;
     SET(m_spi->CR1, SPI_CR1_SSI);
+    m_nss.write(0);
     CLR(GPIOA->ODR, GPIO_ODR_OD4);
 
     return RC::OK;
diff --git a/firmware/Bsp/Drivers/SpiDriver.h b/firmware/Bsp/Drivers/SpiDriver.h
index 2382169..d996db2 100644
--- a/firmware/Bsp/Drivers/SpiDriver.h
+++ b/firmware/Bsp/Drivers/SpiDriver.h
@@ -23,17 +23,19 @@
 
 #include "Bsp/ReturnCode.h"
 #include "Bsp/TaskScheduler.h"
+#include "Bsp/Drivers/GpioDriver.h"
 
-// TODO: Find a better include for this
-#include "stm32l0xx.h"
+#include "Mcu.h"
 
 namespace BSP {
 
 class SpiDriver : public BSP::Schedule::Task {
 
 public:
-    // TODO: Add configurability / provide a real abstraction
-    SpiDriver(BSP::Schedule::TaskScheduler &scheduler);
+    // TODO: Allow other CS pins of either polaritu
+    // TODO: Allow configurable CPHA, CPOL
+    // TODO: Allow other SPI instances (not just SPI1)
+    SpiDriver(BSP::Schedule::TaskScheduler &scheduler, BSP::GpioPin &nss);
 
     void init();
     BSP::Schedule::NextTime execute() override;
@@ -42,6 +44,7 @@ public:
 private:
     BSP::Schedule::TaskScheduler &m_scheduler;
     SPI_TypeDef *m_spi;
+    BSP::GpioPin &m_nss;
 };
 
 }
diff --git a/firmware/Bsp/Mcu/Mcu.h b/firmware/Bsp/Mcu/Mcu.h
new file mode 100644
index 0000000..fc57c27
--- /dev/null
+++ b/firmware/Bsp/Mcu/Mcu.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2020  Max Regan
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#if defined(STM32L031xx)
+#include "stm32l031xx.h"
+#elif defined(STM32L412xx)
+#include "stm32l412xx.h"
+#else
+#error "Unknown device type"
+#endif
diff --git a/firmware/Bsp/Mcu/stm32l412rb.S b/firmware/Bsp/Mcu/stm32l412rb.S
new file mode 100644
index 0000000..bf21462
--- /dev/null
+++ b/firmware/Bsp/Mcu/stm32l412rb.S
@@ -0,0 +1,315 @@
+/* File: startup_ARMCM0.S
+ * Purpose: startup file for Cortex-M0 devices. Should use with
+ *   GCC for ARM Embedded Processors
+ * Version: V2.01
+ * Date: 12 June 2014
+ *
+ */
+/* Copyright (c) 2011 - 2014 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   - Neither the name of ARM nor the names of its contributors may be used
+     to endorse or promote products derived from this software without
+     specific prior written permission.
+   *
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+   ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+   ---------------------------------------------------------------------------*/
+
+
+	.syntax	unified
+	.arch	armv6-m
+
+	.section .stack
+	.align	3
+	.equ	Stack_Size, __STACK_SIZE
+	.globl	__StackTop
+	.globl	__StackLimit
+__StackLimit:
+	.space	Stack_Size
+	.size	__StackLimit, . - __StackLimit
+__StackTop:
+	.size	__StackTop, . - __StackTop
+	.section .heap
+	.align	3
+#ifdef __HEAP_SIZE
+	.equ	Heap_Size, __HEAP_SIZE
+#else
+	.equ	Heap_Size, 0x00000C00
+#endif
+	.globl	__HeapBase
+	.globl	__HeapLimit
+__HeapBase:
+	.if	Heap_Size
+	.space	Heap_Size
+	.endif
+	.size	__HeapBase, . - __HeapBase
+__HeapLimit:
+	.size	__HeapLimit, . - __HeapLimit
+
+	.section .vectors
+	.align 2
+	.globl	__Vectors
+__Vectors:
+	.long	__StackTop            /* Top of Stack */
+	.long	Reset_Handler         /* Reset Handler */
+	.long	NMI_Handler           /* NMI Handler */
+	.long	HardFault_Handler     /* Hard Fault Handler */
+	.long	0                     /* Reserved */
+	.long	0                     /* Reserved */
+	.long	0                     /* Reserved */
+	.long	0                     /* Reserved */
+	.long	0                     /* Reserved */
+	.long	0                     /* Reserved */
+	.long	0                     /* Reserved */
+	.long	SVC_Handler           /* SVCall Handler */
+	.long	0                     /* Reserved */
+	.long	0                     /* Reserved */
+	.long	PendSV_Handler        /* PendSV Handler */
+	.long	SysTick_Handler       /* SysTick Handler */
+
+	/* External interrupts */
+	.long	WWDG_IRQHandler        /*  0:  Watchdog Timer            */
+	.long	PVD_IRQHandler        /*  1:  Real Time Clock           */
+	.long	RTC_IRQHandler       /*  2:  Timer0 / Timer1           */
+	.long	FLASH_IRQHandler       /*  3:  Timer2 / Timer3           */
+	.long	RCC_CRS_IRQHandler       /*  4:  MCIa                      */
+	.long	EXTI_1_0_IRQHandler       /*  5:  MCIb                      */
+	.long	EXTI_3_2_IRQHandler      /*  6:  UART0 - DUT FPGA          */
+	.long	EXTI_15_4_IRQHandler      /*  7:  UART1 - DUT FPGA          */
+	.long	0                             /*  8:  UART2 - reserved */
+    .long	DMA1_CHANNEL1_IRQHandler      /*  8:  UART2 - DUT FPGA          */
+	.long	DMA1_CHANNEL3_2_IRQHandler      /*  9:  UART4 - not connected     */
+	.long	DMA_CHANNEL_7_4_IRQHandler       /* 10: AACI / AC97                */
+	.long	ADC_COMP_IRQHandler       /* 11: CLCD Combined Interrupt    */
+	.long	LPTIM1_IRQHandler       /* 12: Ethernet                   */
+	.long	USART4_USART5_IRQHandler      /* 13: USB Device                 */
+	.long	TIM2_IRQHandler      /* 14: USB Host Controller        */
+	.long	TIM3_IRQHandler      /* 15: Character LCD              */
+	.long	TIM6_IRQHandler    /* 16: Flexray                    */
+	.long	TIM7_IRQHandler        /* 17: CAN                        */
+	.long	0        /* 18: LIN                        */
+	.long	TIM21_IRQHandler        /* 19: I2C ADC/DAC                */
+	.long	I2C3_IRQHandler                     /* 20: Reserved                   */
+	.long	TIM22_IRQHandler                     /* 21: Reserved                   */
+
+    /* TODO: There are more but I'm lazy */
+	.long	0
+	.long	0
+	.long	0
+	.long	0
+	.long	0
+	.long	0
+	.long	0
+	.long	0
+	.long	0
+	.size	__Vectors, . - __Vectors
+
+	.text
+	.thumb
+	.thumb_func
+	.align	1
+	.globl	Reset_Handler
+	.type	Reset_Handler, %function
+Reset_Handler:
+/*  Firstly it copies data from read only memory to RAM. There are two schemes
+ *  to copy. One can copy more than one sections. Another can only copy
+ *  one section.  The former scheme needs more instructions and read-only
+ *  data to implement than the latter.
+ *  Macro __STARTUP_COPY_MULTIPLE is used to choose between two schemes.  */
+
+#ifdef __STARTUP_COPY_MULTIPLE
+/*  Multiple sections scheme.
+ *
+ *  Between symbol address __copy_table_start__ and __copy_table_end__,
+ *  there are array of triplets, each of which specify:
+ *    offset 0: LMA of start of a section to copy from
+ *    offset 4: VMA of start of a section to copy to
+ *    offset 8: size of the section to copy. Must be multiply of 4
+ *
+ *  All addresses must be aligned to 4 bytes boundary.
+ */
+	ldr	r4, =__copy_table_start__
+	ldr	r5, =__copy_table_end__
+
+.L_loop0:
+	cmp	r4, r5
+	bge	.L_loop0_done
+	ldr	r1, [r4]
+	ldr	r2, [r4, #4]
+	ldr	r3, [r4, #8]
+
+.L_loop0_0:
+	subs	r3, #4
+	blt	.L_loop0_0_done
+	ldr	r0, [r1, r3]
+	str	r0, [r2, r3]
+	b	.L_loop0_0
+
+.L_loop0_0_done:
+	adds	r4, #12
+	b	.L_loop0
+
+.L_loop0_done:
+#else
+/*  Single section scheme.
+ *
+ *  The ranges of copy from/to are specified by following symbols
+ *    __etext: LMA of start of the section to copy from. Usually end of text
+ *    __data_start__: VMA of start of the section to copy to
+ *    __data_end__: VMA of end of the section to copy to
+ *
+ *  All addresses must be aligned to 4 bytes boundary.
+ */
+	ldr	r1, =__etext
+	ldr	r2, =__data_start__
+	ldr	r3, =__data_end__
+
+	subs	r3, r2
+	ble	.L_loop1_done
+
+.L_loop1:
+	subs	r3, #4
+	ldr	r0, [r1,r3]
+	str	r0, [r2,r3]
+	bgt	.L_loop1
+
+.L_loop1_done:
+#endif /*__STARTUP_COPY_MULTIPLE */
+
+/*  This part of work usually is done in C library startup code. Otherwise,
+ *  define this macro to enable it in this startup.
+ *
+ *  There are two schemes too. One can clear multiple BSS sections. Another
+ *  can only clear one section. The former is more size expensive than the
+ *  latter.
+ *
+ *  Define macro __STARTUP_CLEAR_BSS_MULTIPLE to choose the former.
+ *  Otherwise efine macro __STARTUP_CLEAR_BSS to choose the later.
+ */
+#ifdef __STARTUP_CLEAR_BSS_MULTIPLE
+/*  Multiple sections scheme.
+ *
+ *  Between symbol address __copy_table_start__ and __copy_table_end__,
+ *  there are array of tuples specifying:
+ *    offset 0: Start of a BSS section
+ *    offset 4: Size of this BSS section. Must be multiply of 4
+ */
+	ldr	r3, =__zero_table_start__
+	ldr	r4, =__zero_table_end__
+
+.L_loop2:
+	cmp	r3, r4
+	bge	.L_loop2_done
+	ldr	r1, [r3]
+	ldr	r2, [r3, #4]
+	movs	r0, 0
+
+.L_loop2_0:
+	subs	r2, #4
+	blt	.L_loop2_0_done
+	str	r0, [r1, r2]
+	b	.L_loop2_0
+.L_loop2_0_done:
+
+	adds	r3, #8
+	b	.L_loop2
+.L_loop2_done:
+#elif defined (__STARTUP_CLEAR_BSS)
+/*  Single BSS section scheme.
+ *
+ *  The BSS section is specified by following symbols
+ *    __bss_start__: start of the BSS section.
+ *    __bss_end__: end of the BSS section.
+ *
+ *  Both addresses must be aligned to 4 bytes boundary.
+ */
+	ldr	r1, =__bss_start__
+	ldr	r2, =__bss_end__
+
+	movs	r0, 0
+
+	subs	r2, r1
+	ble	.L_loop3_done
+
+.L_loop3:
+	subs	r2, #4
+	str	r0, [r1, r2]
+	bgt	.L_loop3
+.L_loop3_done:
+#endif /* __STARTUP_CLEAR_BSS_MULTIPLE || __STARTUP_CLEAR_BSS */
+
+#ifndef __NO_SYSTEM_INIT
+	bl	SystemInit
+#endif
+
+#ifndef __START
+#define __START _start
+#endif
+//	bl	__START
+    bl  main
+	.pool
+	.size	Reset_Handler, . - Reset_Handler
+
+	.align	1
+	.thumb_func
+	.weak	Default_Handler
+	.type	Default_Handler, %function
+Default_Handler:
+	b	.
+	.size	Default_Handler, . - Default_Handler
+
+/*    Macro to define default handlers. Default handler
+ *    will be weak symbol and just dead loops. They can be
+ *    overwritten by other handlers */
+	.macro	def_irq_handler	handler_name
+	.weak	\handler_name
+	.set	\handler_name, Default_Handler
+	.endm
+
+	def_irq_handler	NMI_Handler
+	def_irq_handler	HardFault_Handler
+	def_irq_handler	SVC_Handler
+	def_irq_handler	PendSV_Handler
+	def_irq_handler	SysTick_Handler
+
+	def_irq_handler	WWDG_IRQHandler
+	def_irq_handler	PVD_IRQHandler
+	def_irq_handler	WDT_IRQHandler
+	def_irq_handler	RTC_IRQHandler
+	def_irq_handler	FLASH_IRQHandler
+	def_irq_handler	RCC_CRS_IRQHandler
+	def_irq_handler	EXTI_1_0_IRQHandler
+	def_irq_handler	EXTI_3_2_IRQHandler
+	def_irq_handler	EXTI_15_4_IRQHandler
+    def_irq_handler	DMA1_CHANNEL1_IRQHandler
+	def_irq_handler	DMA1_CHANNEL3_2_IRQHandler
+	def_irq_handler	DMA_CHANNEL_7_4_IRQHandler
+	def_irq_handler	ADC_COMP_IRQHandler
+	def_irq_handler	LPTIM1_IRQHandler
+	def_irq_handler	USART4_USART5_IRQHandler
+	def_irq_handler	TIM2_IRQHandler
+	def_irq_handler	TIM3_IRQHandler
+	def_irq_handler	TIM6_IRQHandler
+	def_irq_handler	TIM7_IRQHandler
+	def_irq_handler	TIM21_IRQHandler
+	def_irq_handler	I2C3_IRQHandler
+	def_irq_handler	TIM22_IRQHandler
+
+	.end
diff --git a/firmware/Bsp/Mcu/stm32l412rb.ld b/firmware/Bsp/Mcu/stm32l412rb.ld
new file mode 100644
index 0000000..87a7fbd
--- /dev/null
+++ b/firmware/Bsp/Mcu/stm32l412rb.ld
@@ -0,0 +1,223 @@
+/**
+Copyright (C) 2009-2015 ARM Limited. All rights reserved.
+Copyright 2019, Max Regan
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+ - Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ - Neither the name of ARM nor the names of its contributors may be used
+   to endorse or promote products derived from this software without
+   specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/* Linker script to configure memory regions. */
+MEMORY
+{
+	FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K
+	RAM (rwx)  : ORIGIN = 0x20000000, LENGTH = 40K
+}
+
+/* Library configurations */
+/* GROUP(libgcc.a libc.a libm.a libnosys.a) */
+
+/* Linker script to place sections and symbol values. Should be used together
+ * with other linker script that defines memory regions FLASH and RAM.
+ * It references following symbols, which must be defined in code:
+ *   Reset_Handler : Entry of reset handler
+ *
+ * It defines following symbols, which code can use without definition:
+ *   __exidx_start
+ *   __exidx_end
+ *   __copy_table_start__
+ *   __copy_table_end__
+ *   __zero_table_start__
+ *   __zero_table_end__
+ *   __etext
+ *   __data_start__
+ *   __preinit_array_start
+ *   __preinit_array_end
+ *   __init_array_start
+ *   __init_array_end
+ *   __fini_array_start
+ *   __fini_array_end
+ *   __data_end__
+ *   __bss_start__
+ *   __bss_end__
+ *   __end__
+ *   end
+ *   __HeapLimit
+ *   __StackLimit
+ *   __StackTop
+ *   __stack
+ *   __Vectors_End
+ *   __Vectors_Size
+ */
+ENTRY(Reset_Handler)
+
+SECTIONS
+{
+	.text :
+	{
+		KEEP(*(.vectors))
+		__Vectors_End = .;
+		__Vectors_Size = __Vectors_End - __Vectors;
+		__end__ = .;
+
+		*(.text*)
+
+		KEEP(*(.init))
+		KEEP(*(.fini))
+
+		/* .ctors */
+		*crtbegin.o(.ctors)
+		*crtbegin?.o(.ctors)
+		*(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+		*(SORT(.ctors.*))
+		*(.ctors)
+
+		/* .dtors */
+ 		*crtbegin.o(.dtors)
+ 		*crtbegin?.o(.dtors)
+ 		*(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+ 		*(SORT(.dtors.*))
+ 		*(.dtors)
+
+		*(.rodata*)
+
+		KEEP(*(.eh_frame*))
+	} > FLASH
+
+	.ARM.extab :
+	{
+		*(.ARM.extab* .gnu.linkonce.armextab.*)
+	} > FLASH
+
+	__exidx_start = .;
+	.ARM.exidx :
+	{
+		*(.ARM.exidx* .gnu.linkonce.armexidx.*)
+	} > FLASH
+	__exidx_end = .;
+
+	/* To copy multiple ROM to RAM sections,
+	 * uncomment .copy.table section and,
+	 * define __STARTUP_COPY_MULTIPLE in startup_ARMCMx.S */
+	/*
+	.copy.table :
+	{
+		. = ALIGN(4);
+		__copy_table_start__ = .;
+		LONG (__etext)
+		LONG (__data_start__)
+		LONG (__data_end__ - __data_start__)
+		LONG (__etext2)
+		LONG (__data2_start__)
+		LONG (__data2_end__ - __data2_start__)
+		__copy_table_end__ = .;
+	} > FLASH
+	*/
+
+	/* To clear multiple BSS sections,
+	 * uncomment .zero.table section and,
+	 * define __STARTUP_CLEAR_BSS_MULTIPLE in startup_ARMCMx.S */
+	/*
+	.zero.table :
+	{
+		. = ALIGN(4);
+		__zero_table_start__ = .;
+		LONG (__bss_start__)
+		LONG (__bss_end__ - __bss_start__)
+		LONG (__bss2_start__)
+		LONG (__bss2_end__ - __bss2_start__)
+		__zero_table_end__ = .;
+	} > FLASH
+	*/
+
+	__etext = .;
+
+	.data : AT (__etext)
+	{
+		__data_start__ = .;
+		*(vtable)
+		*(.data*)
+
+		. = ALIGN(4);
+		/* preinit data */
+		PROVIDE_HIDDEN (__preinit_array_start = .);
+		KEEP(*(.preinit_array))
+		PROVIDE_HIDDEN (__preinit_array_end = .);
+
+		. = ALIGN(4);
+		/* init data */
+		PROVIDE_HIDDEN (__init_array_start = .);
+		KEEP(*(SORT(.init_array.*)))
+		KEEP(*(.init_array))
+		PROVIDE_HIDDEN (__init_array_end = .);
+
+
+		. = ALIGN(4);
+		/* finit data */
+		PROVIDE_HIDDEN (__fini_array_start = .);
+		KEEP(*(SORT(.fini_array.*)))
+		KEEP(*(.fini_array))
+		PROVIDE_HIDDEN (__fini_array_end = .);
+
+		KEEP(*(.jcr*))
+		. = ALIGN(4);
+		/* All data end */
+		__data_end__ = .;
+
+	} > RAM
+
+	.bss :
+	{
+		. = ALIGN(4);
+		__bss_start__ = .;
+		*(.bss*)
+		*(COMMON)
+		. = ALIGN(4);
+		__bss_end__ = .;
+	} > RAM
+
+	.heap (COPY):
+	{
+		__HeapBase = .;
+		__end__ = .;
+		end = __end__;
+		KEEP(*(.heap*))
+		__HeapLimit = .;
+	} > RAM
+
+	/* .stack_dummy section doesn't contains any symbols. It is only
+	 * used for linker to calculate size of stack sections, and assign
+	 * values to stack symbols later */
+	.stack_dummy (COPY):
+	{
+		KEEP(*(.stack*))      /* changed MG 30.05.14 */
+	} > RAM
+
+	/* Set stack top to end of RAM, and stack limit move down by
+	 * size of stack_dummy section */
+	__StackTop = ORIGIN(RAM) + LENGTH(RAM);
+	__StackLimit = __StackTop - SIZEOF(.stack_dummy);
+	PROVIDE(__stack = __StackTop);
+
+	/* Check if data + heap + stack exceeds RAM limit */
+	ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed with stack")
+}
diff --git a/firmware/Bsp/SystemTime.h b/firmware/Bsp/SystemTime.h
index acae7ac..df56c2f 100644
--- a/firmware/Bsp/SystemTime.h
+++ b/firmware/Bsp/SystemTime.h
@@ -26,7 +26,7 @@
 #include "ReturnCode.h"
 #include "Time.h"
 
-#include "stm32l0xx.h"
+#include "Mcu.h"
 
 namespace BSP {
 
diff --git a/firmware/Bsp/macros.h b/firmware/Bsp/macros.h
index 397a928..e2ca70e 100644
--- a/firmware/Bsp/macros.h
+++ b/firmware/Bsp/macros.h
@@ -57,12 +57,12 @@
         (x) |= 1u << (y);                       \
     } while (0)
 
-#define CLR(x, y)                             \
+#define CLR(x, y)                               \
     do {                                        \
         (x) &= ~(y);                            \
     } while (0)
 
-#define CLR_POS(x, y)                         \
+#define CLR_POS(x, y)                           \
     do {                                        \
         (x) &= ~(1u << (y));                    \
     } while (0)
@@ -77,10 +77,17 @@
         (x) ^= 1u << y;                         \
     } while (0)
 
-#define SET_TO(x, clear_mask, val)                                      \
-    do {                                                                \
-        CLR(x, clear_mask);                                           \
-        SET(x, val);                                                    \
+#define SET_TO(x, clear_mask, val)              \
+    do {                                        \
+        CLR(x, clear_mask);                     \
+        SET(x, val);                            \
+    } while (0)
+
+#define SET_STRIDE_TO(var, stride_width, index, val)    \
+    do {                                                \
+        uint32_t mask = (1 << stride_width) - 1;        \
+        CLR(var, mask << (index * stride_width));       \
+        SET(var, val << (index * stride_width));        \
     } while (0)
 
 #endif
diff --git a/firmware/Makefile b/firmware/Makefile
index de0d582..592fce7 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -37,6 +37,10 @@ STM32_PROG  =  STM32_Programmer.sh
 # Device Variables
 #
 
+BOARD ?= watch
+
+ifeq ($(BOARD), watch)
+
 DEVICE_TYPE    ?= stm32l031k6
 DEVICE_FAMILY  =  stm32l031xx
 DEVICE_LINE    =  stm32l0xx
@@ -44,6 +48,21 @@ DEVICE_LINE    =  stm32l0xx
 STACK_SIZE ?= 768
 HEAP_SIZE  ?= 0
 
+else ifeq ($(BOARD), devboard)
+
+DEVICE_TYPE    ?= stm32l412rb
+DEVICE_FAMILY  =  stm32l412xx
+DEVICE_LINE    =  stm32l4xx
+
+STACK_SIZE ?= 768
+HEAP_SIZE  ?= 0
+
+else
+
+$(error Unsupported board: $(BOARD))
+
+endif
+
 #
 # Filenames and paths
 #
@@ -81,7 +100,9 @@ OUTPUT_MAP  ?= $(OUTPUT_NAME).map
 # Flags
 #
 
-DEVICE_DEFINE = $(subst XX,xx,$(shell echo $(DEVICE_FAMILY) | tr '[:lower:]' '[:upper:]'))
+DEVICE_FAMILY_DEFINE = $(subst XX,xx,$(shell echo $(DEVICE_FAMILY) | tr '[:lower:]' '[:upper:]'))
+DEVICE_TYPE_DEFINE = $(shell echo $(DEVICE_TYPE) | tr '[:lower:]' '[:upper:]')
+DEVICE_LINE_DEFINE = $(shell echo $(DEVICE_LINE) | tr '[:lower:]' '[:upper:]')
 
 CPU_FLAGS = -mthumb -mcpu=cortex-m0plus -mfloat-abi=soft
 
@@ -96,8 +117,9 @@ CFLAGS += $(CPU_FLAGS)
 CFLAGS += -ffreestanding
 CFLAGS += -fstack-usage -Wstack-usage=128
 # Defines
-CFLAGS += -D$(DEVICE_DEFINE)
+CFLAGS += -D$(DEVICE_FAMILY_DEFINE) -D$(DEVICE_TYPE_DEFINE) -D$(DEVICE_LINE_DEFINE)
 # Includes
+CFLAGS += -I./Bsp/Mcu/
 CFLAGS += -I./ThirdParty/stm32/$(DEVICE_LINE)/Include
 CFLAGS += -I./ThirdParty/CMSIS/Core/Include
 CFLAGS += -I./build/gen/
@@ -118,6 +140,23 @@ LDFLAGS += -Wl,-print-memory-usage
 # LDFLAGS += -lstdc++ -latomic
 LDFLAGS += -nostartfiles
 
+
+#
+# Build info
+#
+
+$(info ================================)
+$(info Build Parameters)
+$(info Board: $(BOARD))
+$(info Device: $(DEVICE_TYPE))
+$(info Device define: $(DEVICE_TYPE_DEFINE))
+$(info Device type define: $(DEVICE_FAMILY_DEFINE))
+$(info Device line define: $(DEVICE_LINE_DEFINE))
+$(info Stack size: $(STACK_SIZE))
+$(info Heap size: $(HEAP_SIZE))
+$(info ================================)
+
+
 #
 # Default Target
 #
@@ -144,7 +183,7 @@ SMALL_FONT=ThirdParty/fonts/roboto_mono/RobotoMono-Regular.ttf
 $(FONT_GEN_DIR)/small.h $(FONT_GEN_DIR)/small.c: Gen/fixedfont-to-c.py Gen/font.py
 	@echo "GEN $@"
 	@mkdir -p $(FONT_GEN_DIR)
-	@Gen/fixedfont-to-c.py $(patsubst .%,%,$(suffix $@)) $(SMALL_FONT) "$@" -s 24 --header-dir "Bsp/" --name font_small
+	@Gen/fixedfont-to-c.py $(patsubst .%,%,$(suffix $@)) $(SMALL_FONT) "$@" -s 20 --header-dir "Bsp/" --name font_small
 
 LARGE_FONT=ThirdParty/fonts/roboto_mono/RobotoMono-Bold.ttf
 $(FONT_GEN_DIR)/large_digits.h $(FONT_GEN_DIR)/large_digits.c: Gen/fixedfont-to-c.py Gen/font.py
@@ -171,6 +210,12 @@ flash: $(OUTPUT_BIN)
 	@echo "FLASH $(OUTPUT_BIN)"
 	$(STM32_PROG) --connect port=SWD reset=Hwrst -w $(OUTPUT_BIN) 0x8000000 -v --go
 
+jlink: $(OUTPUT_BIN)
+	@echo "FLASH $(OUTPUT_BIN)"
+	JLinkExe -device $$(echo $(DEVICE_TYPE) | tr '[:lower:]' '[:upper:]') -if SWD \
+        -speed auto -autoconnect 1 -CommanderScript cmd.jlink
+
+
 .PHONY: clean
 clean:
 	rm -f $(OBJS) $(OUTPUT_BIN) $(OUTPUT_ELF) $(FONT_C_FILES) $(FONT_H_FILES) $(OUTPUT_MAP) $(addsuffix .su,$(basename $(OBJS)))