From 5168f6ec1f52aa1506b585850c6b9cb748d7259c Mon Sep 17 00:00:00 2001
From: Jeff Epler <jepler@gmail.com>
Date: Wed, 10 Aug 2022 11:13:26 -0500
Subject: [PATCH] Add support for RGB565 images in qrio

Most cameras produce RGB565_SWAPPED data
---
 shared-bindings/qrio/PixelPolicy.c | 10 ++++++++++
 shared-bindings/qrio/PixelPolicy.h |  2 +-
 shared-module/qrio/QRDecoder.c     | 15 +++++++++++++++
 3 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/shared-bindings/qrio/PixelPolicy.c b/shared-bindings/qrio/PixelPolicy.c
index 6887081b24..656045556b 100644
--- a/shared-bindings/qrio/PixelPolicy.c
+++ b/shared-bindings/qrio/PixelPolicy.c
@@ -39,13 +39,23 @@
 //|     ODD_BYTES: PixelPolicy
 //|     """The input buffer to `QRDecoder.decode` consists of greyscale values in positions 1, 3, …, and ignored bytes in positions 0, 2, ….  This can decode directly from YUV images where the odd bytes hold the Y (luminance) data"""
 //|
+//|     RGB565_SWAPPED: PixelPolicy
+//|     """The input buffer to `QRDecoder.decode` consists of RGB565 values in byte-swapped order.  The green component is used."""
+//|
+//|     RGB565: PixelPolicy
+//|     """The input buffer to `QRDecoder.decode` consists of RGB565 values.  The green component is used."""
+//|
 
 MAKE_ENUM_VALUE(qrio_pixel_policy_type, qrio_pixel_policy, EVERY_BYTE, QRIO_EVERY_BYTE);
+MAKE_ENUM_VALUE(qrio_pixel_policy_type, qrio_pixel_policy, RGB565, QRIO_RGB565);
+MAKE_ENUM_VALUE(qrio_pixel_policy_type, qrio_pixel_policy, RGB565_SWAPPED, QRIO_RGB565_SWAPPED);
 MAKE_ENUM_VALUE(qrio_pixel_policy_type, qrio_pixel_policy, EVEN_BYTES, QRIO_EVEN_BYTES);
 MAKE_ENUM_VALUE(qrio_pixel_policy_type, qrio_pixel_policy, ODD_BYTES, QRIO_EVEN_BYTES);
 
 MAKE_ENUM_MAP(qrio_pixel_policy) {
     MAKE_ENUM_MAP_ENTRY(qrio_pixel_policy, EVERY_BYTE),
+    MAKE_ENUM_MAP_ENTRY(qrio_pixel_policy, RGB565),
+    MAKE_ENUM_MAP_ENTRY(qrio_pixel_policy, RGB565_SWAPPED),
     MAKE_ENUM_MAP_ENTRY(qrio_pixel_policy, EVEN_BYTES),
     MAKE_ENUM_MAP_ENTRY(qrio_pixel_policy, ODD_BYTES),
 };
diff --git a/shared-bindings/qrio/PixelPolicy.h b/shared-bindings/qrio/PixelPolicy.h
index 8be5dde1cc..36c1d271fd 100644
--- a/shared-bindings/qrio/PixelPolicy.h
+++ b/shared-bindings/qrio/PixelPolicy.h
@@ -33,7 +33,7 @@
 extern const mp_obj_type_t qrio_pixel_policy_type;
 
 typedef enum {
-    QRIO_EVERY_BYTE, QRIO_EVEN_BYTES, QRIO_ODD_BYTES
+    QRIO_EVERY_BYTE, QRIO_EVEN_BYTES, QRIO_ODD_BYTES, QRIO_RGB565, QRIO_RGB565_SWAPPED
 } qrio_pixel_policy_t;
 
 extern const cp_enum_obj_t qrio_pixel_policy_EVERY_BYTE_obj;
diff --git a/shared-module/qrio/QRDecoder.c b/shared-module/qrio/QRDecoder.c
index 26a609f215..f7b25362e4 100644
--- a/shared-module/qrio/QRDecoder.c
+++ b/shared-module/qrio/QRDecoder.c
@@ -104,6 +104,20 @@ mp_obj_t shared_module_qrio_qrdecoder_decode(qrdecoder_qrdecoder_obj_t *self, co
     uint8_t *src = bufinfo->buf;
 
     switch (policy) {
+        case QRIO_RGB565: {
+            uint16_t *src16 = bufinfo->buf;
+            for (int i = 0; i < width * height; i++) {
+                framebuffer[i] = (src16[i] >> 3) & 0xfc;
+            }
+            break;
+        }
+        case QRIO_RGB565_SWAPPED: {
+            uint16_t *src16 = bufinfo->buf;
+            for (int i = 0; i < width * height; i++) {
+                framebuffer[i] = (__builtin_bswap16(src16[i]) >> 3) & 0xfc;
+            }
+            break;
+        }
         case QRIO_EVERY_BYTE:
             memcpy(framebuffer, src, width * height);
             break;
@@ -116,6 +130,7 @@ mp_obj_t shared_module_qrio_qrdecoder_decode(qrdecoder_qrdecoder_obj_t *self, co
             for (int i = 0; i < width * height; i++) {
                 framebuffer[i] = src[2 * i];
             }
+            break;
     }
     quirc_end(self->quirc);