From 482a6e530aaa19e8c45fa83e7faf06898fac2341 Mon Sep 17 00:00:00 2001
From: Sucareto <28331534+Sucareto@users.noreply.github.com>
Date: Tue, 30 Apr 2024 02:19:10 +0800
Subject: [PATCH] print vfd message

---
 board/vfd.c | 58 +++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 56 insertions(+), 2 deletions(-)

diff --git a/board/vfd.c b/board/vfd.c
index 0442439..b216706 100644
--- a/board/vfd.c
+++ b/board/vfd.c
@@ -26,6 +26,7 @@ static HRESULT vfd_handle_irp(struct irp *irp);
 static struct uart vfd_uart;
 static uint8_t vfd_written[512];
 static uint8_t vfd_readable[512];
+UINT codepage;
 
 HRESULT vfd_hook_init(const struct vfd_config *cfg, unsigned int port_no)
 {
@@ -41,6 +42,7 @@ HRESULT vfd_hook_init(const struct vfd_config *cfg, unsigned int port_no)
     vfd_uart.readable.bytes = vfd_readable;
     vfd_uart.readable.nbytes = sizeof(vfd_readable);
 
+    codepage = GetACP();
     dprintf("VFD: hook enabled.\n");
 
     return iohook_push_handler(vfd_handle_irp);
@@ -62,8 +64,60 @@ static HRESULT vfd_handle_irp(struct irp *irp)
         return hr;
     }
 
-    dprintf("VFD TX:\n");
-    dump_iobuf(&vfd_uart.written);
+    uint8_t cmd = 0;
+    uint8_t str_1[512];
+    uint8_t str_2[512];
+    uint8_t str_1_len = 0;
+    uint8_t str_2_len = 0;
+    for (size_t i = 0; i < vfd_uart.written.pos; i++) {
+        if (vfd_uart.written.bytes[i] == 0x1B) {
+            i++;
+            cmd = vfd_uart.written.bytes[i];
+            if (cmd == 0x30) {
+                i += 3;
+            }
+            else if (cmd == 0x50) {
+                i++;
+            }
+            continue;
+        }
+        if (cmd == 0x30) {
+            str_1[str_1_len++] = vfd_uart.written.bytes[i];
+        }
+        else if (cmd == 0x50) {
+            str_2[str_2_len++] = vfd_uart.written.bytes[i];
+        }
+    }
+
+    if (str_1_len) {
+        str_1[str_1_len++] = '\0';
+        if (codepage != 932) {
+            WCHAR buffer[512];
+            MultiByteToWideChar(932, 0, (LPCSTR)str_1, str_1_len, buffer, str_1_len);
+            char str_recode[str_1_len * 3];
+            WideCharToMultiByte(codepage, 0, buffer, str_1_len, str_recode, str_1_len * 3, NULL, NULL);
+            dprintf("VFD: %s\n", str_recode);
+        }
+        else {
+            dprintf("VFD: %s\n", str_1);
+        }
+    }
+
+    if (str_2_len) {
+        str_2[str_2_len++] = '\0';
+        if (codepage != 932) {
+            WCHAR buffer[512];
+            MultiByteToWideChar(932, 0, (LPCSTR)str_2, str_2_len, buffer, str_2_len);
+            char str_recode[str_2_len * 3];
+            WideCharToMultiByte(codepage, 0, buffer, str_2_len, str_recode, str_2_len * 3, NULL, NULL);
+            dprintf("VFD: %s\n", str_recode);
+        } else {
+            dprintf("VFD: %s\n", str_2);
+        }
+    }
+
+    // dprintf("VFD TX:\n");
+    // dump_iobuf(&vfd_uart.written);
     vfd_uart.written.pos = 0;
 
     return hr;