diff --git a/main/display/lispif_disp_extensions.c b/main/display/lispif_disp_extensions.c index b31c8729..4948e3fd 100644 --- a/main/display/lispif_disp_extensions.c +++ b/main/display/lispif_disp_extensions.c @@ -1646,9 +1646,11 @@ static void img_putc(image_buffer_t *img, int x, int y, int fg, int bg, uint8_t uint8_t w = font_data[0]; uint8_t h = font_data[1]; uint8_t char_num = font_data[2]; - int bytes_per_char = (w * h) / 8; - - if ((w * h) % 8 != 0) { + uint8_t bits_per_pixel = font_data[3]; + + int pixels_per_byte = 8 / bits_per_pixel; + int bytes_per_char = (w * h) / pixels_per_byte; + if ((w * h) % pixels_per_byte != 0) { bytes_per_char += 1; } @@ -1662,14 +1664,25 @@ static void img_putc(image_buffer_t *img, int x, int y, int fg, int bg, uint8_t return; } - for (int i = 0; i < w * h; i++) { - uint8_t byte = font_data[4 + bytes_per_char * ch + (i / 8)]; - uint8_t bit_pos = i % 8; - uint8_t bit = byte & (1 << bit_pos); - if (bit || bg >= 0) { + if (bits_per_pixel == 2) { + for (int i = 0; i < w * h; i++) { + uint8_t byte = font_data[4 + bytes_per_char * ch + (i / 4)]; + uint8_t bit_pos = i % pixels_per_byte; + uint8_t pixel_value = (byte >> (bit_pos * 2)) & 0x03; int x0 = i % w; int y0 = i / w; - putpixel(img, x + x0, y + y0, bit ? fg : bg); + putpixel(img, x + x0, y + y0, pixel_value); + } + } else { + for (int i = 0; i < w * h; i++) { + uint8_t byte = font_data[4 + bytes_per_char * ch + (i / 8)]; + uint8_t bit_pos = i % 8; + uint8_t bit = byte & (1 << bit_pos); + if (bit || bg >= 0) { + int x0 = i % w; + int y0 = i / w; + putpixel(img, x + x0, y + y0, bit ? fg : bg); + } } } }