From f3be03da20708c8dfc990e6e293c4e25a3605e52 Mon Sep 17 00:00:00 2001
From: Stuart Pittaway <1201909+stuartpittaway@users.noreply.github.com>
Date: Mon, 11 Oct 2021 23:42:29 +0100
Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20M261=20S=20I2C=20output=20format=20?=
 =?UTF-8?q?(#22890)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Scott Lahteine <github@thinkyhead.com>
---
 Marlin/src/feature/twibus.cpp              | 61 +++++++++++++++++++---
 Marlin/src/feature/twibus.h                | 10 ++--
 Marlin/src/gcode/feature/i2c/M260_M261.cpp |  7 +--
 Marlin/src/libs/hex_print.h                |  2 +-
 4 files changed, 64 insertions(+), 16 deletions(-)

diff --git a/Marlin/src/feature/twibus.cpp b/Marlin/src/feature/twibus.cpp
index 55e4da75cf..0e23b48a40 100644
--- a/Marlin/src/feature/twibus.cpp
+++ b/Marlin/src/feature/twibus.cpp
@@ -28,11 +28,17 @@
 
 #include <Wire.h>
 
+#include "../libs/hex_print.h"
+
 TWIBus i2c;
 
 TWIBus::TWIBus() {
   #if I2C_SLAVE_ADDRESS == 0
-    Wire.begin();                  // No address joins the BUS as the master
+    Wire.begin(                    // No address joins the BUS as the master
+      #if PINS_EXIST(I2C_SCL, I2C_SDA) && DISABLED(SOFT_I2C_EEPROM)
+        pin_t(I2C_SDA_PIN), pin_t(I2C_SCL_PIN)
+      #endif
+    );
   #else
     Wire.begin(I2C_SLAVE_ADDRESS); // Join the bus as a slave
   #endif
@@ -88,14 +94,53 @@ void TWIBus::echoprefix(uint8_t bytes, FSTR_P const pref, uint8_t adr) {
 }
 
 // static
-void TWIBus::echodata(uint8_t bytes, FSTR_P const pref, uint8_t adr) {
-  echoprefix(bytes, pref, adr);
-  while (bytes-- && Wire.available()) SERIAL_CHAR(Wire.read());
+void TWIBus::echodata(uint8_t bytes, FSTR_P const pref, uint8_t adr, const uint8_t style/*=0*/) {
+  union TwoBytesToInt16 { uint8_t bytes[2]; int16_t integervalue; };
+  TwoBytesToInt16 ConversionUnion;
+
+  echoprefix(bytes, pref, adr);  
+
+  while (bytes-- && Wire.available()) {
+    int value = Wire.read();
+    switch (style) {
+
+      // Style 1, HEX DUMP
+      case 1:
+        SERIAL_CHAR(hex_nybble((value & 0xF0) >> 4));
+        SERIAL_CHAR(hex_nybble(value & 0x0F));
+        if (bytes) SERIAL_CHAR(' ');
+        break;
+
+      // Style 2, signed two byte integer (int16)
+      case 2:
+        if (bytes == 1)
+          ConversionUnion.bytes[1] = (uint8_t)value;
+        else if (bytes == 0) {
+          ConversionUnion.bytes[0] = (uint8_t)value;
+          // Output value in base 10 (standard decimal)
+          SERIAL_ECHO(ConversionUnion.integervalue);
+        }
+        break;
+
+      // Style 3, unsigned byte, base 10 (uint8)
+      case 3:
+        SERIAL_ECHO(value);
+        if (bytes) SERIAL_CHAR(' ');
+        break;
+
+      // Default style (zero), raw serial output
+      default:
+        // This can cause issues with some serial consoles, Pronterface is an example where things go wrong
+        SERIAL_CHAR(value);
+        break;
+    }
+  }
+
   SERIAL_EOL();
 }
 
-void TWIBus::echobuffer(FSTR_P const pref, uint8_t adr) {
-  echoprefix(buffer_s, pref, adr);
+void TWIBus::echobuffer(FSTR_P const prefix, uint8_t adr) {
+  echoprefix(buffer_s, prefix, adr);
   LOOP_L_N(i, buffer_s) SERIAL_CHAR(buffer[i]);
   SERIAL_EOL();
 }
@@ -114,11 +159,11 @@ bool TWIBus::request(const uint8_t bytes) {
   return true;
 }
 
-void TWIBus::relay(const uint8_t bytes) {
+void TWIBus::relay(const uint8_t bytes, const uint8_t style/*=0*/) {
   debug(F("relay"), bytes);
 
   if (request(bytes))
-    echodata(bytes, F("i2c-reply"), addr);
+    echodata(bytes, F("i2c-reply"), addr, style);
 }
 
 uint8_t TWIBus::capture(char *dst, const uint8_t bytes) {
diff --git a/Marlin/src/feature/twibus.h b/Marlin/src/feature/twibus.h
index 2a8a7fef6a..d2c7270303 100644
--- a/Marlin/src/feature/twibus.h
+++ b/Marlin/src/feature/twibus.h
@@ -142,7 +142,7 @@ class TWIBus {
      *
      * @param bytes the number of bytes to request
      */
-    static void echoprefix(uint8_t bytes, FSTR_P prefix, uint8_t adr);
+    static void echoprefix(uint8_t bytes, FSTR_P const prefix, uint8_t adr);
 
     /**
      * @brief Echo data on the bus to serial
@@ -150,8 +150,9 @@ class TWIBus {
      *          to serial in a parser-friendly format.
      *
      * @param bytes the number of bytes to request
+     * @param style Output format for the bytes, 0 = Raw byte [default], 1 = Hex characters, 2 = uint16_t
      */
-    static void echodata(uint8_t bytes, FSTR_P prefix, uint8_t adr);
+    static void echodata(uint8_t bytes, FSTR_P const prefix, uint8_t adr, const uint8_t style=0);
 
     /**
      * @brief Echo data in the buffer to serial
@@ -160,7 +161,7 @@ class TWIBus {
      *
      * @param bytes the number of bytes to request
      */
-    void echobuffer(FSTR_P prefix, uint8_t adr);
+    void echobuffer(FSTR_P const prefix, uint8_t adr);
 
     /**
      * @brief Request data from the slave device and wait.
@@ -192,10 +193,11 @@ class TWIBus {
      * @brief Request data from the slave device, echo to serial.
      * @details Request a number of bytes from a slave device and output
      *          the returned data to serial in a parser-friendly format.
+     * @style Output format for the bytes, 0 = raw byte [default], 1 = Hex characters, 2 = uint16_t
      *
      * @param bytes the number of bytes to request
      */
-    void relay(const uint8_t bytes);
+    void relay(const uint8_t bytes, const uint8_t style=0);
 
     #if I2C_SLAVE_ADDRESS > 0
 
diff --git a/Marlin/src/gcode/feature/i2c/M260_M261.cpp b/Marlin/src/gcode/feature/i2c/M260_M261.cpp
index 438d1527f5..e978fb5048 100644
--- a/Marlin/src/gcode/feature/i2c/M260_M261.cpp
+++ b/Marlin/src/gcode/feature/i2c/M260_M261.cpp
@@ -60,15 +60,16 @@ void GcodeSuite::M260() {
 /**
  * M261: Request X bytes from I2C slave device
  *
- * Usage: M261 A<slave device address base 10> B<number of bytes>
+ * Usage: M261 A<slave device address base 10> B<number of bytes> S<style>
  */
 void GcodeSuite::M261() {
   if (parser.seen('A')) i2c.address(parser.value_byte());
 
-  uint8_t bytes = parser.byteval('B', 1);
+  const uint8_t bytes = parser.byteval('B', 1),   // Bytes to request
+                style = parser.byteval('S');      // Serial output style (ASCII, HEX etc)
 
   if (i2c.addr && bytes && bytes <= TWIBUS_BUFFER_SIZE)
-    i2c.relay(bytes);
+    i2c.relay(bytes, style);
   else
     SERIAL_ERROR_MSG("Bad i2c request");
 }
diff --git a/Marlin/src/libs/hex_print.h b/Marlin/src/libs/hex_print.h
index 40baa15bcc..0baae15bd3 100644
--- a/Marlin/src/libs/hex_print.h
+++ b/Marlin/src/libs/hex_print.h
@@ -27,7 +27,7 @@
 // Utility functions to create and print hex strings as nybble, byte, and word.
 //
 
-FORCE_INLINE char hex_nybble(const uint8_t n) {
+constexpr char hex_nybble(const uint8_t n) {
   return (n & 0xF) + ((n & 0xF) < 10 ? '0' : 'A' - 10);
 }
 char* hex_byte(const uint8_t b);