ch552: Split functionality into separate files

- Move GPIO functions to gpio.c and gpio.h
- Move string handling functions for debug to lib.c and lib.h
- Add main.h and move config defines there
- Add debug print functionality via FPGA with framing protocol
- Add Flash Code and Flash Data functions to flash.c and flash.h

hw/usb_interface/ch552_fw/Makefile

@@ -34,8 +34,11 @@ LFLAGS = \
          $(CFLAGS)
 
 C_FILES = \
-          src/main.c \
           src/debug.c \
+          src/flash.c \
+          src/gpio.c \
+          src/lib.c \
+          src/main.c \
           src/print.c
 
 # Create a .rel file for each .c file in $(OUT_DIR)/

hw/usb_interface/ch552_fw/inc/debug.h

@@ -8,6 +8,7 @@
 #include <stdint.h>
 
 #include "ch554.h"
+#include "gpio.h"
 
 // UART1 baud rates
 //     Setting   Actual      % error
@@ -40,18 +41,6 @@ void CfgFsys(void);        // CH554 clock selection and configuration
 void mDelayuS(uint16_t n); // Delay in units of uS
 void mDelaymS(uint16_t n); // Delay in mS
 
-// Set pin p1.4 and p1.5 to GPIO output mode.
-void gpio_init(void);
-void gpio_set(uint8_t pin);
-void gpio_unset(uint8_t pin);
-uint8_t gpio_get(uint8_t pin);
-
-void gpio_init_p1_4_in(void);
-void gpio_init_p1_5_out(void);
-uint8_t gpio_p1_4_get(void);
-void gpio_p1_5_set(void);
-void gpio_p1_5_unset(void);
-
 /*******************************************************************************
  * Function Name  : CH554UART0Alter()
  * Description    : Set the alternate pin mappings for UART0 (RX on P1.2, TX on P1.3)

hw/usb_interface/ch552_fw/inc/flash.h

@@ -0,0 +1,9 @@
+#ifndef __FLASH_H__
+#define __FLASH_H__
+
+#include <stdint.h>
+
+uint8_t write_code_flash(uint16_t address, uint16_t data);
+uint8_t write_data_flash(uint8_t address, uint8_t data);
+
+#endif

hw/usb_interface/ch552_fw/inc/gpio.h

@@ -0,0 +1,20 @@
+#ifndef __GPIO_H__
+#define __GPIO_H__
+
+#include <stdint.h>
+
+#include "main.h"
+
+// Set pin p1.4 and p1.5 to GPIO output mode.
+void gpio_init(void);
+void gpio_set(uint8_t pin);
+void gpio_unset(uint8_t pin);
+uint8_t gpio_get(uint8_t pin);
+
+void gpio_init_p1_4_in(void);
+void gpio_init_p1_5_out(void);
+uint8_t gpio_p1_4_get(void);
+void gpio_p1_5_set(void);
+void gpio_p1_5_unset(void);
+
+#endif

hw/usb_interface/ch552_fw/inc/lib.h

@@ -0,0 +1,23 @@
+#ifndef __LIB_H__
+#define __LIB_H__
+
+#include <stdint.h>
+
+#include "main.h"
+
+#ifdef USE_NUM_U8
+int8_t uint8_to_str(uint8_t *buf, uint8_t bufsize, uint8_t n);
+#endif
+
+#ifdef USE_NUM_U32
+int8_t uint32_to_str(uint8_t *buf, uint8_t bufsize, uint32_t n);
+#endif
+
+//uint8_t hex_to_uint8(const char *hex, uint8_t len);
+//uint16_t hex_to_uint16(const char *hex, uint8_t len);
+uint32_t hex_to_uint32(const char *hex, uint8_t len);
+
+uint8_t ascii_hex_char_to_byte(uint8_t c);
+//int ascii_hex_string_to_bytes(uint8_t *hex_str, uint8_t *out_bytes, size_t out_len);
+
+#endif

hw/usb_interface/ch552_fw/inc/main.h

@@ -0,0 +1,18 @@
+#ifndef __MAIN_H__
+#define __MAIN_H__
+
+#define USE_DEBUG_PRINT  /* Enable to print debug messages */
+#define DEBUG_PRINT_SW   /* Enable to print debug messages via FPGA */
+//#define DEBUG_PRINT_HW /* Enable to print debug messages via UART0 */
+
+#define USE_NUM_U8
+//#define USE_NUM_U32
+
+//#define DEBUG_SETUP    /* Enable to debug USB setup flow (printStrSetup,printNumU8HexSetup) */
+
+#define USE_NUM_U8HEX
+#define USE_NUM_U16HEX
+//#define USE_NUM_U32HEX
+//#define USE_NEGATIVE_NUMS
+
+#endif

hw/usb_interface/ch552_fw/inc/print.h

@@ -3,30 +3,17 @@
 
 #include <stdint.h>
 
-//#define DEBUG_PRINT
-//#define DEBUG_SETUP
-//#define UART_OUT_DEBUG
-//#define USE_NUM_U8
-#define USE_NUM_U32
-//#define USE_NEGATIVE_NUMS
+#include "main.h"
 
 void printStr(uint8_t *str);
 void printChar(uint8_t c);
 
 #ifdef DEBUG_SETUP
-#define printStrSetup(x)       printStr(x)
-#define printNumHexSetup(x)    printNumHex(x)
+#define printStrSetup       printStr
+#define printNumU8HexSetup  printNumU8Hex
 #else
-#define printStrSetup(x)
-#define printNumHexSetup(x)
-#endif
-
-#ifdef USE_NUM_U8
-int8_t uint8_to_str(uint8_t *buf, uint8_t bufsize, uint8_t n);
-#endif
-
-#ifdef USE_NUM_U32
-int8_t uint32_to_str(uint8_t *buf, uint8_t bufsize, uint32_t n);
+#define printStrSetup
+#define printNumU8HexSetup
 #endif
 
 #ifdef USE_NUM_U8
@@ -37,6 +24,16 @@ void printNumU8(uint8_t num);
 void printNumU32(uint32_t num);
 #endif
 
-void printNumHex(uint8_t num);
+#ifdef USE_NUM_U8HEX
+void printNumU8Hex(uint8_t num);
+#endif
+
+#ifdef USE_NUM_U16HEX
+void printNumU16Hex(uint16_t num);
+#endif
+
+#ifdef USE_NUM_U32HEX
+void printNumU32Hex(uint32_t num);
+#endif
 
 #endif

hw/usb_interface/ch552_fw/src/debug.c

@@ -160,108 +160,3 @@ int getchar(void)
     return SBUF;
 }
 #endif
-
-// Set pin p1.4 and p1.5 to GPIO output mode.
-void gpio_init()
-{
-    // p1.4
-    P1_MOD_OC &= ~0x10;
-    P1_DIR_PU |= 0x10;
-
-    // p1.5
-    P1_MOD_OC &= ~0x20;
-    P1_DIR_PU |= 0x20;
-}
-
-void gpio_set(uint8_t pin)
-{
-    switch (pin) {
-    case 0x10: // p1.4
-        P1 |= 0x10;
-        break;
-    case 0x20: // p1.5
-        P1 |= 0x20;
-        break;
-    default: // do nothing, unsupported pin.
-        break;
-    }
-}
-
-void gpio_unset(uint8_t pin)
-{
-    switch (pin) {
-    case 0x10:
-        P1 &= ~0x10;
-        break;
-    case 0x20:
-        P1 &= ~0x20;
-        break;
-    default: // do nothing, unsupported pin.
-        break;
-    }
-}
-
-uint8_t gpio_get(uint8_t pin)
-{
-    uint8_t ret = 0;
-
-    switch (pin) {
-    case 0x10: // p1.4
-        ret = P1 & 0x10;
-        break;
-    case 0x20: // p1.5
-        ret = P1 & 0x20;
-        break;
-    default: // do nothing, unsupported pin.
-        ret = 0xff;
-        break;
-    }
-
-    return ret;
-}
-
-// Set pin p1.4 to GPIO input mode. (FPGA_CTS)
-void gpio_init_p1_4_in()
-{
-    // p1.4
-    P1_MOD_OC &= ~0x10; // Output Mode: 0 = Push-pull output, 1 = Open-drain output
-    P1_DIR_PU &= ~0x10; // Port Direction Control and Pull-up Enable Register:
-                        //    Push-pull output mode:
-                        //        0 = Input.
-                        //        1 = Output
-                        //    Open-drain output mode:
-                        //        0 = Pull-up resistor disabled
-                        //        1 = Pull-up resistor enabled
-}
-
-// Read status of pin 1.4
-uint8_t gpio_p1_4_get(void)
-{
-    return (P1 & 0x10); // p1.4
-}
-
-// Set pin p1.5 to GPIO output mode. (CH552_CTS)
-void gpio_init_p1_5_out()
-{
-    // p1.5
-    P1_MOD_OC &= ~0x20; // Output Mode: 0 = Push-pull output, 1 = Open-drain output
-    P1_DIR_PU |=  0x20; // Port Direction Control and Pull-up Enable Register:
-                        //    Push-pull output mode:
-                        //        0 = Input.
-                        //        1 = Output
-                        //    Open-drain output mode:
-                        //        0 = Pull-up resistor disabled
-                        //        1 = Pull-up resistor enabled
-}
-
-// Set pin 1.5 high
-void gpio_p1_5_set(void)
-{
-    P1 |= 0x20;
-}
-
-// Set pin 1.5 low
-void gpio_p1_5_unset(void)
-{
-    P1 &= ~0x20;
-}

hw/usb_interface/ch552_fw/src/flash.c

@@ -0,0 +1,79 @@
+#include <stdint.h>
+
+#include "ch554.h"
+
+uint8_t write_code_flash(uint16_t address, uint16_t data)
+{
+    uint8_t ret = 0;
+
+    SAFE_MOD = 0x55;
+    SAFE_MOD = 0xAA;        // Enter Safe mode
+    GLOBAL_CFG |= bCODE_WE; // Enable flash-ROM write
+    SAFE_MOD = 0;           // Exit Safe mode
+
+    // Set address
+    ROM_ADDR = address;
+    ROM_DATA = data;
+
+    // Check that write operation address is valid
+    if ((ROM_STATUS & bROM_ADDR_OK) == 0) {
+        ret |= 0x01;
+    }
+
+    if (ROM_CTRL & bROM_ADDR_OK) {
+        E_DIS = 1; // Disable global interrupts to prevent Flash operation from being interrupted
+        ROM_CTRL = ROM_CMD_WRITE; // Write
+        E_DIS = 0; // Enable global interrupts
+    }
+
+    // Check operation command error
+    if (ROM_STATUS & bROM_CMD_ERR) {
+        ret |= 0x02;
+    }
+
+    SAFE_MOD = 0x55;
+    SAFE_MOD = 0xAA;         // Enter Safe mode
+    GLOBAL_CFG &= ~bCODE_WE; // Disable flash-ROM write
+    SAFE_MOD = 0;            // Exit Safe mode
+
+    return ret;
+}
+
+uint8_t write_data_flash(uint8_t address, uint8_t data)
+{
+    uint8_t ret = 0;
+
+    EA = 0; // Disable global interrupts to prevent Flash operation from being interrupted
+
+    SAFE_MOD = 0x55;
+    SAFE_MOD = 0xAA;        // Enter Safe mode
+    GLOBAL_CFG |= bDATA_WE; // Enable Data-Flash write
+    SAFE_MOD = 0;           // Exit Safe mode
+
+    ROM_ADDR_H = DATA_FLASH_ADDR >> 8;
+    ROM_ADDR_L = address << 1;
+    ROM_DATA_L = data;
+
+    // Check that write operation address is valid
+    if ((ROM_STATUS & bROM_ADDR_OK) == 0) {
+        ret |= 0x01;
+    }
+
+    if (ROM_CTRL & bROM_ADDR_OK) {
+        ROM_CTRL = ROM_CMD_WRITE;  // Write
+    }
+
+    // Check operation command error
+    if (ROM_STATUS & bROM_CMD_ERR) {
+        ret |= 0x02;
+    }
+
+    SAFE_MOD = 0x55;
+    SAFE_MOD = 0xAA;         // Enter Safe mode
+    GLOBAL_CFG &= ~bDATA_WE; // Disable Data-Flash write
+    SAFE_MOD = 0;            // Exit Safe mode
+
+    EA = 1; // Enable global interrupts
+
+    return ret;
+}

hw/usb_interface/ch552_fw/src/gpio.c

@@ -0,0 +1,108 @@
+#include <stdint.h>
+
+#include "ch554.h"
+
+// Set pin p1.4 and p1.5 to GPIO output mode.
+void gpio_init()
+{
+    // p1.4
+    P1_MOD_OC &= ~0x10;
+    P1_DIR_PU |= 0x10;
+
+    // p1.5
+    P1_MOD_OC &= ~0x20;
+    P1_DIR_PU |= 0x20;
+}
+
+void gpio_set(uint8_t pin)
+{
+    switch (pin) {
+    case 0x10: // p1.4
+        P1 |= 0x10;
+        break;
+    case 0x20: // p1.5
+        P1 |= 0x20;
+        break;
+    default: // do nothing, unsupported pin.
+        break;
+    }
+}
+
+void gpio_unset(uint8_t pin)
+{
+    switch (pin) {
+    case 0x10:
+        P1 &= ~0x10;
+        break;
+    case 0x20:
+        P1 &= ~0x20;
+        break;
+    default: // do nothing, unsupported pin.
+        break;
+    }
+}
+
+uint8_t gpio_get(uint8_t pin)
+{
+    uint8_t ret = 0;
+
+    switch (pin) {
+    case 0x10: // p1.4
+        ret = P1 & 0x10;
+        break;
+    case 0x20: // p1.5
+        ret = P1 & 0x20;
+        break;
+    default: // do nothing, unsupported pin.
+        ret = 0xff;
+        break;
+    }
+
+    return ret;
+}
+
+// Set pin p1.4 to GPIO input mode. (FPGA_CTS)
+void gpio_init_p1_4_in()
+{
+    // p1.4
+    P1_MOD_OC &= ~0x10; // Output Mode: 0 = Push-pull output, 1 = Open-drain output
+    P1_DIR_PU &= ~0x10; // Port Direction Control and Pull-up Enable Register:
+                        //    Push-pull output mode:
+                        //        0 = Input.
+                        //        1 = Output
+                        //    Open-drain output mode:
+                        //        0 = Pull-up resistor disabled
+                        //        1 = Pull-up resistor enabled
+}
+
+// Read status of pin 1.4
+uint8_t gpio_p1_4_get(void)
+{
+    return (P1 & 0x10); // p1.4
+}
+
+// Set pin p1.5 to GPIO output mode. (CH552_CTS)
+void gpio_init_p1_5_out()
+{
+    // p1.5
+    P1_MOD_OC &= ~0x20; // Output Mode: 0 = Push-pull output, 1 = Open-drain output
+    P1_DIR_PU |=  0x20; // Port Direction Control and Pull-up Enable Register:
+                        //    Push-pull output mode:
+                        //        0 = Input.
+                        //        1 = Output
+                        //    Open-drain output mode:
+                        //        0 = Pull-up resistor disabled
+                        //        1 = Pull-up resistor enabled
+}
+
+// Set pin 1.5 high
+void gpio_p1_5_set(void)
+{
+    P1 |= 0x20;
+}
+
+// Set pin 1.5 low
+void gpio_p1_5_unset(void)
+{
+    P1 &= ~0x20;
+}

hw/usb_interface/ch552_fw/src/lib.c

@@ -0,0 +1,200 @@
+#include <stdint.h>
+
+#include "main.h"
+
+#ifdef USE_NUM_U8
+int8_t uint8_to_str(uint8_t *buf, uint8_t bufsize, uint8_t n)
+{
+    uint8_t *start;
+
+#ifdef USE_NEGATIVE_NUMS
+    if (n < 0) { // Handle negative numbers.
+        if (!bufsize) {
+            return -1;
+        }
+        *buf++ = '-';
+        bufsize--;
+    }
+#endif
+
+    start = buf; // Remember the start of the string. This will come into play at the end.
+
+    do {
+        // Handle the current digit.
+        uint8_t digit;
+        if (!bufsize) {
+            return -1;
+        }
+        digit = n % 10;
+#ifdef USE_NEGATIVE_NUMS
+        if (digit < 0) {
+            digit *= -1;
+        }
+#endif
+        *buf++ = digit + '0';
+        bufsize--;
+        n /= 10;
+    } while (n);
+
+    // Terminate the string.
+    if (!bufsize) {
+        return -1;
+    }
+    *buf = 0;
+
+    // We wrote the string backwards, i.e. with least significant digits first. Now reverse the string.
+    --buf;
+    while (start < buf) {
+        uint8_t a = *start;
+        *start = *buf;
+        *buf = a;
+        ++start;
+        --buf;
+    }
+
+    return 0;
+}
+#endif
+
+#ifdef USE_NUM_U32
+int8_t uint32_to_str(uint8_t *buf, uint8_t bufsize, uint32_t n)
+{
+    uint8_t *start;
+
+#ifdef USE_NEGATIVE_NUMS
+    if (n < 0) { // Handle negative numbers.
+        if (!bufsize) {
+            return -1;
+        }
+        *buf++ = '-';
+        bufsize--;
+    }
+#endif
+
+    start = buf; // Remember the start of the string. This will come into play at the end.
+
+    do {
+        // Handle the current digit.
+        uint8_t digit;
+        if (!bufsize) {
+            return -1;
+        }
+        digit = n % 10;
+#ifdef USE_NEGATIVE_NUMS
+        if (digit < 0) {
+            digit *= -1;
+        }
+#endif
+        *buf++ = digit + '0';
+        bufsize--;
+        n /= 10;
+    } while (n);
+
+    // Terminate the string.
+    if (!bufsize) {
+        return -1;
+    }
+    *buf = 0;
+
+    // We wrote the string backwards, i.e. with least significant digits first. Now reverse the string.
+    --buf;
+    while (start < buf) {
+        uint8_t a = *start;
+        *start = *buf;
+        *buf = a;
+        ++start;
+        --buf;
+    }
+
+    return 0;
+}
+#endif
+
+#if 0
+uint8_t hex_to_uint8(const char *hex, uint8_t len)
+{
+    uint8_t val = 0;
+    while (len) {
+        char c = *hex++;
+        val <<= 4;
+        if (c >= '0' && c <= '9') {
+            val |= c - '0';
+        } else if (c >= 'A' && c <= 'F') {
+            val |= c - 'A' + 10;
+        } else if (c >= 'a' && c <= 'f') {
+            val |= c - 'a' + 10;
+        } else {
+            return 0; // Invalid character
+        }
+        len--;
+    }
+    return val;
+}
+#endif
+
+#if 0
+uint16_t hex_to_uint16(const char *hex, uint8_t len)
+{
+    uint16_t val = 0;
+    while (len) {
+        char c = *hex++;
+        val <<= 4;
+        if (c >= '0' && c <= '9') {
+            val |= c - '0';
+        } else if (c >= 'A' && c <= 'F') {
+            val |= c - 'A' + 10;
+        } else if (c >= 'a' && c <= 'f') {
+            val |= c - 'a' + 10;
+        } else {
+            return 0; // Invalid character
+        }
+        len--;
+    }
+    return val;
+}
+#endif
+
+uint32_t hex_to_uint32(const char *hex, uint8_t len)
+{
+    uint32_t val = 0;
+    while (len) {
+        char c = *hex++;
+        val <<= 4;
+        if (c >= '0' && c <= '9') {
+            val |= c - '0';
+        } else if (c >= 'A' && c <= 'F') {
+            val |= c - 'A' + 10;
+        } else if (c >= 'a' && c <= 'f') {
+            val |= c - 'a' + 10;
+        } else {
+            return 0; // Invalid character
+        }
+        len--;
+    }
+    return val;
+}
+
+uint8_t ascii_hex_char_to_byte(uint8_t c)
+{
+    if (c >= '0' && c <= '9')
+        return c - '0';
+    if (c >= 'A' && c <= 'F')
+        return c - 'A' + 10;
+    if (c >= 'a' && c <= 'f')
+        return c - 'a' + 10;
+    return 0; // Invalid character, should not happen if input is valid
+}
+
+#if 0
+int ascii_hex_string_to_bytes(uint8_t *hex_str, uint8_t *out_bytes, size_t out_len)
+{
+    if (!hex_str || !out_bytes || out_len < 32)
+        return -1; // Error handling
+
+    for (size_t i = 0; i < 32; i++) {
+        out_bytes[i] = (ascii_hex_char_to_byte(hex_str[i * 2]) << 4) | ascii_hex_char_to_byte(hex_str[i * 2 + 1]);
+    }
+
+    return 0; // Success
+}
+#endif

hw/usb_interface/ch552_fw/src/main.c

@@ -6,13 +6,15 @@
  * Description        : CH554 as CDC device to serial port, select serial port 1
  *******************************************************************************/
 #include <stdint.h>
-#include <stdio.h>
 #include <string.h>
 
 #include <ch554.h>
 #include <ch554_usb.h>
 
 #include "debug.h"
+#include "flash.h"
+#include "lib.h"
+#include "main.h"
 #include "mem.h"
 #include "print.h"
 #include "usb_strings.h"
@@ -550,7 +552,7 @@ void usb_irq_setup_handler(void)
             printStrSetup("Class-Specific ");
             printStrSetup("SetupReq=");
             printStrSetup("0x");
-            printNumHexSetup(SetupReq);
+            printNumU8HexSetup(SetupReq);
             printStrSetup(" ");
             switch(SetupReq) {
             case USB_HID_REQ_TYPE_GET_REPORT:
@@ -984,7 +986,7 @@ void DeviceInterrupt(void)IRQ_USB // USB interrupt service routine, using regist
 
     } else if (UIF_BUS_RST) { // Check device mode USB bus reset interrupt
 
-        printStr("Reset\n");
+        printStrSetup("Reset\n");
 
         UEP0_CTRL =                 UEP_T_RES_NAK | UEP_R_RES_ACK;
         UEP1_CTRL = bUEP_AUTO_TOG | UEP_T_RES_NAK;
@@ -1016,7 +1018,7 @@ void DeviceInterrupt(void)IRQ_USB // USB interrupt service routine, using regist
 
         if (USB_MIS_ST & bUMS_SUSPEND) { // Hang
 
-            printStr("Suspend\n");
+            printStrSetup("Suspend\n");
 
             while (XBUS_AUX & bUART0_TX) {
                 ; // Wait for sending to complete
@@ -1032,7 +1034,7 @@ void DeviceInterrupt(void)IRQ_USB // USB interrupt service routine, using regist
 
         }
     } else { // Unexpected IRQ, should not happen
-        printStr("Unexpected IRQ\n");
+        printStrSetup("Unexpected IRQ\n");
         USB_INT_FG = 0xFF; // Clear interrupt flag
     }
 }
@@ -1241,16 +1243,16 @@ void main()
                 } else { // Invalid mode
                     if (!Halted) {
                         printStr("Invalid header: 0x");
-                        printNumHex(FrameMode);
+                        printNumU8Hex(FrameMode);
                         printStr(", len = ");
-                        printNumU32(UartRxBuf[increment_pointer(UartRxBufOutputPointer,
-                                                                1,
-                                                                UART_RX_BUF_SIZE)]);
+                        printNumU8Hex(UartRxBuf[increment_pointer(UartRxBufOutputPointer,
+                                                                   1,
+                                                                   UART_RX_BUF_SIZE)]);
                         printStr("\n");
                         uint16_t i;
                         uint8_t print_char_count_out = 0;
                         for (i=0; i<UART_RX_BUF_SIZE; i++) {
-                            printNumHex(UartRxBuf[increment_pointer(UartRxBufOutputPointer,
+                            printNumU8Hex(UartRxBuf[increment_pointer(UartRxBufOutputPointer,
                                                                     i,
                                                                     UART_RX_BUF_SIZE)]);
                             print_char_count_out++;

hw/usb_interface/ch552_fw/src/print.c

@@ -1,15 +1,28 @@
 #include <stdint.h>
+#include <string.h>
 
 #include "debug.h"
+#include "lib.h"
+#include "main.h"
+#include "mem.h"
 #include "print.h"
 
+#define MODE_CH552        0x10
+
 void printStr(uint8_t *str)
 {
-#ifdef DEBUG_PRINT
+#ifdef USE_DEBUG_PRINT
+#if defined(DEBUG_PRINT_HW)
     while (*str != 0) {
         CH554UART0SendByte(*str);
         ++str;
     }
+#elif defined(DEBUG_PRINT_SW)
+    uint32_t str_len = strlen(str);
+    CH554UART1SendByte(MODE_CH552);
+    CH554UART1SendByte(str_len);
+    CH554UART1SendBuffer(str, str_len);
+#endif
 #else
     (void)str;
 #endif
@@ -17,131 +30,31 @@ void printStr(uint8_t *str)
 
 void printChar(uint8_t c)
 {
-#ifdef DEBUG_PRINT
+#ifdef USE_DEBUG_PRINT
+#if defined(DEBUG_PRINT_HW)
     CH554UART0SendByte(c);
+#elif defined(DEBUG_PRINT_SW)
+    CH554UART1SendByte(MODE_CH552);
+    CH554UART1SendByte(1);
+    CH554UART1SendByte(c);
+#endif
 #else
     (void)c;
 #endif
 }
 
-#ifdef USE_NUM_U8
-int8_t uint8_to_str(uint8_t *buf, uint8_t bufsize, uint8_t n)
-{
-    uint8_t *start;
-
-#ifdef USE_NEGATIVE_NUMS
-    if (n < 0) { // Handle negative numbers.
-        if (!bufsize) {
-            return -1;
-        }
-        *buf++ = '-';
-        bufsize--;
-    }
-#endif
-
-    start = buf; // Remember the start of the string. This will come into play at the end.
-
-    do {
-        // Handle the current digit.
-        uint8_t digit;
-        if (!bufsize) {
-            return -1;
-        }
-        digit = n % 10;
-#ifdef USE_NEGATIVE_NUMS
-        if (digit < 0) {
-            digit *= -1;
-        }
-#endif
-        *buf++ = digit + '0';
-        bufsize--;
-        n /= 10;
-    } while (n);
-
-    // Terminate the string.
-    if (!bufsize) {
-        return -1;
-    }
-    *buf = 0;
-
-    // We wrote the string backwards, i.e. with least significant digits first. Now reverse the string.
-    --buf;
-    while (start < buf) {
-        uint8_t a = *start;
-        *start = *buf;
-        *buf = a;
-        ++start;
-        --buf;
-    }
-
-    return 0;
-}
-#endif
-
-#ifdef USE_NUM_U32
-int8_t uint32_to_str(uint8_t *buf, uint8_t bufsize, uint32_t n)
-{
-    uint8_t *start;
-
-#ifdef USE_NEGATIVE_NUMS
-    if (n < 0) { // Handle negative numbers.
-        if (!bufsize) {
-            return -1;
-        }
-        *buf++ = '-';
-        bufsize--;
-    }
-#endif
-
-    start = buf; // Remember the start of the string. This will come into play at the end.
-
-    do {
-        // Handle the current digit.
-        uint8_t digit;
-        if (!bufsize) {
-            return -1;
-        }
-        digit = n % 10;
-#ifdef USE_NEGATIVE_NUMS
-        if (digit < 0) {
-            digit *= -1;
-        }
-#endif
-        *buf++ = digit + '0';
-        bufsize--;
-        n /= 10;
-    } while (n);
-
-    // Terminate the string.
-    if (!bufsize) {
-        return -1;
-    }
-    *buf = 0;
-
-    // We wrote the string backwards, i.e. with least significant digits first. Now reverse the string.
-    --buf;
-    while (start < buf) {
-        uint8_t a = *start;
-        *start = *buf;
-        *buf = a;
-        ++start;
-        --buf;
-    }
-
-    return 0;
-}
-#endif
-
 #ifdef USE_NUM_U8
 void printNumU8(uint8_t num)
 {
-#ifdef DEBUG_PRINT
-    uint8_t num_str[4] = { 0 };
+#ifdef USE_DEBUG_PRINT
     int8_t ret;
+    uint8_t num_str[4] = { 0 };
     ret = uint8_to_str(num_str, 4, num);
     if (!ret) {
         printStr(num_str);
     }
+#else
+    (void)num;
 #endif
 }
 #endif
@@ -149,9 +62,9 @@ void printNumU8(uint8_t num)
 #ifdef USE_NUM_U32
 void printNumU32(uint32_t num)
 {
-#ifdef DEBUG_PRINT
-    uint8_t num_str[11] = { 0 };
+#ifdef USE_DEBUG_PRINT
     int8_t ret;
+    uint8_t num_str[11] = { 0 };
     ret = uint32_to_str(num_str, 10, num);
     if (!ret) {
         printStr(num_str);
@@ -164,7 +77,7 @@ void printNumU32(uint32_t num)
 
 void printNumHex(uint8_t num)
 {
-#ifdef DEBUG_PRINT
+#ifdef USE_DEBUG_PRINT
     // High nibble
     uint8_t val = num >> 4;
     if (val <= 9) {
@@ -186,3 +99,47 @@ void printNumHex(uint8_t num)
     (void)num;
 #endif
 }
+
+#ifdef USE_NUM_U8HEX
+void printNumU8Hex(uint8_t num)
+{
+#ifdef USE_DEBUG_PRINT
+    printStr("0x");
+    printNumHex(num);
+#else
+    (void)num;
+#endif
+}
+#endif
+
+#ifdef USE_NUM_U16HEX
+void printNumU16Hex(uint16_t num)
+{
+#ifdef USE_DEBUG_PRINT
+    uint8_t buf[2];
+    memcpy(buf, &num, 2);
+    printStr("0x");
+    for (int8_t i = 1; i > -1; i--) {
+        printNumHex(buf[i]);
+    }
+#else
+    (void)num;
+#endif
+}
+#endif
+
+#ifdef USE_NUM_U32HEX
+void printNumU32Hex(uint32_t num)
+{
+#ifdef USE_DEBUG_PRINT
+    uint8_t buf[4];
+    memcpy(buf, &num, 4);
+    printStr("0x");
+    for (int8_t i = 3; i > -1; i--) {
+        printNumHex(buf[i]);
+    }
+#else
+    (void)num;
+#endif
+}
+#endif