diff --git a/src/shared/drivers/Xbee/Xbee.h b/src/shared/drivers/Xbee/Xbee.h
index d269b1e27052a71a807ac8571cc865dd792cad03..ea5625585ead196ab292591194e8f5e2942fcf47 100644
--- a/src/shared/drivers/Xbee/Xbee.h
+++ b/src/shared/drivers/Xbee/Xbee.h
@@ -119,7 +119,7 @@ public:
          unsigned int send_timeout = 1000)
         : send_timeout(send_timeout), spi_xbee(bus, cs), attn(attn), rst(rst)
     {
-        spi_xbee.config.br = SPIBaudRate::DIV_128;
+        spi_xbee.config.clock_div = SPIClockDivider::DIV128;
 
         // No need to configure before each transaction, we are the only device
         // on the bus.
diff --git a/src/shared/drivers/spi/SPIBus.cpp b/src/shared/drivers/spi/SPIBus.cpp
index c7dc7c803a4972a0150f1b9ac6d916f63ec6af67..afe76d97724c74860d7ed6cafb4423ed24d07707 100644
--- a/src/shared/drivers/spi/SPIBus.cpp
+++ b/src/shared/drivers/spi/SPIBus.cpp
@@ -34,17 +34,22 @@ void SPIBus::configure(SPIBusConfig new_config)
         first_config_applied = true;
         config               = new_config;
 
-        // Clean CR1
+        // Wait until the peripheral is done before changing configuration
+        while ((spi->SR & SPI_SR_TXE) == 0)
+            ;
+        while ((spi->SR & SPI_SR_BSY) == 1)
+            ;
+        
         spi->CR1 = 0;
 
-        // Configure clock division (BR bits)
-        spi->CR1 |= (static_cast<uint16_t>(config.br) & 0x0007) << 3;
         // Configure CPOL & CPHA bits
-        spi->CR1 |= ((uint16_t)config.cpol & 0x0001) << 1 |
-                    ((uint16_t)config.cpha & 0x0001);
+        spi->CR1 |= static_cast<uint32_t>(config.mode);
+
+        // Configure clock division (BR bits)
+        spi->CR1 |= static_cast<uint32_t>(config.clock_div);
 
         // Configure LSBFIRST bit
-        spi->CR1 |= (uint16_t)config.lsb_first << 7;
+        spi->CR1 |= static_cast<uint32_t>(config.bit_order);
 
         spi->CR1 |= SPI_CR1_SSI | SPI_CR1_SSM  // Use software chip-select
                     | SPI_CR1_MSTR             // Master mode
diff --git a/src/shared/drivers/spi/SPIInterface.h b/src/shared/drivers/spi/SPIInterface.h
index e7d9955e9a5c516e250aa65fdfcd9be740b8fd29..7a3eaa1fd3497e99dc517492833fe82d69badd81 100644
--- a/src/shared/drivers/spi/SPIInterface.h
+++ b/src/shared/drivers/spi/SPIInterface.h
@@ -32,24 +32,48 @@ using miosix::delayUs;
 using miosix::GpioPin;
 
 /**
- * @brief SPI baud rate selection parameter.
- * SPI clock frequency will be equal to the SPI peripheral bus clock speed
- * divided by the value specified in this enum.
+ * @brief SPI Clock divider.
+ * SPI clock frequency will be equal to the SPI peripheral bus clock speed (see
+ * datasheet) divided by the value specified in this enum.
  *
  * Eg: DIV_2 --> spi clock freq = f_PCLK / 2
  *
- * See SPI->CR1 on the datasheet for further information.
+ * See register CR1  of the SPI peripheral on the reference manual for further
+ * information.
  */
-enum class SPIBaudRate
+enum class SPIClockDivider : uint8_t
 {
-    DIV_2   = 0,
-    DIV_4   = 1,
-    DIV_8   = 2,
-    DIV_16  = 3,
-    DIV_32  = 4,
-    DIV_64  = 5,
-    DIV_128 = 6,
-    DIV_256 = 7,
+    DIV2   = 0x00,
+    DIV4   = 0x08,
+    DIV8   = 0x10,
+    DIV16  = 0x18,
+    DIV32  = 0x20,
+    DIV64  = 0x28,
+    DIV128 = 0x30,
+    DIV256 = 0x38,
+};
+
+/**
+ * @brief SPI Mode.
+ * See slave device datasheet for information on which one to use.
+ */
+enum class SPIMode : uint8_t
+{
+    MODE0 = 0,  ///> CPOL = 0, CPHA = 0
+    MODE1 = 1,  ///> CPOL = 0, CPHA = 1
+    MODE2 = 2,  ///> CPOL = 1, CPHA = 0
+    MODE3 = 3   ///> CPOL = 1, CPHA = 1
+};
+
+/**
+ * @brief SPI Bit Order
+ * See register CR1  of the SPI peripheral on the reference manual for further
+ * information.
+ */
+enum class SPIBitOrder : uint8_t
+{
+    MSB_FIRST = 0,
+    LSB_FIRST = 0x80
 };
 
 /**
@@ -58,10 +82,10 @@ enum class SPIBaudRate
  */
 struct SPIBusConfig
 {
-    SPIBaudRate br = SPIBaudRate::DIV_2;  ///> Peripheral clock division
-    uint8_t cpol   = 0;                   ///> Clock polarity (0 - 1)
-    uint8_t cpha   = 0;                   ///> Clock phase (0 - 1)
-    bool lsb_first = false;               ///> MSB or LSB first
+    SPIClockDivider clock_div =
+        SPIClockDivider::DIV2;               ///> Peripheral clock division
+    SPIMode mode          = SPIMode::MODE0;  ///> Clock polarity (0 - 1)
+    SPIBitOrder bit_order = SPIBitOrder::MSB_FIRST;  ///> MSB or LSB first
 
     unsigned int cs_setup_time_us = 0;  ///> How long to wait before starting a
                                         ///> a trasmission after CS is set (us)
@@ -73,10 +97,9 @@ struct SPIBusConfig
     {
         // Compare member-by-member
         // clang-format off
-        return  br == other.br 
-             && cpol == other.cpol 
-             && cpha == other.cpha 
-             && lsb_first == other.lsb_first 
+        return  clock_div == other.clock_div 
+             && mode == other.mode
+             && bit_order == other.bit_order
              && cs_setup_time_us == other.cs_setup_time_us 
              && cs_setup_time_us == other.cs_hold_time_us;
         // clang-format on
diff --git a/src/shared/sensors/L3GD20.h b/src/shared/sensors/L3GD20.h
index 65e47100aa8ab0ac8726e31f1dffc8b582d5b933..69c60af52f763be70850af28aceb376d2bad9046 100644
--- a/src/shared/sensors/L3GD20.h
+++ b/src/shared/sensors/L3GD20.h
@@ -73,7 +73,7 @@ public:
         : L3GD20(bus, cs, {}, range, odr, cutoff_freq)
     {
         // Configure SPI
-        spislave.config.br = SPIBaudRate::DIV_32;
+        spislave.config.clock_div = SPIClockDivider::DIV32;
     }
 
     /**
diff --git a/src/shared/sensors/MS580301BA07/MS580301BA07.h b/src/shared/sensors/MS580301BA07/MS580301BA07.h
index 5131e32496e1b7e79068ffd5118a72957949daab..5f8a02d55e805dd9510cc9f9f529834f7154ba20 100644
--- a/src/shared/sensors/MS580301BA07/MS580301BA07.h
+++ b/src/shared/sensors/MS580301BA07/MS580301BA07.h
@@ -37,7 +37,7 @@ public:
     MS580301BA07(SPIBusInterface& bus, GpioPin cs)
         : MS580301BA07(bus, cs, SPIBusConfig{})
     {
-        spi_ms5803.config.br = SPIBaudRate::DIV_128;
+        spi_ms5803.config.clock_div = SPIClockDivider::DIV128;
     }
 
     MS580301BA07(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config)
diff --git a/src/tests/catch/spidriver/FakeSPIBus.h b/src/tests/catch/spidriver/FakeSPIBus.h
index 2a395552b1fe4d9698a7854154b3d543a270b623..94087b088fa4957db9d30b9520a4f3edc7374dde 100644
--- a/src/tests/catch/spidriver/FakeSPIBus.h
+++ b/src/tests/catch/spidriver/FakeSPIBus.h
@@ -151,14 +151,14 @@ void FakeSPIBus::configure(SPIBusConfig new_config)
         // Clean CR1
         spi->CR1 = 0;
 
-        // Configure clock division (BR bits)
-        spi->CR1 |= (static_cast<uint16_t>(config.br) & 0x0007) << 3;
         // Configure CPOL & CPHA bits
-        spi->CR1 |= ((uint16_t)config.cpol & 0x0001) << 1 |
-                    ((uint16_t)config.cpha & 0x0001);
+        spi->CR1 |= static_cast<uint32_t>(config.mode);
 
+        // Configure clock division (BR bits)
+        spi->CR1 |= static_cast<uint32_t>(config.clock_div);
+        
         // Configure LSBFIRST bit
-        spi->CR1 |= (uint16_t)config.lsb_first << 7;
+        spi->CR1 |= static_cast<uint32_t>(config.bit_order);
 
         spi->CR1 |= SPI_CR1_SSI | SPI_CR1_SSM  // Use software chip-select
                     | SPI_CR1_MSTR             // Master mode
diff --git a/src/tests/catch/spidriver/test-spidriver.cpp b/src/tests/catch/spidriver/test-spidriver.cpp
index 5438d812fa9bbff845e426a997a30dfe08c9e993..8b2f47f7edea4fedcaeab4c58f4cd9874d97a343 100644
--- a/src/tests/catch/spidriver/test-spidriver.cpp
+++ b/src/tests/catch/spidriver/test-spidriver.cpp
@@ -56,80 +56,102 @@ TEST_CASE("SPIBus - Bus Configuration")
     SECTION("Configure & check CR1")
     {
         SPIBusConfig config;
-        config.br = SPIBaudRate::DIV_16;
-
-        config.cpha      = 1;
-        config.cpol      = 1;
-        config.lsb_first = true;
-
-        uint32_t expected_CR1 = 0x03DF;
-
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
-
-        // Change config
-        config.br = SPIBaudRate::DIV_256;
-
-        config.cpha      = 0;
-        config.cpol      = 0;
-        config.lsb_first = false;
-
-        expected_CR1 = 0x037C;
-
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
-
-        config.cpha      = 0;
-        config.cpol      = 1;
-        config.lsb_first = false;
-
-        expected_CR1 = 0x037E;
-
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
+        REQUIRE(spi.CR1 == 0);
 
-        config.cpha      = 1;
-        config.cpol      = 0;
-        config.lsb_first = false;
+        SECTION("Mode")
+        {
+            config.mode = SPIMode::MODE0;
+            uint32_t expected_CR1 = 0x0344;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.mode = SPIMode::MODE1;
+            expected_CR1 = 0x0345;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.mode = SPIMode::MODE2;
+            expected_CR1 = 0x0346;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.mode = SPIMode::MODE3;
+            expected_CR1 = 0x0347;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
 
-        expected_CR1 = 0x037D;
+        }
 
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
+        SECTION("Clock Divider")
+        {
+            config.clock_div = SPIClockDivider::DIV2;
+            uint32_t expected_CR1 = 0x0344;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV4;
+            expected_CR1 = 0x034C;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV8;
+            expected_CR1 = 0x0354;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV16;
+            expected_CR1 = 0x035C;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV32;
+            expected_CR1 = 0x0364;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+            
+            config.clock_div = SPIClockDivider::DIV64;
+            expected_CR1 = 0x036C;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV128;
+            expected_CR1 = 0x0374;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV256;
+            expected_CR1 = 0x037C;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+        }
 
-        // Reapply same config
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
+        SECTION("Bit order")
+        {
+            config.bit_order = SPIBitOrder::MSB_FIRST;
+            uint32_t expected_CR1 = 0x0344;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.bit_order = SPIBitOrder::LSB_FIRST;
+            expected_CR1 = 0x03C4;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+        }
+      
     }
 
     SECTION("Disable configuration")
     {
         SPIBusConfig config;
-        config.br = SPIBaudRate::DIV_16;
+        config.clock_div = SPIClockDivider::DIV16;
 
-        config.cpha      = 1;
-        config.cpol      = 1;
-        config.lsb_first = true;
+        config.mode      = SPIMode::MODE3;
+        config.bit_order = SPIBitOrder::LSB_FIRST;
 
         bus.enableSlaveConfiguration(false);
         bus.configure(config);
         REQUIRE(spi.CR1 == 0);
     }
-
-    SECTION("Wrong parameters")
-    {
-        SPIBusConfig config;
-        config.br = SPIBaudRate::DIV_16;
-
-        config.cpha      = 8;
-        config.cpol      = 9;
-        config.lsb_first = true;
-
-        uint32_t expected_CR1 = 0x03DE;
-
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
-    }
 }
 
 TEST_CASE("SPIBus - Chip select")
@@ -157,11 +179,10 @@ TEST_CASE("SPIBus - One byte operations")
     FakeSPIBus bus{&spi};
 
     SPIBusConfig config;
-    config.br = SPIBaudRate::DIV_16;
+    config.clock_div = SPIClockDivider::DIV16;
 
-    config.cpha      = 1;
-    config.cpol      = 1;
-    config.lsb_first = true;
+    config.mode      = SPIMode::MODE3;
+    config.bit_order = SPIBitOrder::LSB_FIRST;
 
     bus.configure(config);
     bus.select(spi.cs);
@@ -209,11 +230,10 @@ TEST_CASE("SPIBus - Multi byte operations")
     FakeSPIBus bus{&spi};
 
     SPIBusConfig config;
-    config.br = SPIBaudRate::DIV_16;
+    config.clock_div = SPIClockDivider::DIV16;
 
-    config.cpha      = 1;
-    config.cpol      = 1;
-    config.lsb_first = true;
+    config.mode      = SPIMode::MODE3;
+    config.bit_order = SPIBitOrder::LSB_FIRST;
 
     bus.configure(config);
     bus.select(spi.cs);
@@ -264,8 +284,8 @@ TEST_CASE("SPITransaction - writes")
     MockSPIBus bus{};
     SPIBusConfig config1{};
 
-    config1.cpha = 1;
-    config1.br   = SPIBaudRate::DIV_32;
+    config1.mode = SPIMode::MODE1;
+    config1.clock_div  = SPIClockDivider::DIV32;
 
     bus.expected_config = config1;
 
@@ -360,8 +380,8 @@ TEST_CASE("SPITransaction - reads")
 
     SPIBusConfig config1;
 
-    config1.cpha = 1;
-    config1.br   = SPIBaudRate::DIV_32;
+    config1.mode = SPIMode::MODE1;
+    config1.clock_div  = SPIClockDivider::DIV32;
 
     bus.expected_config = config1;
 
@@ -450,8 +470,8 @@ TEST_CASE("SPITransaction - transfer")
 
     SPIBusConfig config1;
 
-    config1.cpha = 1;
-    config1.br   = SPIBaudRate::DIV_32;
+    config1.mode = SPIMode::MODE1;
+    config1.clock_div  = SPIClockDivider::DIV32;
 
     bus.expected_config = config1;
 
@@ -460,7 +480,7 @@ TEST_CASE("SPITransaction - transfer")
         SPISlave slave(bus, GpioPin(GPIOA_BASE, 1), config1);
         SPITransaction spi(slave);
 
-        uint8_t buf[4]  = {4, 3, 2, 1};
+        uint8_t buf[4] = {4, 3, 2, 1};
         uint8_t cmp[4] = {4, 3, 2, 1};
 
         spi.transfer(buf, 0);
diff --git a/src/tests/drivers/test-l3gd20.cpp b/src/tests/drivers/test-l3gd20.cpp
index e37d13c76c947f1b2f4f6f6395b5cdab8da0eae1..227dd44f16fc632840792cb352717cbc636b42fd 100644
--- a/src/tests/drivers/test-l3gd20.cpp
+++ b/src/tests/drivers/test-l3gd20.cpp
@@ -66,7 +66,6 @@ volatile uint32_t sample_delta;      // Tick delta between the last 2 watermark
 // L3GD20 SPI
 SPIBus bus(SPI5);
 GpioPin cs(GPIOC_BASE, 1);
-SPIBusConfig cfg;
 
 L3GD20* gyro;
 
@@ -98,8 +97,6 @@ int main()
 
     configure();
 
-    cfg.br = SPIBaudRate::DIV_64;
-
     gyro = new L3GD20(bus, cs, L3GD20::FullScaleRange::FS_250,
                       L3GD20::OutPutDataRate::ODR_760, 0x03);