Fixup driver

modm-io · Feb 14, 2023 · f6d2e00 · f6d2e00
1 parent 8422525
commit f6d2e00
Showing 1 changed file with 58 additions and 33 deletions.
diff --git a/src/modm/platform/i2c/sam_x7x/i2c_master.hpp.in b/src/modm/platform/i2c/sam_x7x/i2c_master.hpp.in
@@ -20,6 +20,7 @@
 #include <modm/platform/gpio/connector.hpp>
 #include <modm/platform/gpio/pin.hpp>
 #include <algorithm>
+#include <bit>
 #include <optional>
 
 namespace modm
@@ -39,56 +40,79 @@ public:
 	static constexpr size_t TransactionBufferSize = {{ options["buffer.transaction"] }};
 
 private:
-	template<class SystemClock, baudrate_t baudrate/*, percent_t tolerance*/>
+	// calculate required bits to represent value
+	static constexpr int
+	bits(uint32_t value)
+	{
+		return 32 - std::countl_zero(value);
+	}
+
+	template<auto clock, baudrate_t baudrate, percent_t tolerance,
+			uint8_t ckdiv, uint8_t cldiv, uint8_t chdiv>
+	static constexpr bool
+	checkBaudrate()
+	{
+		if (ckdiv > 7) {
+			return false;
+		}
+
+		const float tHigh = ((chdiv * (1 << ckdiv)) + 3) / float(clock);
+		const float tLow = ((cldiv * (1 << ckdiv)) + 3) / float(clock);
+		const float freq = 1.f / (tHigh + tLow);
+		const float error = std::fabs(1.f - freq / baudrate);
+
+		return error < pct2f(tolerance);
+	}
+
+	template<class SystemClock, baudrate_t baudrate, percent_t tolerance>
 	static consteval std::optional<uint32_t>
 	calculateTimings()
 	{
-		const auto clock = SystemClock::Twihs{{ id }};
+		constexpr auto clock = SystemClock::Twihs{{ id }};
 
 		// hold time = (HOLD + 3) * (1 / peripheral clock)
 		// => HOLD = hold time * (peripheral clock) - 3
 		constexpr float holdTime = 300.e-9f;
-		const float holdIdeal = holdTime * clock - 3.f;
-		const uint8_t hold = static_cast<uint8_t>(std::clamp(std::ceil(holdIdeal), 0.f, 63.f));
+		constexpr float holdIdeal = holdTime * clock - 3.f;
+		constexpr uint8_t hold = static_cast<uint8_t>(std::clamp(std::ceil(holdIdeal), 0.f, 63.f));
 
 		constexpr bool fastMode = baudrate > 125'000;
 		// Baudrate threshold above which the low time is fixed to the minimum value of
 		// 1.3us in fast mode or 4.7us in standard mode.
 		constexpr auto minLowTime = fastMode ? 1.3e-6 : 4.7e-6;
 		constexpr auto minLowTimeLimit = 1.f / (2*minLowTime);
 
-		constexpr uint32_t maxDiv = 255; // max value of CHDIV, CLDIV
-		constexpr uint32_t maxPreDiv = 7; // max exponent of 2^N divider
-
 		// t_high = ((CHDIV * 2^CKDIV) + 3) * (1 / peripheral clock)
 		// t_low  = ((CLDIV * 2^CKDIV) + 3) * (1 / peripheral clock)
 		if constexpr (baudrate > minLowTimeLimit) {
 			// calculate ideal low and high prescaler values (formula from ASF vendor HAL)
-			auto cldiv = uint32_t(std::round((minLowTime * clock) - 3));
-			const auto tHigh = 1.f / (baudrate + (baudrate - minLowTimeLimit));
-			auto chdiv = uint32_t(std::round((tHigh * clock) - 3));
-			uint32_t ckdiv = 0;
-
-			// increase 2^N pre-divider until prescalers fit into 8 bit
-			while ((chdiv > maxDiv || cldiv > maxDiv) && (ckdiv < maxPreDiv)) {
-				++ckdiv;
-				chdiv /= 2;
-				cldiv /= 2;
-			}
-			return TWIHS_CWGR_HOLD(hold) | TWIHS_CWGR_CLDIV(cldiv) |
-				TWIHS_CWGR_CHDIV(chdiv) | TWIHS_CWGR_CKDIV(ckdiv);
+			constexpr auto cldiv = uint32_t(std::round((minLowTime * clock) - 3));
+			constexpr auto tHigh = 1.f / ((baudrate + (baudrate - minLowTimeLimit)) * 2.f);
+			constexpr auto chdiv = uint32_t(std::round((tHigh * clock) - 3));
+
+			// use 2^N pre-divider if max. prescaler exceeds 8 bits
+			constexpr auto ckdiv = std::max(0, std::max(bits(cldiv), bits(chdiv)) - 8);
+			constexpr uint32_t cldivScaled = std::round(float(cldiv) / (1 << ckdiv));
+			constexpr uint32_t chdivScaled = std::round(float(chdiv) / (1 << ckdiv));
+
+			if (!checkBaudrate<clock, baudrate, tolerance, ckdiv, cldivScaled, chdivScaled>())
+				return std::nullopt;
+
+			return TWIHS_CWGR_HOLD(hold) | TWIHS_CWGR_CLDIV(cldivScaled) |
+				TWIHS_CWGR_CHDIV(chdivScaled) | TWIHS_CWGR_CKDIV(ckdiv);
 		} else {
-			uint32_t ckdiv = 0;
-			auto div = uint32_t(std::round(clock / (baudrate * 2.f) - 3));
-
-			while ((div > maxDiv) && (ckdiv < maxPreDiv)) {
-				++ckdiv;
-				div /= 2;
-			}
-			return TWIHS_CWGR_HOLD(hold) | TWIHS_CWGR_CLDIV(div) |
-				TWIHS_CWGR_CHDIV(div) | TWIHS_CWGR_CKDIV(ckdiv);
+			constexpr auto div = uint32_t(std::round(clock / (baudrate * 2.f) - 3));
+
+			// use 2^N pre-divider if max. prescaler exceeds 8 bits
+			constexpr auto ckdiv = std::max(0, bits(div) - 8);
+			constexpr uint32_t divScaled = std::round(float(div) / (1 << ckdiv));
+
+			if (!checkBaudrate<clock, baudrate, tolerance, ckdiv, divScaled, divScaled>())
+				return std::nullopt;
+
+			return TWIHS_CWGR_HOLD(hold) | TWIHS_CWGR_CLDIV(divScaled) |
+				TWIHS_CWGR_CHDIV(divScaled) | TWIHS_CWGR_CKDIV(ckdiv);
 		}
-		// TODO: tolerance check
 	}
 
 public:
@@ -120,13 +144,14 @@ public:
 	 * @param	rate
 	 *		`Standard` (100 kHz) or `Fast` (400 kHz)
 	 */
-	template<class SystemClock, baudrate_t baudrate=kBd(100)/*, percent_t tolerance=pct(5)*/>
+	template<class SystemClock, baudrate_t baudrate=kBd(100), percent_t tolerance=pct(5)>
 	static void
 	initialize()
 	{
 		static_assert(baudrate <= 400'000, "Baudrate must not exceed 400 kHz for I2C fast mode");
-		constexpr std::optional<uint32_t> registerValue = calculateTimings<SystemClock, baudrate/*, tolerance*/>();
-		static_assert(bool(registerValue), "Could not find a valid clock configuration for the requested baudrate");
+		constexpr std::optional<uint32_t> registerValue = calculateTimings<SystemClock, baudrate, tolerance>();
+		static_assert(bool(registerValue), "Could not find a valid clock configuration for the requested"
+			" baudrate and tolerance");
 
 		initializeWithClockConfig(registerValue.value());
 	}