From 753d397f61424a3ec9b689e6a0cd5ec4c3b6af01 Mon Sep 17 00:00:00 2001
From: DiSlord <dislord@mail.ru>
Date: Sun, 16 May 2021 08:14:10 +0300
Subject: [PATCH] Add lcd_printf function (allow use printf for lcd output)

---
 ili9341.c | 46 +++++++++++++++++++++++++++++++++++++++++++++-
 nanovna.h |  1 +
 2 files changed, 46 insertions(+), 1 deletion(-)

diff --git a/ili9341.c b/ili9341.c
index 0c78c00..78d7257 100644
--- a/ili9341.c
+++ b/ili9341.c
@@ -23,7 +23,7 @@
 #ifdef TINYSA4
 #include "si4432.h"
 #endif
-
+#include "chprintf.h"
 #include "spi.h"
 
 // Pin macros for LCD
@@ -841,6 +841,50 @@ void ili9341_drawstring_10x14(const char *str, int x, int y)
 #endif
 }
 
+typedef struct {
+  const void *vmt;
+  int16_t start_x;
+  int16_t start_y;
+  int16_t x;
+  int16_t y;
+} lcdPrintStream;
+
+static msg_t lcd_put(void *ip, uint8_t ch) {
+  lcdPrintStream *ps = ip;
+  if (ch == '\n') {ps->x = ps->start_x; ps->y+=FONT_STR_HEIGHT; return MSG_OK;}
+  uint16_t w = FONT_GET_WIDTH(ch);
+  ili9341_blitBitmap(ps->x, ps->y, w, FONT_GET_HEIGHT, FONT_GET_DATA(ch));
+  ps->x+= w;
+  return MSG_OK;
+}
+
+#if 0
+static msg_t lcd_put_7x13(void *ip, uint8_t ch) {
+  lcdPrintStream *ps = ip;
+  if (ch == '\n') {ps->x = ps->start_x; ps->y+=FONT_STR_HEIGHT; return MSG_OK;}
+  uint16_t w = FONT_GET_WIDTH(ch);
+  ili9341_blitBitmap(ps->x, ps->y, w, FONT_GET_HEIGHT, FONT_GET_DATA(ch));
+  ps->x+= w;
+  return MSG_OK;
+}
+#endif
+
+// Simple print in buffer function
+int lcd_printf(int16_t x, int16_t y, const char *fmt, ...) {
+  // Init small lcd print stream
+  struct lcd_printStreamVMT {
+    _base_sequential_stream_methods
+  } lcd_vmt = {NULL, NULL, lcd_put, NULL};
+  lcdPrintStream ps = {&lcd_vmt, x, y, x, y};
+  // Performing the print operation using the common code.
+  va_list ap;
+  va_start(ap, fmt);
+  int retval = chvprintf((BaseSequentialStream *)(void *)&ps, fmt, ap);
+  va_end(ap);
+  // Return number of bytes that would have been written.
+  return retval;
+}
+
 void ili9341_drawstringV(const char *str, int x, int y)
 {
   ili9341_set_rotation(DISPLAY_ROTATION_270);
diff --git a/nanovna.h b/nanovna.h
index 6bafbc7..f04bfc0 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -961,6 +961,7 @@ void ili9341_drawchar(uint8_t ch, int x, int y);
 void ili9341_drawstring(const char *str, int x, int y);
 void ili9341_drawstring_7x13(const char *str, int x, int y);
 void ili9341_drawstring_10x14(const char *str, int x, int y);
+int  lcd_printf(int16_t x, int16_t y, const char *fmt, ...);
 void ili9341_drawstringV(const char *str, int x, int y);
 int  ili9341_drawchar_size(uint8_t ch, int x, int y, uint8_t size);
 void ili9341_drawstring_size(const char *str, int x, int y, uint8_t size);