ldlinux: Convert all UsingVGA uses with display_mask_vga()

Introduce display_mask_vga() which does the necessary converting of
UsingVGA into a mask to be AND'd with DisplayMask.

Also, apply TextAttribute when clearing the screen after we've seen a
form feed.

Signed-off-by: Matt Fleming <matt.fleming@intel.com>

1 files changed, 35 insertions, 24 deletions

diff --git a/com32/elflink/ldlinux/msg.c b/com32/elflink/ldlinux/msg.c
index 502b58de..5365eec7 100644
--- a/com32/elflink/ldlinux/msg.c
+++ b/com32/elflink/ldlinux/msg.c
@@ -51,13 +51,18 @@ int get_msg_file(char *filename)
 	return 0;
 }
 
+static inline int display_mask_vga(void)
+{
+	uint8_t mask = UsingVGA & 0x1;
+	return (DisplayMask & ++mask);
+}
+
 static void msg_setbg(uint8_t data)
 {
 	if (unhexchar(&data) == 0) {
 		data <<= 4;
-		if (DisplayMask & UsingVGA) {
+		if (display_mask_vga())
 			TextAttribute = data;
-		}
 
 		NextCharJump = msg_setfg;
 	} else {
@@ -69,7 +74,7 @@ static void msg_setbg(uint8_t data)
 static void msg_setfg(uint8_t data)
 {
 	if (unhexchar(&data) == 0) {
-		if (DisplayMask & UsingVGA) {
+		if (display_mask_vga()) {
 			/* setbg set foreground to 0 */
 			TextAttribute |= data;
 		}
@@ -84,9 +89,32 @@ static inline void msg_ctrl_o(void)
 	NextCharJump = msg_setbg;
 }
 
+/* Convert ANSI colors to PC display attributes */
+static int convert_to_pcdisplay[] = { 0, 4, 2, 6, 1, 5, 3, 7 };
+
+static void set_fgbg(void)
+{
+	uint8_t bg, fg;
+
+	fg = convert_to_pcdisplay[(TextAttribute & 0x7)];
+	bg = convert_to_pcdisplay[((TextAttribute >> 4) & 0x7)];
+
+	printf("\033[");
+	if (TextAttribute & 0x40)
+		printf("1;"); /* Foreground bright */
+
+	printf("3%dm\033[", fg);
+
+	if (TextAttribute & 0x80)
+		printf("5;"); /* Foreground blink */
+
+	printf("4%dm", bg);
+}
+
 static void msg_formfeed(void)
 {
-	printf("\033[2J\033[H");
+	set_fgbg();
+	printf("\033[2J\033[H\033[0m");
 }
 
 static void msg_novga(void)
@@ -138,16 +166,10 @@ static void msg_vga(void)
 	VGAFilePtr = (uint16_t *)VGAFileBuf;
 }
 
-/* Convert ANSI colors to PC display attributes */
-static int convert_to_pcdisplay[] = { 0, 4, 2, 6, 1, 5, 3, 7 };
-
 static void msg_normal(uint8_t data)
 {
-	uint8_t bg, fg;
-	uint8_t mask = UsingVGA & 0x1;
-
 	/* 0x1 = text mode, 0x2 = graphics mode */
-	if (!(DisplayMask & ++mask) || !(DisplayCon & 0x01)) {
+	if (!display_mask_vga() || !(DisplayCon & 0x01)) {
 		/* Write to serial port */
 		if (DisplayMask & 0x4)
 			write_serial(data);
@@ -155,19 +177,8 @@ static void msg_normal(uint8_t data)
 		return;		/* Not screen */
 	}
 
-	fg = convert_to_pcdisplay[(TextAttribute & 0x7)];
-	bg = convert_to_pcdisplay[((TextAttribute >> 4) & 0x7)];
-
-	printf("\033[");
-	if (TextAttribute & 0x40)
-		printf("1;"); /* Foreground bright */
-
-	printf("3%dm\033[", fg);
-
-	if (TextAttribute & 0x80)
-		printf("5;"); /* Foreground blink */
-
-	printf("4%dm%c\033[0m", bg, data);
+	set_fgbg();
+	printf("%c\033[0m", data);
 }
 
 static void msg_modectl(uint8_t data)