print vfd message

This commit is contained in:
Sucareto 2024-04-30 02:19:10 +08:00
parent 4041844ea9
commit 482a6e530a

View File

@ -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;