Add support for SGR 38 and 48

This supports the escape sequences used by fmtlib

nx/include/switch/runtime/devices/console.h

@@ -103,8 +103,8 @@ struct PrintConsole
 	int windowHeight;        ///< Window height in characters
 
 	int tabSize;             ///< Size of a tab
-	int fg;                  ///< Foreground color
+	u16 fg;                  ///< Foreground color
-	int bg;                  ///< Background color
+	u16 bg;                  ///< Background color
 	int flags;               ///< Reverse/bright flags
 
 	bool consoleInitialised; ///< True if the console is initialized
@@ -119,6 +119,8 @@ struct PrintConsole
 #define CONSOLE_COLOR_REVERSE	(1<<6) ///< Reversed color text
 #define CONSOLE_CONCEAL		(1<<7) ///< Concealed text
 #define CONSOLE_CROSSED_OUT	(1<<8) ///< Crossed out text
+#define CONSOLE_FG_CUSTOM	(1<<9) ///< Foreground custom color
+#define CONSOLE_BG_CUSTOM	(1<<10) ///< Background custom color
 
 /// Console debug devices supported by libnx.
 typedef enum {

nx/source/runtime/devices/console.c

@@ -2,9 +2,20 @@
 #include <string.h>
 #include <sys/iosupport.h>
 #include "runtime/devices/console.h"
+#include "display/framebuffer.h"
 
 #include "default_font_bin.h"
 
+static const u8 colorCube[] = {
+	0x00, 0x5f, 0x87, 0xaf, 0xd7, 0xff,
+};
+
+static const u8 grayScale[] = {
+	0x08, 0x12, 0x1c, 0x26, 0x30, 0x3a, 0x44, 0x4e,
+	0x58, 0x62, 0x6c, 0x76, 0x80, 0x8a, 0x94, 0x9e,
+	0xa8, 0xb2, 0xbc, 0xc6, 0xd0, 0xda, 0xe4, 0xee,
+};
+
 //The below width/height is for 720p.
 static PrintConsole defaultConsole =
 {
@@ -32,6 +43,65 @@ static PrintConsole defaultConsole =
 	false	//console initialized
 };
 
+static bool parseColor (char **esc, int *escLen, u16 *color, bool *custom)
+{
+	unsigned int p;
+	unsigned int n;
+	unsigned int r;
+	unsigned int g;
+	unsigned int b;
+	int consumed;
+
+	if (sscanf (*esc, "%d;%n", &p, &consumed) != 1)
+		return false;
+
+	*esc    += consumed;
+	*escLen -= consumed;
+
+	if (p == 5) {
+		if (sscanf (*esc, "%u%n", &n, &consumed) != 1)
+			return false;
+
+		*esc    += consumed;
+		*escLen -= consumed;
+
+		if (n <= 15) {
+			*color  = n;
+			*custom = false;
+		} else if (n <= 231) {
+			n -= 16;
+			r = n / 36;
+			g = (n - r * 36) / 6;
+			b = n - r * 36 - g * 6;
+
+			*color  = RGB565_FROM_RGB8 (colorCube[r], colorCube[g], colorCube[b]);
+			*custom = true;
+		} else if (n <= 255) {
+			n -= 232;
+
+			*color  = RGB565_FROM_RGB8 (grayScale[n], grayScale[n], grayScale[n]);
+			*custom = true;
+		} else {
+			return false;
+		}
+
+		return true;
+	} else if (p == 2) {
+		if (sscanf (*esc, "%u;%u;%u%n", &r, &g, &b, &consumed) != 3)
+			return false;
+
+		*esc    += consumed;
+		*escLen -= consumed;
+
+		*color  = RGB565_FROM_RGB8 (r, g, b);
+		*custom = true;
+
+		return true;
+	}
+
+	return false;
+}
+
 static PrintConsole currentCopy;
 
 static PrintConsole* currentConsole = &currentCopy;
@@ -189,7 +259,7 @@ static ssize_t con_write(struct _reent *r,void *fd,const char *ptr, size_t len)
 		chr = *(tmp++);
 		i++; count++;
 
-		if ( chr == 0x1b && *tmp == '[' ) {
+		if ( chr == 0x1b && len > 1 && *tmp == '[' ) {
 			bool escaping = true;
 			char *escapeseq	= tmp++;
 			int escapelen = 1;
@@ -323,6 +393,7 @@ static ssize_t con_write(struct _reent *r,void *fd,const char *ptr, size_t len)
 						escapelen--;
 
 						do {
+							bool custom;
 							parameter = 0;
 							if (escapelen == 1) {
 								consumed = 1;
@@ -413,19 +484,71 @@ static ssize_t con_write(struct _reent *r,void *fd,const char *ptr, size_t len)
 								break;
 
 							case 30 ... 37: // writing color
+								currentConsole->flags &= ~CONSOLE_FG_CUSTOM;
 								currentConsole->fg     = parameter - 30;
 								break;
 
+							case 38: // custom foreground color
+								if (parseColor (&escapeseq, &escapelen, &currentConsole->fg, &custom)) {
+									if (custom)
+										currentConsole->flags |= CONSOLE_FG_CUSTOM;
+									else
+										currentConsole->flags &= ~CONSOLE_FG_CUSTOM;
+
+									if (!custom && currentConsole->fg < 16) {
+										currentConsole->flags &= ~CONSOLE_COLOR_FAINT;
+										if (currentConsole->fg < 8)
+											currentConsole->flags &= ~CONSOLE_COLOR_BOLD;
+										else
+											currentConsole->flags |= CONSOLE_COLOR_BOLD;
+									}
+
+									// consume next ; or m
+									++escapeseq;
+									--escapelen;
+								} else {
+									// stop processing
+									escapelen = 0;
+								}
+								break;
+
 							case 39: // reset foreground color
+								currentConsole->flags &= ~CONSOLE_FG_CUSTOM;
 								currentConsole->fg     = 7;
 								break;
 
 							case 40 ... 47: // screen color
+								currentConsole->flags &= ~CONSOLE_BG_CUSTOM;
 								currentConsole->bg     = parameter - 40;
 								break;
 
+							case 48: // custom background color
+								if (parseColor (&escapeseq, &escapelen, &currentConsole->bg, &custom)) {
+									if (custom)
+										currentConsole->flags |= CONSOLE_BG_CUSTOM;
+									else
+										currentConsole->flags &= ~CONSOLE_BG_CUSTOM;
+
+									if (!custom && currentConsole->bg < 16) {
+										currentConsole->flags &= ~CONSOLE_COLOR_FAINT;
+										if (currentConsole->bg < 8)
+											currentConsole->flags &= ~CONSOLE_COLOR_BOLD;
+										else
+											currentConsole->flags |= CONSOLE_COLOR_BOLD;
+									}
+
+									// consume next ; or m
+									++escapeseq;
+									--escapelen;
+								} else {
+									// stop processing
+									escapelen = 0;
+								}
+								break;
+
 							case 49: // reset background color
-								currentConsole->fg = 0;
+								currentConsole->flags &= ~CONSOLE_BG_CUSTOM;
+								currentConsole->bg     = 0;
 								break;
 							}
 						} while (escapelen > 0);
@@ -611,7 +734,7 @@ void consolePrintChar(int c) {
 //---------------------------------------------------------------------------------
 void consoleClear(void) {
 //---------------------------------------------------------------------------------
-	printf("\x1b[2J");
+	consoleCls ('2');
 }
 
 //---------------------------------------------------------------------------------

nx/source/runtime/devices/console_sw.c

@@ -109,24 +109,35 @@ static void ConsoleSwRenderer_drawChar(PrintConsole* con, int x, int y, int c)
     u16* frameBuffer = _getFrameBuffer(sw, &stride);
     const u16 *fontdata = (const u16*)con->font.gfx + (16 * c);
 
-    int writingColor = con->fg;
+    u16 fg = con->fg;
-    int screenColor = con->bg;
+    u16 bg = con->bg;
 
+    if (!(con->flags & CONSOLE_FG_CUSTOM)) {
         if (con->flags & CONSOLE_COLOR_BOLD) {
-        writingColor += 8;
+            fg = colorTable[fg + 8];
         } else if (con->flags & CONSOLE_COLOR_FAINT) {
-        writingColor += 16;
+            fg = colorTable[fg + 16];
+        } else {
+            fg = colorTable[fg];
+        }
+    }
+
+    if (!(con->flags & CONSOLE_BG_CUSTOM)) {
+        if (con->flags & CONSOLE_COLOR_BOLD) {
+            bg = colorTable[bg + 8];
+        } else if (con->flags & CONSOLE_COLOR_FAINT) {
+            bg = colorTable[bg + 16];
+        } else {
+            bg = colorTable[bg];
+        }
     }
 
     if (con->flags & CONSOLE_COLOR_REVERSE) {
-        int tmp = writingColor;
+        u16 tmp = fg;
-        writingColor = screenColor;
+        fg = bg;
-        screenColor = tmp;
+        bg = tmp;
     }
 
-    u16 bg = colorTable[screenColor];
-    u16 fg = colorTable[writingColor];
-
     u128 *tmp = (u128*)fontdata;
 
     u128 bvaltop = tmp[0];