From 04b6782ec457fb22759a097892b863a3ec6eaab4 Mon Sep 17 00:00:00 2001
From: BlackMark <me@blackmark.me>
Date: Sun, 12 Apr 2020 23:57:16 +0200
Subject: [PATCH] Add automatic selection of double speed based on error

---
 hardware.hpp | 54 +++++++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 49 insertions(+), 5 deletions(-)

diff --git a/hardware.hpp b/hardware.hpp
index abbb756..f6d15e3 100644
--- a/hardware.hpp
+++ b/hardware.hpp
@@ -4,6 +4,7 @@
 
 #include "../type/type.hpp"
 
+#include <math.h>
 #include <stdint.h>
 
 #define FORCE_INLINE __attribute__((always_inline))
@@ -12,7 +13,6 @@ namespace uart {
 
 enum class Mode {
 	ASYNCHRONOUS,
-	ASYNCHRONOUS_2X,
 	SYNCHRONOUS_MASTER,
 	SYNCHRONOUS_SLAVE,
 	SPI,
@@ -46,7 +46,12 @@ class Hardware {
   public:
 	static void init() FORCE_INLINE
 	{
-		constexpr auto BaudVal = calcBaud();
+		constexpr auto AbsDoubleError = fabs(calcBaudError<true>());
+		constexpr auto AbsNormalError = fabs(calcBaudError<false>());
+		static_assert(AbsDoubleError <= 3.0 || AbsNormalError <= 3.0, "Baud rate error over 3%, probably unusable");
+
+		constexpr auto UseDoubleSpeed = (AbsDoubleError < AbsNormalError);
+		constexpr auto BaudVal = calcBaudVal<UseDoubleSpeed>();
 
 		*getRegPtr<Registers::BAUD_REG_H_ADDR>() = static_cast<uint8_t>(BaudVal >> 8);
 		*getRegPtr<Registers::BAUD_REG_L_ADDR>() = static_cast<uint8_t>(BaudVal);
@@ -58,10 +63,12 @@ class Hardware {
 		constexpr auto EnableRx = calcRxState<true>();
 		constexpr auto EnableTx = calcTxState<true>();
 		constexpr auto InterruptVal = calcInterrupt();
+		constexpr auto DoubleSpeedVal = calcDoubleSpeedVal<UseDoubleSpeed>();
 
 		constexpr uint8_t ControlRegB = DataBitsValues.regBVal | EnableRx | EnableTx | InterruptVal;
 		constexpr uint8_t ControlRegC = DataBitsValues.regCVal | ParityVal | StopBitsVal | ModeVal;
 
+		*getRegPtr<Registers::CTRL_STAT_REG_A_ADDR>() |= DoubleSpeedVal;
 		*getRegPtr<Registers::CTRL_STAT_REG_B_ADDR>() = ControlRegB;
 		*getRegPtr<Registers::CTRL_STAT_REG_C_ADDR>() = ControlRegC;
 	}
@@ -134,13 +141,39 @@ class Hardware {
 		uint8_t regBVal = 0;
 	};
 
-	static constexpr auto calcBaud()
+	template <bool DoubleSpeed = true>
+	static constexpr auto calcBaudVal()
 	{
-		// The actual formula is (F_CPU / (16 * baudRate)) - 1, but this one has the advantage of rounding correctly
-		constexpr auto BaudVal = (F_CPU + 8 * cfg::BAUD_RATE) / (16 * cfg::BAUD_RATE) - 1;
+		if constexpr (DoubleSpeed) {
+			constexpr auto BaudVal = static_cast<uint16_t>(round(F_CPU / (8.0 * cfg::BAUD_RATE) - 1));
+			return BaudVal;
+		}
+
+		constexpr auto BaudVal = static_cast<uint16_t>(round(F_CPU / (16.0 * cfg::BAUD_RATE) - 1));
 		return BaudVal;
 	}
 
+	template <uint16_t BaudVal, bool DoubleSpeed = true>
+	static constexpr auto calcBaudRate()
+	{
+		if constexpr (DoubleSpeed) {
+			constexpr auto BaudRate = static_cast<uint32_t>(round(F_CPU / (8.0 * (BaudVal + 1))));
+			return BaudRate;
+		}
+
+		constexpr auto BaudRate = static_cast<uint32_t>(round(F_CPU / (16.0 * (BaudVal + 1))));
+		return BaudRate;
+	}
+
+	template <bool DoubleSpeed = true>
+	static constexpr auto calcBaudError()
+	{
+		constexpr auto BaudVal = calcBaudVal<DoubleSpeed>();
+		constexpr auto ClosestBaudRate = calcBaudRate<BaudVal, DoubleSpeed>();
+		constexpr auto BaudError = (static_cast<double>(ClosestBaudRate) / cfg::BAUD_RATE - 1) * 100;
+		return BaudError;
+	}
+
 	static constexpr auto calcDataBits()
 	{
 		DataBitsVal dataBitsVal;
@@ -233,6 +266,17 @@ class Hardware {
 
 		return interruptVal;
 	}
+
+	template <bool DoubleSpeed>
+	static constexpr auto calcDoubleSpeedVal()
+	{
+		uint8_t doubleSpeedVal = 0;
+
+		if constexpr (DoubleSpeed)
+			doubleSpeedVal = (1 << CtrlFlagsA::SPEED_2X);
+
+		return doubleSpeedVal;
+	}
 };
 
 template <class Registers, typename CtrlFlagsA, typename CtrlFlagsB, typename CtrlFlagsC, class cfg, Mode mode>