From e310746452cc5c93ae2606a8f826b7a24d72c963 Mon Sep 17 00:00:00 2001 From: Christopher Durand Date: Thu, 13 Jul 2023 16:54:40 +0200 Subject: [PATCH 1/8] [platform] Move common SpiMaster acquire()/release() out of drivers --- src/modm/architecture/interface/spi_lock.hpp | 84 +++++++++++++++++++ src/modm/architecture/module.lb | 1 + .../spi/at90_tiny_mega/spi_master.cpp.in | 49 +---------- .../spi/at90_tiny_mega/spi_master.hpp.in | 19 ++--- .../uart_spi_master.cpp.in | 54 +----------- .../uart_spi_master.hpp.in | 20 ++--- .../spi/bitbang/bitbang_spi_master.hpp | 15 +--- .../spi/bitbang/bitbang_spi_master_impl.hpp | 47 ----------- src/modm/platform/spi/rp/spi_master.cpp.in | 33 -------- src/modm/platform/spi/rp/spi_master.hpp.in | 14 +--- src/modm/platform/spi/sam/spi_master.cpp.in | 35 -------- src/modm/platform/spi/sam/spi_master.hpp.in | 13 +-- .../platform/spi/sam_x7x/spi_master.cpp.in | 33 -------- .../platform/spi/sam_x7x/spi_master.hpp.in | 12 +-- src/modm/platform/spi/stm32/spi_master.cpp.in | 33 -------- src/modm/platform/spi/stm32/spi_master.hpp.in | 15 +--- .../spi/stm32_uart/uart_spi_master.cpp.in | 41 --------- .../spi/stm32_uart/uart_spi_master.hpp.in | 12 +-- 18 files changed, 117 insertions(+), 413 deletions(-) create mode 100644 src/modm/architecture/interface/spi_lock.hpp diff --git a/src/modm/architecture/interface/spi_lock.hpp b/src/modm/architecture/interface/spi_lock.hpp new file mode 100644 index 0000000000..bdf630c860 --- /dev/null +++ b/src/modm/architecture/interface/spi_lock.hpp @@ -0,0 +1,84 @@ +/* +* Copyright (c) 2014-2017, Niklas Hauser +* Copyright (c) 2023, Christopher Durand +* +* This file is part of the modm project. +* +* This Source Code Form is subject to the terms of the Mozilla Public +* License, v. 2.0. If a copy of the MPL was not distributed with this +* file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ +// ---------------------------------------------------------------------------- + +#ifndef MODM_INTERFACE_SPI_LOCK_HPP +#define MODM_INTERFACE_SPI_LOCK_HPP + +#include +#include + +namespace modm +{ + +/** + * Helper class to implement the synchronization mechanism of @ref modm::SpiMaster + * in SPI device drivers. + * + * Inherit from this class publicly and pass the derived class as the template + * argument, as in `class SpiMaster1 : public modm::SpiLock`. + * + * @tparam Derived SPI master derived class + * @ingroup modm_architecture_spi + */ +template +class SpiLock +{ +public: + static uint8_t + acquire(void* ctx, Spi::ConfigurationHandler handler); + + static uint8_t + release(void* ctx); + +private: + static inline uint8_t count{0}; + static inline void* context{nullptr}; + static inline Spi::ConfigurationHandler configuration{nullptr}; +}; + +template +uint8_t +SpiLock::acquire(void* ctx, Spi::ConfigurationHandler handler) +{ + if (context == nullptr) + { + context = ctx; + count = 1; + // if handler is not nullptr and is different from previous configuration + if (handler and configuration != handler) { + configuration = handler; + configuration(); + } + return 1; + } + + if (ctx == context) + return ++count; + + return 0; +} + +template +uint8_t +SpiLock::release(void* ctx) +{ + if (ctx == context) + { + if (--count == 0) + context = nullptr; + } + return count; +} + +} // namespace modm + +#endif // MODM_INTERFACE_SPI_LOCK_HPP diff --git a/src/modm/architecture/module.lb b/src/modm/architecture/module.lb index 7ce177c87f..09a093a0c0 100644 --- a/src/modm/architecture/module.lb +++ b/src/modm/architecture/module.lb @@ -310,6 +310,7 @@ class Spi(Module): env.outbasepath = "modm/src/modm/architecture" env.copy("interface/spi.hpp") env.copy("interface/spi_master.hpp") + env.copy("interface/spi_lock.hpp") # ----------------------------------------------------------------------------- class SpiDevice(Module): diff --git a/src/modm/platform/spi/at90_tiny_mega/spi_master.cpp.in b/src/modm/platform/spi/at90_tiny_mega/spi_master.cpp.in index 8c34ac0110..b93ecdf57d 100644 --- a/src/modm/platform/spi/at90_tiny_mega/spi_master.cpp.in +++ b/src/modm/platform/spi/at90_tiny_mega/spi_master.cpp.in @@ -19,19 +19,6 @@ #include #include -// bit 7 (0x80) is used for transfer 1 byte -// bit 6 (0x40) is used for transfer multiple byte -// bit 5-0 (0x3f) are used to store the acquire count -uint8_t -modm::platform::SpiMaster{{ id }}::state(0); - -void * -modm::platform::SpiMaster{{ id }}::context(nullptr); - -modm::Spi::ConfigurationHandler -modm::platform::SpiMaster{{ id }}::configuration(nullptr); -// ---------------------------------------------------------------------------- - void modm::platform::SpiMaster{{ id }}::initialize(Prescaler prescaler) { @@ -39,42 +26,8 @@ modm::platform::SpiMaster{{ id }}::initialize(Prescaler prescaler) SPCR{{ id }} = (1 << SPE{{ id }}) | (1 << MSTR{{ id }}) | (static_cast(prescaler) & ~0x80); SPSR{{ id }} = (static_cast(prescaler) & 0x80) ? (1 << SPI2X{{ id }}) : 0; - state &= 0x3f; + state = 0; } -// ---------------------------------------------------------------------------- - -uint8_t -modm::platform::SpiMaster{{ id }}::acquire(void *ctx, ConfigurationHandler handler) -{ - if (context == nullptr) - { - context = ctx; - state = (state & ~0x3f) | 1; - // if handler is not nullptr and is different from previous configuration - if (handler and configuration != handler) { - configuration = handler; - configuration(); - } - return 1; - } - - if (ctx == context) - return (++state & 0x3f); - - return 0; -} - -uint8_t -modm::platform::SpiMaster{{ id }}::release(void *ctx) -{ - if (ctx == context) - { - if ((--state & 0x3f) == 0) - context = nullptr; - } - return (state & 0x3f); -} -// ---------------------------------------------------------------------------- modm::ResumableResult modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) diff --git a/src/modm/platform/spi/at90_tiny_mega/spi_master.hpp.in b/src/modm/platform/spi/at90_tiny_mega/spi_master.hpp.in index 317850538e..469547490b 100644 --- a/src/modm/platform/spi/at90_tiny_mega/spi_master.hpp.in +++ b/src/modm/platform/spi/at90_tiny_mega/spi_master.hpp.in @@ -16,6 +16,7 @@ #include +#include #include #include #include @@ -50,11 +51,8 @@ namespace platform * @ingroup modm_platform_spi modm_platform_spi_{{id}} * @author Niklas Hauser */ -class SpiMaster{{ id }} : public ::modm::SpiMaster, private Spi +class SpiMaster{{ id }} : public ::modm::SpiMaster, public SpiLock, private Spi { - static uint8_t state; - static void *context; - static ConfigurationHandler configuration; public: /// Spi Data Mode, Mode0 is the most common mode enum class @@ -120,14 +118,6 @@ public: } } - - static uint8_t - acquire(void *ctx, ConfigurationHandler handler = nullptr); - - static uint8_t - release(void *ctx); - - static uint8_t transferBlocking(uint8_t data) { @@ -158,6 +148,11 @@ public: protected: static void initialize(Prescaler prescaler); + +private: + // bit 7 (0x80) is used for transfer 1 byte + // bit 6 (0x40) is used for transfer multiple byte + static inline uint8_t state{}; }; } // namespace platform diff --git a/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.cpp.in b/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.cpp.in index 4801a690aa..e7af684dc8 100644 --- a/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.cpp.in +++ b/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.cpp.in @@ -21,24 +21,6 @@ #include #include -%% if not extended -modm::platform::UartSpiMaster{{id}}::DataOrder -modm::platform::UartSpiMaster{{id}}::dataOrder = DataOrder::MsbFirst; -%% endif - -// bit 7 (0x80) is used for transfer 1 byte -// bit 6 (0x40) is used for transfer multiple byte -// bit 5-0 (0x3f) are used to store the acquire count -uint8_t -modm::platform::UartSpiMaster{{id}}::state(0); - -void * -modm::platform::UartSpiMaster{{id}}::context(nullptr); - -modm::Spi::ConfigurationHandler -modm::platform::UartSpiMaster{{id}}::configuration(nullptr); -// ---------------------------------------------------------------------------- - void modm::platform::UartSpiMaster{{id}}::initialize(uint16_t prescaler) { @@ -71,43 +53,9 @@ modm::platform::UartSpiMaster{{id}}::initialize(uint16_t prescaler) %% if not extended dataOrder = DataOrder::MsbFirst; %% endif - state &= 0x3f; -} -// ---------------------------------------------------------------------------- - -uint8_t -modm::platform::UartSpiMaster{{id}}::acquire(void *ctx, ConfigurationHandler handler) -{ - if (context == nullptr) - { - context = ctx; - state = (state & ~0x3f) | 1; - // if handler is not nullptr and is different from previous configuration - if (handler and configuration != handler) { - configuration = handler; - configuration(); - } - return 1; - } - - if (ctx == context) - return (++state & 0x3f); - - return 0; + state = 0; } -uint8_t -modm::platform::UartSpiMaster{{id}}::release(void *ctx) -{ - if (ctx == context) - { - if ((--state & 0x3f) == 0) - context = nullptr; - } - return (state & 0x3f); -} -// ---------------------------------------------------------------------------- - modm::ResumableResult modm::platform::UartSpiMaster{{id}}::transfer(uint8_t data) { diff --git a/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.hpp.in b/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.hpp.in index cdd8a62fb4..cbbfd22d15 100644 --- a/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.hpp.in +++ b/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.hpp.in @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -32,11 +33,8 @@ namespace platform * @ingroup modm_platform_uart_spi modm_platform_uart_spi_{{id}} * @author Niklas Hauser */ -class UartSpiMaster{{ id }} : public ::modm::SpiMaster +class UartSpiMaster{{ id }} : public ::modm::SpiMaster, public SpiLock { - static uint8_t state; - static void *context; - static ConfigurationHandler configuration; public: /// Spi Data Mode, Mode0 is the most common mode %% if not extended @@ -144,14 +142,6 @@ public: %% endif } - - static uint8_t - acquire(void *ctx, ConfigurationHandler handler = nullptr); - - static uint8_t - release(void *ctx); - - static uint8_t transferBlocking(uint8_t data) { @@ -176,8 +166,12 @@ protected: initialize(uint16_t prescaler); private: + // bit 7 (0x80) is used for transfer 1 byte + // bit 6 (0x40) is used for transfer multiple byte + static inline uint8_t state{}; + %% if not extended - static DataOrder dataOrder; + static inline DataOrder dataOrder = DataOrder::MsbFirst; %% endif }; diff --git a/src/modm/platform/spi/bitbang/bitbang_spi_master.hpp b/src/modm/platform/spi/bitbang/bitbang_spi_master.hpp index 4cc6496bea..6de402fd96 100644 --- a/src/modm/platform/spi/bitbang/bitbang_spi_master.hpp +++ b/src/modm/platform/spi/bitbang/bitbang_spi_master.hpp @@ -16,7 +16,8 @@ #ifndef MODM_SOFTWARE_BITBANG_SPI_MASTER_HPP #define MODM_SOFTWARE_BITBANG_SPI_MASTER_HPP -#include +#include +#include #include #include #include @@ -41,7 +42,8 @@ namespace platform template< typename Sck, typename Mosi, typename Miso = GpioUnused > -class BitBangSpiMaster : public ::modm::SpiMaster +class BitBangSpiMaster : public ::modm::SpiMaster, + public SpiLock> { public: template< class... Signals > @@ -59,21 +61,12 @@ class BitBangSpiMaster : public ::modm::SpiMaster static void setDataOrder(DataOrder order); - - static uint8_t - acquire(void *ctx, ConfigurationHandler handler = nullptr); - - static uint8_t - release(void *ctx); - - static uint8_t transferBlocking(uint8_t data); static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length); - static modm::ResumableResult transfer(uint8_t data); diff --git a/src/modm/platform/spi/bitbang/bitbang_spi_master_impl.hpp b/src/modm/platform/spi/bitbang/bitbang_spi_master_impl.hpp index 140576ac54..8c8c418210 100644 --- a/src/modm/platform/spi/bitbang/bitbang_spi_master_impl.hpp +++ b/src/modm/platform/spi/bitbang/bitbang_spi_master_impl.hpp @@ -26,17 +26,6 @@ template uint8_t modm::platform::BitBangSpiMaster::operationMode(0); -template -uint8_t -modm::platform::BitBangSpiMaster::count(0); - -template -void * -modm::platform::BitBangSpiMaster::context(nullptr); - -template -modm::Spi::ConfigurationHandler -modm::platform::BitBangSpiMaster::configuration(nullptr); // ---------------------------------------------------------------------------- template @@ -90,42 +79,6 @@ modm::platform::BitBangSpiMaster::setDataOrder(DataOrder order) else operationMode &= ~0b100; } -// ---------------------------------------------------------------------------- - -template -uint8_t -modm::platform::BitBangSpiMaster::acquire(void *ctx, ConfigurationHandler handler) -{ - if (context == nullptr) - { - context = ctx; - count = 1; - // if handler is not nullptr and is different from previous configuration - if (handler and configuration != handler) { - configuration = handler; - configuration(); - } - return 1; - } - - if (ctx == context) - return ++count; - - return 0; -} - -template -uint8_t -modm::platform::BitBangSpiMaster::release(void *ctx) -{ - if (ctx == context) - { - if (--count == 0) - context = nullptr; - } - return count; -} -// ---------------------------------------------------------------------------- template uint8_t diff --git a/src/modm/platform/spi/rp/spi_master.cpp.in b/src/modm/platform/spi/rp/spi_master.cpp.in index 74997c31fc..622f3c2e74 100644 --- a/src/modm/platform/spi/rp/spi_master.cpp.in +++ b/src/modm/platform/spi/rp/spi_master.cpp.in @@ -25,39 +25,6 @@ void modm::platform::SpiMaster{{ id }}::unreset() Resets::unresetWait(RESETS_RESET_SPI{{ id }}_BITS); } -uint8_t -modm::platform::SpiMaster{{ id }}::acquire(void *ctx, ConfigurationHandler handler) -{ - if (context == nullptr) - { - context = ctx; - count = 1; - // if handler is not nullptr and is different from previous configuration - if (handler and configuration != handler) { - configuration = handler; - configuration(); - } - return 1; - } - - if (ctx == context) - return ++count; - - return 0; -} - -uint8_t -modm::platform::SpiMaster{{ id }}::release(void *ctx) -{ - if (ctx == context) - { - if (--count == 0) - context = nullptr; - } - return count; -} -// ---------------------------------------------------------------------------- - modm::ResumableResult modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { diff --git a/src/modm/platform/spi/rp/spi_master.hpp.in b/src/modm/platform/spi/rp/spi_master.hpp.in index 86b0590539..e8676bc22b 100644 --- a/src/modm/platform/spi/rp/spi_master.hpp.in +++ b/src/modm/platform/spi/rp/spi_master.hpp.in @@ -10,6 +10,7 @@ // ---------------------------------------------------------------------------- #pragma once +#include #include #include #include @@ -25,18 +26,13 @@ namespace modm::platform * @author Andrey Kunitsyn * @ingroup modm_platform_spi modm_platform_spi_{{id}} */ -class SpiMaster{{ id }} : public modm::SpiMaster +class SpiMaster{{ id }} : public modm::SpiMaster, public SpiLock { protected: // Bit0: single transfer state // Bit1: block transfer state static inline uint8_t state{0}; -private: - static inline uint8_t count{0}; - static inline void *context{nullptr}; - static inline ConfigurationHandler configuration{nullptr}; -protected: static inline bool isBusy() { return (spi{{ id }}_hw->sr & SPI_SSPSR_BSY_BITS); } @@ -168,12 +164,6 @@ public: modm_assert(order == DataOrder::MsbFirst, "SPI DataOrder", "SPI hardware does not support alternate transmit order"); } - static uint8_t - acquire(void *ctx, ConfigurationHandler handler = nullptr); - - static uint8_t - release(void *ctx); - static uint8_t transferBlocking(uint8_t data) { diff --git a/src/modm/platform/spi/sam/spi_master.cpp.in b/src/modm/platform/spi/sam/spi_master.cpp.in index 1630e3d0a8..1c5991ce07 100644 --- a/src/modm/platform/spi/sam/spi_master.cpp.in +++ b/src/modm/platform/spi/sam/spi_master.cpp.in @@ -17,41 +17,6 @@ #include "spi_master_{{id}}.hpp" -// ---------------------------------------------------------------------------- - -uint8_t -modm::platform::SpiMaster{{ id }}::acquire(void *ctx, ConfigurationHandler handler) -{ - if (context == nullptr) - { - context = ctx; - count = 1; - // if handler is not nullptr and is different from previous configuration - if (handler and configuration != handler) { - configuration = handler; - configuration(); - } - return 1; - } - - if (ctx == context) - return ++count; - - return 0; -} - -uint8_t -modm::platform::SpiMaster{{ id }}::release(void *ctx) -{ - if (ctx == context) - { - if (--count == 0) - context = nullptr; - } - return count; -} -// ---------------------------------------------------------------------------- - modm::ResumableResult modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { diff --git a/src/modm/platform/spi/sam/spi_master.hpp.in b/src/modm/platform/spi/sam/spi_master.hpp.in index 98dbffeb08..ed5726dd46 100644 --- a/src/modm/platform/spi/sam/spi_master.hpp.in +++ b/src/modm/platform/spi/sam/spi_master.hpp.in @@ -10,6 +10,7 @@ // ---------------------------------------------------------------------------- #pragma once +#include #include #include #include @@ -23,13 +24,11 @@ namespace modm::platform { * @author Jeff McBride * @ingroup modm_platform_spi modm_platform_spi_{{id}} */ -class SpiMaster{{ id }} : public modm::SpiMaster { +class SpiMaster{{ id }} : public modm::SpiMaster, public SpiLock +{ // Bit0: single transfer state // Bit1: block transfer state static inline uint8_t state{0}; - static inline uint8_t count{0}; - static inline void *context{nullptr}; - static inline ConfigurationHandler configuration{nullptr}; // Work around a name collision between 'struct modm::Spi' and 'struct Spi' // defined in the CMSIS headers @@ -115,12 +114,6 @@ public: modm_assert(order == DataOrder::MsbFirst, "SPI DataOrder", "SPI hardware does not support alternate transmit order"); } - static uint8_t - acquire(void *ctx, ConfigurationHandler handler = nullptr); - - static uint8_t - release(void *ctx); - static uint8_t transferBlocking(uint8_t data) { return RF_CALL_BLOCKING(transfer(data)); diff --git a/src/modm/platform/spi/sam_x7x/spi_master.cpp.in b/src/modm/platform/spi/sam_x7x/spi_master.cpp.in index e32b7b2995..218c7dc44d 100644 --- a/src/modm/platform/spi/sam_x7x/spi_master.cpp.in +++ b/src/modm/platform/spi/sam_x7x/spi_master.cpp.in @@ -17,39 +17,6 @@ #include "spi_master_{{id}}.hpp" -uint8_t -modm::platform::SpiMaster{{ id }}::acquire(void* ctx, ConfigurationHandler handler) -{ - if (context == nullptr) - { - context = ctx; - count = 1; - // if handler is not nullptr and is different from previous configuration - if (handler and configuration != handler) { - configuration = handler; - configuration(); - } - return 1; - } - - if (ctx == context) - return ++count; - - return 0; -} - -uint8_t -modm::platform::SpiMaster{{ id }}::release(void* ctx) -{ - if (ctx == context) - { - if (--count == 0) - context = nullptr; - } - return count; -} -// ---------------------------------------------------------------------------- - modm::ResumableResult modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { diff --git a/src/modm/platform/spi/sam_x7x/spi_master.hpp.in b/src/modm/platform/spi/sam_x7x/spi_master.hpp.in index 597ca59293..9f40cc092f 100644 --- a/src/modm/platform/spi/sam_x7x/spi_master.hpp.in +++ b/src/modm/platform/spi/sam_x7x/spi_master.hpp.in @@ -18,6 +18,7 @@ #ifndef MODM_SAMX7X_SPI_MASTER{{ id }}_HPP #define MODM_SAMX7X_SPI_MASTER{{ id }}_HPP +#include #include #include #include @@ -33,15 +34,12 @@ namespace modm::platform * * @ingroup modm_platform_spi modm_platform_spi_{{id}} */ -class SpiMaster{{ id }} : public modm::SpiMaster, public SpiBase +class SpiMaster{{ id }} : public modm::SpiMaster, public SpiLock, public SpiBase { private: // Bit0: single transfer state // Bit1: block transfer state static inline uint8_t state{0}; - static inline uint8_t count{0}; - static inline void* context{nullptr}; - static inline ConfigurationHandler configuration{nullptr}; public: using DataMode = SpiBase::DataMode; using Hal = SpiHal{{ id }}; @@ -95,12 +93,6 @@ public: SpiHal{{ id }}::setDataSize(size); } - static uint8_t - acquire(void* ctx, ConfigurationHandler handler = nullptr); - - static uint8_t - release(void* ctx); - static uint8_t transferBlocking(uint8_t data) { diff --git a/src/modm/platform/spi/stm32/spi_master.cpp.in b/src/modm/platform/spi/stm32/spi_master.cpp.in index 57dae48e59..825fc911fe 100644 --- a/src/modm/platform/spi/stm32/spi_master.cpp.in +++ b/src/modm/platform/spi/stm32/spi_master.cpp.in @@ -16,39 +16,6 @@ #include "spi_master_{{id}}.hpp" -uint8_t -modm::platform::SpiMaster{{ id }}::acquire(void *ctx, ConfigurationHandler handler) -{ - if (context == nullptr) - { - context = ctx; - count = 1; - // if handler is not nullptr and is different from previous configuration - if (handler and configuration != handler) { - configuration = handler; - configuration(); - } - return 1; - } - - if (ctx == context) - return ++count; - - return 0; -} - -uint8_t -modm::platform::SpiMaster{{ id }}::release(void *ctx) -{ - if (ctx == context) - { - if (--count == 0) - context = nullptr; - } - return count; -} -// ---------------------------------------------------------------------------- - modm::ResumableResult modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { diff --git a/src/modm/platform/spi/stm32/spi_master.hpp.in b/src/modm/platform/spi/stm32/spi_master.hpp.in index 87999cfa98..bb94cee464 100644 --- a/src/modm/platform/spi/stm32/spi_master.hpp.in +++ b/src/modm/platform/spi/stm32/spi_master.hpp.in @@ -18,6 +18,7 @@ #ifndef MODM_STM32_SPI_MASTER{{ id }}_HPP #define MODM_STM32_SPI_MASTER{{ id }}_HPP +#include #include #include #include @@ -37,17 +38,13 @@ namespace platform * @author Niklas Hauser * @ingroup modm_platform_spi modm_platform_spi_{{id}} */ -class SpiMaster{{ id }} : public modm::SpiMaster +class SpiMaster{{ id }} : public modm::SpiMaster, public SpiLock { protected: // `state` must be protected to allow access from SpiMasterDma subclass // Bit0: single transfer state // Bit1: block transfer state static inline uint8_t state{0}; -private: - static inline uint8_t count{0}; - static inline void* context{nullptr}; - static inline ConfigurationHandler configuration{nullptr}; public: using Hal = SpiHal{{ id }}; @@ -127,14 +124,6 @@ public: SpiHal{{ id }}::enableTransfer(); } - - static uint8_t - acquire(void *ctx, ConfigurationHandler handler = nullptr); - - static uint8_t - release(void *ctx); - - static uint8_t transferBlocking(uint8_t data) { diff --git a/src/modm/platform/spi/stm32_uart/uart_spi_master.cpp.in b/src/modm/platform/spi/stm32_uart/uart_spi_master.cpp.in index fa234bf778..282303cd39 100644 --- a/src/modm/platform/spi/stm32_uart/uart_spi_master.cpp.in +++ b/src/modm/platform/spi/stm32_uart/uart_spi_master.cpp.in @@ -23,47 +23,6 @@ modm::platform::UartSpiMaster{{ id }}::DataOrder uint8_t modm::platform::UartSpiMaster{{ id }}::state(0); -uint8_t -modm::platform::UartSpiMaster{{ id }}::count(0); - -void * -modm::platform::UartSpiMaster{{ id }}::context(nullptr); - -modm::Spi::ConfigurationHandler -modm::platform::UartSpiMaster{{ id }}::configuration(nullptr); -// ---------------------------------------------------------------------------- - -uint8_t -modm::platform::UartSpiMaster{{ id }}::acquire(void *ctx, ConfigurationHandler handler) -{ - if (context == nullptr) - { - context = ctx; - count = 1; - // if handler is not nullptr and is different from previous configuration - if (handler and configuration != handler) { - configuration = handler; - configuration(); - } - return 1; - } - - if (ctx == context) - return ++count; - - return 0; -} - -uint8_t -modm::platform::UartSpiMaster{{ id }}::release(void *ctx) -{ - if (ctx == context) - { - if (--count == 0) - context = nullptr; - } - return count; -} // ---------------------------------------------------------------------------- modm::ResumableResult diff --git a/src/modm/platform/spi/stm32_uart/uart_spi_master.hpp.in b/src/modm/platform/spi/stm32_uart/uart_spi_master.hpp.in index 504e8d49f1..9915fc19cb 100644 --- a/src/modm/platform/spi/stm32_uart/uart_spi_master.hpp.in +++ b/src/modm/platform/spi/stm32_uart/uart_spi_master.hpp.in @@ -14,6 +14,7 @@ #ifndef MODM_STM32_UART_SPI_MASTER{{ id }}_HPP #define MODM_STM32_UART_SPI_MASTER{{ id }}_HPP +#include #include #include #include "../uart/uart_hal_{{ id }}.hpp" @@ -33,7 +34,8 @@ namespace platform * @author Niklas Hauser * @ingroup modm_platform_uart_spi modm_platform_uart_spi_{{id}} */ -class UartSpiMaster{{ id }} : public modm::SpiMaster, public UartBase +class UartSpiMaster{{ id }} : public modm::SpiMaster, public UartBase, + public SpiLock { static DataOrder dataOrder; static uint8_t state; @@ -103,14 +105,6 @@ public: dataOrder = order; } - - static uint8_t - acquire(void *ctx, ConfigurationHandler handler = nullptr); - - static uint8_t - release(void *ctx); - - static uint8_t transferBlocking(uint8_t data) { From 0b0ddd4f4b73bfec5f24d4ce705f272c85320035 Mon Sep 17 00:00:00 2001 From: Christopher Durand Date: Thu, 13 Jul 2023 20:22:02 +0200 Subject: [PATCH 2/8] [stm32] Add interface to query DMA transfer status without interrupt --- src/modm/platform/dma/stm32/dma.hpp.in | 50 ++++++++++++++++++--- src/modm/platform/dma/stm32/dma_base.hpp.in | 8 ++-- 2 files changed, 48 insertions(+), 10 deletions(-) diff --git a/src/modm/platform/dma/stm32/dma.hpp.in b/src/modm/platform/dma/stm32/dma.hpp.in index adec14cc0e..9da23cf00b 100644 --- a/src/modm/platform/dma/stm32/dma.hpp.in +++ b/src/modm/platform/dma/stm32/dma.hpp.in @@ -3,7 +3,7 @@ * Copyright (c) 2014-2017, Niklas Hauser * Copyright (c) 2020, Mike Wolfram * Copyright (c) 2021, Raphael Lehmann - * Copyright (c) 2021, Christopher Durand + * Copyright (c) 2021-2023, Christopher Durand * * This file is part of the modm project. * @@ -116,6 +116,10 @@ public: using ChannelHal = DmaChannelHal; +%% if dmaType in ["stm32-stream-channel", "stm32-mux-stream"] + static constexpr uint8_t FlagOffsetLut[8] = {0, 6, 16, 22, 0+32, 6+32, 16+32, 22+32}; +%% endif + public: /** * Configure the DMA channel @@ -145,7 +149,7 @@ public: } /** - * Start the transfer of the DMA channel + * Start the transfer of the DMA channel and clear all interrupt flags. */ static void start() @@ -321,15 +325,14 @@ public: uint32_t(InterruptFlags::Error) << (uint32_t(ChannelID) * 4) }; %% elif dmaType in ["stm32-stream-channel", "stm32-mux-stream"] - constexpr uint8_t offsetLut[8] = {0, 6, 16, 22, 0+32, 6+32, 16+32, 22+32}; static const uint64_t HT_Flag { - uint64_t(InterruptFlags::HalfTransferComplete) << offsetLut[uint32_t(ChannelID)] + uint64_t(InterruptFlags::HalfTransferComplete) << FlagOffsetLut[uint32_t(ChannelID)] }; static const uint64_t TC_Flag { - uint64_t(InterruptFlags::TransferComplete) << offsetLut[uint32_t(ChannelID)] + uint64_t(InterruptFlags::TransferComplete) << FlagOffsetLut[uint32_t(ChannelID)] }; static const uint64_t TE_Flag { - uint64_t(InterruptFlags::Error) << offsetLut[uint32_t(ChannelID)] + uint64_t(InterruptFlags::Error) << FlagOffsetLut[uint32_t(ChannelID)] }; %% endif @@ -349,6 +352,41 @@ public: ControlHal::clearInterruptFlags(InterruptFlags::Global, ChannelID); } + /** + * Read channel status flags when channel interrupts are disabled. + * This function is useful to query the transfer state when the use of + * the channel interrupt is not required for the application. + * + * @warning Flags are automatically cleared in the ISR if the channel + * interrupt is enabled or when start() is called. + */ + static InterruptFlags_t + getInterruptFlags() + { + const auto globalFlags = ControlHal::getInterruptFlags(); + const auto mask = static_cast(InterruptFlags::All); +%% if dmaType in ["stm32-channel-request", "stm32-channel", "stm32-mux"] + const auto shift = static_cast(ChannelID) * 4; +%% elif dmaType in ["stm32-stream-channel", "stm32-mux-stream"] + const auto shift = FlagOffsetLut[uint32_t(ChannelID)]; +%% endif + const auto channelFlags = static_cast((globalFlags >> shift) & mask); + return InterruptFlags_t{channelFlags}; + } + + /** + * Clear channel interrupt flags. + * Use only when the channel interrupt is disabled. + * + * @warning Flags are automatically cleared in the ISR if the channel + * interrupt is enabled or when start() is called. + */ + static void + clearInterruptFlags(InterruptFlags_t flags = InterruptFlags::All) + { + ControlHal::clearInterruptFlags(flags, ChannelID); + } + /** * Enable the IRQ vector of the channel * diff --git a/src/modm/platform/dma/stm32/dma_base.hpp.in b/src/modm/platform/dma/stm32/dma_base.hpp.in index 0b1f12a7d8..b2b250f6a0 100644 --- a/src/modm/platform/dma/stm32/dma_base.hpp.in +++ b/src/modm/platform/dma/stm32/dma_base.hpp.in @@ -228,7 +228,7 @@ public: }; %% if dmaType in ["stm32-stream-channel", "stm32-mux-stream"] - enum class InterruptEnable { + enum class InterruptEnable : uint32_t { DirectModeError = DMA_SxCR_DMEIE, TransferError = DMA_SxCR_TEIE, HalfTransfer = DMA_SxCR_HTIE, @@ -236,7 +236,7 @@ public: }; MODM_FLAGS32(InterruptEnable); - enum class InterruptFlags { + enum class InterruptFlags : uint8_t { FifoError = 0b00'0001, DirectModeError = 0b00'0100, Error = 0b00'1000, @@ -247,14 +247,14 @@ public: }; MODM_FLAGS32(InterruptFlags); %% elif dmaType in ["stm32-channel-request", "stm32-channel", "stm32-mux"] - enum class InterruptEnable { + enum class InterruptEnable : uint32_t { TransferComplete = DMA_CCR_TCIE, HalfTransfer = DMA_CCR_HTIE, TransferError = DMA_CCR_TEIE, }; MODM_FLAGS32(InterruptEnable); - enum class InterruptFlags { + enum class InterruptFlags : uint8_t { Global = 0b0001, TransferComplete = 0b0010, HalfTransferComplete = 0b0100, From 3b2530fe8b42f337aa9d2086c2c4397f9cf634bd Mon Sep 17 00:00:00 2001 From: Christopher Durand Date: Thu, 13 Jul 2023 21:19:14 +0200 Subject: [PATCH 3/8] [board] Fix SPI1, SPI2 and SPI3 clock for Nucleo H723ZG --- src/modm/board/nucleo_h723zg/board.hpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modm/board/nucleo_h723zg/board.hpp b/src/modm/board/nucleo_h723zg/board.hpp index 6b3fbd6575..ed286d3422 100644 --- a/src/modm/board/nucleo_h723zg/board.hpp +++ b/src/modm/board/nucleo_h723zg/board.hpp @@ -32,6 +32,7 @@ struct SystemClock { // Max 550MHz static constexpr uint32_t SysClk = 550_MHz; + static constexpr uint32_t Pll1Q = SysClk / 4; // Max 550MHz static constexpr uint32_t Hclk = SysClk / 1; // D1CPRE static constexpr uint32_t Frequency = Hclk; @@ -53,9 +54,9 @@ struct SystemClock static constexpr uint32_t Dac1 = Apb1; - static constexpr uint32_t Spi1 = Apb2; - static constexpr uint32_t Spi2 = Apb1; - static constexpr uint32_t Spi3 = Apb1; + static constexpr uint32_t Spi1 = Pll1Q; + static constexpr uint32_t Spi2 = Pll1Q; + static constexpr uint32_t Spi3 = Pll1Q; static constexpr uint32_t Spi4 = Apb2; static constexpr uint32_t Spi5 = Apb2; static constexpr uint32_t Spi6 = Apb4; @@ -115,9 +116,9 @@ struct SystemClock .range = Rcc::PllInputRange::MHz1_2, .pllM = 4, // 8 MHz / 4 = 2 MHz .pllN = 275, // 2 MHz * 275 = 550 MHz - .pllP = 1, // 500 MHz / 1 = 550 MHz - .pllQ = 2, // 500 MHz / 2 = 275 MHz - .pllR = 2, // 500 MHz / 2 = 275 MHz + .pllP = 1, // 550 MHz / 1 = 550 MHz + .pllQ = 4, // 550 MHz / 4 = 137.5 MHz + .pllR = 2, // 550 MHz / 2 = 275 MHz }; Rcc::enablePll1(Rcc::PllSource::Hse, pllFactors1); Rcc::setFlashLatency(); From 13c1cd2187254526eb64cbc1b23dc6155f2c0b50 Mon Sep 17 00:00:00 2001 From: Christopher Durand Date: Tue, 18 Jul 2023 23:16:05 +0200 Subject: [PATCH 4/8] [processing] Do not require protothread module to use SPI with fibers --- src/modm/platform/spi/at90_tiny_mega/module.lb | 2 +- src/modm/platform/spi/rp/module.lb | 2 +- src/modm/platform/spi/sam/module.lb | 2 +- src/modm/platform/spi/sam_x7x/module.lb | 2 +- src/modm/platform/spi/stm32/module.lb | 2 +- src/modm/platform/spi/stm32_uart/module.lb | 2 +- src/modm/processing/fiber/module.lb | 9 ++++++++- src/modm/processing/resumable/module.lb | 2 +- 8 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modm/platform/spi/at90_tiny_mega/module.lb b/src/modm/platform/spi/at90_tiny_mega/module.lb index ff52edd2a0..60a006e169 100644 --- a/src/modm/platform/spi/at90_tiny_mega/module.lb +++ b/src/modm/platform/spi/at90_tiny_mega/module.lb @@ -15,7 +15,7 @@ def get_properties(env): device = env[":target"] driver = device.get_driver("spi:avr") properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", False) + properties["use_fiber"] = env.query(":processing:fiber:__enabled", False) properties["target"] = device.identifier properties["partname"] = device.partname properties["driver"] = driver diff --git a/src/modm/platform/spi/rp/module.lb b/src/modm/platform/spi/rp/module.lb index ba4c4598e8..c072bf9d15 100644 --- a/src/modm/platform/spi/rp/module.lb +++ b/src/modm/platform/spi/rp/module.lb @@ -25,7 +25,7 @@ class Instance(Module): def build(self, env): env.substitutions = { "id": self.instance, - "use_fiber": env.get(":processing:protothread:use_fiber", False) + "use_fiber": env.query(":processing:fiber:__enabled", False) } env.outbasepath = "modm/src/modm/platform/spi" diff --git a/src/modm/platform/spi/sam/module.lb b/src/modm/platform/spi/sam/module.lb index f3509f493b..4c0e7818f4 100644 --- a/src/modm/platform/spi/sam/module.lb +++ b/src/modm/platform/spi/sam/module.lb @@ -14,7 +14,7 @@ def get_properties(env): device = env[":target"] driver = device.get_driver("spi") properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", False) + properties["use_fiber"] = env.query(":processing:fiber:__enabled", False) properties["target"] = device.identifier properties["features"] = driver["feature"] if "feature" in driver else [] return properties diff --git a/src/modm/platform/spi/sam_x7x/module.lb b/src/modm/platform/spi/sam_x7x/module.lb index a1cd54519c..459b51e5e9 100644 --- a/src/modm/platform/spi/sam_x7x/module.lb +++ b/src/modm/platform/spi/sam_x7x/module.lb @@ -15,7 +15,7 @@ def get_properties(env): device = env[":target"] properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", False) + properties["use_fiber"] = env.query(":processing:fiber:__enabled", False) properties["target"] = device.identifier return properties diff --git a/src/modm/platform/spi/stm32/module.lb b/src/modm/platform/spi/stm32/module.lb index 2329a5bc84..7d4ee68e92 100644 --- a/src/modm/platform/spi/stm32/module.lb +++ b/src/modm/platform/spi/stm32/module.lb @@ -16,7 +16,7 @@ def get_properties(env): device = env[":target"] driver = device.get_driver("spi") properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", False) + properties["use_fiber"] = env.query(":processing:fiber:__enabled", False) properties["target"] = device.identifier properties["features"] = driver["feature"] if "feature" in driver else [] return properties diff --git a/src/modm/platform/spi/stm32_uart/module.lb b/src/modm/platform/spi/stm32_uart/module.lb index a2c4942e02..ee10191197 100644 --- a/src/modm/platform/spi/stm32_uart/module.lb +++ b/src/modm/platform/spi/stm32_uart/module.lb @@ -17,7 +17,7 @@ def get_properties(env): "target": device.identifier, "driver": driver, "over8_sampling": ("feature" in driver) and ("over8" in driver["feature"]), - "use_fiber": env.get(":processing:protothread:use_fiber", False), + "use_fiber": env.query(":processing:fiber:__enabled", False) } return properties diff --git a/src/modm/processing/fiber/module.lb b/src/modm/processing/fiber/module.lb index 30bcfca10c..22e09dbe3f 100644 --- a/src/modm/processing/fiber/module.lb +++ b/src/modm/processing/fiber/module.lb @@ -2,7 +2,7 @@ # -*- coding: utf-8 -*- # # Copyright (c) 2020, Erik Henriksson -# Copyright (c) 2021, Christopher Durand +# Copyright (c) 2021-2023, Christopher Durand # Copyright (c) 2021, Niklas Hauser # # This file is part of the modm project. @@ -16,9 +16,16 @@ def init(module): module.name = ":processing:fiber" module.description = FileReader("module.md") +def is_enabled(env): + return env.get(":processing:protothread:use_fiber", False) or \ + not env.has_module(":processing:protothread") def prepare(module, options): module.depends(":processing:timer") + + module.add_query( + EnvironmentQuery(name="__enabled", factory=is_enabled)) + # No ARM64 support yet! return "arm64" not in options[":target"].get_driver("core")["type"] diff --git a/src/modm/processing/resumable/module.lb b/src/modm/processing/resumable/module.lb index eae81c1431..b68e2bee09 100644 --- a/src/modm/processing/resumable/module.lb +++ b/src/modm/processing/resumable/module.lb @@ -29,7 +29,7 @@ def prepare(module, options): def build(env): env.outbasepath = "modm/src/modm/processing/resumable" - if env.get(":processing:protothread:use_fiber", False): + if env.query(":processing:fiber:__enabled", False): env.copy("macros_fiber.hpp", "macros.hpp") env.copy("resumable_fiber.hpp", "resumable.hpp") else: From 53796b08c5989b8f380aa99267f21ece6681bdf4 Mon Sep 17 00:00:00 2001 From: Christopher Durand Date: Thu, 13 Jul 2023 21:48:47 +0200 Subject: [PATCH 5/8] [stm32] Add STM32H7 SPI driver with DMA support --- README.md | 2 +- src/modm/platform/spi/stm32h7/module.lb | 93 +++++ src/modm/platform/spi/stm32h7/spi_base.hpp.in | 204 ++++++++++ src/modm/platform/spi/stm32h7/spi_hal.hpp.in | 209 +++++++++++ .../platform/spi/stm32h7/spi_hal_impl.hpp.in | 353 ++++++++++++++++++ .../platform/spi/stm32h7/spi_master.cpp.in | 159 ++++++++ .../platform/spi/stm32h7/spi_master.hpp.in | 138 +++++++ .../spi/stm32h7/spi_master_dma.hpp.in | 132 +++++++ .../spi/stm32h7/spi_master_dma_impl.hpp.in | 254 +++++++++++++ 9 files changed, 1543 insertions(+), 1 deletion(-) create mode 100644 src/modm/platform/spi/stm32h7/module.lb create mode 100644 src/modm/platform/spi/stm32h7/spi_base.hpp.in create mode 100644 src/modm/platform/spi/stm32h7/spi_hal.hpp.in create mode 100644 src/modm/platform/spi/stm32h7/spi_hal_impl.hpp.in create mode 100644 src/modm/platform/spi/stm32h7/spi_master.cpp.in create mode 100644 src/modm/platform/spi/stm32h7/spi_master.hpp.in create mode 100644 src/modm/platform/spi/stm32h7/spi_master_dma.hpp.in create mode 100644 src/modm/platform/spi/stm32h7/spi_master_dma_impl.hpp.in diff --git a/README.md b/README.md index 80bf649f33..2f33b19e22 100644 --- a/README.md +++ b/README.md @@ -454,7 +454,7 @@ Please [discover modm's peripheral drivers for your specific device][discover]. ✅ ✅ ✅ -○ +✅ ✅ ✅ ✅ diff --git a/src/modm/platform/spi/stm32h7/module.lb b/src/modm/platform/spi/stm32h7/module.lb new file mode 100644 index 0000000000..6056bc2c83 --- /dev/null +++ b/src/modm/platform/spi/stm32h7/module.lb @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# Copyright (c) 2016-2018, Niklas Hauser +# Copyright (c) 2017, Fabian Greif +# Copyright (c) 2023, Christopher Durand +# +# This file is part of the modm project. +# +# This Source Code Form is subject to the terms of the Mozilla Public +# License, v. 2.0. If a copy of the MPL was not distributed with this +# file, You can obtain one at http://mozilla.org/MPL/2.0/. +# ----------------------------------------------------------------------------- + +def get_properties(env): + device = env[":target"] + driver = device.get_driver("spi") + properties = {} + properties["use_fiber"] = env.query(":processing:fiber:__enabled", False) + properties["target"] = device.identifier + + return properties + +class Instance(Module): + def __init__(self, driver, instance): + self.instance = int(instance) + self.driver = driver + + def init(self, module): + module.name = str(self.instance) + module.description = "Instance {}".format(self.instance) + + def prepare(self, module, options): + module.depends(":platform:spi") + return True + + def build(self, env): + properties = get_properties(env) + properties["id"] = self.instance + + device = env[":target"] + if device.identifier.family in ["h7"]: + properties["fifo_size"] = 16 if self.instance in (1, 2, 3) else 8 + properties["max_data_bits"] = 32 if self.instance in (1, 2, 3) else 16 + properties["max_crc_bits"] = properties["max_data_bits"] + properties["max_transfer_size"] = 2**16 - 1 # is smaller for some instances on U5 + properties["i2s"] = self.instance in (1, 2, 3, 6) + else: + raise NotImplementedError("Unsupported device") + + env.substitutions = properties + env.outbasepath = "modm/src/modm/platform/spi" + + env.template("spi_hal.hpp.in", "spi_hal_{}.hpp".format(self.instance)) + env.template("spi_hal_impl.hpp.in", "spi_hal_{}_impl.hpp".format(self.instance)) + env.template("spi_master.hpp.in", "spi_master_{}.hpp".format(self.instance)) + env.template("spi_master.cpp.in", "spi_master_{}.cpp".format(self.instance)) + if env.has_module(":platform:dma"): + env.template("spi_master_dma.hpp.in", "spi_master_{}_dma.hpp".format(self.instance)) + env.template("spi_master_dma_impl.hpp.in", "spi_master_{}_dma_impl.hpp".format(self.instance)) + + +def init(module): + module.name = ":platform:spi" + module.description = "Serial Peripheral Interface (SPI)" + +def prepare(module, options): + device = options[":target"] + if not device.has_driver("spi:stm32-extended"): + return False + if device.identifier.family not in ["h7"]: + # U5 is not supported yet + return False + + module.depends( + ":architecture:register", + ":architecture:spi", + ":cmsis:device", + ":math:algorithm", + ":platform:gpio", + ":platform:rcc") + + for driver in device.get_all_drivers("spi:stm32-extended"): + for instance in driver["instance"]: + module.add_submodule(Instance(driver, instance)) + + return True + +def build(env): + env.substitutions = get_properties(env) + env.outbasepath = "modm/src/modm/platform/spi" + + env.template("spi_base.hpp.in") diff --git a/src/modm/platform/spi/stm32h7/spi_base.hpp.in b/src/modm/platform/spi/stm32h7/spi_base.hpp.in new file mode 100644 index 0000000000..2c6a1e8480 --- /dev/null +++ b/src/modm/platform/spi/stm32h7/spi_base.hpp.in @@ -0,0 +1,204 @@ +/* + * Copyright (c) 2013, Kevin Läufer + * Copyright (c) 2013-2017, Niklas Hauser + * Copyright (c) 2014, Daniel Krebs + * Copyright (c) 2020, Mike Wolfram + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_STM32H7_SPI_BASE_HPP +#define MODM_STM32H7_SPI_BASE_HPP + +#include +#include "../device.hpp" +#include + +namespace modm::platform +{ + +/** + * Base class for the SPI classes + * + * Provides common definitions that do not depend on the specific SPI instance. + * + * @ingroup modm_platform_spi + */ +class SpiBase +{ +public: + enum class + Interrupt : uint32_t + { + RxPacketAvailable = SPI_IER_RXPIE, + TxPacketSpaceAvailable = SPI_IER_TXPIE, + DuplexPacket = SPI_IER_DXPIE, + EndOfTransfer = SPI_IER_EOTIE, + TxTransferFilled = SPI_IER_TXTFIE, + Underrun = SPI_IER_UDRIE, + Overrun = SPI_IER_OVRIE, + CrcError = SPI_IER_CRCEIE, + TiFrameError = SPI_IER_TIFREIE, + ModeFault = SPI_IER_MODFIE, + Reload = SPI_IER_TSERFIE + }; + MODM_FLAGS32(Interrupt); + + enum class + StatusFlag : uint32_t + { + RxPacketAvailable = SPI_SR_RXP, + TxPacketSpaceAvailable = SPI_SR_TXP, + DuplexPacket = SPI_SR_DXP, + EndOfTransfer = SPI_SR_EOT, + TxTransferFilled = SPI_SR_TXTF, + Underrun = SPI_SR_UDR, + Overrun = SPI_SR_OVR, + CrcError = SPI_SR_CRCE, + TiFrameError = SPI_SR_TIFRE, + ModeFault = SPI_SR_MODF, + Reload = SPI_SR_TSERF, + Suspension = SPI_SR_SUSP, + TxTransferComplete = SPI_SR_TXC, + RxFifoLevel1 = SPI_SR_RXPLVL_1, + RxFifoLevel0 = SPI_SR_RXPLVL_0, + RxFifoWordNotEmpty = SPI_SR_RXWNE + }; + MODM_FLAGS32(StatusFlag); + + enum class + MasterSelection : uint32_t + { + Slave = 0, + Master = SPI_CFG2_MASTER, + Mask = Master, + }; + + enum class + DataMode : uint32_t + { + Mode0 = 0b00, ///< clock normal, sample on rising edge + Mode1 = SPI_CFG2_CPHA, ///< clock normal, sample on falling edge + Mode2 = SPI_CFG2_CPOL, ///< clock inverted, sample on falling edge + /// clock inverted, sample on rising edge + Mode3 = SPI_CFG2_CPOL | SPI_CFG2_CPHA, + Mask = Mode3 + }; + + enum class + DataOrder : uint32_t + { + MsbFirst = 0b0, + LsbFirst = SPI_CFG2_LSBFRST, + Mask = LsbFirst, + }; + + enum class + Prescaler : uint32_t + { + Div2 = 0, + Div4 = SPI_CFG1_MBR_0, + Div8 = SPI_CFG1_MBR_1, + Div16 = SPI_CFG1_MBR_1 | SPI_CFG1_MBR_0, + Div32 = SPI_CFG1_MBR_2, + Div64 = SPI_CFG1_MBR_2 | SPI_CFG1_MBR_0, + Div128 = SPI_CFG1_MBR_2 | SPI_CFG1_MBR_1, + Div256 = SPI_CFG1_MBR_2 | SPI_CFG1_MBR_1 | SPI_CFG1_MBR_0 + }; + + enum class + DataSize : uint32_t + { + Bit4 = 3, + Bit5 = 4, + Bit6 = 5, + Bit7 = 6, + Bit8 = 7, + Bit9 = 8, + Bit10 = 9, + Bit11 = 10, + Bit12 = 11, + Bit13 = 12, + Bit14 = 13, + Bit15 = 14, + Bit16 = 15, + Bit17 = 16, + Bit18 = 17, + Bit19 = 18, + Bit20 = 19, + Bit21 = 20, + Bit22 = 21, + Bit23 = 22, + Bit24 = 23, + Bit25 = 24, + Bit26 = 25, + Bit27 = 26, + Bit28 = 27, + Bit29 = 28, + Bit30 = 29, + Bit31 = 30, + Bit32 = 31 + }; + static_assert(SPI_CFG1_DSIZE_Pos == 0); + + enum class DmaMode : uint32_t + { + None = 0, + Tx = SPI_CFG1_TXDMAEN, + Rx = SPI_CFG1_RXDMAEN, + Mask = SPI_CFG1_TXDMAEN | SPI_CFG1_RXDMAEN + }; + MODM_FLAGS32(DmaMode); + + enum class DuplexMode : uint32_t + { + FullDuplex = 0, + TransmitOnly = SPI_CFG2_COMM_0, + ReceiveOnly = SPI_CFG2_COMM_1, + HalfDuplex = SPI_CFG2_COMM_1 | SPI_CFG2_COMM_0, + Mask = HalfDuplex + }; + + enum class SlaveSelectMode + { + HardwareGpio, + Software = SPI_CFG2_SSM + }; + + enum class SlaveSelectPolarity + { + ActiveLow = 0, + ActiveHigh = SPI_CFG2_SSIOP + }; + + enum class CrcInit : uint32_t + { + AllZeros = 0, + AllOnes = SPI_CR1_TCRCINI | SPI_CR1_RCRCINI, + Mask = AllOnes + }; +}; + +constexpr auto +operator<=>(SpiBase::DataSize s0, SpiBase::DataSize s1) +{ + const auto v0 = static_cast(s0); + const auto v1 = static_cast(s1); + if (v0 < v1) { + return std::strong_ordering::less; + } else if (v0 > v1) { + return std::strong_ordering::greater; + } else { + return std::strong_ordering::equal; + } +} + +} // namespace modm::platform + +#endif // MODM_STM32H7_SPI_BASE_HPP diff --git a/src/modm/platform/spi/stm32h7/spi_hal.hpp.in b/src/modm/platform/spi/stm32h7/spi_hal.hpp.in new file mode 100644 index 0000000000..214cb1d296 --- /dev/null +++ b/src/modm/platform/spi/stm32h7/spi_hal.hpp.in @@ -0,0 +1,209 @@ +/* + * Copyright (c) 2013, Kevin Läufer + * Copyright (c) 2013-2018, Niklas Hauser + * Copyright (c) 2014, Daniel Krebs + * Copyright (c) 2020, Mike Wolfram + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_STM32H7_SPI_HAL{{ id }}_HPP +#define MODM_STM32H7_SPI_HAL{{ id }}_HPP + +#include "spi_base.hpp" + +namespace modm::platform +{ + +/** + * Serial peripheral interface (SPI{{ id }}) + * + * @ingroup modm_platform_spi modm_platform_spi_{{id}} + */ +class SpiHal{{ id }} : public SpiBase +{ +public: + static constexpr auto peripheral = Peripheral::Spi{{ id }}; + + static constexpr DataSize MaxDataBits = DataSize::Bit{{ max_data_bits }}; + static constexpr DataSize MaxCrcBits = DataSize::Bit{{ max_crc_bits }}; + + /// Maximum permitted size for fixed size transfers + static constexpr std::size_t MaxTransferSize = {{ max_transfer_size }}; + + /// Size of FIFO in bytes + static constexpr std::size_t FifoSize = {{ fifo_size }}; + + static void + enableClock(); + + static void + disableClock(); + + static void + initialize(Prescaler prescaler, + MasterSelection masterSelection = MasterSelection::Master, + DataMode dataMode = DataMode::Mode0, + DataOrder dataOrder = DataOrder::MsbFirst, + DataSize dataSize = DataSize::Bit8); + + static void + enableTransfer(); + + static void + disableTransfer(); + + static bool + isTransferEnabled(); + + static void + startMasterTransfer(); + + static void + suspendMasterTransfer(); + + static void + setDataMode(DataMode dataMode); + + static void + setDataOrder(DataOrder dataOrder); + + static void + setDataSize(DataSize dataSize); + + static void + setMasterSelection(MasterSelection masterSelection); + + static void + setDuplexMode(DuplexMode mode); + + static void + setCrcEnabled(bool enabled); + + static void + setCrcSize(DataSize crcSize); + + static void + setCrcPolynomial(uint32_t poly); + + static void + setCrcInitialValue(CrcInit init); + + static void + setDmaMode(DmaMode_t mode); + + /** + * Configure total amount of data items of transfer + * Set size to 0 for unlimited transfers. + * @param reload Next transfer size after reload + */ + static void + setTransferSize(uint16_t size, uint16_t reload = 0); + + static void + setSlaveSelectMode(SlaveSelectMode mode); + + static void + setSlaveSelectPolarity(SlaveSelectPolarity polarity); + + static void + setSlaveSelectState(bool state); + + static uint32_t + transmitCrc(); + + static uint32_t + receiveCrc(); + + static volatile uint32_t* + transmitRegister(); + + static const volatile uint32_t* + receiveRegister(); + + static StatusFlag_t + status(); + + /// @return true if SPI_SR_EOT is set + static bool + isTransferCompleted(); + + /// @return true if SPI_SR_TXC is set + static bool + isTxCompleted(); + + /// @return true if SPI_SR_TXP is not set + static bool + isTxFifoFull(); + + /// @return true if SPI_SR_RXP is set + static bool + isRxDataAvailable(); + + /** + * Write up to 8 Bit to the transmit data register + * + * @warning Writing with a size smaller than the configured data size is not allowed. + */ + static void + write(uint8_t data); + + /** + * Write up to 16 Bit to the data register + * @warning Writing with a size smaller than the configured data size is not allowed. + */ + static void + write16(uint16_t data); + + /** + * Write up to 32 Bit to the transmit data register + */ + static void + write32(uint32_t data); + + /** + * Read an 8-bit value from the receive data register. + * + * @warning Reading with a size smaller than the configured data size is not allowed. + */ + static uint8_t + read(); + + /** + * Read a 16-bit value from the receive data register. + * + * @warning Reading with a size smaller than the configured data size is not allowed. + */ + static uint16_t + read16(); + + /** + * Read a 32-bit value from the receive data register. + */ + static uint32_t + read32(); + + static void + enableInterruptVector(bool enable, uint32_t priority); + + static void + enableInterrupt(Interrupt_t interrupt); + + static void + disableInterrupt(Interrupt_t interrupt); + + static void + acknowledgeInterruptFlags(StatusFlag_t flags); +}; + +} // namespace modm::platform + +#include "spi_hal_{{ id }}_impl.hpp" + +#endif // MODM_STM32H7_SPI_HAL{{ id }}_HPP diff --git a/src/modm/platform/spi/stm32h7/spi_hal_impl.hpp.in b/src/modm/platform/spi/stm32h7/spi_hal_impl.hpp.in new file mode 100644 index 0000000000..77770cf4bd --- /dev/null +++ b/src/modm/platform/spi/stm32h7/spi_hal_impl.hpp.in @@ -0,0 +1,353 @@ +/* +* Copyright (c) 2013, Kevin Läufer +* Copyright (c) 2013-2017, Niklas Hauser +* Copyright (c) 2014, Daniel Krebs +* Copyright (c) 2020, Mike Wolfram +* Copyright (c) 2023, Christopher Durand +* +* This file is part of the modm project. +* +* This Source Code Form is subject to the terms of the Mozilla Public +* License, v. 2.0. If a copy of the MPL was not distributed with this +* file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ +// ---------------------------------------------------------------------------- + +#ifndef MODM_STM32H7_SPI_HAL{{ id }}_HPP +# error "Don't include this file directly, use 'spi_hal{{ id }}.hpp' instead!" +#endif +#include + +namespace modm::platform +{ + +inline void +SpiHal{{ id }}::enableClock() +{ + Rcc::enable(); +} + +inline void +SpiHal{{ id }}::disableClock() +{ + Rcc::disable(); +} + +inline void +SpiHal{{ id }}::initialize(Prescaler prescaler, + MasterSelection masterSelection, DataMode dataMode, + DataOrder dataOrder, DataSize dataSize) +{ + enableClock(); + disableTransfer(); + + acknowledgeInterruptFlags( + StatusFlag::EndOfTransfer | + StatusFlag::TxTransferFilled | + StatusFlag::Underrun | + StatusFlag::Overrun | + StatusFlag::CrcError | + StatusFlag::TiFrameError | + StatusFlag::ModeFault | + StatusFlag::Reload | + StatusFlag::Suspension + ); + + // initialize with unlimited transfer size + setTransferSize(0); + + // Pause master transfer if RX FIFO is full + SPI{{ id }}->CR1 = SPI_CR1_MASRX; + + SPI{{ id }}->CFG2 = static_cast(dataMode) + | static_cast(dataOrder) + | static_cast(masterSelection) + | SPI_CFG2_SSOE; // disable multi-master support to prevent spurious mode fault + + SPI{{ id }}->CFG1 = static_cast(prescaler) + | static_cast(dataSize); +} + +inline void +SpiHal{{ id }}::setDataMode(DataMode dataMode) +{ + SPI{{ id }}->CFG2 = (SPI{{ id }}->CFG2 & ~static_cast(DataMode::Mask)) + | static_cast(dataMode); +} + +inline void +SpiHal{{ id }}::setDataOrder(DataOrder dataOrder) +{ + SPI{{ id }}->CFG2 = (SPI{{ id }}->CFG2 & ~static_cast(DataOrder::Mask)) + | static_cast(dataOrder); +} + +inline void +SpiHal{{ id }}::setDataSize(DataSize dataSize) +{ + SPI{{ id }}->CFG1 = (SPI{{ id }}->CFG1 & ~SPI_CFG1_DSIZE_Msk) + | static_cast(dataSize); +} + +inline void +SpiHal{{ id }}::setMasterSelection(MasterSelection masterSelection) +{ + SPI{{ id }}->CFG2 = (SPI{{ id }}->CFG2 & ~static_cast(MasterSelection::Mask)) + | static_cast(masterSelection); +} + +inline void +SpiHal{{ id }}::setDuplexMode(DuplexMode mode) +{ + SPI{{ id }}->CFG2 = (SPI{{ id }}->CFG2 & ~static_cast(DuplexMode::Mask)) + | static_cast(mode); +} + +inline void +SpiHal{{ id }}::setCrcEnabled(bool enabled) +{ + if (enabled) { + SPI{{ id }}->CFG1 |= SPI_CFG1_CRCEN; + } else { + SPI{{ id }}->CFG1 &= ~SPI_CFG1_CRCEN; + } +} + +inline void +SpiHal{{ id }}::setCrcSize(DataSize crcSize) +{ + if ((crcSize == DataSize::Bit16) or (crcSize == DataSize::Bit32)) { + SPI{{ id }}->CR1 |= SPI_CR1_CRC33_17; + } else { + SPI{{ id }}->CR1 &= ~SPI_CR1_CRC33_17; + } + SPI{{ id }}->CFG1 = (SPI{{ id }}->CFG1 & ~SPI_CFG1_CRCSIZE_Msk) + | (static_cast(crcSize) << SPI_CFG1_CRCSIZE_Pos); +} + +inline void +SpiHal{{ id }}::setCrcPolynomial(uint32_t poly) +{ + SPI{{ id }}->CRCPOLY = poly; +} + +inline void +SpiHal{{ id }}::setCrcInitialValue(CrcInit init) +{ + SPI{{ id }}->CR1 = (SPI{{ id }}->CR1 & ~static_cast(CrcInit::Mask)) | + static_cast(init); +} + +inline void +SpiHal{{ id }}::setTransferSize(uint16_t size, uint16_t reload) +{ + static_assert(SPI_CR2_TSIZE_Pos == 0); + SPI{{ id }}->CR2 = (reload << SPI_CR2_TSER_Pos) | size; +} + +inline void +SpiHal{{ id }}::setDmaMode(DmaMode_t mode) +{ + SPI{{ id }}->CFG1 = (SPI{{ id }}->CFG1 & ~(DmaMode::Tx | DmaMode::Rx).value) + | mode.value; +} + +inline void +SpiHal{{ id }}::setSlaveSelectMode(SlaveSelectMode mode) +{ + SPI{{ id }}->CFG2 = (SPI{{ id }}->CFG2 & ~SPI_CFG2_SSM_Msk) + | uint32_t(mode); +} + +inline void +SpiHal{{ id }}::setSlaveSelectPolarity(SlaveSelectPolarity polarity) +{ + SPI{{ id }}->CFG2 = (SPI{{ id }}->CFG2 & ~SPI_CFG2_SSIOP_Msk) + | uint32_t(polarity); +} + +inline void +SpiHal{{ id }}::setSlaveSelectState(bool state) +{ + if (state) { + SPI{{ id }}->CR1 |= SPI_CR1_SSI; + } else { + SPI{{ id }}->CR1 &= ~SPI_CR1_SSI; + } +} + +inline void +SpiHal{{ id }}::write(uint8_t data) +{ + // Write with 8-bit access + auto* const ptr = reinterpret_cast<__IO uint8_t*>(&SPI{{ id }}->TXDR); + *ptr = data; +} + +inline void +SpiHal{{ id }}::write16(uint16_t data) +{ + // Write with 16-bit access + // SPI{{ id }}->TXDR is of type "volatile uint32_t". + // [[gnu::may_alias]] is required to avoid undefined behaviour due to strict aliasing violations. + auto* const [[gnu::may_alias]] ptr = reinterpret_cast<__IO uint16_t*>(&SPI{{ id }}->TXDR); + *ptr = data; +} + +inline void +SpiHal{{ id }}::write32(uint32_t data) +{ + SPI{{ id }}->TXDR = data; +} + +inline uint8_t +SpiHal{{ id }}::read() +{ + // Read with 8-bit access + return *reinterpret_cast(&SPI{{ id }}->RXDR); +} + +inline uint16_t +SpiHal{{ id }}::read16() +{ + // Read with 16-bit access + // SPI{{ id }}->RXDR is of type "const volatile uint32_t". + // [[gnu::may_alias]] is required to avoid undefined behaviour due to strict aliasing violations. + auto* const [[gnu::may_alias]] ptr = reinterpret_cast(&SPI{{ id }}->RXDR); + return *ptr; +} + +inline uint32_t +SpiHal{{ id }}::read32() +{ + return SPI{{ id }}->RXDR; +} + +inline void +SpiHal{{ id }}::enableInterruptVector(bool enable, uint32_t priority) +{ + if (enable) { + // Set priority for the interrupt vector + NVIC_SetPriority(SPI{{ id }}_IRQn, priority); + // register IRQ at the NVIC + NVIC_EnableIRQ(SPI{{ id }}_IRQn); + } + else { + NVIC_DisableIRQ(SPI{{ id }}_IRQn); + } +} + +inline void +SpiHal{{ id }}::enableInterrupt(Interrupt_t interrupt) +{ + SPI{{ id }}->IER |= interrupt.value; +} + +inline void +SpiHal{{ id }}::disableInterrupt(Interrupt_t interrupt) +{ + SPI{{ id }}->IER &= ~interrupt.value; +} + +inline void +SpiHal{{ id }}::acknowledgeInterruptFlags(StatusFlag_t flags) +{ + constexpr auto mask = + SPI_IFCR_EOTC | + SPI_IFCR_TXTFC | + SPI_IFCR_UDRC | + SPI_IFCR_OVRC | + SPI_IFCR_CRCEC | + SPI_IFCR_TIFREC | + SPI_IFCR_MODFC | + SPI_IFCR_TSERFC | + SPI_IFCR_SUSPC; + + SPI{{ id }}->IFCR = flags.value & mask; +} + +inline SpiHal{{ id }}::StatusFlag_t +SpiHal{{ id }}::status() +{ + return StatusFlag_t(SPI{{ id }}->SR); +} + +inline bool +SpiHal{{ id }}::isTransferCompleted() +{ + return bool(status() & StatusFlag::EndOfTransfer); +} + +inline bool +SpiHal{{ id }}::isTxCompleted() +{ + return bool(status() & StatusFlag::TxTransferComplete); +} + +inline bool +SpiHal{{ id }}::isTxFifoFull() +{ + return !(status() & StatusFlag::TxPacketSpaceAvailable); +} + +inline bool +SpiHal{{ id }}::isRxDataAvailable() +{ + return bool(status() & StatusFlag::RxPacketAvailable); +} + +inline void +SpiHal{{ id }}::enableTransfer() +{ + SPI{{ id }}->CR1 |= SPI_CR1_SPE; +} + +inline void +SpiHal{{ id }}::disableTransfer() +{ + SPI{{ id }}->CR1 &= ~SPI_CR1_SPE; +} + +inline bool +SpiHal{{ id }}::isTransferEnabled() +{ + return (SPI{{ id }}->CR1 & SPI_CR1_SPE); +} + +inline void +SpiHal{{ id }}::startMasterTransfer() +{ + SPI{{ id }}->CR1 |= SPI_CR1_CSTART; +} + +inline void +SpiHal{{ id }}::suspendMasterTransfer() +{ + SPI{{ id }}->CR1 |= SPI_CR1_CSUSP; +} + +inline uint32_t +SpiHal{{ id }}::transmitCrc() +{ + return SPI{{ id }}->TXCRC; +} + +inline uint32_t +SpiHal{{ id }}::receiveCrc() +{ + return SPI{{ id }}->RXCRC; +} + +inline volatile uint32_t* +SpiHal{{ id }}::transmitRegister() +{ + return &SPI{{ id }}->TXDR; +} + +inline const volatile uint32_t* +SpiHal{{ id }}::receiveRegister() +{ + return &SPI{{ id }}->RXDR; +} + +} // namespace modm::platform diff --git a/src/modm/platform/spi/stm32h7/spi_master.cpp.in b/src/modm/platform/spi/stm32h7/spi_master.cpp.in new file mode 100644 index 0000000000..02b498a617 --- /dev/null +++ b/src/modm/platform/spi/stm32h7/spi_master.cpp.in @@ -0,0 +1,159 @@ +/* +* Copyright (c) 2009, Martin Rosekeit +* Copyright (c) 2009-2012, Fabian Greif +* Copyright (c) 2010, Georgi Grinshpun +* Copyright (c) 2012-2017, Niklas Hauser +* Copyright (c) 2013, Kevin Läufer +* Copyright (c) 2014, Sascha Schade +* Copyright (c) 2023, Christopher Durand +* +* This file is part of the modm project. +* +* This Source Code Form is subject to the terms of the Mozilla Public +* License, v. 2.0. If a copy of the MPL was not distributed with this +* file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ +// ---------------------------------------------------------------------------- + +#include "spi_master_{{id}}.hpp" + +namespace modm::platform +{ + +modm::ResumableResult +SpiMaster{{ id }}::transfer(uint8_t data) +{ +%% if use_fiber + while (Hal::isTxFifoFull()) + modm::fiber::yield(); + + Hal::write(data); + + while (!Hal::isRxDataAvailable()) + modm::fiber::yield(); + + return Hal::read(); +%% else + // this is a manually implemented "fast resumable function" + // there is no context or nesting protection, since we don't need it. + // there are only two states encoded into 1 bit (LSB of state): + // 1. waiting to start, and + // 2. waiting to finish. + + if (!(state & Bit0)) + { + // wait for previous transfer to finish + if (Hal::isTxFifoFull()) + return {modm::rf::Running}; + + Hal::write(data); + + // set LSB = Bit0 + state |= Bit0; + } + + if (!Hal::isRxDataAvailable()) + return {modm::rf::Running}; + + data = Hal::read(); + + state &= ~Bit0; + return {modm::rf::Stop, data}; +%% endif +} + +modm::ResumableResult +SpiMaster{{ id }}::transfer( + const uint8_t* tx, uint8_t* rx, std::size_t length) +{ +%% if use_fiber + std::size_t rxIndex = 0; + std::size_t txIndex = 0; + + while (rxIndex < length) { + while ((txIndex < length) and !Hal::isTxFifoFull()) { + Hal::write(tx ? tx[txIndex] : 0); + ++txIndex; + } + while ((rxIndex < length) and Hal::isRxDataAvailable()) { + const uint8_t data = Hal::read(); + if (rx) { + rx[rxIndex] = data; + } + ++rxIndex; + } + if (rxIndex < length) { + modm::fiber::yield(); + } + } +%% else + // this is a manually implemented "fast resumable function" + // there is no context or nesting protection, since we don't need it. + // there are only two states encoded into 1 bit (Bit1 of state): + // 1. initialize index, and + // 2. wait for transfer to finish. + + // we need to globally remember which byte we are currently transferring + static std::size_t rxIndex = 0; + static std::size_t txIndex = 0; + + // we are only interested in Bit1 + switch(state & Bit1) + { + case 0: + // we will only visit this state once + state |= Bit1; + + // initialize index and check range + rxIndex = 0; + txIndex = 0; + while (rxIndex < length) { + default: + { + while ((txIndex < length) and !Hal::isTxFifoFull()) { + Hal::write(tx ? tx[txIndex] : 0); + ++txIndex; + } + while ((rxIndex < length) and Hal::isRxDataAvailable()) { + if (rx) { + rx[rxIndex] = Hal::read(); + } else { + Hal::read(); + } + ++rxIndex; + } + if (rxIndex < length) { + return {modm::rf::Running}; + } + } + } + + // clear the state + state &= ~Bit1; + return {modm::rf::Stop}; + } +%% endif +} + +void +SpiMaster{{ id }}::finishTransfer() +{ + if (Hal::isTransferEnabled()) { + while (!(Hal::status() & Hal::StatusFlag::TxTransferComplete)); + Hal::disableTransfer(); + } + + Hal::acknowledgeInterruptFlags( + Hal::StatusFlag::EndOfTransfer | + Hal::StatusFlag::TxTransferFilled | + Hal::StatusFlag::Underrun | + Hal::StatusFlag::Overrun | + Hal::StatusFlag::CrcError | + Hal::StatusFlag::TiFrameError | + Hal::StatusFlag::ModeFault | + Hal::StatusFlag::Reload | + Hal::StatusFlag::Suspension + ); +} + +} // namespace modm::platform diff --git a/src/modm/platform/spi/stm32h7/spi_master.hpp.in b/src/modm/platform/spi/stm32h7/spi_master.hpp.in new file mode 100644 index 0000000000..f44a0883a9 --- /dev/null +++ b/src/modm/platform/spi/stm32h7/spi_master.hpp.in @@ -0,0 +1,138 @@ +/* + * Copyright (c) 2009-2011, Fabian Greif + * Copyright (c) 2010, Martin Rosekeit + * Copyright (c) 2011-2017, Niklas Hauser + * Copyright (c) 2012, Georgi Grinshpun + * Copyright (c) 2013, Kevin Läufer + * Copyright (c) 2014, Sascha Schade + * Copyright (c) 2022-2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_STM32H7_SPI_MASTER{{ id }}_HPP +#define MODM_STM32H7_SPI_MASTER{{ id }}_HPP + +#include +#include +#include +#include +%% if use_fiber +#include +%% endif +#include "spi_hal_{{ id }}.hpp" + +namespace modm::platform +{ + +/** + * Serial peripheral interface (SPI{{ id }}). + * + * @ingroup modm_platform_spi modm_platform_spi_{{id}} + */ +class SpiMaster{{ id }} : public modm::SpiMaster, public SpiLock +{ +%% if not use_fiber + // Bit0: single transfer state + // Bit1: block transfer state + static inline uint8_t state{0}; +%% endif +public: + using Hal = SpiHal{{ id }}; + + using DataMode = Hal::DataMode; + using DataOrder = Hal::DataOrder; + using DataSize = Hal::DataSize; + + template< class... Signals > + static void + connect() + { + using Connector = GpioConnector; + using Sck = typename Connector::template GetSignal; + using Mosi = typename Connector::template GetSignal; + using Miso = typename Connector::template GetSignal; + + Sck::setOutput(Gpio::OutputType::PushPull); + Mosi::setOutput(Gpio::OutputType::PushPull); + Miso::setInput(Gpio::InputType::Floating); + Connector::connect(); + } + + template< class SystemClock, baudrate_t baudrate, percent_t tolerance=pct(5) > + static void + initialize() + { + constexpr auto result = modm::Prescaler::from_power(SystemClock::Spi{{ id }}, baudrate, 2, 256); + assertBaudrateInTolerance< result.frequency, baudrate, tolerance >(); + + // translate the prescaler into the bitmapping + constexpr Hal::Prescaler prescaler{result.index << SPI_CFG1_MBR_Pos}; +%% if not use_fiber + state = 0; +%% endif + + Hal::initialize(prescaler); + Hal::enableTransfer(); + Hal::startMasterTransfer(); + } + + static void + setDataMode(DataMode mode) + { + finishTransfer(); + Hal::setDataMode(mode); + Hal::enableTransfer(); + Hal::startMasterTransfer(); + } + + static void + setDataOrder(DataOrder order) + { + finishTransfer(); + Hal::setDataOrder(order); + Hal::enableTransfer(); + Hal::startMasterTransfer(); + } + + static void + setDataSize(DataSize size) + { + finishTransfer(); + Hal::setDataSize(static_cast(size)); + Hal::enableTransfer(); + Hal::startMasterTransfer(); + } + + static uint8_t + transferBlocking(uint8_t data) + { + return RF_CALL_BLOCKING(transfer(data)); + } + + static void + transferBlocking(const uint8_t* tx, uint8_t* rx, std::size_t length) + { + RF_CALL_BLOCKING(transfer(tx, rx, length)); + } + + + static modm::ResumableResult + transfer(uint8_t data); + + static modm::ResumableResult + transfer(const uint8_t* tx, uint8_t* rx, std::size_t length); + +private: + static void + finishTransfer(); +}; + +} // namespace modm::platform + +#endif // MODM_STM32H7_SPI_MASTER{{ id }}_HPP diff --git a/src/modm/platform/spi/stm32h7/spi_master_dma.hpp.in b/src/modm/platform/spi/stm32h7/spi_master_dma.hpp.in new file mode 100644 index 0000000000..009fabe238 --- /dev/null +++ b/src/modm/platform/spi/stm32h7/spi_master_dma.hpp.in @@ -0,0 +1,132 @@ +/* + * Copyright (c) 2020, Mike Wolfram + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_STM32H7_SPI_MASTER{{ id }}_DMA_HPP +#define MODM_STM32H7_SPI_MASTER{{ id }}_DMA_HPP + +#include +#include "spi_master_{{ id }}.hpp" + +namespace modm::platform +{ + +/** + * Serial peripheral interface (SPI{{ id }}) with DMA support. + * + * This class uses the DMA controller for sending and receiving data, which + * greatly reduces the CPU load. Beside passing the DMA channels as template + * parameters the class can be used in the same way like the SpiMaster{{ id }}. + * + * @tparam DmaChannelRX DMA channel for receiving + * @tparam DmaChannelTX DMA channel for sending + * + * @ingroup modm_platform_spi modm_platform_spi_{{id}} + */ +template +class SpiMaster{{ id }}_Dma : public modm::SpiMaster, + public SpiLock> +{ +protected: + struct Dma { + using RxChannel = typename DmaChannelRx::template RequestMapping< + Peripheral::Spi{{ id }}, DmaBase::Signal::Rx>::Channel; + using TxChannel = typename DmaChannelTx::template RequestMapping< + Peripheral::Spi{{ id }}, DmaBase::Signal::Tx>::Channel; + static constexpr DmaBase::Request RxRequest = DmaChannelRx::template RequestMapping< + Peripheral::Spi{{ id }}, DmaBase::Signal::Rx>::Request; + static constexpr DmaBase::Request TxRequest = DmaChannelTx::template RequestMapping< + Peripheral::Spi{{ id }}, DmaBase::Signal::Tx>::Request; + }; + +%% if not use_fiber + // Bit0: single transfer state + // Bit1: block transfer state + // Bit2: block transfer rx dma transfer completed + static inline uint8_t state{0}; +%% endif +public: + using Hal = SpiHal{{ id }}; + + using DataMode = Hal::DataMode; + using DataOrder = Hal::DataOrder; + using DataSize = Hal::DataSize; + + template< class... Signals > + static void + connect() + { + using Connector = GpioConnector; + using Sck = typename Connector::template GetSignal; + using Mosi = typename Connector::template GetSignal; + using Miso = typename Connector::template GetSignal; + + Sck::setOutput(Gpio::OutputType::PushPull); + Mosi::setOutput(Gpio::OutputType::PushPull); + Miso::setInput(Gpio::InputType::Floating); + Connector::connect(); + } + + template< class SystemClock, baudrate_t baudrate, percent_t tolerance=pct(5) > + static void + initialize(); + + static void + setDataMode(DataMode mode) + { + Hal::setDataMode(mode); + } + + static void + setDataOrder(DataOrder order) + { + Hal::setDataOrder(order); + } + + static void + setDataSize(DataSize size) + { + Hal::setDataSize(static_cast(size)); + } + + static uint8_t + transferBlocking(uint8_t data) + { + return RF_CALL_BLOCKING(transfer(data)); + } + + /// @pre At least one of tx or rx must not be nullptr + static void + transferBlocking(const uint8_t* tx, uint8_t* rx, std::size_t length) + { + RF_CALL_BLOCKING(transfer(tx, rx, length)); + } + + static modm::ResumableResult + transfer(uint8_t data); + + /// @pre At least one of tx or rx must not be nullptr + static modm::ResumableResult + transfer(const uint8_t* tx, uint8_t* rx, std::size_t length); + +protected: + static void + finishTransfer(); + + static void + startDmaTransfer(const uint8_t* tx, uint8_t* rx, std::size_t length); +}; + +} // namespace modm::platform + +#include "spi_master_{{ id }}_dma_impl.hpp" + +#endif // MODM_STM32_SPI_MASTER{{ id }}_DMA_HPP diff --git a/src/modm/platform/spi/stm32h7/spi_master_dma_impl.hpp.in b/src/modm/platform/spi/stm32h7/spi_master_dma_impl.hpp.in new file mode 100644 index 0000000000..10aa8d2ab2 --- /dev/null +++ b/src/modm/platform/spi/stm32h7/spi_master_dma_impl.hpp.in @@ -0,0 +1,254 @@ +/* + * Copyright (c) 2020, Mike Wolfram + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_STM32H7_SPI_MASTER{{ id }}_DMA_HPP +# error "Don't include this file directly, use 'spi_master_{{ id }}_dma.hpp' instead!" +#endif + +namespace modm::platform +{ + +template +template +void +SpiMaster{{ id }}_Dma::initialize() +{ + Dma::RxChannel::configure(DmaBase::DataTransferDirection::PeripheralToMemory, + DmaBase::MemoryDataSize::Byte, DmaBase::PeripheralDataSize::Byte, + DmaBase::MemoryIncrementMode::Increment, DmaBase::PeripheralIncrementMode::Fixed, + DmaBase::Priority::Medium); + Dma::RxChannel::disableInterruptVector(); + Dma::RxChannel::setPeripheralAddress(reinterpret_cast(Hal::receiveRegister())); + Dma::RxChannel::template setPeripheralRequest(); + + Dma::TxChannel::configure(DmaBase::DataTransferDirection::MemoryToPeripheral, + DmaBase::MemoryDataSize::Byte, DmaBase::PeripheralDataSize::Byte, + DmaBase::MemoryIncrementMode::Increment, DmaBase::PeripheralIncrementMode::Fixed, + DmaBase::Priority::Medium); + Dma::TxChannel::disableInterruptVector(); + Dma::TxChannel::setPeripheralAddress(reinterpret_cast(Hal::transmitRegister())); + Dma::TxChannel::template setPeripheralRequest(); + +%% if not use_fiber + state = 0; +%% endif + + constexpr auto result = modm::Prescaler::from_power(SystemClock::Spi{{ id }}, baudrate, 2, 256); + assertBaudrateInTolerance< result.frequency, baudrate, tolerance >(); + + constexpr Hal::Prescaler prescaler{result.index << SPI_CFG1_MBR_Pos}; + Hal::initialize(prescaler); +} + +template +modm::ResumableResult +SpiMaster{{ id }}_Dma::transfer(uint8_t data) +{ + // DMA is not used for single byte transfers +%% if use_fiber + Hal::setDuplexMode(Hal::DuplexMode::FullDuplex); + Hal::setTransferSize(1); + Hal::enableTransfer(); + Hal::write(data); + Hal::startMasterTransfer(); + + // wait for transfer to complete + while (!Hal::isTransferCompleted()) + modm::fiber::yield(); + + data = SpiHal{{ id }}::read(); + finishTransfer(); + + return data; +%% else + // this is a manually implemented "fast resumable function" + // there is no context or nesting protection, since we don't need it. + // there are only two states encoded into 1 bit (LSB of state): + // 1. waiting to start, and + // 2. waiting to finish. + // LSB != Bit0 ? + if ( !(state & Bit0) ) + { + Hal::setDuplexMode(Hal::DuplexMode::FullDuplex); + Hal::setTransferSize(1); + Hal::enableTransfer(); + Hal::write(data); + Hal::startMasterTransfer(); + + // set LSB = Bit0 + state |= Bit0; + } + + if (!Hal::isTransferCompleted()) + return {modm::rf::Running}; + + data = SpiHal{{ id }}::read(); + finishTransfer(); + + // transfer finished + state &= ~Bit0; + return {modm::rf::Stop, data}; +%% endif +} + +template +void +SpiMaster{{ id }}_Dma::startDmaTransfer( + const uint8_t* tx, uint8_t* rx, std::size_t length) +{ + Hal::setTransferSize(length); + + if (tx and rx) { + Hal::setDuplexMode(Hal::DuplexMode::FullDuplex); + } else if (rx) { + Hal::setDuplexMode(Hal::DuplexMode::ReceiveOnly); + } else { // tx only + Hal::setDuplexMode(Hal::DuplexMode::TransmitOnly); + } + + /* + * Required order of operations according to the reference manual: + * 1. Enable SPI RX DMA + * 2. Enable DMA channels + * 3. Enable SPI TX DMA + * 4. Start transfer + */ + if (rx) { + Dma::RxChannel::setMemoryAddress(reinterpret_cast(rx)); + Dma::RxChannel::setDataLength(length); + Hal::setDmaMode(Hal::DmaMode::Rx); + Dma::RxChannel::start(); + } + + if (tx) { + Dma::TxChannel::setMemoryAddress(reinterpret_cast(tx)); + Dma::TxChannel::setDataLength(length); + Dma::TxChannel::start(); + if (rx) { + Hal::setDmaMode(Hal::DmaMode::Tx | Hal::DmaMode::Rx); + } else { + Hal::setDmaMode(Hal::DmaMode::Tx); + } + } + + Hal::enableTransfer(); + Hal::startMasterTransfer(); +} + + +template +modm::ResumableResult +SpiMaster{{ id }}_Dma::transfer( + const uint8_t* tx, uint8_t* rx, std::size_t length) +{ + using Flags = DmaBase::InterruptFlags; +%% if use_fiber + startDmaTransfer(tx, rx, length); + + bool dmaRxFinished = (rx == nullptr); + while (!Hal::isTransferCompleted() or !dmaRxFinished) { + if(rx) { + const auto flags = DmaChannelRx::getInterruptFlags(); + if (flags & Flags::Error) { + break; + } + if (flags & Flags::TransferComplete) { + dmaRxFinished = true; + } + } + if(tx) { + const auto flags = DmaChannelTx::getInterruptFlags(); + if (flags & Flags::Error) { + break; + } + } + modm::fiber::yield(); + } + finishTransfer(); +%% else + // this is a manually implemented "fast resumable function" + // there is no context or nesting protection, since we don't need it. + // there are only two states encoded into 1 bit (Bit1 of state): + // 1. initialize index, and + // 2. wait for 1-byte transfer to finish. + + // we are only interested in Bit1 + switch(state & Bit1) + { + case 0: + // we will only visit this state once + state |= Bit1; + startDmaTransfer(tx, rx, length); + if (!rx) { + state |= Bit2; + } + [[fallthrough]]; + + default: + if (!Hal::isTransferCompleted() or !(state & Bit2)) { + if(rx) { + static DmaBase::InterruptFlags_t flags; + flags = DmaChannelRx::getInterruptFlags(); + if (flags & Flags::Error) { + // abort on DMA error + finishTransfer(); + state &= ~(Bit2 | Bit1); + return {modm::rf::Stop}; + } + if (flags & Flags::TransferComplete) { + state |= Bit2; + } + } + if(tx) { + if (DmaChannelTx::getInterruptFlags() & Flags::Error) { + // abort on DMA error + finishTransfer(); + state &= ~(Bit2 | Bit1); + return {modm::rf::Stop}; + } + } + return { modm::rf::Running }; + } + + finishTransfer(); + + // clear the state + state &= ~(Bit2 | Bit1); + return {modm::rf::Stop}; + } +%% endif +} + +template +void +SpiMaster{{ id }}_Dma::finishTransfer() +{ + DmaChannelTx::stop(); + DmaChannelRx::stop(); + + Hal::setDmaMode(Hal::DmaMode::None); + Hal::disableTransfer(); + + Hal::acknowledgeInterruptFlags( + Hal::StatusFlag::EndOfTransfer | + Hal::StatusFlag::TxTransferFilled | + Hal::StatusFlag::Underrun | + Hal::StatusFlag::Overrun | + Hal::StatusFlag::CrcError | + Hal::StatusFlag::TiFrameError | + Hal::StatusFlag::ModeFault | + Hal::StatusFlag::Reload | + Hal::StatusFlag::Suspension + ); +} + +} // namespace modm::platform From f1f4f867ff275af5186fb7dd3cd078f28b926f3e Mon Sep 17 00:00:00 2001 From: Christopher Durand Date: Thu, 20 Jul 2023 16:56:13 +0200 Subject: [PATCH 6/8] [driver] Add BMI088 driver --- README.md | 27 +- src/modm/driver/inertial/bmi088.hpp | 330 ++++++++++++++++ src/modm/driver/inertial/bmi088.lb | 38 ++ src/modm/driver/inertial/bmi088_impl.hpp | 355 ++++++++++++++++++ src/modm/driver/inertial/bmi088_transport.hpp | 232 ++++++++++++ .../driver/inertial/bmi088_transport_impl.hpp | 188 ++++++++++ 6 files changed, 1157 insertions(+), 13 deletions(-) create mode 100644 src/modm/driver/inertial/bmi088.hpp create mode 100644 src/modm/driver/inertial/bmi088.lb create mode 100644 src/modm/driver/inertial/bmi088_impl.hpp create mode 100644 src/modm/driver/inertial/bmi088_transport.hpp create mode 100644 src/modm/driver/inertial/bmi088_transport_impl.hpp diff --git a/README.md b/README.md index 2f33b19e22..a2ad41c854 100644 --- a/README.md +++ b/README.md @@ -723,96 +723,97 @@ your specific needs. SPI Flash BME280 +BMI088 BMP085 BNO055 CAT24AA CYCLE-COUNTER -DRV832X +DRV832X DS1302 DS1631 DS18B20 EA-DOG Encoder Input -Encoder Input BitBang +Encoder Input BitBang Encoder Output BitBang FT245 FT6x06 Gpio Sampler HCLAx -HD44780 +HD44780 HMC58x HMC6343 HX711 I2C-EEPROM ILI9341 -IS31FL3733 +IS31FL3733 ITG3200 IXM42XXX L3GD20 LAN8720A LAWICEL -LIS302DL +LIS302DL LIS3DSH LIS3MDL LM75 LP503x LSM303A -LSM6DS33 +LSM6DS33 LSM6DSO LTC2984 MAX31855 MAX31865 MAX6966 -MAX7219 +MAX7219 MCP23x17 MCP2515 MCP3008 MCP7941x MCP990X -MMC5603 +MMC5603 MS5611 MS5837 NOKIA5110 NRF24 TFT-DISPLAY -PAT9125EL +PAT9125EL PCA8574 PCA9535 PCA9548A PCA9685 QMC5883L -SH1106 +SH1106 SIEMENS-S65 SIEMENS-S75 SK6812 SK9822 SSD1306 -ST7586S +ST7586S ST7789 STTS22H STUSB4500 SX1276 SX128X -TCS3414 +TCS3414 TCS3472 TLC594x TMP102 TMP12x TMP175 -TOUCH2046 +TOUCH2046 VL53L0 VL6180 WS2812 diff --git a/src/modm/driver/inertial/bmi088.hpp b/src/modm/driver/inertial/bmi088.hpp new file mode 100644 index 0000000000..10156921c4 --- /dev/null +++ b/src/modm/driver/inertial/bmi088.hpp @@ -0,0 +1,330 @@ +/* + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_BMI088_HPP +#define MODM_BMI088_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "bmi088_transport.hpp" + +namespace modm +{ + +/// @ingroup modm_driver_bmi088 +struct bmi088 +{ + enum class AccRange : uint8_t + { + Range3g = 0x00, //< +- 3g + Range6g = 0x01, //< +- 6g (default) + Range12g = 0x02, //< +- 12g + Range24g = 0x03 //< +- 24g + }; + + enum class GyroRange : uint8_t + { + Range2000dps = 0x00, //< +- 2000 deg/s (default) + Range1000dps = 0x01, //< +- 1000 deg/s + Range500dps = 0x02, //< +- 500 deg/s + Range250dps = 0x03, //< +- 250 deg/s + Range125dps = 0x04 //< +- 125 deg/s + }; + + struct AccData + { + /// acceleration in milli-g + Vector3f getFloat() const; + + Vector3i raw; + AccRange range; + }; + + struct GyroData + { + /// angular rate in deg/s + Vector3f getFloat() const; + + Vector3i raw; + GyroRange range; + }; + + /// Accelerometer data rate and filter bandwidth config + enum class AccRate : uint8_t + { + Rate12Hz_Bw5Hz = 0x5 | 0xA0, + Rate12Hz_Bw2Hz = 0x5 | 0x90, + Rate12Hz_Bw1Hz = 0x5 | 0x80, + Rate25Hz_Bw10Hz = 0x6 | 0xA0, + Rate25Hz_Bw5Hz = 0x6 | 0x90, + Rate25Hz_Bw3Hz = 0x6 | 0x80, + Rate50Hz_Bw20Hz = 0x7 | 0xA0, + Rate50Hz_Bw9Hz = 0x7 | 0x90, + Rate50Hz_Bw5Hz = 0x7 | 0x80, + Rate100Hz_Bw40Hz = 0x8 | 0xA0, + Rate100Hz_Bw19Hz = 0x8 | 0x90, + Rate100Hz_Bw10Hz = 0x8 | 0x80, + Rate200Hz_Bw80Hz = 0x9 | 0xA0, + Rate200Hz_Bw38Hz = 0x9 | 0x90, + Rate200Hz_Bw20Hz = 0x9 | 0x80, + Rate400Hz_Bw145Hz = 0xA | 0xA0, + Rate400Hz_Bw75Hz = 0xA | 0x90, + Rate400Hz_Bw40Hz = 0xA | 0x80, + Rate800Hz_Bw230Hz = 0xB | 0xA0, + Rate800Hz_Bw140Hz = 0xB | 0x90, + Rate800Hz_Bw80Hz = 0xB | 0x80, + Rate1600Hz_Bw280Hz = 0xC | 0xA0, + Rate1600Hz_Bw234Hz = 0xC | 0x90, + Rate1600Hz_Bw145Hz = 0xC | 0x80 + }; + + enum class AccGpioConfig : uint8_t + { + ActiveHigh = Bit1, //< Set GPIO polarity + OpenDrain = Bit2, //< Configure open-drain mode, push-pull if not set + EnableOutput = Bit3, //< Activate GPIO output function + EnableInput = Bit4 //< Activate GPIO intput function + }; + MODM_FLAGS8(AccGpioConfig); + + /// Configuration flags to map accelerometer interrupt signal to GPIO + enum class AccGpioMap : uint8_t + { + Int1FifoFull = Bit0, + Int1FifoWatermark = Bit1, + Int1DataReady = Bit2, + Int2FifoFull = Bit4, + Int2FifoWatermark = Bit5, + Int2DataReady = Bit6 + }; + MODM_FLAGS8(AccGpioMap); + + enum class AccStatus : uint8_t + { + DataReady = Bit7 + }; + MODM_FLAGS8(AccStatus); + + enum class AccPowerConf : uint8_t + { + Active = 0x00, + Suspend = 0x03 + }; + + enum class AccPowerControl : uint8_t + { + Off = 0x00, + On = 0x04 + }; + + enum class AccSelfTest : uint8_t + { + Off = 0x00, + Positive = 0x0D, + Negative = 0x09 + }; + + /// Gyroscope data rate and filter bandwidth config + enum class GyroRate : uint8_t + { + Rate2000Hz_Bw532Hz = 0x00, + Rate2000Hz_Bw230Hz = 0x01, + Rate1000Hz_Bw116Hz = 0x02, + Rate400Hz_Bw47Hz = 0x03, + Rate200Hz_Bw23Hz = 0x04, + Rate100Hz_Bw12Hz = 0x05, + Rate200Hz_Bw64Hz = 0x06, + Rate100Hz_Bw32Hz = 0x07 + }; + + enum class GyroGpioConfig : uint8_t + { + Int3ActiveHigh = Bit0, + Int3OpenDrain = Bit1, + Int4ActiveHigh = Bit2, + Int4OpenDrain = Bit3 + }; + MODM_FLAGS8(GyroGpioConfig); + + enum class GyroGpioMap : uint8_t + { + Int3DataReady = Bit0, + Int3FifoInterrupt = Bit2, + Int4FifoInterrupt = Bit5, + Int4DataReady = Bit7 + }; + MODM_FLAGS8(GyroGpioMap); + + enum class GyroSelfTest : uint8_t + { + // read-only bits + RateOk = Bit4, + Fail = Bit2, + Ready = Bit1, + // write-only bit + Trigger = Bit0 + }; + MODM_FLAGS8(GyroSelfTest); + + enum class GyroStatus : uint8_t + { + DataReady = Bit7, + FifoInterrupt = Bit4 + }; + MODM_FLAGS8(GyroStatus); + + enum class GyroInterruptControl : uint8_t + { + Fifo = Bit6, + DataReady = Bit7 + }; + MODM_FLAGS8(GyroInterruptControl); +}; + +/** + * Bosch BMI088 IMU + * + * The device contains an accelerometer and a gyroscope which operate + * independently. + * + * The device supports both I2C and SPI. For applications with high data + * rates or strict timing requirements SPI is recommended. + * + * The "MM Feature Set" accelerometer functions are not supported yet. + * + * @tparam Transport Transport layer (use @ref Bmi088SpiTransport or @ref Bmi088I2cTransport) + * @ingroup modm_driver_bmi088 + */ +template +class Bmi088 : public bmi088, public Transport +{ +public: + /// @arg transportArgs Arguments to transport layer. + /// Pass addresses for I2C, none for SPI. + template + Bmi088(Args... transportArgs); + + /// Initialize device. Call before any other member function. + /// @return true on success, false on error + bool + initialize(bool runSelfTest); + + // Accelerometer functions + + std::optional + readAccData(); + + /// Read data ready flag from ACC_STATUS register. Cleared on data read. + bool + readAccDataReady(); + + /// Read and clear data ready flag in ACC_INT_STAT_1 register. Not cleared on data read. + bool + clearAccDataReadyInterrupt(); + + /// @return true on success, false on error + bool + setAccRate(AccRate config); + + /// @return true on success, false on error + bool + setAccRange(AccRange range); + + /// @return true on success, false on error + bool + setAccInt1GpioConfig(AccGpioConfig_t config); + + /// @return true on success, false on error + bool + setAccInt2GpioConfig(AccGpioConfig_t config); + + /// @return true on success, false on error + bool + setAccGpioMap(AccGpioMap_t map); + + // Gyroscope functions + + std::optional + readGyroData(); + + /** + * Read Data ready interrupt status. + * @warning The interrupt is cleared automatically after 280-400 μs. + * @return true on success, false on error + */ + bool + readGyroDataReady(); + + /// @return true on success, false on error + bool + setGyroRate(GyroRate config); + + /// @return true on success, false on error + bool + setGyroRange(GyroRange range); + + /// @return true on success, false on error + bool + setGyroGpioConfig(GyroGpioConfig_t config); + + /// @return true on success, false on error + bool + setGyroGpioMap(GyroGpioMap_t map); + +private: + using AccRegister = Transport::AccRegister; + using GyroRegister = Transport::GyroRegister; + + static constexpr std::chrono::milliseconds ResetTimeout{30}; + static constexpr std::chrono::microseconds WriteTimeout{2}; + static constexpr std::chrono::microseconds AccSuspendTimeout{450}; + + static constexpr uint8_t ResetCommand{0xB6}; + static constexpr uint8_t AccChipId{0x1E}; + static constexpr uint8_t GyroChipId{0x0F}; + + bool + checkChipId(); + + bool + reset(); + + bool + selfTest(); + + bool + enableAccelerometer(); + + void + timerWait(); + + std::optional + readRegister(auto reg); + + modm::PreciseTimeout timer_; + AccRange accRange_{AccRange::Range6g}; + GyroRange gyroRange_{GyroRange::Range2000dps}; +}; + + +} // modm namespace + +#include "bmi088_impl.hpp" + +#endif // MODM_ADIS16470_HPP diff --git a/src/modm/driver/inertial/bmi088.lb b/src/modm/driver/inertial/bmi088.lb new file mode 100644 index 0000000000..1d16099368 --- /dev/null +++ b/src/modm/driver/inertial/bmi088.lb @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# Copyright (c) 2023, Christopher Durand +# +# This file is part of the modm project. +# +# This Source Code Form is subject to the terms of the Mozilla Public +# License, v. 2.0. If a copy of the MPL was not distributed with this +# file, You can obtain one at http://mozilla.org/MPL/2.0/. +# ----------------------------------------------------------------------------- + + +def init(module): + module.name = ":driver:bmi088" + module.description = """\ +# BMI088 Inertial Measurement Unit + +[Datasheet](https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi088-ds001.pdf) +""" + +def prepare(module, options): + module.depends( + ":architecture:gpio", + ":architecture:register", + ":architecture:spi.device", + ":architecture:i2c.device", + ":math:geometry", + ":processing:fiber", + ":processing:timer") + return True + +def build(env): + env.outbasepath = "modm/src/modm/driver/inertial" + env.copy("bmi088.hpp") + env.copy("bmi088_impl.hpp") + env.copy("bmi088_transport.hpp") + env.copy("bmi088_transport_impl.hpp") diff --git a/src/modm/driver/inertial/bmi088_impl.hpp b/src/modm/driver/inertial/bmi088_impl.hpp new file mode 100644 index 0000000000..b94dac0604 --- /dev/null +++ b/src/modm/driver/inertial/bmi088_impl.hpp @@ -0,0 +1,355 @@ +/* + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#include +#ifndef MODM_BMI088_HPP +#error "Don't include this file directly, use 'bmi088.hpp' instead!" +#endif + +#include + +namespace modm +{ + +template +template +Bmi088::Bmi088(Args... transportArgs) + : Transport{transportArgs...} +{ +} + +template +bool +Bmi088::initialize(bool runSelfTest) +{ + Transport::initialize(); + + if (!checkChipId() or !reset() or !enableAccelerometer()) { + return false; + } + + if (runSelfTest and !selfTest()) { + return false; + } + + timerWait(); + const bool ok = this->writeRegister(GyroRegister::InterruptControl, + uint8_t(GyroInterruptControl::DataReady)); + timer_.restart(WriteTimeout); + return ok; +} + +template +std::optional +Bmi088::readAccData() +{ + const auto data = this->readRegisters(AccRegister::DataXLow, 6); + + if (data.empty()) { + return {}; + } + + return AccData { + .raw = Vector3i( + data[0] | data[1] << 8, + data[2] | data[3] << 8, + data[4] | data[5] << 8 + ), + .range = accRange_ + }; +} + +template +bool +Bmi088::readAccDataReady() +{ + const auto value = readRegister(AccRegister::Status).value_or(0); + return value & uint8_t(AccStatus::DataReady); +} + +template +bool +Bmi088::clearAccDataReadyInterrupt() +{ + const auto result = readRegister(AccRegister::InterruptStatus).value_or(0); + return result & uint8_t(AccStatus::DataReady); +} + +template +bool +Bmi088::setAccRate(AccRate config) +{ + timerWait(); + const bool ok = this->writeRegister(AccRegister::Config, uint8_t(config)); + timer_.restart(ResetTimeout); + return ok; +} + +template +bool +Bmi088::setAccRange(AccRange range) +{ + timerWait(); + const bool ok = this->writeRegister(AccRegister::Range, uint8_t(range)); + timer_.restart(ResetTimeout); + if (ok) { + accRange_ = range; + return true; + } + return false; +} + +template +bool +Bmi088::setAccInt1GpioConfig(AccGpioConfig_t config) +{ + timerWait(); + const bool ok = this->writeRegister(AccRegister::Int1Control, config.value); + timer_.restart(ResetTimeout); + return ok; +} + +template +bool +Bmi088::setAccInt2GpioConfig(AccGpioConfig_t config) +{ + timerWait(); + const bool ok = this->writeRegister(AccRegister::Int2Control, config.value); + timer_.restart(ResetTimeout); + return ok; +} + +template +bool +Bmi088::setAccGpioMap(AccGpioMap_t map) +{ + timerWait(); + const bool ok = this->writeRegister(AccRegister::IntMap, map.value); + timer_.restart(ResetTimeout); + return ok; +} + +template +std::optional +Bmi088::readGyroData() +{ + const auto data = this->readRegisters(GyroRegister::RateXLow, 6); + + if (data.empty()) { + return {}; + } + + return GyroData { + .raw = Vector3i( + data[0] | data[1] << 8, + data[2] | data[3] << 8, + data[4] | data[5] << 8 + ), + .range = gyroRange_ + }; +} + +template +bool +Bmi088::readGyroDataReady() +{ + const auto value = readRegister(GyroRegister::InterruptStatus).value_or(0); + return bool(GyroStatus_t{value} & GyroStatus::DataReady); +} + +template +bool +Bmi088::setGyroRate(GyroRate rate) +{ + timerWait(); + const bool ok = this->writeRegister(GyroRegister::Bandwidth, uint8_t(rate)); + timer_.restart(ResetTimeout); + return ok; +} + +template +bool +Bmi088::setGyroRange(GyroRange range) +{ + timerWait(); + const bool ok = this->writeRegister(GyroRegister::Range, uint8_t(range)); + timer_.restart(ResetTimeout); + if (ok) { + gyroRange_ = range; + return true; + } + return false; +} + +template +bool +Bmi088::setGyroGpioConfig(GyroGpioConfig_t config) +{ + timerWait(); + const bool ok = this->writeRegister(GyroRegister::Int3Int4Conf, config.value); + timer_.restart(ResetTimeout); + return ok; +} + +template +bool +Bmi088::setGyroGpioMap(GyroGpioMap_t map) +{ + timerWait(); + const bool ok = this->writeRegister(GyroRegister::Int3Int4Map, map.value); + timer_.restart(ResetTimeout); + return ok; +} + +template +bool +Bmi088::checkChipId() +{ + const std::optional gyroId = readRegister(GyroRegister::ChipId); + const std::optional accId = readRegister(AccRegister::ChipId); + const bool gyroIdValid = gyroId.value_or(0) == GyroChipId; + const bool accIdValid = accId.value_or(0) == AccChipId; + return gyroIdValid and accIdValid; +} + +template +void +Bmi088::timerWait() +{ + while (timer_.isArmed()) { + modm::fiber::yield(); + } +} + +template +std::optional +Bmi088::readRegister(auto reg) +{ + const auto data = this->readRegisters(reg, 1); + if (data.empty()) { + return std::nullopt; + } else { + return data[0]; + } +} + +template +bool +Bmi088::reset() +{ + const bool gyroOk = this->writeRegister(GyroRegister::SoftReset, ResetCommand); + const bool accOk = this->writeRegister(AccRegister::SoftReset, ResetCommand); + + if (!gyroOk or !accOk) { + return false; + } + accRange_ = AccRange::Range6g; + gyroRange_ = GyroRange::Range2000dps; + + timer_.restart(ResetTimeout); + timerWait(); + + // required to switch the accelerometer to SPI mode if SPI is used + Transport::initialize(); + + return true; +} + +template +bool +Bmi088::selfTest() +{ + bool ok = setAccRange(AccRange::Range24g); + ok &= setAccRate(AccRate::Rate1600Hz_Bw280Hz); + if (!ok) { + return false; + } + timer_.restart(2ms); + timerWait(); + ok = this->writeRegister(AccRegister::SelfTest, uint8_t(AccSelfTest::Positive)); + if (!ok) { + return false; + } + timer_.restart(50ms); + timerWait(); + const auto positiveData = readAccData(); + if (!positiveData) { + return false; + } + ok = this->writeRegister(AccRegister::SelfTest, uint8_t(AccSelfTest::Negative)); + if (!ok) { + return false; + } + timer_.restart(50ms); + timerWait(); + const auto negativeData = readAccData(); + if (!negativeData) { + return false; + } + const Vector3f diff = positiveData->getFloat() - negativeData->getFloat(); + const bool accOk = (diff[0] >= 1000.f) and (diff[1] >= 1000.f) and (diff[2] >= 500.f); + this->writeRegister(AccRegister::SelfTest, uint8_t(AccSelfTest::Off)); + if (!accOk) { + return false; + } + ok = this->writeRegister(GyroRegister::SelfTest, uint8_t(GyroSelfTest::Trigger)); + if (!ok) { + return false; + } + timer_.restart(30ms); + timerWait(); + const auto gyroTest = GyroSelfTest_t(readRegister(GyroRegister::SelfTest).value_or(0)); + if (gyroTest != (GyroSelfTest::Ready | GyroSelfTest::RateOk)) { + return false; + } + return reset() and enableAccelerometer(); +} + +template +bool +Bmi088::enableAccelerometer() +{ + timerWait(); + bool ok = this->writeRegister(AccRegister::PowerConfig, uint8_t(AccPowerConf::Active)); + timer_.restart(AccSuspendTimeout); + if (!ok) { + return false; + } + timerWait(); + + ok = this->writeRegister(AccRegister::PowerControl, uint8_t(AccPowerControl::On)); + timer_.restart(WriteTimeout); + return ok; +} + +inline Vector3f +bmi088::AccData::getFloat() const +{ + const float factor = (1500 << (int(range) + 1)) * (1 / 32768.f); + return Vector3f { + raw[0] * factor, + raw[1] * factor, + raw[2] * factor + }; +} + +inline Vector3f +bmi088::GyroData::getFloat() const +{ + const float factor = (2000 >> int(range)) * (1 / 32768.f); + return Vector3f { + raw[0] * factor, + raw[1] * factor, + raw[2] * factor + }; +} + +} // namespace modm diff --git a/src/modm/driver/inertial/bmi088_transport.hpp b/src/modm/driver/inertial/bmi088_transport.hpp new file mode 100644 index 0000000000..569bdd71dc --- /dev/null +++ b/src/modm/driver/inertial/bmi088_transport.hpp @@ -0,0 +1,232 @@ +/* + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_BMI088_TRANSPORT_HPP +#define MODM_BMI088_TRANSPORT_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace modm +{ + +/// @ingroup modm_driver_bmi088 +struct Bmi088TransportBase +{ + enum class + AccRegister : uint8_t + { + ChipId = 0x00, + // 0x01: reserved + Error = 0x02, + Status = 0x03, + // 0x04-0x11: reserved + DataXLow = 0x12, + DataXHigh = 0x13, + DataYLow = 0x14, + DataYHigh = 0x15, + DataZLow = 0x16, + DataZHigh = 0x17, + SensorTime0 = 0x18, + SensorTime1 = 0x19, + SensorTime2 = 0x1A, + // 0x1B-0x1C: reserved + InterruptStatus = 0x1D, + // 0x1E-0x21: reserved + TempHigh = 0x22, + TempLow = 0x23, + FifoLength0 = 0x24, + FifoLength1 = 0x25, + FifoData = 0x26, + // 0x27-0x3F: reserved + Config = 0x40, + Range = 0x41, + // 0x42-0x44: reserved + FifoDownsampling = 0x45, + FifoWatermark0 = 0x46, + FifoWatermark1 = 0x47, + FifoConfig0 = 0x48, + FifoConfig1 = 0x49, + // 0x4A-0x52: reserved + Int1Control = 0x53, + Int2Control = 0x54, + // 0x55-0x57: reserved + IntMap = 0x58, + // 0x59-0x6C: reserved + SelfTest = 0x6D, + // 0x6E-0x7B: reserved + PowerConfig = 0x7C, + PowerControl = 0x7D, + SoftReset = 0x7E + }; + + enum class + GyroRegister : uint8_t + { + ChipId = 0x00, + // 0x01: reserved + RateXLow = 0x02, + RateXHigh = 0x03, + RateYLow = 0x04, + RateYHigh = 0x05, + RateZLow = 0x06, + RateZHigh = 0x07, + // 0x08-0x09: reserved + InterruptStatus = 0x0A, + // 0x0B-0x0D: reserved + FifoStatus = 0x0E, + Range = 0x0F, + Bandwidth = 0x10, + LowPowerMode1 = 0x11, + // 0x12-0x13: reserved + SoftReset = 0x14, + InterruptControl = 0x15, + Int3Int4Conf = 0x16, + // 0x17: reserved + Int3Int4Map = 0x18, + // 0x19-0x1D: reserved + FifoWatermark = 0x1E, + // 0x1F-0x33: reserved + FifoExtInt = 0x34, + // 0x35-0x3B: reserved + SelfTest = 0x3C, + FifoConfig0 = 0x3D, + FifoConfig1 = 0x3E, + FifoData = 0x3F + }; + static constexpr uint8_t MaxRegisterSequence{6}; +}; + +/// @ingroup modm_driver_bmi088 +template +concept Bmi088Transport = requires(T& transport, Bmi088TransportBase::AccRegister reg1, + Bmi088TransportBase::GyroRegister reg2, uint8_t data) +{ + { transport.initialize() }; + { transport.readRegisters(reg1, /* count= */data) } -> std::same_as>; + { transport.readRegisters(reg2, /* count= */data) } -> std::same_as>; + { transport.writeRegister(reg1, data) } -> std::same_as; + { transport.writeRegister(reg2, data) } -> std::same_as; +}; + +/** + * BMI088 SPI transport. Pass as template parameter to Bmi088 driver class to + * use the driver with SPI. + * + * The transport class contains internal buffers which allow to read all three + * gyroscope or accelerometer axes in a single DMA transaction. + * + * @tparam SpiMaster SPI master the device is connected to + * @tparam AccCs accelerometer chip-select GPIO + * @tparam GyroCs gyroscope chip-select GPIO + * @ingroup modm_driver_bmi088 + */ +template +class Bmi088SpiTransport : public Bmi088TransportBase, + public SpiDevice +{ +public: + Bmi088SpiTransport() = default; + + Bmi088SpiTransport(const Bmi088SpiTransport&) = delete; + + Bmi088SpiTransport& + operator=(const Bmi088SpiTransport&) = delete; + + void + initialize(); + + std::span + readRegisters(AccRegister startReg, uint8_t count); + + std::span + readRegisters(GyroRegister startReg, uint8_t count); + + bool + writeRegister(AccRegister reg, uint8_t data); + + bool + writeRegister(GyroRegister reg, uint8_t data); + +private: + template + std::span + readRegisters(uint8_t reg, uint8_t count, bool dummyByte); + + template + bool + writeRegister(uint8_t reg, uint8_t data); + + static constexpr uint8_t ReadFlag{0b1000'0000}; + + std::array rxBuffer_{}; + std::array txBuffer_{}; +}; + + +/** + * BMI088 I2C transport. Pass as template parameter to Bmi088 driver class to + * use the driver with I2C. + * + * @tparam I2cMaster I2c master the device is connected to + * @ingroup modm_driver_bmi088 + */ +template +class Bmi088I2cTransport : public Bmi088TransportBase, public I2cDevice +{ +public: + Bmi088I2cTransport(uint8_t accAddress, uint8_t gyroAddress); + + Bmi088I2cTransport(const Bmi088I2cTransport&) = delete; + + Bmi088I2cTransport& + operator=(const Bmi088I2cTransport&) = delete; + + void + initialize(); + + std::span + readRegisters(AccRegister startReg, uint8_t count); + + std::span + readRegisters(GyroRegister startReg, uint8_t count); + + bool + writeRegister(AccRegister reg, uint8_t data); + + bool + writeRegister(GyroRegister reg, uint8_t data); + +private: + std::span + readRegisters(uint8_t reg, uint8_t count); + + bool + writeRegister(uint8_t reg, uint8_t data); + + uint8_t accAddress_; + uint8_t gyroAddress_; + std::array buffer_{}; +}; + +} // modm namespace + +#include "bmi088_transport_impl.hpp" + +#endif // MODM_BMI088_TRANSPORT_HPP diff --git a/src/modm/driver/inertial/bmi088_transport_impl.hpp b/src/modm/driver/inertial/bmi088_transport_impl.hpp new file mode 100644 index 0000000000..8e62894091 --- /dev/null +++ b/src/modm/driver/inertial/bmi088_transport_impl.hpp @@ -0,0 +1,188 @@ +/* + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_BMI088_TRANSPORT_HPP +#error "Don't include this file directly, use 'bmi088_transport.hpp' instead!" +#endif + +#include + +namespace modm +{ + +template +void +Bmi088SpiTransport::initialize() +{ + // rising edge on CS required to enable accelerometer SPI mode + AccCs::setOutput(false); + modm::delay_ns(100); + AccCs::setOutput(true); + + GyroCs::setOutput(true); +} + +template +std::span +Bmi088SpiTransport::readRegisters(AccRegister startReg, + uint8_t count) +{ + return readRegisters(static_cast(startReg), count, true); +} + +template +std::span +Bmi088SpiTransport::readRegisters(GyroRegister startReg, + uint8_t count) +{ + return readRegisters(static_cast(startReg), count, false); +} + +template +template +std::span +Bmi088SpiTransport::readRegisters(uint8_t startReg, + uint8_t count, bool dummyByte) +{ + if (count > MaxRegisterSequence) { + return {}; + } + + while (!this->acquireMaster()) { + modm::fiber::yield(); + } + Cs::reset(); + + const uint8_t dataOffset = (dummyByte ? 2 : 1); + + txBuffer_[0] = startReg | ReadFlag; + txBuffer_[1] = 0; + + SpiMaster::transfer(&txBuffer_[0], &rxBuffer_[0], count + dataOffset); + + if (this->releaseMaster()) { + Cs::set(); + } + + return std::span{&rxBuffer_[dataOffset], count}; +} + +template +bool +Bmi088SpiTransport::writeRegister(AccRegister reg, uint8_t data) +{ + return writeRegister(static_cast(reg), data); +} + +template +bool +Bmi088SpiTransport::writeRegister(GyroRegister reg, uint8_t data) +{ + return writeRegister(static_cast(reg), data); +} + +template +template +bool +Bmi088SpiTransport::writeRegister(uint8_t reg, uint8_t data) +{ + while (!this->acquireMaster()) { + modm::fiber::yield(); + } + Cs::reset(); + + txBuffer_[0] = reg; + txBuffer_[1] = data; + SpiMaster::transfer(&txBuffer_[0], nullptr, 2); + + if (this->releaseMaster()) { + Cs::set(); + } + + return true; +} + +// ------------------------------------------------------------------------------------------------ + +template +Bmi088I2cTransport::Bmi088I2cTransport(uint8_t accAddress, uint8_t gyroAddress) + : I2cDevice{accAddress}, accAddress_{accAddress}, gyroAddress_{gyroAddress} +{ +} + +template +void +Bmi088I2cTransport::initialize() +{ +} + +template +std::span +Bmi088I2cTransport::readRegisters(AccRegister startReg, uint8_t count) +{ + this->transaction.setAddress(accAddress_); + return readRegisters(static_cast(startReg), count); +} + +template +std::span +Bmi088I2cTransport::readRegisters(GyroRegister startReg, uint8_t count) +{ + this->transaction.setAddress(gyroAddress_); + return readRegisters(static_cast(startReg), count); +} + +template +std::span +Bmi088I2cTransport::readRegisters(uint8_t startReg, uint8_t count) +{ + if (count > MaxRegisterSequence) { + return {}; + } + + this->transaction.configureWriteRead(&startReg, 1, &buffer_[0], count); + const bool success = this->runTransaction(); + + if (success) { + return std::span{&buffer_[0], count}; + } else { + return {}; + } +} + +template +bool +Bmi088I2cTransport::writeRegister(AccRegister reg, uint8_t data) +{ + this->transaction.setAddress(accAddress_); + return writeRegister(static_cast(reg), data); +} + +template +bool +Bmi088I2cTransport::writeRegister(GyroRegister reg, uint8_t data) +{ + this->transaction.setAddress(gyroAddress_); + return writeRegister(static_cast(reg), data); +} + +template +bool +Bmi088I2cTransport::writeRegister(uint8_t reg, uint8_t data) +{ + buffer_[0] = reg; + buffer_[1] = data; + this->transaction.configureWrite(&buffer_[0], 2); + + return this->runTransaction(); +} + +} // namespace modm From 06c07006827e8e065256b76bc10394c56b92461f Mon Sep 17 00:00:00 2001 From: Christopher Durand Date: Thu, 20 Jul 2023 16:57:18 +0200 Subject: [PATCH 7/8] [example] Add BMI088 SPI example for Nucleo H723ZG --- examples/nucleo_h723zg/bmi088/spi/main.cpp | 103 ++++++++++++++++++ examples/nucleo_h723zg/bmi088/spi/project.xml | 15 +++ 2 files changed, 118 insertions(+) create mode 100644 examples/nucleo_h723zg/bmi088/spi/main.cpp create mode 100644 examples/nucleo_h723zg/bmi088/spi/project.xml diff --git a/examples/nucleo_h723zg/bmi088/spi/main.cpp b/examples/nucleo_h723zg/bmi088/spi/main.cpp new file mode 100644 index 0000000000..9704a8c852 --- /dev/null +++ b/examples/nucleo_h723zg/bmi088/spi/main.cpp @@ -0,0 +1,103 @@ +/* + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#include + +#include +#include + +using namespace Board; + +using Spi = SpiMaster2_Dma; +using CsGyro = GpioC0; +using CsAcc = GpioD6; +using Mosi = GpioC3; +using Miso = GpioC2; +using Sck = GpioD3; + +using AccInt1 = GpioC8; +using GyroInt3 = GpioC9; + +using Transport = modm::Bmi088SpiTransport; +using Imu = modm::Bmi088; +Imu imu; + +void initializeImu() +{ + AccInt1::setInput(AccInt1::InputType::PullDown); + GyroInt3::setInput(GyroInt3::InputType::PullDown); + + constexpr bool selfTest = true; + while (!imu.initialize(selfTest)) { + MODM_LOG_ERROR << "Initialization failed, retrying ...\n"; + modm::delay(500ms); + } + + bool ok = imu.setAccRate(Imu::AccRate::Rate12Hz_Bw5Hz); + ok &= imu.setAccRange(Imu::AccRange::Range3g); + + const auto int1Config = (Imu::AccGpioConfig::ActiveHigh | Imu::AccGpioConfig::EnableOutput); + ok &= imu.setAccInt1GpioConfig(int1Config); + ok &= imu.setAccGpioMap(Imu::AccGpioMap::Int1DataReady); + + ok &= imu.setGyroRate(Imu::GyroRate::Rate100Hz_Bw12Hz); + ok &= imu.setGyroRange(Imu::GyroRange::Range250dps); + ok &= imu.setGyroGpioConfig(Imu::GyroGpioConfig::Int3ActiveHigh); + ok &= imu.setGyroGpioMap(Imu::GyroGpioMap::Int3DataReady); + + if (!ok) { + MODM_LOG_ERROR << "Configuration failed!\n"; + } +} + +int main() +{ + Board::initialize(); + Leds::setOutput(); + Dma1::enable(); + Spi::connect(); + Spi::initialize(); + + MODM_LOG_INFO << "BMI088 SPI Test\n"; + initializeImu(); + + std::atomic_bool accReady = false; + std::atomic_bool gyroReady = false; + + Exti::connect(Exti::Trigger::RisingEdge, [&accReady](auto){ + accReady = true; + }); + + Exti::connect(Exti::Trigger::RisingEdge, [&gyroReady](auto){ + gyroReady = true; + }); + + while (true) + { + while(!accReady or !gyroReady); + + const std::optional accResult = imu.readAccData(); + accReady = false; + const std::optional gyroResult = imu.readGyroData(); + gyroReady = false; + + if (accResult) { + const modm::Vector3f data = accResult->getFloat(); + MODM_LOG_INFO.printf("Acc [mg]\tx:\t%5.1f\ty: %5.1f\tz: %5.1f\n", data[0], data[1], data[2]); + } + if (gyroResult) { + const modm::Vector3f data = gyroResult->getFloat(); + MODM_LOG_INFO.printf("Gyro [deg/s]\tx:\t%5.2f\ty: %5.2f\tz: %5.2f\n", data[0], data[1], data[2]); + } + } + + return 0; +} diff --git a/examples/nucleo_h723zg/bmi088/spi/project.xml b/examples/nucleo_h723zg/bmi088/spi/project.xml new file mode 100644 index 0000000000..0ee7b8a80b --- /dev/null +++ b/examples/nucleo_h723zg/bmi088/spi/project.xml @@ -0,0 +1,15 @@ + + modm:nucleo-h723zg + + + + + + modm:build:scons + modm:processing:fiber + modm:platform:dma + modm:platform:exti + modm:platform:spi:2 + modm:driver:bmi088 + + From a771042eb5d503acdae5ac18d08c61932b7c3618 Mon Sep 17 00:00:00 2001 From: Christopher Durand Date: Wed, 4 Oct 2023 22:44:50 +0200 Subject: [PATCH 8/8] [example] Add BMI088 I2C example for Nucleo H723ZG --- examples/nucleo_h723zg/bmi088/i2c/main.cpp | 102 ++++++++++++++++++ examples/nucleo_h723zg/bmi088/i2c/project.xml | 14 +++ 2 files changed, 116 insertions(+) create mode 100644 examples/nucleo_h723zg/bmi088/i2c/main.cpp create mode 100644 examples/nucleo_h723zg/bmi088/i2c/project.xml diff --git a/examples/nucleo_h723zg/bmi088/i2c/main.cpp b/examples/nucleo_h723zg/bmi088/i2c/main.cpp new file mode 100644 index 0000000000..17b6847d17 --- /dev/null +++ b/examples/nucleo_h723zg/bmi088/i2c/main.cpp @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2023, Christopher Durand + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#include + +#include +#include + +using namespace Board; + +using I2c = I2cMaster1; +using Scl = GpioB8; // D15 +using Sda = GpioB9; // D14 + +using AccInt1 = GpioC6; +using GyroInt3 = GpioB15; + +using Transport = modm::Bmi088I2cTransport; +using Imu = modm::Bmi088; + +constexpr uint8_t AccAddress = 0x18; +constexpr uint8_t GyroAddress = 0x68; +Imu imu{AccAddress, GyroAddress}; + +void initializeImu() +{ + AccInt1::setInput(AccInt1::InputType::PullDown); + GyroInt3::setInput(GyroInt3::InputType::PullDown); + + constexpr bool selfTest = true; + while (!imu.initialize(selfTest)) { + MODM_LOG_ERROR << "Initialization failed, retrying ...\n"; + modm::delay(500ms); + } + + bool ok = imu.setAccRate(Imu::AccRate::Rate12Hz_Bw5Hz); + ok &= imu.setAccRange(Imu::AccRange::Range3g); + + const auto int1Config = (Imu::AccGpioConfig::ActiveHigh | Imu::AccGpioConfig::EnableOutput); + ok &= imu.setAccInt1GpioConfig(int1Config); + ok &= imu.setAccGpioMap(Imu::AccGpioMap::Int1DataReady); + + ok &= imu.setGyroRate(Imu::GyroRate::Rate100Hz_Bw12Hz); + ok &= imu.setGyroRange(Imu::GyroRange::Range250dps); + ok &= imu.setGyroGpioConfig(Imu::GyroGpioConfig::Int3ActiveHigh); + ok &= imu.setGyroGpioMap(Imu::GyroGpioMap::Int3DataReady); + + if (!ok) { + MODM_LOG_ERROR << "Configuration failed!\n"; + } +} + +int main() +{ + Board::initialize(); + Leds::setOutput(); + I2c::connect(I2c::PullUps::Internal); + I2c::initialize(); + + MODM_LOG_INFO << "BMI088 I2C Test\n"; + initializeImu(); + + std::atomic_bool accReady = false; + std::atomic_bool gyroReady = false; + + Exti::connect(Exti::Trigger::RisingEdge, [&accReady](auto){ + accReady = true; + }); + + Exti::connect(Exti::Trigger::RisingEdge, [&gyroReady](auto){ + gyroReady = true; + }); + + while (true) + { + while(!accReady or !gyroReady); + + const std::optional accResult = imu.readAccData(); + accReady = false; + const std::optional gyroResult = imu.readGyroData(); + gyroReady = false; + + if (accResult) { + const modm::Vector3f data = accResult->getFloat(); + MODM_LOG_INFO.printf("Acc [mg]\tx:\t%5.1f\ty: %5.1f\tz: %5.1f\n", data[0], data[1], data[2]); + } + if (gyroResult) { + const modm::Vector3f data = gyroResult->getFloat(); + MODM_LOG_INFO.printf("Gyro [deg/s]\tx:\t%5.2f\ty: %5.2f\tz: %5.2f\n", data[0], data[1], data[2]); + } + } + + return 0; +} diff --git a/examples/nucleo_h723zg/bmi088/i2c/project.xml b/examples/nucleo_h723zg/bmi088/i2c/project.xml new file mode 100644 index 0000000000..797c04a836 --- /dev/null +++ b/examples/nucleo_h723zg/bmi088/i2c/project.xml @@ -0,0 +1,14 @@ + + modm:nucleo-h723zg + + + + + + modm:build:scons + modm:processing:fiber + modm:platform:exti + modm:platform:i2c:1 + modm:driver:bmi088 + +