Add writeBytes function to allow writing multiple bytes

hardware.hpp

@@ -6,22 +6,18 @@
 
 namespace i2c {
 
-#if defined(__AVR_ATmega1284P__)
+#if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega328P__)
 
-enum class Prescaler {
+template <uint32_t Clock>
-	PRESCALER_1 = 0b00,
-	PRESCALER_4 = 0b01,
-	PRESCALER_16 = 0b10,
-	PRESCALER_64 = 0b11,
-};
-
-template <Prescaler Pres, uint32_t Clock>
 class Hardware {
   public:
 	static void init()
 	{
-		TWSR = static_cast<uint8_t>(Pres);
+		constexpr auto clock = calcClock();
-		TWBR = 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;
 	}
 };
 

i2c.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <stddef.h>
 #include <stdint.h>
 
 #include "hardware.hpp"
@@ -24,12 +25,34 @@ struct I2c {
 		return Driver::write(data);
 	}
 
+	template <size_t Bytes>
+	static bool writeBytes(const uint8_t *buffer)
+	{
+		for (size_t i = 0; i < Bytes; ++i) {
+			if (!write(buffer[i]))
+				return false;
+		}
+
+		return true;
+	}
+
 	template <bool LastByte = false>
 	static uint8_t read()
 	{
 		return Driver::template read<LastByte>();
 	}
 
+	template <size_t Bytes>
+	static void readBytes(uint8_t *buffer)
+	{
+		static_assert(Bytes > 0, "Must read at least 1 byte");
+
+		for (size_t i = 0; i < Bytes - 1; ++i) {
+			buffer[i] = read();
+		}
+		buffer[Bytes - 1] = read<true>();
+	}
+
 	static void stop()
 	{
 		Driver::stop();