diff --git a/CMakeLists.txt b/CMakeLists.txt
index 58d59c7..138f023 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -186,7 +186,7 @@ target_compile_options(firmware PRIVATE -Wdouble-promotion)
 
 # target_link_libraries( firmware PRIVATE A3idesHeaders )
 
-target_sources(firmware PRIVATE src/main.cpp src/hal/avr/cpu.cpp src/hal/avr/spi.cpp)
+target_sources(firmware PRIVATE src/main.cpp src/hal/avr/cpu.cpp)
 
 set_property(
   SOURCE src/version.c
diff --git a/src/hal/avr/spi.cpp b/src/hal/avr/spi.cpp
deleted file mode 100644
index 73e260a..0000000
--- a/src/hal/avr/spi.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#include "../spi.h"
-
-namespace hal {
-namespace spi {
-
-    // void Init(SPI_TypeDef *const hspi, SPI_InitTypeDef *const conf)
-    // {
-    //     using namespace hal;
-    //     // gpio::Init(conf->miso_pin, gpio::GPIO_InitTypeDef(gpio::Mode::input, gpio::Pull::none));
-    //     // gpio::Init(conf->mosi_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
-    //     // gpio::Init(conf->sck_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
-    //     // gpio::Init(conf->ss_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::high));
-
-    //     const uint8_t spi2x = (conf->prescaler == 7) ? 0 : (conf->prescaler & 0x01);
-    //     const uint8_t spr = ((conf->prescaler - 1) >> 1) & 0x03;
-
-    //     hspi->SPCRx = (0 << SPIE) | (1 << SPE) | (0 << DORD) | (1 << MSTR) | (0 << CPOL) | (0 << CPHA) | (spr << SPR0);
-    //     hspi->SPSRx = (spi2x << SPI2X);
-    // }
-}
-}
diff --git a/src/hal/gpio.h b/src/hal/gpio.h
index 0ba9687..7b974e8 100644
--- a/src/hal/gpio.h
+++ b/src/hal/gpio.h
@@ -47,22 +47,22 @@ namespace gpio {
             , pin(pin) {};
     };
 
-    inline void WritePin(const GPIO_pin portPin, Level level) {
+    __attribute__((always_inline)) inline void WritePin(const GPIO_pin portPin, Level level) {
         if (level == Level::high)
             portPin.port->PORTx |= (1 << portPin.pin);
         else
             portPin.port->PORTx &= ~(1 << portPin.pin);
     }
 
-    inline Level ReadPin(const GPIO_pin portPin) {
+    __attribute__((always_inline)) inline Level ReadPin(const GPIO_pin portPin) {
         return (Level)(portPin.port->PINx & (1 << portPin.pin));
     }
 
-    inline void TogglePin(const GPIO_pin portPin) {
+    __attribute__((always_inline)) inline void TogglePin(const GPIO_pin portPin) {
         portPin.port->PINx |= (1 << portPin.pin);
     }
 
-    inline void Init(const GPIO_pin portPin, GPIO_InitTypeDef GPIO_Init) {
+    __attribute__((always_inline)) inline void Init(const GPIO_pin portPin, GPIO_InitTypeDef GPIO_Init) {
         if (GPIO_Init.mode == Mode::output) {
             WritePin(portPin, GPIO_Init.level);
             portPin.port->DDRx |= (1 << portPin.pin);
diff --git a/src/hal/spi.h b/src/hal/spi.h
index 3c22ec4..dbf3c1a 100644
--- a/src/hal/spi.h
+++ b/src/hal/spi.h
@@ -13,29 +13,26 @@ namespace spi {
     };
 
     struct SPI_InitTypeDef {
-        // hal::gpio::GPIO_pin miso_pin;
-        // hal::gpio::GPIO_pin mosi_pin;
-        // hal::gpio::GPIO_pin sck_pin;
-        // hal::gpio::GPIO_pin ss_pin;
+        hal::gpio::GPIO_pin miso_pin;
+        hal::gpio::GPIO_pin mosi_pin;
+        hal::gpio::GPIO_pin sck_pin;
+        hal::gpio::GPIO_pin ss_pin;
         uint8_t prescaler;
     };
 
-    // void Init(SPI_TypeDef *const hspi, SPI_InitTypeDef *const conf);
-
-    inline void Init(SPI_TypeDef *const hspi, uint8_t prescaler) {
+    __attribute__((always_inline)) inline void Init(SPI_TypeDef *const hspi, SPI_InitTypeDef *const conf) {
         using namespace hal;
-        // gpio::Init(conf->miso_pin, gpio::GPIO_InitTypeDef(gpio::Mode::input, gpio::Pull::none));
-        // gpio::Init(conf->mosi_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
-        // gpio::Init(conf->sck_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
-        // gpio::Init(conf->ss_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::high));
+        gpio::Init(conf->miso_pin, gpio::GPIO_InitTypeDef(gpio::Mode::input, gpio::Pull::none));
+        gpio::Init(conf->mosi_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
+        gpio::Init(conf->sck_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
+        gpio::Init(conf->ss_pin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::high));
 
-        const uint8_t spi2x = (prescaler == 7) ? 0 : (prescaler & 0x01);
-        const uint8_t spr = ((prescaler - 1) >> 1) & 0x03;
+        const uint8_t spi2x = (conf->prescaler == 7) ? 0 : (conf->prescaler & 0x01);
+        const uint8_t spr = ((conf->prescaler - 1) >> 1) & 0x03;
 
         hspi->SPCRx = (0 << SPIE) | (1 << SPE) | (0 << DORD) | (1 << MSTR) | (0 << CPOL) | (0 << CPHA) | (spr << SPR0);
         hspi->SPSRx = (spi2x << SPI2X);
     }
-
 }
 }
 
diff --git a/src/main.cpp b/src/main.cpp
index ca2ab7f..fe31630 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -4,18 +4,17 @@
 
 /// One-time setup of HW and SW components
 /// Called before entering the loop() function
-void setup(){
+void setup() {
     using namespace hal;
 
-    gpio::Init(gpio::GPIO_pin(GPIOB, 3), gpio::GPIO_InitTypeDef(gpio::Mode::input, gpio::Pull::none));
-    gpio::Init(gpio::GPIO_pin(GPIOB, 2), gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
-    gpio::Init(gpio::GPIO_pin(GPIOB, 1), gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
-    // gpio::Init(gpio::GPIO_pin(GPIOB, 0), gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
-
-    // spi::SPI_InitTypeDef spi_conf = {
-    //     .prescaler = 2, //4mhz
-    // };
-    spi::Init(SPI0, 2);
+    spi::SPI_InitTypeDef spi_conf = {
+        .miso_pin = gpio::GPIO_pin(GPIOB, 3),
+        .mosi_pin = gpio::GPIO_pin(GPIOB, 2),
+        .sck_pin = gpio::GPIO_pin(GPIOB, 1),
+        .ss_pin = gpio::GPIO_pin(GPIOB, 0),
+        .prescaler = 2, //4mhz
+    };
+    spi::Init(SPI0, &spi_conf);
 }
 
 /// Main loop of the firmware
@@ -32,13 +31,12 @@ void setup(){
 ///   StepWhateverElseNeedsStepping();
 /// The idea behind the Step* routines is to keep each automaton non-blocking allowing for some “concurrency”.
 /// Some FW components will leverage ISR to do their stuff (UART, motor stepping?, etc.)
-void loop(){
-
+void loop() {
 }
 
 int main() {
     setup();
-    for(;;){
+    for (;;) {
         using namespace hal::gpio;
         WritePin(GPIO_pin(GPIOB, 5), Level::low);
         TogglePin(GPIO_pin(GPIOB, 6));