Added automatic clock calculation with compile-time checks

hardware.hpp

@@ -8,20 +8,16 @@ namespace i2c {
 
 #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega328P__)
 
-enum class Prescaler {
-	PRESCALER_1 = 0b00,
-	PRESCALER_4 = 0b01,
-	PRESCALER_16 = 0b10,
-	PRESCALER_64 = 0b11,
-};
-
-template <Prescaler Pres, uint32_t Clock>
+template <uint32_t Clock>
 class Hardware {
   public:
 	static void init()
 	{
-		TWSR = static_cast<uint8_t>(Pres);
-		TWBR = calcClock();
+		constexpr auto clock = calcClock();
+		static_assert(checkClock(clock), "Unable to set requested I2C frequency");
+
+		TWSR = clock.prescaler;
+		TWBR = clock.bitrate;
 	}
 
 	template <uint8_t Addr>
@@ -82,9 +78,46 @@ class Hardware {
 	}
 
   private:
-	static constexpr uint8_t calcClock()
+	struct ClockCfg {
+		uint8_t prescaler;
+		uint8_t bitrate;
+	};
+
+	static constexpr auto calcScl(ClockCfg clk)
 	{
-		return ((F_CPU / Clock) - 16) / 2;
+		return F_CPU / (16 + 2 * clk.bitrate * clk.prescaler);
+	}
+
+	static constexpr auto checkClock(ClockCfg clk)
+	{
+		constexpr auto constexprAbs = [](auto val) {
+			if (val < 0)
+				return -val;
+			return val;
+		};
+
+		constexpr auto CLOCK_EPSILON = 10;
+		auto scl = calcScl(clk);
+		if (constexprAbs(static_cast<int>(scl) - static_cast<int>(Clock)) < CLOCK_EPSILON)
+			return true;
+		return false;
+	}
+
+	static constexpr auto calcClock()
+	{
+		constexpr auto calcBitRate = [](uint8_t prescaler) { return (F_CPU / Clock - 16) / (2 * prescaler); };
+
+		auto clk = ClockCfg{1, 0};
+
+		for (uint8_t prescaler = 1; prescaler <= 64; prescaler *= 4) {
+			clk.prescaler = prescaler;
+			clk.bitrate = calcBitRate(prescaler);
+
+			if (checkClock(clk))
+				break;
+		}
+
+		return clk;
 	}
 };