From 05c799c466673ab8a0b18785de3768683f567218 Mon Sep 17 00:00:00 2001
From: BlackMark <me@blackmark.me>
Date: Fri, 21 Feb 2020 21:24:10 +0100
Subject: [PATCH] Implemented mux register initialization

---
 adc.hpp      | 97 +++++++++++++++++++++++++++++++++++++++++-----------
 hardware.hpp |  6 ++++
 2 files changed, 83 insertions(+), 20 deletions(-)

diff --git a/adc.hpp b/adc.hpp
index 859f14d..4b9ce97 100644
--- a/adc.hpp
+++ b/adc.hpp
@@ -14,22 +14,40 @@ namespace detail {
 
 extern void (*fnAdcIntHandler)(uint16_t);
 
+using reg_ptr_t = volatile uint8_t *;
+
+template <uintptr_t Address>
+static inline reg_ptr_t getRegPtr()
+{
+	return reinterpret_cast<reg_ptr_t>(Address);
+}
+
+template <typename Cfg>
 class AdcImpl {
-  private:
-	using callback_t = void (*)(uint16_t);
-
   public:
-	static void init(callback_t callback)
-	{
-		detail::fnAdcIntHandler = callback;
-	}
-
-	static void init() {}
-
 	static uint16_t read()
 	{
 		return 0;
 	}
+
+  protected:
+	static void init(uint8_t muxVal)
+	{
+		*getRegPtr<Registers::MUX_SEL_ADDR>() = muxVal | calcRef();
+	}
+
+	static constexpr auto calcRef()
+	{
+		uint8_t muxVal = 0;
+
+		if constexpr (Cfg::VREF == VoltageRef::AVCC) {
+			muxVal |= 1 << ControlFlagsMUX::REF_SEL_0;
+		} else if constexpr (Cfg::VREF == VoltageRef::INTERNAL) {
+			muxVal |= (1 << ControlFlagsMUX::REF_SEL_0) | (1 << ControlFlagsMUX::REF_SEL_1);
+		}
+
+		return muxVal;
+	}
 };
 
 } // namespace detail
@@ -41,13 +59,60 @@ class Adc {
 };
 
 template <typename Cfg, io::P pin>
-class Adc<Cfg, io::P, pin> : public detail::AdcImpl {
+class Adc<Cfg, io::P, pin> : public detail::AdcImpl<Cfg> {
+	using callback_t = void (*)(uint16_t);
+
   public:
 	static_assert(detail::supports_adc_v<pin>, "Pin does not support ADC");
+
+	static void init(callback_t callback)
+	{
+		detail::fnAdcIntHandler = callback;
+		init();
+	}
+
+	static void init()
+	{
+		constexpr auto muxVal = calcChannel();
+		detail::AdcImpl<Cfg>::init(muxVal);
+	}
+
+  private:
+	static constexpr auto calcChannel()
+	{
+		return static_cast<uint8_t>(pin) - static_cast<uint8_t>(io::P::C0);
+	}
 };
 
 template <typename Cfg, InputSource src>
-class Adc<Cfg, InputSource, src> : public detail::AdcImpl {
+class Adc<Cfg, InputSource, src> : public detail::AdcImpl<Cfg> {
+	using callback_t = void (*)(uint16_t);
+
+  public:
+	static void init(callback_t callback)
+	{
+		detail::fnAdcIntHandler = callback;
+		init();
+	}
+
+	static void init()
+	{
+		constexpr auto muxVal = calcChannel();
+		detail::AdcImpl<Cfg>::init(muxVal);
+	}
+
+  private:
+	static constexpr auto calcChannel()
+	{
+		using mx = detail::ControlFlagsMUX;
+		// clang-format off
+		switch (src) {
+		case InputSource::TEMP: return 1 << mx::CHANNEL_SEL_3;
+		case InputSource::VBG: return (1 << mx::CHANNEL_SEL_3) | (1 << mx::CHANNEL_SEL_2) | (1 << mx::CHANNEL_SEL_1);
+		case InputSource::GND: return (1 << mx::CHANNEL_SEL_3) | (1 << mx::CHANNEL_SEL_2) | (1 << mx::CHANNEL_SEL_1) | (1 << mx::CHANNEL_SEL_0);
+		}
+		// clang-format on
+	}
 };
 
 } // namespace adc
@@ -69,14 +134,6 @@ namespace detail {
 
 void (*fnAdcIntHandler)(uint16_t) = nullptr;
 
-using reg_ptr_t = volatile uint8_t *;
-
-template <uintptr_t Address>
-static inline reg_ptr_t getRegPtr()
-{
-	return reinterpret_cast<reg_ptr_t>(Address);
-}
-
 ISR(ADC_vect)
 {
 	if (fnAdcIntHandler) {
diff --git a/hardware.hpp b/hardware.hpp
index 0b67e2a..903c08f 100644
--- a/hardware.hpp
+++ b/hardware.hpp
@@ -70,6 +70,12 @@ enum class ControlFlagsDigInDis {
 	DIGITAL_INPUT_DISABLE_5 = ADC5D,
 };
 
+// clang-format off
+constexpr int operator<<(const int &lhs, const ControlFlagsMUX &rhs) { return lhs << static_cast<int>(rhs); }
+constexpr int operator<<(const int &lhs, const ControlFlagsA &rhs) { return lhs << static_cast<int>(rhs); }
+constexpr int operator<<(const int &lhs, const ControlFlagsB &rhs) { return lhs << static_cast<int>(rhs); }
+// clang-format on
+
 template <io::P pin>
 struct supports_adc {
 	static constexpr auto value = (io::detail::getBus(pin) == io::Bus::C) ? true : false;