1 files changed, 43 insertions, 5 deletions

diff --git a/extmod/modframebuf.c b/extmod/modframebuf.c
index 792a3a7fa2..33985dd00a 100644
--- a/extmod/modframebuf.c
+++ b/extmod/modframebuf.c
@@ -53,6 +53,41 @@ typedef struct _mp_framebuf_p_t {
     fill_rect_t fill_rect;
 } mp_framebuf_p_t;
 
+// constants for formats
+#define FRAMEBUF_MVLSB    (0)
+#define FRAMEBUF_RGB565   (1)
+#define FRAMEBUF_GS4_HMSB (2)
+#define FRAMEBUF_MHLSB    (3)
+#define FRAMEBUF_MHMSB    (4)
+
+// Functions for MHLSB and MHMSB
+
+STATIC void mono_horiz_setpixel(const mp_obj_framebuf_t *fb, int x, int y, uint32_t color) {
+    size_t index = (x + y * fb->stride) >> 3;
+    int offset = fb->format == FRAMEBUF_MHMSB ? x & 0x07 : 7 - (x & 0x07);
+    ((uint8_t*)fb->buf)[index] = (((uint8_t*)fb->buf)[index] & ~(0x01 << offset)) | ((color != 0) << offset);
+}
+
+STATIC uint32_t mono_horiz_getpixel(const mp_obj_framebuf_t *fb, int x, int y) {
+    size_t index = (x + y * fb->stride) >> 3;
+    int offset = fb->format == FRAMEBUF_MHMSB ? x & 0x07 : 7 - (x & 0x07);
+    return (((uint8_t*)fb->buf)[index] >> (offset)) & 0x01;
+}
+
+STATIC void mono_horiz_fill_rect(const mp_obj_framebuf_t *fb, int x, int y, int w, int h, uint32_t col) {
+    int reverse = fb->format == FRAMEBUF_MHMSB;
+    int advance = fb->stride >> 3;
+    while (w--) {
+        uint8_t *b = &((uint8_t*)fb->buf)[(x >> 3) + y * advance];
+        int offset = reverse ?  x & 7 : 7 - (x & 7);
+        for (int hh = h; hh; --hh) {
+            *b = (*b & ~(0x01 << offset)) | ((col != 0) << offset);
+            b += advance;
+        }
+        ++x;
+    }
+}
+
 // Functions for MVLSB format
 
 STATIC void mvlsb_setpixel(const mp_obj_framebuf_t *fb, int x, int y, uint32_t color) {
@@ -148,15 +183,12 @@ STATIC void gs4_hmsb_fill_rect(const mp_obj_framebuf_t *fb, int x, int y, int w,
     }
 }
 
-// constants for formats
-#define FRAMEBUF_MVLSB    (0)
-#define FRAMEBUF_RGB565   (1)
-#define FRAMEBUF_GS4_HMSB (2)
-
 STATIC mp_framebuf_p_t formats[] = {
     [FRAMEBUF_MVLSB] = {mvlsb_setpixel, mvlsb_getpixel, mvlsb_fill_rect},
     [FRAMEBUF_RGB565] = {rgb565_setpixel, rgb565_getpixel, rgb565_fill_rect},
     [FRAMEBUF_GS4_HMSB] = {gs4_hmsb_setpixel, gs4_hmsb_getpixel, gs4_hmsb_fill_rect},
+    [FRAMEBUF_MHLSB] = {mono_horiz_setpixel, mono_horiz_getpixel, mono_horiz_fill_rect},
+    [FRAMEBUF_MHMSB] = {mono_horiz_setpixel, mono_horiz_getpixel, mono_horiz_fill_rect},
 };
 
 static inline void setpixel(const mp_obj_framebuf_t *fb, int x, int y, uint32_t color) {
@@ -207,6 +239,10 @@ STATIC mp_obj_t framebuf_make_new(const mp_obj_type_t *type, size_t n_args, size
         case FRAMEBUF_RGB565:
         case FRAMEBUF_GS4_HMSB:
             break;
+        case FRAMEBUF_MHLSB:
+        case FRAMEBUF_MHMSB:
+            o->stride = (o->stride + 7) & ~7;
+            break;
         default:
             nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_ValueError,
                 "invalid format"));
@@ -545,6 +581,8 @@ STATIC const mp_rom_map_elem_t framebuf_module_globals_table[] = {
     { MP_ROM_QSTR(MP_QSTR_MVLSB), MP_OBJ_NEW_SMALL_INT(FRAMEBUF_MVLSB) },
     { MP_ROM_QSTR(MP_QSTR_RGB565), MP_OBJ_NEW_SMALL_INT(FRAMEBUF_RGB565) },
     { MP_ROM_QSTR(MP_QSTR_GS4_HMSB), MP_OBJ_NEW_SMALL_INT(FRAMEBUF_GS4_HMSB) },
+    { MP_ROM_QSTR(MP_QSTR_MHLSB), MP_OBJ_NEW_SMALL_INT(FRAMEBUF_MHLSB) },
+    { MP_ROM_QSTR(MP_QSTR_MHMSB), MP_OBJ_NEW_SMALL_INT(FRAMEBUF_MHMSB) },
 };
 
 STATIC MP_DEFINE_CONST_DICT(framebuf_module_globals, framebuf_module_globals_table);