diff --git a/Grbl_Esp32.sln b/Grbl_Esp32.sln
index 5479c1cb..2aa43d42 100644
--- a/Grbl_Esp32.sln
+++ b/Grbl_Esp32.sln
@@ -16,6 +16,7 @@ Global
 	EndGlobalSection
 	GlobalSection(ProjectConfigurationPlatforms) = postSolution
 		{11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Debug|x64.ActiveCfg = Debug|x64
+		{11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Debug|x64.Build.0 = Debug|x64
 		{11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Debug|x86.ActiveCfg = Debug|Win32
 		{11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Debug|x86.Build.0 = Debug|Win32
 		{11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Release|x64.ActiveCfg = Release|x64
@@ -23,7 +24,6 @@ Global
 		{11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Release|x86.ActiveCfg = Release|Win32
 		{11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Release|x86.Build.0 = Release|Win32
 		{33ECE513-60D1-4949-A4A9-C95D353C2CF0}.Debug|x64.ActiveCfg = Debug|x64
-		{33ECE513-60D1-4949-A4A9-C95D353C2CF0}.Debug|x64.Build.0 = Debug|x64
 		{33ECE513-60D1-4949-A4A9-C95D353C2CF0}.Debug|x86.ActiveCfg = Debug|Win32
 		{33ECE513-60D1-4949-A4A9-C95D353C2CF0}.Debug|x86.Build.0 = Debug|Win32
 		{33ECE513-60D1-4949-A4A9-C95D353C2CF0}.Release|x64.ActiveCfg = Release|x64
diff --git a/Grbl_Esp32/src/Configuration/Configurable.h b/Grbl_Esp32/src/Configuration/Configurable.h
index 79580bf4..53be0fe0 100644
--- a/Grbl_Esp32/src/Configuration/Configurable.h
+++ b/Grbl_Esp32/src/Configuration/Configurable.h
@@ -5,14 +5,23 @@
 
 namespace Configuration
 {
+    class HandlerBase;
+
     class Configurable
     {
         Configurable(const Configurable&) = delete;
+        Configurable(Configurable&&) = default;
+
         Configurable& operator=(const Configurable&) = delete;
+        Configurable& operator=(Configurable&&) = default;
 
     public:
         Configurable() = default;
 
-        virtual void generate(Generator& generator) const = 0;
+        virtual void validate() const = 0;
+        virtual void handle(HandlerBase& handler) = 0;
+        // virtual const char* name() const = 0;
+
+        virtual ~Configurable() {}
     };
 }
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/Generator.cpp b/Grbl_Esp32/src/Configuration/Generator.cpp
index cc15bcbb..fee14f7f 100644
--- a/Grbl_Esp32/src/Configuration/Generator.cpp
+++ b/Grbl_Esp32/src/Configuration/Generator.cpp
@@ -3,70 +3,40 @@
 #include "Configurable.h"
 
 #include <cstring>
+#include <cstdio>
 
 namespace Configuration
 {
     void Generator::enter(const char* name)
     {
         indent();
-        addStr(name);
-        addStr(":\n");
+        dst_ << name << ":\n";
         indent_++;
     }
 
-    void Generator::add(const char* key, const std::string& value)
-    {
-        add(key, value.c_str());
-    }
-
-    void Generator::add(const char* key, const char* value)
-    {
-        indent();
-        addStr(key);
-        addStr(": ");
-        addStr(value);
-        addStr("\n");
-    }
-
-    void Generator::add(const char* key, bool value)
-    {
-        if (value) { add(key, "true"); } 
-        else { add(key, "false"); }
-    }
-
-    void Generator::add(const char* key, int value)
-    {
-        char tmp[11];
-        snprintf(tmp, 11, "%d", value);
-        add(key, tmp);
-    }
-
-    void Generator::add(const char* key, double value)
-    {
-        char tmp[20];
-        snprintf(tmp, 20, "%f", value);
-        add(key, tmp);
-    }
-
-    void Generator::add(const char* key, Pin value)
-    {
-        if (!value.undefined())
-        {
-            add(key, value.str());
-        }
-    }
-
     void Generator::add(Configuration::Configurable* configurable)
     {
         if (configurable != nullptr)
         {
-            configurable->generate(*this);
+            configurable->handle(*this);
         }
     }
 
     void Generator::leave()
     {
-        addStr("\n");
+        if (!lastIsNewline_)
+        {
+            dst_ << '\n';
+            lastIsNewline_ = true;
+        }
+
         indent_--;
     }
+
+    void Generator::handleDetail(const char* name, Configurable* value) {
+        enter(name);
+        value->handle(*this);
+        leave();
+    }
+
 }
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/Generator.h b/Grbl_Esp32/src/Configuration/Generator.h
index c3b38b6c..73e5ff31 100644
--- a/Grbl_Esp32/src/Configuration/Generator.h
+++ b/Grbl_Esp32/src/Configuration/Generator.h
@@ -1,51 +1,63 @@
 #pragma once
 
-#include <string>
 #include <vector>
 
 #include "../Pin.h"
+#include "../StringRange.h"
+#include "../StringStream.h"
+#include "HandlerBase.h"
 
 namespace Configuration
 {
     class Configurable;
 
-    class Generator
+    class Generator : public HandlerBase
     {
         Generator(const Generator&) = delete;
         Generator& operator=(const Generator&) = delete;
 
-        std::vector<char> config_;
         int indent_;
-
-        inline void addStr(const char* text) {
-            for (auto it = text; *it; ++it)
-            {
-                config_.push_back(*it);
-            }
-        }
+        SimpleOutputStream& dst_;
+        bool lastIsNewline_ = false;
 
         inline void indent() {
+            lastIsNewline_ = false;
             for (int i = 0; i < indent_ * 2; ++i)
             {
-                config_.push_back(' ');
+                dst_ << ' ';
             }
         }
 
-    public:
-        Generator() = default;
-
         void enter(const char* name);
-        void add(const char* key, const std::string& value);
-        void add(const char* key, const char* value);
-        void add(const char* key, bool value);
-        void add(const char* key, int value);
-        void add(const char* key, double value);
-        void add(const char* key, Pin value);
         void add(Configuration::Configurable* configurable);
         void leave();
+        
+    protected:
+        void handleDetail(const char* name, Configurable* value) override;
+        bool matchesUninitialized(const char* name) override { return false; }
+        HandlerType handlerType() override { return HandlerType::Generator; }
 
-        inline std::string str() const {
-            return std::string(config_.begin(), config_.end());
+    public:
+        Generator(SimpleOutputStream& dst) : indent_(0), dst_(dst) {}
+
+        void handle(const char* name, int& value) override {
+            indent();
+            dst_ << name << ": " << value << '\n';
+        }
+
+        void handle(const char* name, double& value) override {
+            indent();
+            dst_ << name << ": " << value << '\n';
+        }
+
+        void handle(const char* name, StringRange value) override {
+            indent();
+            dst_ << name << ": " << value << '\n';
+        }
+
+        void handle(const char* name, Pin& value) override {
+            indent();
+            dst_ << name << ": " << value << '\n';
         }
     };
-}
\ No newline at end of file
+}
diff --git a/Grbl_Esp32/src/Configuration/GenericFactory.h b/Grbl_Esp32/src/Configuration/GenericFactory.h
index ebe6e5f2..fe2fb31a 100644
--- a/Grbl_Esp32/src/Configuration/GenericFactory.h
+++ b/Grbl_Esp32/src/Configuration/GenericFactory.h
@@ -1,8 +1,9 @@
 #pragma once
 
-#include <string>
 #include <vector>
 
+#include "HandlerBase.h"
+
 namespace Configuration {
     template <typename BaseType>
     class GenericFactory
@@ -26,11 +27,8 @@ namespace Configuration {
             BuilderBase(const BuilderBase& o) = delete;
             BuilderBase& operator=(const BuilderBase& o) = delete;
 
-            virtual BaseType* create(Configuration::Parser& parser) const = 0;
-
-            inline bool matches(const char* name) {
-                return !strcmp(name, name_);
-            }
+            virtual BaseType* create() const = 0;
+            const char* name() const { return name_; }
 
             virtual ~BuilderBase() = default;
         };
@@ -51,25 +49,29 @@ namespace Configuration {
                 instance().registerBuilder(this);
             }
 
-            BaseType* create(Configuration::Parser& parser) const override
+            BaseType* create() const override
             {
-                return new DerivedType(parser);
+                return new DerivedType();
             }
         };
 
-        static const BuilderBase* find(const char* name) {
-            for (auto it : instance().builders_)
+        static void handle(Configuration::HandlerBase& handler, BaseType*& inst)
+        {
+            if (inst == nullptr)
             {
-                if (it->matches(name))
-                {
-                    return it;
+                for (auto it : instance().builders_) {
+                    if (handler.matchesUninitialized(it->name())) {
+                        inst = it->create();
+                        handler.handle(it->name(), *inst);
+
+                        return;
+                    }
                 }
             }
-            return nullptr;
-        }
-
-        inline static const BuilderBase* find(const std::string& name) {
-            return find(name.c_str());
+            else 
+            {
+                handler.handleDetail(inst->name(), inst);
+            }
         }
     };
 }
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/HandlerBase.h b/Grbl_Esp32/src/Configuration/HandlerBase.h
new file mode 100644
index 00000000..54f88da7
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/HandlerBase.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include "HandlerType.h"
+#include "../Pin.h"
+#include "../StringRange.h"
+
+namespace Configuration
+{
+    class Configurable;
+
+    template <typename BaseType>
+    class GenericFactory;
+
+    class HandlerBase
+    {
+    protected:
+        virtual void handleDetail(const char* name, Configurable* value) = 0;
+        virtual bool matchesUninitialized(const char* name) = 0;
+
+        template <typename BaseType>
+        friend class GenericFactory;
+
+    public:
+        virtual void handle(const char* name, bool& value) = 0;
+        virtual void handle(const char* name, int& value) = 0;
+        virtual void handle(const char* name, double& value) = 0;
+        virtual void handle(const char* name, StringRange value) = 0;
+        virtual void handle(const char* name, Pin& value) = 0;
+
+        virtual HandlerType handlerType() = 0;
+
+        template <typename T>
+        void handle(const char* name, T*& value) {
+            if (handlerType() == HandlerType::Parser)
+            {
+                if (value == nullptr && matchesUninitialized(name))
+                {
+                    value = new T();
+                    handleDetail(name, value);
+                }
+            }
+            else {
+                if (value != nullptr) {
+                    handleDetail(name, value);
+                }
+            }
+        }
+
+        template <typename T>
+        void handle(const char* name, T& value) { handleDetail(name, &value); }
+    };
+}
diff --git a/Grbl_Esp32/src/Configuration/HandlerType.h b/Grbl_Esp32/src/Configuration/HandlerType.h
new file mode 100644
index 00000000..2514f3c4
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/HandlerType.h
@@ -0,0 +1,12 @@
+#pragma once
+
+namespace Configuration
+{
+    enum struct HandlerType
+    {
+        Parser,
+        Runtime,
+        Generator,
+        Validator
+    };
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/LegacySettingHandler.h b/Grbl_Esp32/src/Configuration/LegacySettingHandler.h
new file mode 100644
index 00000000..a5fbf94f
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/LegacySettingHandler.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include "LegacySettingRegistry.h"
+
+namespace Configuration
+{
+    class LegacySettingHandler
+    {
+    public:
+        inline LegacySettingHandler() {
+            LegacySettingRegistry::registerHandler(this);
+        }
+
+        LegacySettingHandler(const LegacySettingHandler&) = delete;
+        LegacySettingHandler(LegacySettingHandler&&) = delete;
+        LegacySettingHandler& operator=(const LegacySettingHandler&) = delete;
+        LegacySettingHandler& operator=(LegacySettingHandler&&) = delete;
+
+        virtual int index() = 0;
+        virtual void handle(const char* value) = 0;
+        virtual ~LegacySettingHandler() {
+            // Remove from factory? We shouldn't remove handlers...
+        }
+    };
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/LegacySettingRegistry.cpp b/Grbl_Esp32/src/Configuration/LegacySettingRegistry.cpp
new file mode 100644
index 00000000..66c5e8cf
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/LegacySettingRegistry.cpp
@@ -0,0 +1,59 @@
+#include "LegacySettingRegistry.h"
+
+#include "LegacySettingHandler.h"
+
+namespace Configuration 
+{
+    bool LegacySettingRegistry::isLegacySetting(const char* str)
+    {
+        return str[0] == '$' && (str[1] >= '0' && str[1] <= '9');
+    }
+
+    void LegacySettingRegistry::registerHandler(LegacySettingHandler* handler)
+    {
+        instance().handlers_.push_back(handler);
+    }
+
+    bool LegacySettingRegistry::tryHandleLegacy(const char* str) {
+        if (isLegacySetting(str)) {
+            auto start = str;
+
+            int value = 0;
+            ++str;
+
+            while (*str && *str >= '0' && *str <= '9')
+            {
+                value = value * 10 + (*str - '0');
+                ++str;
+            }
+
+            if (*str == '=') {
+                ++str;
+
+                handleLegacy(value, str);
+            }
+            else {
+                warn("Incorrect setting '" << start << "': cannot find '='.");
+            }
+            return true;
+        }
+        else {
+            return false;
+        }
+    }
+
+    void LegacySettingRegistry::handleLegacy(int index, const char* value) {
+        bool handled = false;
+        for (auto it : instance().handlers_) {
+            if (it->index() == index)
+            {
+                handled = true;
+                it->handle(value);
+            }
+        }
+
+        if (!handled) {
+            warn("Cannot find handler for $" << index << ". Setting was ignored.");
+        }
+    }
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/LegacySettingRegistry.h b/Grbl_Esp32/src/Configuration/LegacySettingRegistry.h
new file mode 100644
index 00000000..2b0500b3
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/LegacySettingRegistry.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <vector>
+#include "../Logging.h"
+
+namespace Configuration {
+    class LegacySettingHandler;
+
+    class LegacySettingRegistry
+    {
+        static LegacySettingRegistry& instance() {
+            static LegacySettingRegistry instance_;
+            return instance_;
+        }
+
+        LegacySettingRegistry() = default;
+
+        LegacySettingRegistry(const LegacySettingRegistry&) = delete;
+        LegacySettingRegistry& operator=(const LegacySettingRegistry&) = delete;
+
+        std::vector<LegacySettingHandler*> handlers_;
+
+        static bool isLegacySetting(const char* str);
+        static void handleLegacy(int index, const char* value);
+
+    public:
+        static void registerHandler(LegacySettingHandler* handler);
+        static bool tryHandleLegacy(const char* str);
+    };
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/Parser.cpp b/Grbl_Esp32/src/Configuration/Parser.cpp
index c3e397ef..98477374 100644
--- a/Grbl_Esp32/src/Configuration/Parser.cpp
+++ b/Grbl_Esp32/src/Configuration/Parser.cpp
@@ -2,6 +2,8 @@
 
 #include "ParseException.h"
 
+#include <climits>
+
 namespace Configuration {
     Parser::Parser(const char* start, const char* end) : Tokenizer(start, end), current_() {
         Tokenize();
@@ -77,18 +79,18 @@ namespace Configuration {
         if (last == token_.indent_) {
             // Yes, the token continues where we left off:
             current_ = token_;
-            // Tokenize(); --> No need, this is handled by MoveNext!
+            Tokenize();
         } else {
             current_         = TokenData();
             current_.indent_ = last;
         }
     }
 
-    std::string Parser::stringValue() const {
+    StringRange Parser::stringValue() const {
         if (current_.kind_ != TokenKind::String) {
             parseError("Expected a string value (e.g. 'foo')");
         }
-        return std::string(current_.sValueStart_, current_.sValueEnd_);
+        return StringRange(current_.sValueStart_, current_.sValueEnd_);
     }
     
     bool Parser::boolValue() const {
@@ -99,14 +101,14 @@ namespace Configuration {
     }
     
     int Parser::intValue() const {
-        if (current_.kind_ != TokenKind::Boolean) {
+        if (current_.kind_ != TokenKind::IntegerValue) {
             parseError("Expected an integer value (e.g. 123456)");
         }
         return current_.iValue_;
     }
     
-    double Parser::floatValue() const {
-        if (current_.kind_ != TokenKind::Boolean) {
+    double Parser::doubleValue() const {
+        if (current_.kind_ != TokenKind::FloatingPoint) {
             parseError("Expected a float value (e.g. 123.456)");
         }
         return current_.fValue_;
@@ -117,7 +119,8 @@ namespace Configuration {
         if (current_.kind_ != TokenKind::String) {
             parseError("Expected a string value (e.g. 'foo')");
         }
-        return Pin(std::string(current_.sValueStart_, current_.sValueEnd_));
+
+        return Pin::create(StringRange(current_.sValueStart_, current_.sValueEnd_));
     }
 
 }
diff --git a/Grbl_Esp32/src/Configuration/Parser.h b/Grbl_Esp32/src/Configuration/Parser.h
index 765acf3f..f68e2b8b 100644
--- a/Grbl_Esp32/src/Configuration/Parser.h
+++ b/Grbl_Esp32/src/Configuration/Parser.h
@@ -2,6 +2,7 @@
 
 #include "Tokenizer.h"
 #include "../Pin.h"
+#include "../StringRange.h"
 
 #include <stack>
 #include <cstring>
@@ -38,15 +39,16 @@ namespace Configuration {
         void leave();
 
         inline bool is(const char* expected) const {
-            return !strncmp(expected, current_.keyStart_, size_t(current_.keyEnd_ - current_.keyStart_));
+            return current_.keyStart_ != nullptr && 
+                   !strncmp(expected, current_.keyStart_, size_t(current_.keyEnd_ - current_.keyStart_));
         }
 
-        inline std::string key() const { return std::string(current_.keyStart_, current_.keyEnd_); }
+        inline StringRange key() const { return StringRange(current_.keyStart_, current_.keyEnd_); }
 
-        std::string stringValue() const;
+        StringRange stringValue() const;
         bool boolValue() const;
         int intValue() const;
-        double floatValue() const;
+        double doubleValue() const;
         Pin pinValue() const;
     };
 }
diff --git a/Grbl_Esp32/src/Configuration/ParserHandler.h b/Grbl_Esp32/src/Configuration/ParserHandler.h
new file mode 100644
index 00000000..0bb42ab4
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/ParserHandler.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include "HandlerBase.h"
+#include "Parser.h"
+#include "Configurable.h"
+
+#include "../Logging.h"
+
+namespace Configuration
+{
+    class ParserHandler : public Configuration::HandlerBase
+    {
+    private:
+        Configuration::Parser& parser_;
+
+    protected:
+        void handleDetail(const char* name, Configuration::Configurable* value) override {
+            if (value != nullptr && parser_.is(name)) {
+                debug("Parsing configurable " << name);
+
+                parser_.enter();
+                for (; !parser_.isEndSection(); parser_.moveNext()) {
+                    value->handle(*this);
+                }
+                parser_.leave();
+            }
+        }
+
+        bool matchesUninitialized(const char* name) override {
+            return parser_.is(name);
+        }
+
+    public:
+        ParserHandler(Configuration::Parser& parser) : parser_(parser) {}
+
+        void handle(const char* name, int& value) override {
+            if (parser_.is(name)) { value = parser_.intValue(); }
+        }
+
+        void handle(const char* name, double& value) override {
+            if (parser_.is(name)) { value = parser_.doubleValue(); }
+        }
+
+        void handle(const char* name, StringRange value) override {
+            if (parser_.is(name)) { value = parser_.stringValue(); }
+        }
+
+        void handle(const char* name, Pin& value) override {
+            if (parser_.is(name)) { value = parser_.pinValue(); }
+        }
+
+        HandlerType handlerType() override { return HandlerType::Parser; }
+    };
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/RuntimeSetting.cpp b/Grbl_Esp32/src/Configuration/RuntimeSetting.cpp
new file mode 100644
index 00000000..025cb03e
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/RuntimeSetting.cpp
@@ -0,0 +1,65 @@
+#include "RuntimeSetting.h"
+
+#include <cstdlib>
+
+namespace Configuration
+{
+    void RuntimeSetting::handleDetail(const char* name, Configuration::Configurable* value)
+    {
+        if (is(name) && this->value() == nullptr)
+        {
+            auto previous = start_;
+
+            // Figure out next node
+            auto next = start_;
+            for (; *next && *next != '=' && *next != '/'; ++next)
+            {
+            }
+            
+            // Do we have a child?
+            if (*next == '/') {
+                ++next;
+                start_ = next;
+
+                // Handle child:
+                value->handle(*this);
+            }
+
+            // Restore situation:
+            start_ = previous;
+        }
+    }
+
+    void RuntimeSetting::handle(const char* name, int& value)
+    {
+        if (is(name) && this->value() != nullptr)
+        {
+            value = atoi(this->value());
+        }
+    }
+
+    void RuntimeSetting::handle(const char* name, double& value)
+    {
+        if (is(name) && this->value() != nullptr)
+        {
+            char* floatEnd;
+            value = strtod(this->value(), &floatEnd);
+        }
+    }
+
+    void RuntimeSetting::handle(const char* name, StringRange value)
+    {
+        if (is(name) && this->value() != nullptr)
+        {
+            value = this->value();
+        }
+    }
+
+    void RuntimeSetting::handle(const char* name, Pin& value)
+    {
+        if (is(name) && this->value() != nullptr)
+        {
+            value = Pin::create(StringRange(this->value()));
+        }
+    }
+}
diff --git a/Grbl_Esp32/src/Configuration/RuntimeSetting.h b/Grbl_Esp32/src/Configuration/RuntimeSetting.h
new file mode 100644
index 00000000..3b112184
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/RuntimeSetting.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "HandlerBase.h"
+#include "Configurable.h"
+
+namespace Configuration
+{
+    class RuntimeSetting : public Configuration::HandlerBase
+    {
+    private:
+        const char* setting_; // $foo/bar=12
+        const char* start_;
+
+        bool is(const char* name) const {
+            if (start_ != nullptr) {
+                auto len = strlen(name);
+                return !strncmp(name, start_, len) && (start_[len] == '=' || start_[len] == '/');
+            }
+            else {
+                return false;
+            }
+        }
+
+        const char* value() const
+        {
+            for (const char* it = start_; *it; ++it)
+            {
+                if (*it == '/') { return nullptr; }
+                else if (*it == '=') { return it + 1; }
+            }
+            return nullptr;
+        }
+
+    protected:
+        void handleDetail(const char* name, Configuration::Configurable* value) override;
+
+        bool matchesUninitialized(const char* name) override { return false; }
+
+    public:
+        RuntimeSetting(const char* runtimeSetting) : setting_(runtimeSetting + 1), start_(runtimeSetting+1) {}
+
+        void handle(const char* name, int& value) override;
+        void handle(const char* name, double& value) override;
+        void handle(const char* name, StringRange value) override;
+        void handle(const char* name, Pin& value) override;
+
+        HandlerType handlerType() override { return HandlerType::Runtime; }
+    };
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/Tokenizer.cpp b/Grbl_Esp32/src/Configuration/Tokenizer.cpp
index 141f8fa7..f04483f1 100644
--- a/Grbl_Esp32/src/Configuration/Tokenizer.cpp
+++ b/Grbl_Esp32/src/Configuration/Tokenizer.cpp
@@ -1,11 +1,12 @@
 #include "Tokenizer.h"
 
 #include "ParseException.h"
+
 #include <cstdlib>
 
 namespace Configuration {
 
-    Tokenizer::Tokenizer(const char* start, const char* end) : start_(start), current_(start), end_(end), token_() {
+    Tokenizer::Tokenizer(const char* start, const char* end) :  current_(start), end_(end), start_(start), token_() {
         // If start is a yaml document start ('---' [newline]), skip that first.
         if (EqualsCaseInsensitive("---")) {
             for (int i = 0; i < 3; ++i) {
diff --git a/Grbl_Esp32/src/Configuration/Tokenizer.h b/Grbl_Esp32/src/Configuration/Tokenizer.h
index 335915cb..f8696ad4 100644
--- a/Grbl_Esp32/src/Configuration/Tokenizer.h
+++ b/Grbl_Esp32/src/Configuration/Tokenizer.h
@@ -2,8 +2,6 @@
 
 #include "TokenKind.h"
 
-#include <string>
-
 namespace Configuration {
 
     class Tokenizer {
diff --git a/Grbl_Esp32/src/Configuration/Validator.cpp b/Grbl_Esp32/src/Configuration/Validator.cpp
new file mode 100644
index 00000000..b78f80d3
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/Validator.cpp
@@ -0,0 +1,13 @@
+#include "Validator.h"
+
+#include "Configurable.h"
+
+#include <cstring>
+
+namespace Configuration
+{
+    void Validator::handleDetail(const char* name, Configurable* value) {
+        value->validate();
+        value->handle(*this);
+    }
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Configuration/Validator.h b/Grbl_Esp32/src/Configuration/Validator.h
new file mode 100644
index 00000000..afa3591a
--- /dev/null
+++ b/Grbl_Esp32/src/Configuration/Validator.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <vector>
+
+#include "../Pin.h"
+#include "HandlerBase.h"
+
+namespace Configuration
+{
+    class Configurable;
+
+    class Validator : public HandlerBase
+    {
+        Validator(const Validator&) = delete;
+        Validator& operator=(const Validator&) = delete;
+
+    protected:
+        void handleDetail(const char* name, Configurable* value) override;
+        bool matchesUninitialized(const char* name) override { return false; }
+        HandlerType handlerType() override { return HandlerType::Validator; }
+
+    public:
+        Validator() = default;
+
+        void handle(const char* name, int& value) override { }
+        void handle(const char* name, double& value) override { }
+        void handle(const char* name, StringRange value) override { }
+        void handle(const char* name, Pin& value) override { }
+    };
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Logging.cpp b/Grbl_Esp32/src/Logging.cpp
new file mode 100644
index 00000000..a1debcae
--- /dev/null
+++ b/Grbl_Esp32/src/Logging.cpp
@@ -0,0 +1,29 @@
+#include "Logging.h"
+
+#ifndef ESP32
+
+#include <iostream>
+
+DebugStream::DebugStream(const char* name) {
+    std::cout << '[' << name << ": ";
+}
+void DebugStream::add(char c)
+{
+    std::cout << c;
+}
+
+DebugStream::~DebugStream() { std::cout << ']' << std::endl; }
+
+#else
+
+DebugStream::DebugStream(const char* name) {
+    Serial.print("[");
+    Serial.print(name);
+    Serial.print(": ");
+}
+
+void DebugStream::add(char c) { Serial.print(c); }
+
+DebugStream::~DebugStream() { Serial.println("]"); }
+
+#endif
diff --git a/Grbl_Esp32/src/Logging.h b/Grbl_Esp32/src/Logging.h
new file mode 100644
index 00000000..0c33acc6
--- /dev/null
+++ b/Grbl_Esp32/src/Logging.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "SimpleOutputStream.h"
+
+class DebugStream : public SimpleOutputStream
+{
+public:
+    DebugStream(const char* name);
+    void add(char c) override;
+    ~DebugStream();
+};
+
+#include "StringStream.h"
+
+#define debug(x) { DebugStream ss("DBG "); ss << x; }
+#define info(x)  { DebugStream ss("INFO"); ss << x; }
+#define warn(x)  { DebugStream ss("WARN"); ss << x; }
+#define error(x) { DebugStream ss("ERR "); ss << x; }
+
diff --git a/Grbl_Esp32/src/MachineConfig.cpp b/Grbl_Esp32/src/MachineConfig.cpp
new file mode 100644
index 00000000..96dc79f4
--- /dev/null
+++ b/Grbl_Esp32/src/MachineConfig.cpp
@@ -0,0 +1,114 @@
+#include "MachineConfig.h"
+
+#include "Motors/Motor.h"
+
+// Configuration system helpers:
+void Axis::validate() const  {
+    Assert(motor_ != nullptr, "Motor should be defined when an axis is defined.");
+}
+
+void Axis::handle(Configuration::HandlerBase& handler)  {
+    Motors::MotorFactory::handle(handler, motor_);
+}
+
+// Checks if a motor matches this axis:
+bool Axis::hasMotor(const Motors::Motor* const motor) const {
+    return motor_ == motor;
+}
+
+Axis::~Axis() {
+    delete motor_;
+}
+
+Axes::Axes() : axis_()
+{
+    for (int i = 0; i < MAX_NUMBER_AXIS; ++i)
+    {
+        for (int j = 0; j <= MAX_NUMBER_GANGED; ++j)
+        {
+            axis_[i][j] = nullptr;
+        }
+    }
+}
+
+// Some small helpers to find the axis index and axis ganged index for a given motor. This 
+// is helpful for some motors that need this info, as well as debug information.
+size_t Axes::findAxisIndex(const Motors::Motor* const motor) const {
+    for (int i = 0; i < MAX_NUMBER_AXIS; ++i)
+    {
+        for (int j = 0; j <= MAX_NUMBER_GANGED; ++j)
+        {
+            if (axis_[i][j] != nullptr && axis_[i][j]->hasMotor(motor)) {
+                return i;
+            }
+        }
+    }
+
+    Assert(false, "Cannot find axis for motor. Something wonky is going on here..."); 
+    return SIZE_MAX;
+}
+
+size_t Axes::findAxisGanged(const Motors::Motor* const motor) const {
+    for (int i = 0; i < MAX_NUMBER_AXIS; ++i)
+    {
+        for (int j = 0; j <= MAX_NUMBER_GANGED; ++j)
+        {
+            if (axis_[i][j] != nullptr && axis_[i][j]->hasMotor(motor)) {
+                return j;
+            }
+        }
+    }
+
+    Assert(false, "Cannot find axis for motor. Something wonky is going on here...");
+    return SIZE_MAX;
+}
+
+// Configuration helpers:
+void Axes::validate() const { }
+
+void Axes::handle(Configuration::HandlerBase& handler) {
+    const char* allAxis = "xyzabc";
+
+    char tmp[3];
+    tmp[2] = '\0';
+
+    for (size_t a = 0; a < MAX_NUMBER_AXIS; ++a)
+    {
+        tmp[0] = allAxis[a];
+        tmp[1] = '\0';
+
+        if (handler.handlerType() == Configuration::HandlerType::Runtime ||
+            handler.handlerType() == Configuration::HandlerType::Parser)
+        {
+            // 'x' is a shorthand for 'x1', so we don't generate it.
+            handler.handle(tmp, axis_[a][1]);
+        }
+
+        for (size_t g = 1; g <= MAX_NUMBER_GANGED; ++g)
+        {
+            tmp[1] = char('0' + g);
+            handler.handle(tmp, axis_[a][g]);
+        }
+    }
+}
+
+Axes::~Axes() {
+    for (int i = 0; i < MAX_NUMBER_AXIS; ++i)
+    {
+        for (int j = 0; j <= MAX_NUMBER_GANGED; ++j)
+        {
+            delete axis_[i][j];
+        }
+    }
+}
+
+
+void MachineConfig::validate() const  { }
+
+void MachineConfig::handle(Configuration::HandlerBase& handler)  {
+    handler.handle("axes", axes_);
+}
+
+MachineConfig::~MachineConfig() {
+    delete axes_;
+}
diff --git a/Grbl_Esp32/src/MachineConfig.h b/Grbl_Esp32/src/MachineConfig.h
new file mode 100644
index 00000000..fcd87cfd
--- /dev/null
+++ b/Grbl_Esp32/src/MachineConfig.h
@@ -0,0 +1,65 @@
+#pragma once
+
+#include "Assert.h"
+#include "Configuration/GenericFactory.h"
+#include "Configuration/HandlerBase.h"
+#include "Configuration/Configurable.h"
+
+namespace Motors {
+    class Motor;
+}
+
+class Axis : public Configuration::Configurable {
+    Motors::Motor* motor_ = nullptr;
+
+public:
+    Axis() = default;
+
+    // Configuration system helpers:
+    void validate() const override;
+    void handle(Configuration::HandlerBase& handler) override;
+
+    // Checks if a motor matches this axis:
+    bool hasMotor(const Motors::Motor* const motor) const;
+
+    ~Axis();
+};
+
+class Axes : public Configuration::Configurable {
+    static const int MAX_NUMBER_AXIS   = 6;
+    static const int MAX_NUMBER_GANGED = 2;
+
+    Axis* axis_[MAX_NUMBER_AXIS][MAX_NUMBER_GANGED + 1];
+
+public:
+    Axes();
+
+    // Some small helpers to find the axis index and axis ganged index for a given motor. This
+    // is helpful for some motors that need this info, as well as debug information.
+    size_t findAxisIndex(const Motors::Motor* const motor) const;
+
+    size_t findAxisGanged(const Motors::Motor* const motor) const;
+
+    // Configuration helpers:
+    void validate() const override;
+    void handle(Configuration::HandlerBase& handler) override;
+
+    ~Axes();
+};
+
+class MachineConfig : public Configuration::Configurable {
+public:
+    Axes* axes_ = nullptr;
+
+    MachineConfig() = default;
+
+    static MachineConfig*& instance() {
+        static MachineConfig* instance = nullptr;
+        return instance;
+    }
+
+    void validate() const override;
+    void handle(Configuration::HandlerBase& handler) override;
+
+    ~MachineConfig();
+};
diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp
index 2326323d..530d57f6 100644
--- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp
+++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp
@@ -30,18 +30,11 @@ namespace Motors {
     bool    Motors::Dynamixel2::uart_ready         = false;
     uint8_t Motors::Dynamixel2::ids[MAX_N_AXIS][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } };
 
-    Dynamixel2::Dynamixel2(uint8_t axis_index, uint8_t id, Pin tx_pin, Pin rx_pin, Pin rts_pin) :
-        Servo(axis_index), _id(id), _tx_pin(tx_pin), _rx_pin(rx_pin), _rts_pin(rts_pin) {
-        if (_tx_pin == Pin::UNDEFINED || _rx_pin == Pin::UNDEFINED || _rts_pin == Pin::UNDEFINED) {
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dynamixel Error. Missing pin definitions");
-            _has_errors = true;
-        } else {
-            _has_errors = false;  // The motor can be used
-        }
-    }
-
     void Dynamixel2::init() {
-        init_uart(_id, _axis_index, _dual_axis_index);  // static and only allows one init
+        _has_errors = false; // let's start with the assumption we're good.
+        _axis_index = axis_index();
+
+        init_uart(_id, axis_index(), dual_axis_index());  // static and only allows one init
 
         read_settings();
 
@@ -67,7 +60,7 @@ namespace Motors {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
                        "%s Dynamixel Servo ID:%d Count(%5.0f,%5.0f) %s",
-                       reportAxisNameMsg(_axis_index, _dual_axis_index),
+                       reportAxisNameMsg(axis_index(), dual_axis_index()),
                        _id,
                        _dxl_count_min,
                        _dxl_count_max,
@@ -89,14 +82,14 @@ namespace Motors {
                 grbl_msg_sendf(CLIENT_SERIAL,
                                MsgLevel::Info,
                                "%s Dynamixel Detected ID %d Model XL430-W250 F/W Rev %x",
-                               reportAxisNameMsg(_axis_index, _dual_axis_index),
+                               reportAxisNameMsg(axis_index(), dual_axis_index()),
                                _id,
                                _dxl_rx_message[11]);
             } else {
                 grbl_msg_sendf(CLIENT_SERIAL,
                                MsgLevel::Info,
                                "%s Dynamixel Detected ID %d M/N %d F/W Rev %x",
-                               reportAxisNameMsg(_axis_index, _dual_axis_index),
+                               reportAxisNameMsg(axis_index(), dual_axis_index()),
                                _id,
                                model_num,
                                _dxl_rx_message[11]);
@@ -104,7 +97,7 @@ namespace Motors {
 
         } else {
             grbl_msg_sendf(
-                CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index), _id);
+                CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(axis_index(), dual_axis_index()), _id);
             return false;
         }
 
@@ -452,4 +445,11 @@ namespace Motors {
 
         return crc_accum;
     }
+
+    // Configuration registration
+    namespace 
+    {
+        MotorFactory::InstanceBuilder<Dynamixel2> registration("dynamixel2");
+    }
 }
+
diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.h b/Grbl_Esp32/src/Motors/Dynamixel2.h
index e6592706..7e567f08 100644
--- a/Grbl_Esp32/src/Motors/Dynamixel2.h
+++ b/Grbl_Esp32/src/Motors/Dynamixel2.h
@@ -73,19 +73,6 @@ const int DXL_CONTROL_MODE_POSITION = 3;
 
 namespace Motors {
     class Dynamixel2 : public Servo {
-    public:
-        Dynamixel2(uint8_t axis_index, uint8_t address, Pin tx_pin, Pin rx_pin, Pin rts_pin);
-
-        // Overrides for inherited methods
-        void init() override;
-        void read_settings() override;
-        bool set_homing_mode(bool isHoming) override;
-        void set_disable(bool disable) override;
-        void update() override;
-
-        static bool    uart_ready;
-        static uint8_t ids[MAX_N_AXIS][2];
-
     protected:
         void config_message() override;
 
@@ -118,8 +105,43 @@ namespace Motors {
         Pin         _rx_pin;
         Pin         _rts_pin;
         uart_port_t _uart_num;
+        int         _axis_index;
 
         bool _disabled;
         bool _has_errors;
+
+    public:
+        Dynamixel2() : _id(255), _disabled(true), _has_errors(true) {}
+
+        // Overrides for inherited methods
+        void init() override;
+        void read_settings() override;
+        bool set_homing_mode(bool isHoming) override;
+        void set_disable(bool disable) override;
+        void update() override;
+
+        static bool    uart_ready;
+        static uint8_t ids[MAX_N_AXIS][2];
+
+        // Configuration handlers:
+        void validate() const override {
+            Assert(!_tx_pin.undefined(), "TX pin should be configured.");
+            Assert(!_rx_pin.undefined(), "RX pin should be configured.");
+            Assert(!_rts_pin.undefined(), "RTS pin should be configured.");
+            Assert(_id != 255, "Dynamixel ID should be configured.");
+        }
+
+        void handle(Configuration::HandlerBase& handler) override {
+            handler.handle("tx", _tx_pin);
+            handler.handle("rx", _rx_pin);
+            handler.handle("rts", _rts_pin);
+            
+            int id = _id;
+            handler.handle("id", id);
+            _id = id;
+        }
+
+        // Name of the configurable. Must match the name registered in the cpp file.
+        const char* name() const override { return "dynamixel2"; }
     };
 }
diff --git a/Grbl_Esp32/src/Motors/Motor.cpp b/Grbl_Esp32/src/Motors/Motor.cpp
index 195601f8..f3bf022b 100644
--- a/Grbl_Esp32/src/Motors/Motor.cpp
+++ b/Grbl_Esp32/src/Motors/Motor.cpp
@@ -32,13 +32,22 @@
 */
 
 #include "Motor.h"
+#include "../MachineConfig.h"
 
 namespace Motors {
-    Motor::Motor(uint8_t axis_index) :
-        _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {}
-
     void Motor::debug_message() {}
 
     bool Motor::test() { return true; };  // true = OK
 
+    uint8_t Motor::axis_index() const {
+        Assert(MachineConfig::instance() != nullptr &&
+            MachineConfig::instance()->axes_ != nullptr, "Expected machine to be configured before this is called.");
+        return MachineConfig::instance()->axes_->findAxisIndex(this);
+    }
+    uint8_t Motor::dual_axis_index() const {
+        Assert(MachineConfig::instance() != nullptr &&
+            MachineConfig::instance()->axes_ != nullptr, "Expected machine to be configured before this is called.");
+        return MachineConfig::instance()->axes_->findAxisGanged(this);
+    }
+
 }
diff --git a/Grbl_Esp32/src/Motors/Motor.h b/Grbl_Esp32/src/Motors/Motor.h
index c9c0300b..be2f7a65 100644
--- a/Grbl_Esp32/src/Motors/Motor.h
+++ b/Grbl_Esp32/src/Motors/Motor.h
@@ -1,5 +1,11 @@
 #pragma once
 
+
+#include "Assert.h"
+#include "../Configuration/GenericFactory.h"
+#include "../Configuration/HandlerBase.h"
+#include "../Configuration/Configurable.h"
+
 /*
     Motor.h
     Header file for Motor Classes
@@ -34,9 +40,9 @@
 #include <cstdint>
 
 namespace Motors {
-    class Motor {
+    class Motor : public Configuration::Configurable {
     public:
-        Motor(uint8_t axis_index);
+        Motor() = default;
 
         // init() establishes configured motor parameters.  It is called after
         // all motor objects have been constructed.
@@ -94,6 +100,12 @@ namespace Motors {
         // called from a periodic task.
         virtual void update() {}
 
+        // Name is required for the configuration factory to work.
+        virtual const char* name() const = 0;
+
+        // Virtual base classes require a virtual destructor.
+        virtual ~Motor() {}
+
     protected:
         // config_message(), called from init(), displays a message describing
         // the motor configuration - pins and other motor-specific items
@@ -111,7 +123,9 @@ namespace Motors {
         //   tables can be indexed by these variables.
         // TODO Architecture: It might be useful to cache a
         // reference to the axis settings entry.
-        uint8_t _axis_index;       // X_AXIS, etc
-        uint8_t _dual_axis_index;  // 0 = primary 1=ganged
+        uint8_t axis_index() const;       // X_AXIS, etc
+        uint8_t dual_axis_index() const;  // 0 = primary 1=ganged, etc
     };
+
+    using MotorFactory = Configuration::GenericFactory<Motor>;
 }
diff --git a/Grbl_Esp32/src/Motors/Motors.h b/Grbl_Esp32/src/Motors/Motors.h
index a6451eff..339419df 100644
--- a/Grbl_Esp32/src/Motors/Motors.h
+++ b/Grbl_Esp32/src/Motors/Motors.h
@@ -33,8 +33,6 @@
 
 // These are used for setup and to talk to the motors as a group.
 void    init_motors();
-uint8_t get_next_trinamic_driver_index();
-void    readSgTask(void* pvParameters);
 void    motors_read_settings();
 
 // The return value is a bitmask of axes that can home
@@ -42,5 +40,3 @@ uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming);
 void    motors_set_disable(bool disable);
 void    motors_step(uint8_t step_mask, uint8_t dir_mask);
 void    motors_unstep();
-
-void servoUpdateTask(void* pvParameters);
diff --git a/Grbl_Esp32/src/Motors/NullMotor.cpp b/Grbl_Esp32/src/Motors/NullMotor.cpp
index be5b34a2..99c31ab0 100644
--- a/Grbl_Esp32/src/Motors/NullMotor.cpp
+++ b/Grbl_Esp32/src/Motors/NullMotor.cpp
@@ -22,7 +22,9 @@
 #include "NullMotor.h"
 
 namespace Motors {
-   Nullmotor::Nullmotor(uint8_t axis_index) :
-       Motor(axis_index)
-   {}
+    // Configuration registration
+    namespace
+    {
+        MotorFactory::InstanceBuilder<Nullmotor> registration("null_motor");
+    }
 }
diff --git a/Grbl_Esp32/src/Motors/NullMotor.h b/Grbl_Esp32/src/Motors/NullMotor.h
index 6e859855..9e393897 100644
--- a/Grbl_Esp32/src/Motors/NullMotor.h
+++ b/Grbl_Esp32/src/Motors/NullMotor.h
@@ -5,7 +5,14 @@
 namespace Motors {
     class Nullmotor : public Motor {
     public:
-        Nullmotor(uint8_t axis_index);
+        Nullmotor() = default;
+
         bool set_homing_mode(bool isHoming) { return false; }
+
+        // Configuration handlers:
+        void validate() const override { }
+        void handle(Configuration::HandlerBase& handler) override { }
+
+        const char* name() const override { return "null_motor"; }
     };
 }
diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp
index ebdca33f..40ace5a5 100644
--- a/Grbl_Esp32/src/Motors/RcServo.cpp
+++ b/Grbl_Esp32/src/Motors/RcServo.cpp
@@ -31,9 +31,11 @@
 #include "RcServo.h"
 
 namespace Motors {
-    RcServo::RcServo(uint8_t axis_index, Pin pwm_pin) : Servo(axis_index), _pwm_pin(pwm_pin) {}
+    // RcServo::RcServo(Pin pwm_pin) : Servo(), _pwm_pin(pwm_pin) {}
 
     void RcServo::init() {
+        _axis_index = axis_index();
+
         // TODO FIXME: This is leaking if init() is called multiple times.
         char* setting_cal_min = (char*)malloc(20);
         snprintf(setting_cal_min, 20, "%c/RcServo/Cal/Min", report_get_axis_letter(_axis_index));
@@ -59,11 +61,11 @@ namespace Motors {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
                        "%s RC Servo Pin:%d Pulse Len(%.0f,%.0f) %s",
-                       reportAxisNameMsg(_axis_index, _dual_axis_index),
+                       reportAxisNameMsg(axis_index(), dual_axis_index()),
                        _pwm_pin,
                        _pwm_pulse_min,
                        _pwm_pulse_max,
-                       reportAxisLimitsMsg(_axis_index));
+                       reportAxisLimitsMsg(axis_index()));
     }
 
     void RcServo::_write_pwm(uint32_t duty) {
@@ -135,4 +137,10 @@ namespace Motors {
             swap(_pwm_pulse_min, _pwm_pulse_max);
         }
     }
+
+    // Configuration registration
+    namespace
+    {
+        MotorFactory::InstanceBuilder<RcServo> registration("rc_servo");
+    }
 }
diff --git a/Grbl_Esp32/src/Motors/RcServo.h b/Grbl_Esp32/src/Motors/RcServo.h
index 17b9ba0b..45767240 100644
--- a/Grbl_Esp32/src/Motors/RcServo.h
+++ b/Grbl_Esp32/src/Motors/RcServo.h
@@ -26,18 +26,6 @@
 
 namespace Motors {
     class RcServo : public Servo {
-    public:
-        RcServo(uint8_t axis_index, Pin pwm_pin);
-
-        // Overrides for inherited methods
-        void init() override;
-        void read_settings() override;
-        bool set_homing_mode(bool isHoming) override;
-        void set_disable(bool disable) override;
-        void update() override;
-
-        void _write_pwm(uint32_t duty);
-
     protected:
         void config_message() override;
 
@@ -56,5 +44,31 @@ namespace Motors {
 
         FloatSetting* rc_servo_cal_min;
         FloatSetting* rc_servo_cal_max;
+
+        int _axis_index = -1;
+
+    public:
+        RcServo() = default;
+
+        // Overrides for inherited methods
+        void init() override;
+        void read_settings() override;
+        bool set_homing_mode(bool isHoming) override;
+        void set_disable(bool disable) override;
+        void update() override;
+
+        void _write_pwm(uint32_t duty);
+
+        // Configuration handlers:
+        void validate() const override {
+            Assert(!_pwm_pin.undefined(), "PWM pin should be configured.");
+        }
+
+        void handle(Configuration::HandlerBase& handler) override {
+            handler.handle("pwm", _pwm_pin);
+        }
+
+        // Name of the configurable. Must match the name registered in the cpp file.
+        const char* name() const override { return "rc_servo"; }
     };
 }
diff --git a/Grbl_Esp32/src/Motors/Servo.cpp b/Grbl_Esp32/src/Motors/Servo.cpp
index 65e213a1..335c4db4 100644
--- a/Grbl_Esp32/src/Motors/Servo.cpp
+++ b/Grbl_Esp32/src/Motors/Servo.cpp
@@ -34,7 +34,7 @@
 namespace Motors {
     Servo* Servo::List = NULL;
 
-    Servo::Servo(uint8_t axis_index) : Motor(axis_index) {
+    Servo::Servo() : Motor() {
         link = List;
         List = this;
     }
@@ -70,5 +70,4 @@ namespace Motors {
             reportTaskStackSize(uxHighWaterMark);
         }
     }
-
 }
diff --git a/Grbl_Esp32/src/Motors/Servo.h b/Grbl_Esp32/src/Motors/Servo.h
index 09b6e35c..5656dbea 100644
--- a/Grbl_Esp32/src/Motors/Servo.h
+++ b/Grbl_Esp32/src/Motors/Servo.h
@@ -28,7 +28,7 @@
 namespace Motors {
     class Servo : public Motor {
     public:
-        Servo(uint8_t axis_index);
+        Servo();
 #if 0
         // Overrides for inherited methods
         void init() override;
diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp
index c7bd7553..aa618fab 100644
--- a/Grbl_Esp32/src/Motors/StandardStepper.cpp
+++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp
@@ -39,17 +39,15 @@ namespace Motors {
         return rmt_channel_t(next_RMT_chan_num);
     }
 
-    StandardStepper::StandardStepper(uint8_t axis_index, Pin step_pin, Pin dir_pin, Pin disable_pin) :
-        Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) {}
-
     void StandardStepper::init() {
         init_step_dir_pins();
         config_message();
     }
 
     void StandardStepper::init_step_dir_pins() {
-        _invert_step_pin = bitnum_istrue(step_invert_mask->get(), _axis_index);
-        _invert_dir_pin  = bitnum_istrue(dir_invert_mask->get(), _axis_index);
+        auto axisIndex = axis_index();
+        _invert_step_pin = bitnum_istrue(step_invert_mask->get(), axisIndex);
+        _invert_dir_pin  = bitnum_istrue(dir_invert_mask->get(), axisIndex);
         _dir_pin.setAttr(Pin::Attr::Output);
 
 #ifdef USE_RMT_STEPS
@@ -100,11 +98,11 @@ namespace Motors {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
                        "%s Standard Stepper Step:%s Dir:%s Disable:%s %s",
-                       reportAxisNameMsg(_axis_index, _dual_axis_index),
+                       reportAxisNameMsg(axis_index(), dual_axis_index()),
                        _step_pin.name().c_str(),
                        _dir_pin.name().c_str(),
                        _disable_pin.name().c_str(),
-                       reportAxisLimitsMsg(_axis_index));
+                       reportAxisLimitsMsg(axis_index()));
     }
 
     void StandardStepper::step() {
@@ -125,4 +123,10 @@ namespace Motors {
     void StandardStepper::set_direction(bool dir) { _dir_pin.write(dir ^ _invert_dir_pin); }
 
     void StandardStepper::set_disable(bool disable) { _disable_pin.write(disable); }
+
+    // Configuration registration
+    namespace
+    {
+        MotorFactory::InstanceBuilder<StandardStepper> registration("standard_stepper");
+    }
 }
diff --git a/Grbl_Esp32/src/Motors/StandardStepper.h b/Grbl_Esp32/src/Motors/StandardStepper.h
index 62458747..ae0a9fa5 100644
--- a/Grbl_Esp32/src/Motors/StandardStepper.h
+++ b/Grbl_Esp32/src/Motors/StandardStepper.h
@@ -5,7 +5,9 @@
 namespace Motors {
     class StandardStepper : public Motor {
     public:
-        StandardStepper(uint8_t axis_index, Pin step_pin, Pin dir_pin, Pin disable_pin);
+        //StandardStepper(uint8_t axis_index, Pin step_pin, Pin dir_pin, Pin disable_pin);
+
+        StandardStepper() = default;
 
         // Overrides for inherited methods
         void init() override;
@@ -31,6 +33,21 @@ namespace Motors {
         Pin  _dir_pin;
         Pin  _disable_pin;
 
+        // Configuration handlers:
+        void validate() const override {
+            Assert(!_step_pin.undefined(), "Step pin should be configured.");
+            Assert(!_dir_pin.undefined(), "Direction pin should be configured.");
+        }
+
+        void handle(Configuration::HandlerBase& handler) override {
+            handler.handle("step", _step_pin);
+            handler.handle("direction", _dir_pin);
+            handler.handle("disable", _disable_pin);
+        }
+
+        // Name of the configurable. Must match the name registered in the cpp file.
+        const char* name() const override { return "standard_stepper"; }
+
     private:
         static rmt_channel_t get_next_RMT_chan_num();
         static rmt_item32_t  rmtItem[2];
diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.h b/Grbl_Esp32/src/Motors/TrinamicDriver.h
index 75a1cd01..fcfe378d 100644
--- a/Grbl_Esp32/src/Motors/TrinamicDriver.h
+++ b/Grbl_Esp32/src/Motors/TrinamicDriver.h
@@ -70,27 +70,6 @@ namespace Motors {
     };
 
     class TrinamicDriver : public StandardStepper {
-    public:
-        TrinamicDriver(uint8_t axis_index, Pin step_pin, Pin dir_pin, Pin disable_pin, Pin cs_pin, uint16_t driver_part_number, float r_sense) :
-            TrinamicDriver(axis_index, step_pin, dir_pin, disable_pin, cs_pin, driver_part_number, r_sense, get_next_index()) {}
-
-        TrinamicDriver(uint8_t  axis_index,
-                       Pin      step_pin,
-                       Pin      dir_pin,
-                       Pin      disable_pin,
-                       Pin      cs_pin,
-                       uint16_t driver_part_number,
-                       float    r_sense,
-                       int8_t   spi_index);
-
-        // Overrides for inherited methods
-        void init() override;
-        void read_settings() override;
-        bool set_homing_mode(bool ishoming) override;
-        void set_disable(bool disable) override;
-
-        void debug_message();
-
     private:
         uint32_t calc_tstep(float speed, float percent);
 
@@ -124,5 +103,27 @@ namespace Motors {
 
     protected:
         void config_message() override;
+
+    public:
+        TrinamicDriver(uint8_t axis_index, Pin step_pin, Pin dir_pin, Pin disable_pin, Pin cs_pin, uint16_t driver_part_number, float r_sense) :
+            TrinamicDriver(axis_index, step_pin, dir_pin, disable_pin, cs_pin, driver_part_number, r_sense, get_next_index()) {}
+
+        TrinamicDriver(uint8_t  axis_index,
+                       Pin      step_pin,
+                       Pin      dir_pin,
+                       Pin      disable_pin,
+                       Pin      cs_pin,
+                       uint16_t driver_part_number,
+                       float    r_sense,
+                       int8_t   spi_index);
+
+        // Overrides for inherited methods
+        void init() override;
+        void read_settings() override;
+        bool set_homing_mode(bool ishoming) override;
+        void set_disable(bool disable) override;
+
+        void debug_message();
+
     };
 }
diff --git a/Grbl_Esp32/src/Motors/UnipolarMotor.cpp b/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
index 4869ffae..86e9aee2 100644
--- a/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
+++ b/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
@@ -1,12 +1,6 @@
 #include "UnipolarMotor.h"
 
 namespace Motors {
-    UnipolarMotor::UnipolarMotor(uint8_t axis_index, Pin pin_phase0, Pin pin_phase1, Pin pin_phase2, Pin pin_phase3) :
-        Motor(axis_index), _pin_phase0(pin_phase0), _pin_phase1(pin_phase1), _pin_phase2(pin_phase2), _pin_phase3(pin_phase3),
-
-        _half_step(true)  // TODO read from settings ... microstep > 1 = half step
-    {}
-
     void UnipolarMotor::init() {
         _pin_phase0.setAttr(Pin::Attr::Output);
         _pin_phase1.setAttr(Pin::Attr::Output);
@@ -20,12 +14,12 @@ namespace Motors {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
                        "%s Unipolar Stepper Ph0:%s Ph1:%s Ph2:%s Ph3:%s %s",
-                       reportAxisNameMsg(_axis_index, _dual_axis_index),
+                       reportAxisNameMsg(axis_index(), dual_axis_index()),
                        _pin_phase0.name().c_str(),
                        _pin_phase1.name().c_str(),
                        _pin_phase2.name().c_str(),
                        _pin_phase3.name().c_str(),
-                       reportAxisLimitsMsg(_axis_index));
+                       reportAxisLimitsMsg(axis_index()));
     }
 
     void UnipolarMotor::set_disable(bool disable) {
@@ -126,4 +120,10 @@ namespace Motors {
         _pin_phase2.write(_phase[2]);
         _pin_phase3.write(_phase[3]);
     }
+
+    // Configuration registration
+    namespace
+    {
+        MotorFactory::InstanceBuilder<UnipolarMotor> registration("unipolar");
+    }
 }
diff --git a/Grbl_Esp32/src/Motors/UnipolarMotor.h b/Grbl_Esp32/src/Motors/UnipolarMotor.h
index afb24aea..9c0afc9b 100644
--- a/Grbl_Esp32/src/Motors/UnipolarMotor.h
+++ b/Grbl_Esp32/src/Motors/UnipolarMotor.h
@@ -5,7 +5,7 @@
 namespace Motors {
     class UnipolarMotor : public Motor {
     public:
-        UnipolarMotor(uint8_t axis_index, Pin pin_phase0, Pin pin_phase1, Pin pin_phase2, Pin pin_phase3);
+        UnipolarMotor() = default;
 
         // Overrides for inherited methods
         void init() override;
@@ -14,15 +14,34 @@ namespace Motors {
         void set_direction(bool) override;
         void step() override;
 
+        // Configuration handlers:
+        void validate() const override {
+            Assert(!_pin_phase0.undefined(), "Phase 0 pin should be configured.");
+            Assert(!_pin_phase1.undefined(), "Phase 1 pin should be configured.");
+            Assert(!_pin_phase2.undefined(), "Phase 2 pin should be configured.");
+            Assert(!_pin_phase3.undefined(), "Phase 3 pin should be configured.");
+        }
+
+        void handle(Configuration::HandlerBase& handler) override {
+            handler.handle("phase0", _pin_phase0);
+            handler.handle("phase1", _pin_phase1);
+            handler.handle("phase2", _pin_phase2);
+            handler.handle("phase3", _pin_phase3);
+            handler.handle("half_step", _half_step);
+        }
+
+        // Name of the configurable. Must match the name registered in the cpp file.
+        const char* name() const override { return "unipolar"; }
+
     private:
         Pin     _pin_phase0;
         Pin     _pin_phase1;
         Pin     _pin_phase2;
         Pin     _pin_phase3;
-        uint8_t _current_phase;
-        bool    _half_step;
-        bool    _enabled;
-        bool    _dir;
+        uint8_t _current_phase = 0;
+        bool    _half_step = true;
+        bool    _enabled = false;
+        bool    _dir = true;
 
     protected:
         void config_message() override;
diff --git a/Grbl_Esp32/src/Pin.cpp b/Grbl_Esp32/src/Pin.cpp
index 60ad03d7..8591293e 100644
--- a/Grbl_Esp32/src/Pin.cpp
+++ b/Grbl_Esp32/src/Pin.cpp
@@ -14,7 +14,9 @@
 #    include "Grbl.h"  // grbl_sendf
 #endif
 
-bool Pin::parse(String str, Pins::PinDetail*& pinImplementation) {
+bool Pin::parse(StringRange tmp, Pins::PinDetail*& pinImplementation) {
+    String str = tmp.str();
+
     // Initialize pinImplementation first! Callers might want to delete it, and we don't want a random pointer.
     pinImplementation = nullptr;
 
@@ -115,6 +117,10 @@ bool Pin::parse(String str, Pins::PinDetail*& pinImplementation) {
 }
 
 Pin Pin::create(const String& str) {
+    return create(StringRange(str));
+}
+
+Pin Pin::create(const StringRange& str) {
     Pins::PinDetail* pinImplementation = nullptr;
     try {
 #if defined PIN_DEBUG && defined ESP32
diff --git a/Grbl_Esp32/src/Pin.h b/Grbl_Esp32/src/Pin.h
index 7d54e3fe..2e75d248 100644
--- a/Grbl_Esp32/src/Pin.h
+++ b/Grbl_Esp32/src/Pin.h
@@ -4,6 +4,7 @@
 #include "Pins/PinDetail.h"
 #include "Pins/PinCapabilities.h"
 #include "Pins/PinAttributes.h"
+#include "StringRange.h"
 
 #include <Arduino.h>  // for IRAM_ATTR
 #include <cstdint>
@@ -37,7 +38,7 @@ class Pin {
 
     inline Pin(uint8_t index) : _index(index) {}
 
-    static bool parse(String str, Pins::PinDetail*& detail);
+    static bool parse(StringRange str, Pins::PinDetail*& detail);
 
 public:
     using Capabilities = Pins::PinCapabilities;
@@ -49,6 +50,7 @@ public:
     static const bool On  = true;
     static const bool Off = false;
 
+    static Pin  create(const StringRange& str);
     static Pin  create(const String& str);
     static bool validate(const String& str);
 
@@ -64,6 +66,8 @@ public:
     inline bool operator==(const Pin& o) const { return _index == o._index; }
     inline bool operator!=(const Pin& o) const { return _index != o._index; }
 
+    inline bool undefined() const { return (*this) == UNDEFINED; }
+
     inline uint8_t getNative(Capabilities expectedBehavior) const {
         auto detail = Pins::PinLookup::_instance.GetPin(_index);
         Assert(detail->capabilities().has(expectedBehavior), "Requested pin does not have the expected behavior.");
diff --git a/Grbl_Esp32/src/SimpleOutputStream.cpp b/Grbl_Esp32/src/SimpleOutputStream.cpp
new file mode 100644
index 00000000..2c8bc1b9
--- /dev/null
+++ b/Grbl_Esp32/src/SimpleOutputStream.cpp
@@ -0,0 +1,55 @@
+#include "SimpleOutputStream.h"
+
+#include <cstring>
+
+char* SimpleOutputStream::intToBuf(int value, char* dst)
+{
+#ifdef ESP32
+    return itoa(value, dst, 10);
+#else 
+    _itoa_s(value, dst, 10, 10);
+    return dst + strlen(dst);
+#endif
+}
+
+void SimpleOutputStream::add(const char* s) {
+    for (; *s; ++s) { add(*s); }
+}
+
+void SimpleOutputStream::add(int value) {
+    char buf[10];
+    intToBuf(value, buf);
+    add(buf);
+}
+
+void SimpleOutputStream::add(double value, int numberDigits, int precision)
+{
+    if (isnan(value)) {
+        add("NaN");
+    }
+    else if (isinf(value)) {
+        add("Inf");
+    }
+
+    char buf[30]; // that's already quite big
+    char fmt[10];
+    fmt[0] = '%';
+    fmt[1] = '0';
+
+    char* next = intToBuf(numberDigits, fmt + 2);
+    *next++ = '.';
+    intToBuf(precision, next);
+
+    snprintf(buf, sizeof(buf) - 1, fmt, value);
+    add(buf);
+}
+
+void SimpleOutputStream::add(StringRange range)
+{
+    for (auto ch : range) { add(ch); }
+}
+
+void SimpleOutputStream::add(const Pin& pin)
+{
+    add(pin.str());
+}
diff --git a/Grbl_Esp32/src/SimpleOutputStream.h b/Grbl_Esp32/src/SimpleOutputStream.h
new file mode 100644
index 00000000..5e3d0541
--- /dev/null
+++ b/Grbl_Esp32/src/SimpleOutputStream.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <cstring>
+
+#include "StringRange.h"
+#include "Pin.h"
+
+class SimpleOutputStream
+{
+    static char* intToBuf(int value, char* dst);
+
+public:
+    SimpleOutputStream() = default;
+
+    SimpleOutputStream(const SimpleOutputStream& o) = delete;
+    SimpleOutputStream(SimpleOutputStream&& o) = delete;
+
+    SimpleOutputStream& operator=(const SimpleOutputStream& o) = delete;
+    SimpleOutputStream& operator=(SimpleOutputStream&& o) = delete;
+
+    virtual void add(char c) = 0;
+    virtual void flush() {}
+
+    void add(const char* s);
+    void add(int value);
+    void add(double value, int numberDigits, int precision);
+    void add(StringRange range);
+    void add(const Pin& pin);
+
+    virtual ~SimpleOutputStream() {
+    }
+};
+
+inline SimpleOutputStream& operator<<(SimpleOutputStream& lhs, char c) {
+    lhs.add(c);
+    return lhs;
+}
+
+inline SimpleOutputStream& operator<<(SimpleOutputStream& lhs, const char* v) {
+    lhs.add(v);
+    return lhs;
+}
+
+inline SimpleOutputStream& operator<<(SimpleOutputStream& lhs, int v) {
+    lhs.add(v);
+    return lhs;
+}
+
+inline SimpleOutputStream& operator<<(SimpleOutputStream& lhs, double v) {
+    lhs.add(v, 4, 3);
+    return lhs;
+}
+
+inline SimpleOutputStream& operator<<(SimpleOutputStream& lhs, StringRange v) {
+    lhs.add(v);
+    return lhs;
+}
+
+inline SimpleOutputStream& operator<<(SimpleOutputStream& lhs, const Pin& v) {
+    lhs.add(v);
+    return lhs;
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/StringRange.h b/Grbl_Esp32/src/StringRange.h
new file mode 100644
index 00000000..cb41770c
--- /dev/null
+++ b/Grbl_Esp32/src/StringRange.h
@@ -0,0 +1,80 @@
+#pragma once
+
+#include <cstring>
+
+#ifndef ESP32
+#    include <string>
+#else
+#    include "WString.h"
+#endif
+
+class StringRange {
+    const char* start_;
+    const char* end_;
+
+public:
+    StringRange() : start_(nullptr), end_(nullptr) {}
+
+    StringRange(const char* str) : start_(str), end_(str + strlen(str)) {}
+
+    StringRange(const char* start, const char* end) : start_(start), end_(end) {}
+
+    StringRange(const StringRange& o) = default;
+    StringRange(StringRange&& o)      = default;
+
+    StringRange(const String& str) : StringRange(str.begin(), str.end()) {}
+
+    StringRange& operator=(const StringRange& o) = default;
+    StringRange& operator=(StringRange&& o) = default;
+
+    int find(char c) const {
+        const char* s = start_;
+        for (; s != end_ && *s != c; ++s) {}
+        return (*s) ? (s - start_) : -1;
+    }
+
+    StringRange subString(int index, int length) const {
+        const char* s = start_ + index;
+        if (s > end_) {
+            s = end_;
+        }
+        const char* e = s + length;
+        if (e > end_) {
+            e = end_;
+        }
+        return StringRange(s, e);
+    }
+
+    bool equals(const StringRange& o) const {
+        auto l = length();
+        return l == o.length() && !strncmp(start_, o.start_, l);
+    }
+
+    bool equals(const char* o) const {
+        const char* c  = start_;
+        const char* oc = o;
+        for (; *c != '\0' && *oc != '\0' && *c == *oc; ++c, ++oc) {}
+        return c == end_ && *oc == '\0';
+    }
+
+    int length() const { return end_ - start_; }
+
+    // Iterator support:
+    const char* begin() const { return start_; }
+    const char* end() const { return end_; }
+
+#ifndef ESP32
+    std::string str() const { return std::string(begin(), end()); }
+#else
+    String str() const {
+        // TODO: Check if we can eliminate this function. I'm pretty sure we can.
+        auto len = length();
+        char* buf = new char[len + 1];
+        memcpy(buf, begin(), len);
+        buf[len] = 0;
+        String tmp(buf);
+        delete[] buf;
+        return tmp;
+    }
+#endif
+};
diff --git a/Grbl_Esp32/src/StringStream.h b/Grbl_Esp32/src/StringStream.h
new file mode 100644
index 00000000..62630d45
--- /dev/null
+++ b/Grbl_Esp32/src/StringStream.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "SimpleOutputStream.h"
+#include "StringRange.h"
+
+#include <vector>
+
+class StringStream : public SimpleOutputStream
+{
+    std::vector<char> data_;
+
+public: 
+    void add(char c) override {
+        data_.push_back(c);
+    }
+
+    StringRange str() const {
+        return StringRange(data_.data(), data_.data() + data_.size());
+    }
+};
\ No newline at end of file