From fb087fc535c531303580147b082b2d7fbdaa282a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Gu=C3=B0ni=20M=C3=A1r=20Gilbert?= <gudni.m.g@gmail.com>
Date: Sat, 27 Aug 2022 08:58:14 +0000
Subject: [PATCH] PFW-1386 Add M707 and M708

WriteRegister and ReadRegister function will create new compiler
warnings due to unused parameters, we can ignore it for now.
---
 Firmware/Marlin_main.cpp | 63 ++++++++++++++++++++++++++++++++++++++++
 Firmware/mmu2.cpp        | 10 +++++++
 Firmware/mmu2.h          | 13 +++++++++
 3 files changed, 86 insertions(+)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 04a94d7be..2d4a141eb 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -8654,6 +8654,69 @@ Sigma_Exit:
     }
     break;
 
+    /*!
+    ### M707 - Read from MMU register
+    #### Usage
+
+        M707 [ A | C ]
+
+        M707 A14 C2 - Read two bytes from address 0x14
+
+    #### Parameters
+    - `A` - Address of register in hexidecimal. Default value is 0.
+    - `C` - Number of bytes to read. Default value is 0.
+    */
+    case 707:
+    {
+        uint8_t addr = 0;
+        uint8_t nrbytes = 0;
+        if ( MMU2::mmu2.Enabled() )
+        {
+            if( code_seen('A') ) {
+                addr = code_value_uint8();
+            }
+            if( code_seen('C') ) {
+                nrbytes = code_value_uint8();
+            }
+            MMU2::mmu2.ReadRegister(addr, nrbytes);
+        }
+    }
+    break;
+
+    /*!
+    ### M708 - Write to MMU register
+    #### Usage
+
+        M708 [ A | X | C ]
+
+        M708 A14 X30 C1 - Write to register 0x14 the value 30 which is 1 byte.
+
+    #### Parameters
+    - `A` - Address of register in hexidecimal. Default value is 0.
+    - `X` - Data to write. Default value is 0.
+    - `C` - Number of bytes to write. Default value is 0.
+    */
+    case 708:
+    {
+        uint8_t addr = 0;
+        uint8_t data = 0;
+        uint8_t nrbytes = 0;
+        if ( MMU2::mmu2.Enabled() )
+        {
+            if( code_seen('A') ) {
+                addr = code_value_uint8();
+            }
+            if( code_seen('X') ) {
+                data = code_value_uint8();
+            }
+            if( code_seen('C') ) {
+                nrbytes = code_value_uint8();
+            }
+            MMU2::mmu2.WriteRegister(addr, data, nrbytes);
+        }
+    }
+    break;
+
     /*!
     ### M709 - MMU turn on/off/reset
     The MK3S cannot not power off the MMU, for that reason the functionality is not supported.
diff --git a/Firmware/mmu2.cpp b/Firmware/mmu2.cpp
index f7567e5fb..a0c838220 100644
--- a/Firmware/mmu2.cpp
+++ b/Firmware/mmu2.cpp
@@ -179,6 +179,16 @@ void MMU2::PowerOn(){
     power_on();
 }
 
+void MMU2::ReadRegister(uint8_t address, uint8_t nrbytes){
+    // TODO, implement for gcode M707
+    // Currently this function is NOP
+}
+
+void MMU2::WriteRegister(uint8_t address, uint8_t data, uint8_t nrbytes){
+    // TODO, implement for gcode M708
+    // Currently this function is NOP
+}
+
 void MMU2::mmu_loop() {
     // We only leave this method if the current command was successfully completed - that's the Marlin's way of blocking operation
     // Atomic compare_exchange would have been the most appropriate solution here, but this gets called only in Marlin's task,
diff --git a/Firmware/mmu2.h b/Firmware/mmu2.h
index 0167fa1d8..5a6736e70 100644
--- a/Firmware/mmu2.h
+++ b/Firmware/mmu2.h
@@ -85,6 +85,19 @@ public:
     /// Power on the MMU
     void PowerOn();
 
+    /// Read from a MMU register (See gcode M707)
+    /// @param address Address of register in hexidecimal
+    /// @param nrbytes Number of bytes to read
+    /// @returns no return
+    void ReadRegister(uint8_t address, uint8_t nrbytes);
+
+    /// Write from a MMU register (See gcode M708)
+    /// @param address Address of register in hexidecimal
+    /// @param data Data to write to register
+    /// @param nrbytes Number of bytes to write
+    /// @returns no return
+    void WriteRegister(uint8_t address, uint8_t data, uint8_t nrbytes);
+
 
     /// The main loop of MMU processing.
     /// Doesn't loop (block) inside, performs just one step of logic state machines.