Merge pull request #1998 from tannewt/monocolor

Add support for grayscale displays

1 files changed, 28 insertions, 7 deletions

diff --git a/shared-module/displayio/ColorConverter.c b/shared-module/displayio/ColorConverter.c
index 3928e115a..6b9bd5840 100644
--- a/shared-module/displayio/ColorConverter.c
+++ b/shared-module/displayio/ColorConverter.c
@@ -29,15 +29,36 @@
 void common_hal_displayio_colorconverter_construct(displayio_colorconverter_t* self) {
 }
 
-bool common_hal_displayio_colorconverter_convert(displayio_colorconverter_t *self, uint32_t input_color, uint16_t* output_color) {
-    // TODO(tannewt): Validate the color input against the input format.
-    uint32_t r5 = (input_color >> 19);
-    uint32_t g6 = (input_color >> 10) & 0x3f;
-    uint32_t b5 = (input_color >> 3) & 0x1f;
+uint16_t displayio_colorconverter_compute_rgb565(uint32_t color_rgb888) {
+    uint32_t r5 = (color_rgb888 >> 19);
+    uint32_t g6 = (color_rgb888 >> 10) & 0x3f;
+    uint32_t b5 = (color_rgb888 >> 3) & 0x1f;
     uint32_t packed = r5 << 11 | g6 << 5 | b5;
     // swap bytes
-    *output_color = __builtin_bswap16(packed);
-    return true;
+    return __builtin_bswap16(packed);
+}
+
+uint8_t displayio_colorconverter_compute_luma(uint32_t color_rgb888) {
+    uint32_t r8 = (color_rgb888 >> 16);
+    uint32_t g8 = (color_rgb888 >> 8) & 0xff;
+    uint32_t b8 = color_rgb888 & 0xff;
+    return (r8 * 19) / 255 + (g8 * 182) / 255 + (b8 + 54) / 255;
+}
+
+bool displayio_colorconverter_convert(displayio_colorconverter_t *self, const _displayio_colorspace_t* colorspace, uint32_t input_color, uint32_t* output_color) {
+    if (colorspace->depth == 16) {
+        *output_color = displayio_colorconverter_compute_rgb565(input_color);
+        return true;
+    } else if (colorspace->grayscale && colorspace->depth <= 8) {
+        uint8_t luma = displayio_colorconverter_compute_luma(input_color);
+        *output_color = luma >> (8 - colorspace->depth);
+        return true;
+    }
+    return false;
+}
+
+void common_hal_displayio_colorconverter_convert(displayio_colorconverter_t *self, const _displayio_colorspace_t* colorspace, uint32_t input_color, uint32_t* output_color) {
+    displayio_colorconverter_convert(self, colorspace, input_color, output_color);
 }
 
 // Currently no refresh logic is needed for a ColorConverter.