diff --git a/huanyang.c b/huanyang.c
index c5c180f..c2a9b21 100644
--- a/huanyang.c
+++ b/huanyang.c
@@ -27,7 +27,7 @@
 #include "driver.h"
 #endif
 
-#if VFD_ENABLE == 1 || VFD_ENABLE == 2
+#if VFD_ENABLE == 1 || VFD_ENABLE == 2 || VFD_ENABLE == -1
 
 #include <math.h>
 #include <string.h>
@@ -46,10 +46,6 @@
 
 #include "modbus.h"
 
-#ifdef SPINDLE_PWM_DIRECT
-#error "Uncomment SPINDLE_RPM_CONTROLLED in grbl/config.h to add Huanyang spindle support!"
-#endif
-
 #ifndef VFD_ADDRESS
 #define VFD_ADDRESS 0x01
 #endif
@@ -64,6 +60,8 @@ typedef enum {
     VFD_SetStatus
 } vfd_response_t;
 
+static spindle_id_t v1_spindle_id = -1, v2_spindle_id = -1;
+static bool v1_active = false,  v2_active = false;
 static float rpm_programmed = -1.0f;
 static spindle_state_t vfd_state = {0};
 static spindle_data_t spindle_data = {0};
@@ -71,36 +69,25 @@ static on_report_options_ptr on_report_options;
 static on_spindle_select_ptr on_spindle_select;
 static driver_reset_ptr driver_reset;
 static uint32_t rpm_max = 0;
-#if VFD_ENABLE == 1
-static float rpm_max50 = 3000;
-#endif
 
-static void rx_packet (modbus_message_t *msg);
 static void rx_exception (uint8_t code, void *context);
+static spindle_data_t *spindleGetData (spindle_data_request_t request);
+
+#if VFD_ENABLE == 1 || VFD_ENABLE == -1
+
+static float rpm_max50 = 3000;
 
-static const modbus_callbacks_t callbacks = {
-    .on_rx_packet = rx_packet,
+static void v1_rx_packet (modbus_message_t *msg);
+
+static const modbus_callbacks_t v1_callbacks = {
+    .on_rx_packet = v1_rx_packet,
     .on_rx_exception = rx_exception
 };
 
 // Read maximum configured RPM from spindle, value is used later for calculating current RPM
 // In the case of the original Huanyang protocol, the value is the configured RPM at 50Hz
-static void spindleGetMaxRPM (void)
+static void v1_spindleGetMaxRPM (void)
 {
-#if VFD_ENABLE == 2
-    modbus_message_t cmd = {
-        .context = (void *)VFD_GetMaxRPM,
-        .adu[0] = VFD_ADDRESS,
-        .adu[1] = ModBus_ReadHoldingRegisters,
-        .adu[2] = 0xB0,
-        .adu[3] = 0x05,
-        .adu[4] = 0x00,
-        .adu[5] = 0x02,
-        .tx_length = 8,
-        .rx_length = 8
-    };
-    modbus_send(&cmd, &callbacks, true);
-#else
     modbus_message_t cmd = {
         .context = (void *)VFD_GetMaxRPM50,
         .adu[0] = VFD_ADDRESS,
@@ -112,33 +99,14 @@ static void spindleGetMaxRPM (void)
         .tx_length = 8,
         .rx_length = 8
     };
-    modbus_send(&cmd, &callbacks, true);
-#endif
+
+    modbus_send(&cmd, &v1_callbacks, true);
 }
 
-static void spindleSetRPM (float rpm, bool block)
+static void v1_spindleSetRPM (float rpm, bool block)
 {
-
     if (rpm != rpm_programmed) {
 
-#if VFD_ENABLE == 2
-
-        uint16_t data = (uint32_t)(rpm) * 10000UL / rpm_max;
-
-        modbus_message_t rpm_cmd = {
-            .context = (void *)VFD_SetRPM,
-            .crc_check = false,
-            .adu[0] = VFD_ADDRESS,
-            .adu[1] = ModBus_WriteRegister,
-            .adu[2] = 0x10,
-            .adu[4] = data >> 8,
-            .adu[5] = data & 0xFF,
-            .tx_length = 8,
-            .rx_length = 8
-        };
-
-#else
-
         uint32_t data = lroundf(rpm * 5000.0f / (float)rpm_max50); // send Hz * 10  (Ex:1500 RPM = 25Hz .... Send 2500)
 
         modbus_message_t rpm_cmd = {
@@ -153,11 +121,9 @@ static void spindleSetRPM (float rpm, bool block)
             .rx_length = 6
         };
 
-#endif
-
         vfd_state.at_speed = false;
 
-        modbus_send(&rpm_cmd, &callbacks, block);
+        modbus_send(&rpm_cmd, &v1_callbacks, block);
 
         if(settings.spindle.at_speed_tolerance > 0.0f) {
             spindle_data.rpm_low_limit = rpm / (1.0f + settings.spindle.at_speed_tolerance);
@@ -167,29 +133,14 @@ static void spindleSetRPM (float rpm, bool block)
     }
 }
 
-static void spindleUpdateRPM (float rpm)
+static void v1_spindleUpdateRPM (float rpm)
 {
-    spindleSetRPM(rpm, false);
+    v1_spindleSetRPM(rpm, false);
 }
 
 // Start or stop spindle
-static void spindleSetState (spindle_state_t state, float rpm)
+static void v1_spindleSetState (spindle_state_t state, float rpm)
 {
-#if VFD_ENABLE == 2
-
-    modbus_message_t mode_cmd = {
-        .context = (void *)VFD_SetStatus,
-        .crc_check = false,
-        .adu[0] = VFD_ADDRESS,
-        .adu[1] = ModBus_WriteRegister,
-        .adu[2] = 0x20,
-        .adu[5] = (!state.on || rpm == 0.0f) ? 6 : (state.ccw ? 2 : 1),
-        .tx_length = 8,
-        .rx_length = 8
-    };
-
-#else
-
     modbus_message_t mode_cmd = {
         .context = (void *)VFD_SetStatus,
         .crc_check = false,
@@ -201,58 +152,192 @@ static void spindleSetState (spindle_state_t state, float rpm)
         .rx_length = 6
     };
 
-#endif
-
     if(vfd_state.ccw != state.ccw)
         rpm_programmed = 0.0f;
 
     vfd_state.on = state.on;
     vfd_state.ccw = state.ccw;
 
-    if(modbus_send(&mode_cmd, &callbacks, true))
-        spindleSetRPM(rpm, true);
+    if(modbus_send(&mode_cmd, &v1_callbacks, true))
+        v1_spindleSetRPM(rpm, true);
 }
 
-static spindle_data_t *spindleGetData (spindle_data_request_t request)
+static spindle_data_t *v1_spindleGetData (spindle_data_request_t request)
 {
     return &spindle_data;
 }
 
 // Returns spindle state in a spindle_state_t variable
-static spindle_state_t spindleGetState (void)
+static spindle_state_t v1_spindleGetState (void)
 {
-
-#if VFD_ENABLE == 2
-
     modbus_message_t mode_cmd = {
         .context = (void *)VFD_GetRPM,
         .crc_check = false,
         .adu[0] = VFD_ADDRESS,
+        .adu[1] = ModBus_ReadInputRegisters,
+        .adu[2] = 0x03,
+        .adu[3] = 0x01,
+        .tx_length = 8,
+        .rx_length = 8
+    };
+
+    modbus_send(&mode_cmd, &v1_callbacks, false); // TODO: add flag for not raising alarm?
+
+    // Get the actual RPM from spindle encoder input when available.
+    if(hal.spindle.get_data && hal.spindle.get_data != v1_spindleGetData) {
+        float rpm = hal.spindle.get_data(SpindleData_RPM)->rpm;
+        vfd_state.at_speed = settings.spindle.at_speed_tolerance <= 0.0f || (rpm >= spindle_data.rpm_low_limit && rpm <= spindle_data.rpm_high_limit);
+    }
+
+    return vfd_state; // return previous state as we do not want to wait for the response
+}
+
+static void v1_rx_packet (modbus_message_t *msg)
+{
+    if(!(msg->adu[0] & 0x80)) {
+
+        switch((vfd_response_t)msg->context) {
+
+            case VFD_GetRPM:
+                spindle_data.rpm = (float)((msg->adu[4] << 8) | msg->adu[5]) * (float)rpm_max50 / 5000.0f;
+                vfd_state.at_speed = settings.spindle.at_speed_tolerance <= 0.0f || (spindle_data.rpm >= spindle_data.rpm_low_limit && spindle_data.rpm <= spindle_data.rpm_high_limit);
+                break;
+
+            case VFD_GetMaxRPM:
+                rpm_max = (msg->adu[4] << 8) | msg->adu[5];
+                break;
+
+            case VFD_GetMaxRPM50:
+                rpm_max50 = (msg->adu[4] << 8) | msg->adu[5];
+                break;
+
+            default:
+                break;
+        }
+    }
+}
+
+bool v1_spindle_config (void)
+{
+    static bool init_ok = false;
+
+    if(!modbus_isup())
+        return false;
+
+    if(!init_ok) {
+        init_ok = true;
+        v1_spindleGetMaxRPM();
+    }
+
+    return true;
+}
+
+#endif // VFD_ENABLE == 1 || VFD_ENABLE == -1
+
+#if VFD_ENABLE == 2 || VFD_ENABLE == -1
+
+static void v2_rx_packet (modbus_message_t *msg);
+
+static const modbus_callbacks_t v2_callbacks = {
+    .on_rx_packet = v2_rx_packet,
+    .on_rx_exception = rx_exception
+};
+
+// Read maximum configured RPM from spindle, value is used later for calculating current RPM
+// In the case of the original Huanyang protocol, the value is the configured RPM at 50Hz
+static void v2_spindleGetMaxRPM (void)
+{
+    modbus_message_t cmd = {
+        .context = (void *)VFD_GetMaxRPM,
+        .adu[0] = VFD_ADDRESS,
         .adu[1] = ModBus_ReadHoldingRegisters,
-        .adu[2] = 0x70,
-        .adu[3] = 0x0C,
+        .adu[2] = 0xB0,
+        .adu[3] = 0x05,
         .adu[4] = 0x00,
         .adu[5] = 0x02,
         .tx_length = 8,
         .rx_length = 8
     };
 
-#else
+    modbus_send(&cmd, &v2_callbacks, true);
+}
+
+static void v2_spindleSetRPM (float rpm, bool block)
+{
+    if (rpm != rpm_programmed) {
+
+        uint16_t data = (uint32_t)(rpm) * 10000UL / rpm_max;
+
+        modbus_message_t rpm_cmd = {
+            .context = (void *)VFD_SetRPM,
+            .crc_check = false,
+            .adu[0] = VFD_ADDRESS,
+            .adu[1] = ModBus_WriteRegister,
+            .adu[2] = 0x10,
+            .adu[4] = data >> 8,
+            .adu[5] = data & 0xFF,
+            .tx_length = 8,
+            .rx_length = 8
+        };
+
+        vfd_state.at_speed = false;
+
+        modbus_send(&rpm_cmd, &v2_callbacks, block);
+
+        if(settings.spindle.at_speed_tolerance > 0.0f) {
+            spindle_data.rpm_low_limit = rpm / (1.0f + settings.spindle.at_speed_tolerance);
+            spindle_data.rpm_high_limit = rpm * (1.0f + settings.spindle.at_speed_tolerance);
+        }
+        rpm_programmed = rpm;
+    }
+}
+
+static void v2_spindleUpdateRPM (float rpm)
+{
+    v2_spindleSetRPM(rpm, false);
+}
 
+// Start or stop spindle
+static void v2_spindleSetState (spindle_state_t state, float rpm)
+{
     modbus_message_t mode_cmd = {
-        .context = (void *)VFD_GetRPM,
+        .context = (void *)VFD_SetStatus,
         .crc_check = false,
         .adu[0] = VFD_ADDRESS,
-        .adu[1] = ModBus_ReadInputRegisters,
-        .adu[2] = 0x03,
-        .adu[3] = 0x01,
+        .adu[1] = ModBus_WriteRegister,
+        .adu[2] = 0x20,
+        .adu[5] = (!state.on || rpm == 0.0f) ? 6 : (state.ccw ? 2 : 1),
         .tx_length = 8,
         .rx_length = 8
     };
 
-#endif
+    if(vfd_state.ccw != state.ccw)
+        rpm_programmed = 0.0f;
+
+    vfd_state.on = state.on;
+    vfd_state.ccw = state.ccw;
 
-    modbus_send(&mode_cmd, &callbacks, false); // TODO: add flag for not raising alarm?
+    if(modbus_send(&mode_cmd, &v2_callbacks, true))
+        v2_spindleSetRPM(rpm, true);
+}
+
+// Returns spindle state in a spindle_state_t variable
+static spindle_state_t v2_spindleGetState (void)
+{
+    modbus_message_t mode_cmd = {
+        .context = (void *)VFD_GetRPM,
+        .crc_check = false,
+        .adu[0] = VFD_ADDRESS,
+        .adu[1] = ModBus_ReadHoldingRegisters,
+        .adu[2] = 0x70,
+        .adu[3] = 0x0C,
+        .adu[4] = 0x00,
+        .adu[5] = 0x02,
+        .tx_length = 8,
+        .rx_length = 8
+    };
+
+    modbus_send(&mode_cmd, &v2_callbacks, false); // TODO: add flag for not raising alarm?
 
     // Get the actual RPM from spindle encoder input when available.
     if(hal.spindle.get_data && hal.spindle.get_data != spindleGetData) {
@@ -263,18 +348,15 @@ static spindle_state_t spindleGetState (void)
     return vfd_state; // return previous state as we do not want to wait for the response
 }
 
-static void rx_packet (modbus_message_t *msg)
+static void v2_rx_packet (modbus_message_t *msg)
 {
     if(!(msg->adu[0] & 0x80)) {
 
         switch((vfd_response_t)msg->context) {
 
             case VFD_GetRPM:
-#if VFD_ENABLE == 2
                 spindle_data.rpm = (float)((msg->adu[4] << 8) | msg->adu[5]);
-#else
                 spindle_data.rpm = (float)((msg->adu[4] << 8) | msg->adu[5]) * (float)rpm_max50 / 5000.0f;
-#endif
                 vfd_state.at_speed = settings.spindle.at_speed_tolerance <= 0.0f || (spindle_data.rpm >= spindle_data.rpm_low_limit && spindle_data.rpm <= spindle_data.rpm_high_limit);
                 break;
 
@@ -282,18 +364,34 @@ static void rx_packet (modbus_message_t *msg)
                 rpm_max = (msg->adu[4] << 8) | msg->adu[5];
                 break;
 
-#if VFD_ENABLE == 1
-            case VFD_GetMaxRPM50:
-                rpm_max50 = (msg->adu[4] << 8) | msg->adu[5];
-                break;
-#endif
-
             default:
                 break;
         }
     }
 }
 
+bool v2_spindle_config (void)
+{
+    static bool init_ok = false;
+
+    if(!modbus_isup())
+        return false;
+
+    if(!init_ok) {
+        init_ok = true;
+        v2_spindleGetMaxRPM();
+    }
+
+    return true;
+}
+
+#endif // VFD_ENABLE == 2 || VFD_ENABLE == -1
+
+static spindle_data_t *spindleGetData (spindle_data_request_t request)
+{
+    return &spindle_data;
+}
+
 static void raise_alarm (uint_fast16_t state)
 {
     system_raise_alarm(Alarm_Spindle);
@@ -314,10 +412,12 @@ static void onReportOptions (bool newopt)
     on_report_options(newopt);
 
     if(!newopt) {
-#if VFD_ENABLE == 2
-        hal.stream.write("[PLUGIN:HUANYANG VFD P2A v0.07]" ASCII_EOL);
-#else
+#if VFD_ENABLE == 1
         hal.stream.write("[PLUGIN:HUANYANG VFD v0.07]" ASCII_EOL);
+#elif VFD_ENABLE == 2
+        hal.stream.write("[PLUGIN:HUANYANG P2A VFD v0.07]" ASCII_EOL);
+#else
+        hal.stream.write("[PLUGIN:HUANYANG v1 VFD v0.07]" ASCII_EOL);
 #endif
     }
 }
@@ -325,77 +425,77 @@ static void onReportOptions (bool newopt)
 static void huanyang_reset (void)
 {
     driver_reset();
-    spindleGetMaxRPM();
+
+#if VFD_ENABLE == 1 || VFD_ENABLE == -1
+    if(v1_active)
+        v1_spindleGetMaxRPM();
+#endif
+#if VFD_ENABLE == 2 || VFD_ENABLE == -1
+    if(v2_active)
+        v2_spindleGetMaxRPM();
+#endif
 }
 
-bool huanyang_spindle_select (uint_fast8_t spindle_id)
+static bool huanyang_spindle_select (spindle_id_t spindle_id)
 {
-    static bool init_ok = false, vfd_active = false;
-    static driver_cap_t driver_cap;
-    static spindle_ptrs_t spindle_org;
-
-    if(vfd_active && spindle_id != 1 && spindle_org.set_state != NULL) {
+    if((v1_active = spindle_id == v1_spindle_id) || (v2_active = spindle_id == v2_spindle_id)) {
 
-        vfd_active = false;
-
-        gc_spindle_off();
+        if(settings.spindle.ppr == 0)
+            hal.spindle.get_data = spindleGetData;
 
-        hal.driver_cap = driver_cap;
-        memcpy(&hal.spindle, &spindle_org, sizeof(spindle_ptrs_t));
-    }
+    } else if(hal.spindle.get_data == spindleGetData)
+        hal.spindle.get_data = NULL;
 
     if(on_spindle_select && on_spindle_select(spindle_id))
         return true;
 
-    if(!modbus_isup())
-        return false;
-
-    if((vfd_active = spindle_id == 1)) {
-
-        if(hal.spindle.set_state != spindleSetState) {
-
-            if(spindle_org.set_state == NULL) {
-                driver_cap = hal.driver_cap;
-                memcpy(&spindle_org, &hal.spindle, sizeof(spindle_ptrs_t));
-            }
-
-            if(spindle_org.set_state)
-                gc_spindle_off();
-
-            hal.spindle.set_state = spindleSetState;
-            hal.spindle.get_state = spindleGetState;
-            hal.spindle.update_rpm = spindleUpdateRPM;
-            hal.spindle.reset_data = NULL;
-
-            hal.driver_cap.variable_spindle = On;
-            hal.driver_cap.spindle_at_speed = On;
-            hal.driver_cap.spindle_dir = On;
-        }
-
-        if(settings.spindle.ppr == 0)
-            hal.spindle.get_data = spindleGetData;
-
-        if(!init_ok) {
-            init_ok = true;
-            spindleGetMaxRPM();
-        }
-    }
-
     return true;
 }
 
-void vfd_init (void)
+void vfd_huanyang_init (void)
 {
+#if VFD_ENABLE == 1 || VFD_ENABLE == -1
+    static const spindle_ptrs_t v1_spindle = {
+        .cap.variable = On,
+        .cap.at_speed = On,
+        .cap.direction = On,
+        .config = v1_spindle_config,
+        .set_state = v1_spindleSetState,
+        .get_state = v1_spindleGetState,
+        .update_rpm = v1_spindleUpdateRPM
+    };
+#endif
+#if VFD_ENABLE == 2 || VFD_ENABLE == -1
+    static const spindle_ptrs_t v2_spindle = {
+        .cap.variable = On,
+        .cap.at_speed = On,
+        .cap.direction = On,
+        .config = v2_spindle_config,
+        .set_state = v2_spindleSetState,
+        .get_state = v2_spindleGetState,
+        .update_rpm = v2_spindleUpdateRPM
+    };
+#endif
+
     if(modbus_enabled()) {
 
-        on_spindle_select = grbl.on_spindle_select;
-        grbl.on_spindle_select = huanyang_spindle_select;
+#if VFD_ENABLE == 1 || VFD_ENABLE == -1
+        v1_spindle_id = spindle_register(&v1_spindle, "Huanyang v1");
+#endif
+#if VFD_ENABLE == 2 || VFD_ENABLE == -1
+        v2_spindle_id = spindle_register(&v2_spindle, "Huanyang P2A");
+#endif
+        if(v1_spindle_id != -1 || v2_spindle_id != -1) {
+
+            on_spindle_select = grbl.on_spindle_select;
+            grbl.on_spindle_select = huanyang_spindle_select;
 
-        on_report_options = grbl.on_report_options;
-        grbl.on_report_options = onReportOptions;
+            on_report_options = grbl.on_report_options;
+            grbl.on_report_options = onReportOptions;
 
-        driver_reset = hal.driver_reset;
-        hal.driver_reset = huanyang_reset;
+            driver_reset = hal.driver_reset;
+            hal.driver_reset = huanyang_reset;
+        }
     }
 }
 
diff --git a/modbus.c b/modbus.c
index 7bcca4b..34f90bc 100644
--- a/modbus.c
+++ b/modbus.c
@@ -4,7 +4,7 @@
 
   Part of grblHAL
 
-  Copyright (c) 2020-2021 Terje Io
+  Copyright (c) 2020-2022 Terje Io
   except modbus_CRC16x which is Copyright (c) 2006 Christian Walter <wolti@sil.at>
   Lifted from his FreeModbus Libary
 
@@ -439,7 +439,7 @@ static void onReportOptions (bool newopt)
     on_report_options(newopt);
 
     if(!newopt)
-        hal.stream.write("[PLUGIN:MODBUS v0.10]" ASCII_EOL);
+        hal.stream.write("[PLUGIN:MODBUS v0.12]" ASCII_EOL);
 }
 
 bool modbus_isup (void)
@@ -504,49 +504,13 @@ static bool claim_stream (io_stream_properties_t const *sstream)
                 hal.periph_port.set_pin_description(Output_TX, (pin_group_t)(PinGroup_UART + claimed->instance), "Modbus");
                 hal.periph_port.set_pin_description(Input_RX, (pin_group_t)(PinGroup_UART + claimed->instance), "Modbus");
             }
-        }
+        } else
+            claimed = NULL;
     }
 
     return claimed != NULL;
 }
 
-#if VFD_ENABLE
-
-// Dummy spindle, installed if modbus_init() fails.
-
-static settings_changed_ptr settings_changed;
-
-static void spindleUpdateRPM (float rpm)
-{
-}
-
-static void spindleSetState (spindle_state_t state, float rpm)
-{
-}
-
-static spindle_state_t spindleGetState (void)
-{
-    return (spindle_state_t){0};
-}
-
-static void _settings_changed (settings_t *settings)
-{
-    if(settings_changed)
-        settings_changed(settings);
-
-    if(hal.spindle.set_state == NULL) {
-        hal.spindle.set_state = spindleSetState;
-        hal.spindle.get_state = spindleGetState;
-        hal.spindle.update_rpm = spindleUpdateRPM;
-        hal.spindle.reset_data = NULL;
-        hal.driver_cap.variable_spindle = Off;
-        hal.driver_cap.spindle_at_speed = Off;
-        hal.driver_cap.spindle_dir = Off;
-    }
-}
-
-#endif
-
 void modbus_init (void)
 {
 
@@ -591,11 +555,6 @@ void modbus_init (void)
             queue[idx].next = idx == MODBUS_QUEUE_LENGTH - 1 ? &queue[0] : &queue[idx + 1];
 
     } else {
-
-#if VFD_ENABLE
-        settings_changed = hal.settings_changed;
-        hal.settings_changed = _settings_changed;
-#endif
         protocol_enqueue_rt_command(pos_failed);
         system_raise_alarm(Alarm_SelftestFailed);
     }
diff --git a/select.c b/select.c
index d243174..0f9c390 100644
--- a/select.c
+++ b/select.c
@@ -41,7 +41,7 @@ static status_code_t validate (parser_block_t *gc_block, parameter_words_t *depr
 {
     status_code_t state = Status_OK;
 
-    if(gc_block->user_mcode == Spindle_Select && grbl.on_spindle_select) {
+    if(gc_block->user_mcode == Spindle_Select) {
 
         if(!gc_block->words.p || isnanf(gc_block->values.p))
             state = Status_GcodeValueWordMissing;
@@ -60,14 +60,14 @@ static status_code_t validate (parser_block_t *gc_block, parameter_words_t *depr
 static void execute (sys_state_t state, parser_block_t *gc_block)
 {
     if(gc_block->user_mcode == Spindle_Select)
-        grbl.on_spindle_select((uint_fast8_t)(gc_block->values.p));
+        spindle_select((spindle_id_t)(gc_block->values.p == 0.0f ? 0 : settings.spindle.flags.type));
     else if(user_mcode.execute)
         user_mcode.execute(state, gc_block);
 }
 
 void spindle_select_init (void)
 {
-    if(grbl.on_spindle_select) {
+    if(spindle_get_count() > 1) {
 
         memcpy(&user_mcode, &hal.user_mcode, sizeof(user_mcode_ptrs_t));