aboot: Add "fastboot oem screenshot"

Usage: "fastboot oem screenshot && fastboot get_staged lk2nd.ppm"

Produces a PPM image of current lk2nd screen contents. I felt like
writing some NEON assembly for this, probably a bit overkill but why not?

app/aboot/aboot.c

@@ -3707,6 +3707,40 @@ int fetch_image_from_partition()
 	}
 }
 
+static void cmd_oem_screenshot(const char *arg, void *data, unsigned sz)
+{
+	struct fbcon_config *fb = fbcon_display();
+	unsigned hdr;
+
+	if (!fb) {
+		fastboot_fail("display not initialized");
+		return;
+	}
+
+	if (fb->format != FB_FORMAT_RGB888 || fb->bpp != 24) {
+		fastboot_fail("unsupported fb format\n");
+		return;
+	}
+
+	sz = fb->width * fb->height;
+	if (sz % sizeof(uint64_t) != 0) {
+		fastboot_fail("unsupported display resolution\n");
+		return;
+	}
+
+	/* PPM image header, see http://netpbm.sourceforge.net/doc/ppm.html */
+	hdr = sprintf(data, "P6\n\n%7u %7u\n255\n", fb->width, fb->height);
+	ASSERT(hdr % sizeof(uint64_t) == 0);
+
+	sz = fb->width * fb->height;
+
+	/* PPM expects RGB but this seems to be BGR, so do some fancy swapping! */
+	//memcpy(data + hdr, fb->base, sz * 3);
+	rgb888_swap(fb->base, data + hdr, sz / sizeof(uint64_t));
+
+	fastboot_stage(data, hdr + sz*3);
+}
+
 /* Get the size from partiton name */
 static void get_partition_size(const char *arg, char *response)
 {
@@ -3846,6 +3880,9 @@ void aboot_fastboot_register_commands(void)
 						{"oem off-mode-charge", cmd_oem_off_mode_charger},
 						{"oem select-display-panel", cmd_oem_select_display_panel},
 #endif
+#endif
+#if DISPLAY_SPLASH_SCREEN
+						{"oem screenshot", cmd_oem_screenshot},
 #endif
 						};
 

arch/arm/ops.S

@@ -286,3 +286,14 @@ FUNCTION(arch_cycle_count)
 FUNCTION(smc_call)
 	smc		#0
 	bx		lr
+
+/* void rgb888_swap(const void *in, void *out, uint32_t npixels8) */
+/* https://developer.arm.com/documentation/den0018/a/NEON-Code-Examples-with-Intrinsics/Swapping-color-channels */
+FUNCTION(rgb888_swap)
+	.fpu neon
+0:	vld3.8  {d0,d1,d2}, [r0]!
+	vswp    d0, d2
+	vst3.8  {d0,d1,d2}, [r1]!
+	subs    r2, r2, #1
+	bne     0b
+	bx      lr

include/arch/ops.h

@@ -40,6 +40,8 @@ int atomic_add(volatile int *ptr, int val);
 int atomic_and(volatile int *ptr, int val);
 int atomic_or(volatile int *ptr, int val);
 
+void rgb888_swap(const void *in, void *out, uint32_t npixels8);
+
 #endif // !ASSEMBLY
 #define ICACHE 1
 #define DCACHE 2