QMK Modifier Stripes for OLED Display

oled.h

// include the static image variable
// static const char display [] PROGMEM
#include "images.h"

oled_rotation_t oled_init_user(oled_rotation_t rotation) {
    return OLED_ROTATION_0;
}

// helper to shift bits across an array.
void shiftright(char* buf, int size, int num) {
  for (int i = size-1; i > 0; i--) {
    buf[i] >>= 1;
    if (buf[i-1] & 0x01) {
      buf[i] |= 0x80;
    }
  }
  buf[0] >>= 1;
}

// Display is loaded from bottom right to top left in 1 byte vertical strips.
void mask(char* enable) {
  char rowenable[3];
  const char* data = display;
  int i = 0;
  char blockmask[2] = { 0x00, 0x00 };

  while (i < sizeof(display)) {
    // shift enable-mask right 1 at each row, reinitialize rowenable copy
    if (i % 128 == 0) {
      // skip first iteration
      if (i != 0) {
        shiftright(enable, 3, 2);
      }
      rowenable[0] = enable[0];
      rowenable[1] = enable[1];
      rowenable[2] = enable[2];
    }

    // For each 1 byte by 1 byte block, determine whether mask is fully enabled,
    // or partially enabled. If it is partially enabled a triangle of the underlying
    // image is displayed based on whether or not the preceding block is enabled.
    // This is controlled by shifting in the first byte into the second as each 
    // verticle strip is loaded.
    if (i % 8 == 0) {
      if (rowenable[2] & 0x01) {
        blockmask[1] = 0xff;
      } else {
        blockmask[1] = 0x00;
      }

      if (rowenable[2] & 0x02) {
        blockmask[0] = 0xff;
      } else {
        blockmask[0] = 0x00;
      }
      shiftright(rowenable, 3, 2);
    }

    uint8_t c = pgm_read_byte(data++);
    oled_write_raw_byte(c & blockmask[1], i++);
    shiftright(blockmask, 2, 1);
  }
}

// set 3 byte mask according to active modifiers
void setmask(char* mask) {
  mask[0] = 0x00;
  mask[1] = 0x00;
  mask[2] = 0x00;

  uint8_t mod = get_mods() | get_oneshot_mods();
  if (mod & MOD_MASK_CTRL) {
    mask[1] |= 0xf0;
  }
  if (mod & MOD_MASK_ALT) {
    mask[1] |= 0x0f;
  }
  if (mod & MOD_MASK_GUI) {
    mask[0] |= 0xf0;
  }

  switch (get_highest_layer(layer_state)) {
    case _LOWER:
      mask[2] |= 0xf0;
      break;
    case _RAISE:
      mask[0] |= 0x0f;
      break;
  }
}

bool oled_task_user(void) {
    char m[3];
    setmask(m);
    mask(m);
    return true;
}