Merge branch 'ssd1306_more_functions' of https://github.com/Zaltora/esp-open-rtos into Zaltora-ssd1306_more_functions

examples/ssd1306_fps/Makefile

@@ -0,0 +1,3 @@
+PROGRAM=SSD1306_fps
+EXTRA_COMPONENTS = extras/ssd1306 extras/i2c extras/fonts
+include ../../common.mk

examples/ssd1306_fps/main.c

@@ -0,0 +1,150 @@
+#include <espressif/esp_common.h>
+#include <esp/uart.h>
+#include <FreeRTOS.h>
+#include <task.h>
+#include <queue.h>
+#include <timers.h>
+#include <string.h>
+#include <ssd1306/ssd1306.h>
+
+#define LOAD_ICON_X 54
+#define LOAD_ICON_Y 32
+#define LOAD_ICON_SIZE 20
+
+#define CIRCLE_COUNT_ICON_X 94
+#define CIRCLE_COUNT_ICON_Y 42
+
+
+/* Remove this line if your display connected by SPI */
+#define I2C_CONNECTION
+
+#ifdef I2C_CONNECTION
+    #include <i2c/i2c.h>
+#endif
+#include "fonts/fonts.h"
+
+/* Change this according to you schematics and display size */
+#define DISPLAY_WIDTH  128
+#define DISPLAY_HEIGHT 64
+
+#ifdef I2C_CONNECTION
+    #define PROTOCOL SSD1306_PROTO_I2C
+    #define ADDR     SSD1306_I2C_ADDR_0
+    #define SCL_PIN  14
+    #define SDA_PIN  13
+#else
+    #define PROTOCOL SSD1306_PROTO_SPI4
+    #define CS_PIN   5
+    #define DC_PIN   4
+#endif
+
+/* Declare device descriptor */
+static const ssd1306_t dev = {
+    .protocol = PROTOCOL,
+#ifdef I2C_CONNECTION
+    .addr     = ADDR,
+#else
+    .cs_pin   = CS_PIN,
+    .dc_pin   = DC_PIN,
+#endif
+    .width    = DISPLAY_WIDTH,
+    .height   = DISPLAY_HEIGHT
+};
+
+/* Local frame buffer */
+static uint8_t buffer[DISPLAY_WIDTH * DISPLAY_HEIGHT / 8];
+
+TimerHandle_t timerHandle = 0 ; // Timer handler
+uint8_t frame_done = 0 ; // number of frame send.
+uint8_t fps = 0 ; // image per second.
+
+
+static void ssd1306_task(void *pvParameters)
+{
+    printf("%s: Started user interface task\n", __FUNCTION__);
+    vTaskDelay(1000/portTICK_PERIOD_MS);
+
+    ssd1306_set_whole_display_lighting(&dev, false);
+
+
+    char text[20] ;
+    uint8_t x0 = LOAD_ICON_X ;
+    uint8_t y0 = LOAD_ICON_Y ;
+    uint8_t x1 = LOAD_ICON_X + LOAD_ICON_SIZE ;
+    uint8_t y1 = LOAD_ICON_Y + LOAD_ICON_SIZE ;
+    uint16_t count = 0 ;
+    ssd1306_select_font(1);
+
+    while (1) {
+
+        ssd1306_draw_string(&dev, buffer, 10, 10,"Hello, esp-open-rtos !", OLED_COLOR_WHITE, OLED_COLOR_BLACK) ;
+        sprintf(text,"FPS: %u",fps);
+        ssd1306_draw_string(&dev, buffer, 10, 40, text, OLED_COLOR_WHITE, OLED_COLOR_BLACK) ;
+
+        //generate loading icon
+        ssd1306_draw_line(&dev, buffer, x0, y0, x1, y1, OLED_COLOR_BLACK);
+        if (x0 < (LOAD_ICON_X + LOAD_ICON_SIZE)) { x0++ ; x1-- ; }
+        else if(y0< (LOAD_ICON_Y + LOAD_ICON_SIZE)) { y0++ ; y1-- ; }
+        else { x0 = LOAD_ICON_X ; y0 = LOAD_ICON_Y ; x1 = LOAD_ICON_X + LOAD_ICON_SIZE ; y1 = LOAD_ICON_Y + LOAD_ICON_SIZE ; }
+        ssd1306_draw_line(&dev, buffer, x0, y0, x1, y1, OLED_COLOR_WHITE);
+        ssd1306_draw_rectangle(&dev, buffer, LOAD_ICON_X, LOAD_ICON_Y, LOAD_ICON_SIZE+1, LOAD_ICON_SIZE+1, OLED_COLOR_WHITE);
+
+        //generate circle counting
+        for (uint8_t i = 0 ; i < 10 ; i++ ) {
+            if ((count>>i) & 0x01)  ssd1306_draw_circle(&dev,buffer,CIRCLE_COUNT_ICON_X, CIRCLE_COUNT_ICON_Y, i, OLED_COLOR_BLACK);
+        }
+
+        if(count != 0x03FF) count++;
+        else count = 0 ;
+
+        for (uint8_t i = 0 ; i < 10 ; i++ ) {
+            if ((count>>i) & 0x01) ssd1306_draw_circle(&dev,buffer, CIRCLE_COUNT_ICON_X, CIRCLE_COUNT_ICON_Y, i, OLED_COLOR_WHITE);
+        }
+
+        if (ssd1306_load_frame_buffer(&dev, buffer))
+            goto error_loop;
+        frame_done++ ;
+    }
+
+error_loop:
+    printf("%s: error while loading framebuffer into SSD1306\n", __func__);
+    for(;;){
+        vTaskDelay(2000 / portTICK_PERIOD_MS);
+        printf("%s: error loop\n", __FUNCTION__);
+    }
+}
+
+void SoftTimer( TimerHandle_t xTimer ){
+    fps = frame_done ; // Save number of frame already send to screen
+    frame_done = 0 ;
+
+}
+
+
+void user_init(void)
+{
+    //uncomment to test with CPU overclocked
+    //sdk_system_update_cpu_freq(160);
+
+    // Setup HW
+    uart_set_baud(0, 115200);
+
+    printf("SDK version:%s\n", sdk_system_get_sdk_version());
+
+#ifdef I2C_CONNECTION
+    i2c_init(SCL_PIN, SDA_PIN);
+#endif
+
+    while (ssd1306_init(&dev) != 0)
+    {
+        printf("%s: failed to init SSD1306 lcd\n", __func__);
+        vTaskDelay(1000/portTICK_PERIOD_MS);
+    }
+    ssd1306_set_whole_display_lighting(&dev, true);
+    vTaskDelay(1000/portTICK_PERIOD_MS);
+    // Create user interface task
+
+    xTaskCreate(ssd1306_task, "ssd1306_task", 256, NULL, 2, NULL);
+    timerHandle = xTimerCreate("Timer", 1000/portTICK_PERIOD_MS, pdTRUE, NULL, SoftTimer);
+    xTimerStart(timerHandle,0);
+}

extras/fonts/component.mk

@@ -0,0 +1,9 @@
+# Component makefile for extras/fonts
+
+# expected anyone using bmp driver includes it as 'fonts/fonts.h'
+INC_DIRS += $(fonts_ROOT)..
+
+# args for passing into compile rule generation
+fonts_SRC_DIR =  $(fonts_ROOT)
+
+$(eval $(call component_compile_rules,fonts))

extras/fonts/config.h

@@ -0,0 +1,8 @@
+#ifndef _EXTRAS_FONTS_CONFIG_H_
+#define _EXTRAS_FONTS_CONFIG_H_
+
+#ifndef NUM_FONTS
+#define NUM_FONTS 3
+#endif
+
+#endif /* _EXTRAS_FONTS_CONFIG_H_ */

extras/fonts/fonts.c

@@ -0,0 +1,18 @@
+/*
+ * oled_fonts.c
+ *
+ *  Created on: 8 dec. 2016
+ *      Author: zaltora
+ */
+#include "fonts.h"
+
+extern const font_info_t  glcd_5x7_font_info;
+extern const font_info_t  roboto_8ptFontInfo;
+extern const font_info_t  roboto_10ptFontInfo;
+
+const font_info_t*  fonts[NUM_FONTS] =
+{
+    &glcd_5x7_font_info,
+    &roboto_8ptFontInfo,
+    &roboto_10ptFontInfo,
+};

extras/fonts/fonts.h

@@ -0,0 +1,38 @@
+/*
+ * fonts.h
+ *
+ *  Created on: 8 dec. 2016
+ *      Author: zaltora
+ */
+
+#ifndef FONTS_H
+#define FONTS_H
+
+#include <FreeRTOS.h>
+#include "config.h"
+
+/**
+ * Character descriptor
+ */
+typedef struct _font_char_desc
+{
+    uint8_t width;      //Character width in pixel
+    uint16_t offset;    //Offset of this character in bitmap
+} font_char_desc_t;
+
+/**
+ * Font information
+ */
+typedef struct _font_info
+{
+    uint8_t height;         //Character height in pixel, all characters have same height
+    uint8_t c;              //Simulation of "C" width in TrueType term, the space between adjacent characters
+    char char_start;        //First character
+    char char_end;          //Last character
+    const font_char_desc_t* char_descriptors; //descriptor for each character
+    const uint8_t *bitmap;  //Character bitmap
+} font_info_t;
+
+extern const font_info_t* fonts[NUM_FONTS];  //Built-in fonts
+
+#endif // _FONTS__H_

extras/ssd1306/ssd1306.c

@@ -19,6 +19,7 @@
 #include <esp/gpio.h>
 #include <FreeRTOS.h>
 #include <task.h>
+#include <fonts/fonts.h>
 
 #define SPI_BUS 1
 
@@ -68,6 +69,11 @@
 #define debug(fmt, ...)
 #endif
 
+#define abs(x) ((x)<0 ? -(x) : (x))
+#define swap(x, y) do { typeof(x) temp##x##y = x; x = y; y = temp##x##y; } while (0)
+
+const font_info_t* font ; // save font selection
+
 /* Issue a command to SSD1306 device
  * I2C proto format:
  * |S|Slave Address|W|ACK|0x00|Command|Ack|P|
@@ -403,4 +409,586 @@ int ssd1306_load_xbm(const ssd1306_t *dev, uint8_t *xbm, uint8_t *fb)
     return ssd1306_load_frame_buffer(dev, fb);
 }
 
+int ssd1306_draw_pixel(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, ssd1306_color_t color)
+{
+    uint16_t index;
 
+    if ((x >= dev->width) || (x < 0) || (y >= dev->height) || (y < 0))
+        return -EINVAL;
+
+    index = x + (y / 8) * dev->width;
+    switch (color)
+    {
+    case OLED_COLOR_WHITE:
+        fb[index] |= (1 << (y & 7));
+        break;
+    case OLED_COLOR_BLACK:
+        fb[index] &= ~(1 << (y & 7));
+        break;
+    case OLED_COLOR_INVERT:
+        fb[index] ^= (1 << (y & 7));
+        break;
+    default:
+        break;
+    }
+    return 0 ;
+}
+
+int ssd1306_draw_hline(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, uint8_t w, ssd1306_color_t color)
+{
+    uint16_t index;
+    uint8_t mask, t;
+
+    // boundary check
+    if ((x >= dev->width) || (x < 0) || (y >= dev->height) || (y < 0))
+        return -EINVAL;
+    if (w == 0)
+        return -EINVAL;
+    if (x + w > dev->width)
+        w = dev->width - x;
+
+    t = w;
+    index = x + (y / 8) * dev->width;
+    mask = 1 << (y & 7);
+    switch (color)
+    {
+    case OLED_COLOR_WHITE:
+        while (t--)
+        {
+            fb[index] |= mask;
+            ++index;
+        }
+        break;
+    case OLED_COLOR_BLACK:
+        mask = ~mask;
+        while (t--)
+        {
+            fb[index] &= mask;
+            ++index;
+        }
+        break;
+    case OLED_COLOR_INVERT:
+        while (t--)
+        {
+            fb[index] ^= mask;
+            ++index;
+        }
+        break;
+    default:
+        break;
+    }
+    return 0 ;
+}
+
+int ssd1306_draw_vline(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, uint8_t h, ssd1306_color_t color)
+{
+    uint16_t index;
+    uint8_t mask, mod, t;
+
+    // boundary check
+    if ((x >= dev->width) || (x < 0) || (y >= dev->height) || (y < 0))
+        return -EINVAL;
+    if (h == 0)
+        return -EINVAL;
+    if (y + h > dev->height)
+        h = dev->height - y;
+
+    t = h;
+    index = x + (y / 8) * dev->width;
+    mod = y & 7;
+    if (mod) // partial line that does not fit into byte at top
+    {
+        // Magic from Adafruit
+        mod = 8 - mod;
+        static const uint8_t premask[8] = { 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0xFE };
+        mask = premask[mod];
+        if (t < mod)
+            mask &= (0xFF >> (mod - t));
+        switch (color)
+        {
+        case OLED_COLOR_WHITE:
+            fb[index] |= mask;
+            break;
+        case OLED_COLOR_BLACK:
+            fb[index] &= ~mask;
+            break;
+        case OLED_COLOR_INVERT:
+            fb[index] ^= mask;
+            break;
+        default:
+            break;
+        }
+
+        if (t < mod)
+            return 0;
+        t -= mod;
+        index += dev->width;
+    }
+    if (t >= 8) // byte aligned line at middle
+    {
+        switch (color)
+        {
+        case OLED_COLOR_WHITE:
+            do
+           {
+               fb[index] = 0xff;
+               index += dev->width;
+               t -= 8;
+           } while (t >= 8);
+            break;
+        case OLED_COLOR_BLACK:
+            do
+            {
+               fb[index] = 0x00;
+               index += dev->width;
+               t -= 8;
+            } while (t >= 8);
+            break;
+        case OLED_COLOR_INVERT:
+            do
+            {
+                fb[index] = ~fb[index];
+                index += dev->width;
+                t -= 8;
+            } while (t >= 8);
+            break;
+        default:
+            break;
+        }
+    }
+    if (t) // // partial line at bottom
+    {
+        mod = t & 7;
+        static const uint8_t postmask[8] = {0x00, 0x01, 0x03, 0x07, 0x0F, 0x1F, 0x3F, 0x7F };
+        mask = postmask[mod];
+        switch (color)
+        {
+        case OLED_COLOR_WHITE:
+            fb[index] |= mask;
+            break;
+        case OLED_COLOR_BLACK:
+            fb[index] &= ~mask;
+            break;
+        case OLED_COLOR_INVERT:
+            fb[index] ^= mask;
+            break;
+        default:
+            break;
+        }
+
+
+    }
+    return 0;
+}
+
+int ssd1306_draw_rectangle(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, uint8_t w, uint8_t h, ssd1306_color_t color)
+{
+    int err = 0;
+    if ((err = ssd1306_draw_hline(dev, fb, x, y, w, color)))
+        return err;
+    if ((err = ssd1306_draw_hline(dev, fb, x, y + h - 1, w, color)))
+        return err;
+    if ((err = ssd1306_draw_vline(dev, fb, x, y, h, color)))
+        return err;
+    return ssd1306_draw_vline(dev, fb, x + w - 1, y, h, color);
+
+}
+
+
+int ssd1306_fill_rectangle(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, uint8_t w, uint8_t h, ssd1306_color_t color)
+{
+    // Can be optimized?
+    uint8_t i;
+    int err = 0;
+    for (i = x; i < x + w; ++i)
+        if ((err = ssd1306_draw_vline(dev, fb, i, y, h, color)))
+            return err;
+    return 0 ;
+}
+
+int ssd1306_draw_circle(const ssd1306_t *dev, uint8_t *fb, int8_t x0, int8_t y0, uint8_t r, ssd1306_color_t color)
+{
+    // Refer to http://en.wikipedia.org/wiki/Midpoint_circle_algorithm for the algorithm
+
+    int8_t x = r;
+    int8_t y = 1;
+    int16_t radius_err = 1 - x;
+    int err = 0;
+
+    if (r == 0)
+        return -EINVAL;
+
+    if ((err = ssd1306_draw_pixel(dev, fb, x0 - r, y0,     color)))
+        return err;
+    if ((err = ssd1306_draw_pixel(dev, fb, x0 + r, y0,     color)))
+        return err;
+    if ((err = ssd1306_draw_pixel(dev, fb, x0,     y0 - r, color)))
+        return err;
+    if ((err = ssd1306_draw_pixel(dev, fb, x0,     y0 + r, color)))
+        return err;
+
+    while (x >= y)
+    {
+        if ((err = ssd1306_draw_pixel(dev, fb, x0 + x, y0 + y, color)))
+            return err;
+        if ((err = ssd1306_draw_pixel(dev, fb, x0 - x, y0 + y, color)))
+            return err;
+        if ((err = ssd1306_draw_pixel(dev, fb, x0 + x, y0 - y, color)))
+            return err;
+        if ((err = ssd1306_draw_pixel(dev, fb, x0 - x, y0 - y, color)))
+            return err;
+        if (x != y)
+        {
+            /* Otherwise the 4 drawings below are the same as above, causing
+             * problem when color is INVERT
+             */
+            if ((err = ssd1306_draw_pixel(dev, fb, x0 + y, y0 + x, color)))
+                return err;
+            if ((err = ssd1306_draw_pixel(dev, fb, x0 - y, y0 + x, color)))
+                return err;
+            if ((err = ssd1306_draw_pixel(dev, fb, x0 + y, y0 - x, color)))
+                return err;
+            if ((err = ssd1306_draw_pixel(dev, fb, x0 - y, y0 - x, color)))
+                return err;
+        }
+        ++y;
+        if (radius_err < 0)
+        {
+            radius_err += 2 * y + 1;
+        }
+        else
+        {
+            --x;
+            radius_err += 2 * (y - x + 1);
+        }
+
+    }
+    return 0 ;
+}
+
+int ssd1306_fill_circle(const ssd1306_t *dev, uint8_t *fb, int8_t x0, int8_t y0, uint8_t r, ssd1306_color_t color)
+{
+    int8_t x = 1;
+    int8_t y = r;
+    int16_t radius_err = 1 - y;
+    int8_t x1;
+    int err = 0;
+
+    if (r == 0)
+        return -EINVAL;
+
+    if ((err = ssd1306_draw_vline(dev, fb, x0, y0 - r, 2 * r + 1, color))) // Center vertical line
+        return err;
+    while (y >= x)
+    {
+        if ((err = ssd1306_draw_vline(dev, fb, x0 - x, y0 - y, 2 * y + 1, color)))
+            return err;
+        if ((err = ssd1306_draw_vline(dev, fb, x0 + x, y0 - y, 2 * y + 1, color)))
+            return err;
+        if (color != OLED_COLOR_INVERT)
+        {
+            if ((err = ssd1306_draw_vline(dev, fb, x0 - y, y0 - x, 2 * x + 1, color)))
+                return err;
+            if ((err = ssd1306_draw_vline(dev, fb, x0 + y, y0 - x, 2 * x + 1, color)))
+                return err;
+        }
+        ++x;
+        if (radius_err < 0)
+        {
+            radius_err += 2 * x + 1;
+        }
+        else
+        {
+            --y;
+            radius_err += 2 * (x - y + 1);
+        }
+    }
+
+    if (color == OLED_COLOR_INVERT)
+    {
+        x1 = x; // Save where we stopped
+
+        y = 1;
+        x = r;
+        radius_err = 1 - x;
+        if ((err = ssd1306_draw_hline(dev, fb, x0 + x1, y0, r - x1 + 1, color)))
+            return err;
+        if ((err = ssd1306_draw_hline(dev, fb, x0 - r, y0, r - x1 + 1, color)))
+            return err;
+        while (x >= y)
+        {
+            if ((err = ssd1306_draw_hline(dev, fb, x0 + x1, y0 - y, x - x1 + 1, color)))
+                return err;
+            if ((err = ssd1306_draw_hline(dev, fb, x0 + x1, y0 + y, x - x1 + 1, color)))
+                return err;
+            if ((err = ssd1306_draw_hline(dev, fb, x0 - x,  y0 - y, x - x1 + 1, color)))
+                return err;
+            if ((err = ssd1306_draw_hline(dev, fb, x0 - x,  y0 + y, x - x1 + 1, color)))
+                return err;
+            ++y;
+            if (radius_err < 0)
+            {
+                radius_err += 2 * y + 1;
+            }
+            else
+            {
+                --x;
+                radius_err += 2 * (y - x + 1);
+            }
+        }
+    }
+    return 0 ;
+}
+
+int ssd1306_draw_line(const ssd1306_t *dev, uint8_t *fb, int16_t x0, int16_t y0, int16_t x1, int16_t y1, ssd1306_color_t color) {
+
+    if ((x0 >= dev->width) || (x0 < 0) || (y0 >= dev->height) || (y0 < 0))
+        return -EINVAL;
+    if ((x1 >= dev->width) || (x1 < 0) || (y1 >= dev->height) || (y1 < 0))
+        return -EINVAL;
+
+   int err ;
+   int16_t steep = abs(y1 - y0) > abs(x1 - x0);
+   if (steep) {
+       swap(x0, y0);
+       swap(x1, y1);
+   }
+
+   if (x0 > x1) {
+       swap(x0, x1);
+       swap(y0, y1);
+   }
+
+   int16_t dx, dy;
+   dx = x1 - x0;
+   dy = abs(y1 - y0);
+
+   int16_t errx = dx / 2;
+   int16_t ystep;
+
+   if (y0 < y1) {
+       ystep = 1;
+   } else {
+       ystep = -1;
+   }
+
+   for (; x0<=x1; x0++) {
+       if (steep) {
+           if ((err = ssd1306_draw_pixel(dev, fb, y0, x0, color)))
+               return err;
+       } else {
+           if ((err = ssd1306_draw_pixel(dev, fb, x0, y0, color)))
+               return err;
+       }
+       errx -= dy;
+       if (errx < 0) {
+           y0 += ystep;
+           errx += dx;
+       }
+   }
+   return 0;
+}
+
+int ssd1306_draw_triangle(const ssd1306_t *dev, uint8_t *fb, int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, ssd1306_color_t color) {
+    int err ;
+    if ((err = ssd1306_draw_line(dev, fb, x0, y0, x1, y1, color)))
+        return err;
+    if ((err = ssd1306_draw_line(dev, fb, x1, y1, x2, y2, color)))
+        return err;
+    return ssd1306_draw_line(dev, fb, x2, y2, x0, y0, color);
+}
+
+int ssd1306_fill_triangle(const ssd1306_t *dev, uint8_t *fb, int16_t x0, int16_t y0,int16_t x1, int16_t y1, int16_t x2, int16_t y2, ssd1306_color_t color) {
+
+   int16_t a, b, y, last;
+   int err ;
+
+   // Sort coordinates by Y order (y2 >= y1 >= y0)
+   if (y0 > y1) {
+       swap(y0, y1); swap(x0, x1);
+   }
+   if (y1 > y2) {
+       swap(y2, y1); swap(x2, x1);
+   }
+   if (y0 > y1) {
+       swap(y0, y1); swap(x0, x1);
+   }
+
+   if(y0 == y2) { // Handle awkward all-on-same-line case as its own thing
+       a = b = x0;
+       if(x1 < a)      a = x1;
+       else if(x1 > b) b = x1;
+       if(x2 < a)      a = x2;
+       else if(x2 > b) b = x2;
+       if ((err = ssd1306_draw_hline(dev, fb, a, y0, b-a+1, color)))
+           return err;
+       return 0;
+   }
+
+   int16_t
+   dx01 = x1 - x0,
+   dy01 = y1 - y0,
+   dx02 = x2 - x0,
+   dy02 = y2 - y0,
+   dx12 = x2 - x1,
+   dy12 = y2 - y1,
+   sa   = 0,
+   sb   = 0;
+
+   // For upper part of triangle, find scanline crossings for segments
+   // 0-1 and 0-2.  If y1=y2 (flat-bottomed triangle), the scanline y1
+   // is included here (and second loop will be skipped, avoiding a /0
+   // error there), otherwise scanline y1 is skipped here and handled
+   // in the second loop...which also avoids a /0 error here if y0=y1
+   // (flat-topped triangle).
+   if(y1 == y2) last = y1;   // Include y1 scanline
+   else         last = y1-1; // Skip it
+
+   for(y=y0; y<=last; y++) {
+       a   = x0 + sa / dy01;
+       b   = x0 + sb / dy02;
+       sa += dx01;
+       sb += dx02;
+       /* longhand:
+   a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
+   b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
+        */
+       if(a > b) swap(a,b);
+       if ((err = ssd1306_draw_hline(dev, fb, a, y, b-a+1, color)))
+           return err;
+   }
+
+   // For lower part of triangle, find scanline crossings for segments
+   // 0-2 and 1-2.  This loop is skipped if y1=y2.
+   sa = dx12 * (y - y1);
+   sb = dx02 * (y - y0);
+   for(; y<=y2; y++) {
+       a   = x1 + sa / dy12;
+       b   = x0 + sb / dy02;
+       sa += dx12;
+       sb += dx02;
+       /* longhand:
+   a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
+   b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
+        */
+       if(a > b) swap(a,b);
+       if ((err = ssd1306_draw_hline(dev, fb, a, y, b-a+1, color)))
+           return err;
+   }
+   return 0 ;
+}
+
+int ssd1306_select_font(uint8_t idx)
+{
+    if (idx < NUM_FONTS)
+        font = fonts[idx];
+    else
+        return -EINVAL;
+    return 0 ;
+}
+
+uint8_t ssd1306_draw_char(const ssd1306_t *dev, uint8_t *fb, uint8_t x, uint8_t y, char c, ssd1306_color_t foreground, ssd1306_color_t background)
+{
+    uint8_t i, j;
+    const uint8_t *bitmap;
+    uint8_t line = 0 ;
+
+    if (font == NULL)
+        return 0;
+
+    // we always have space in the font set
+    if ((c < font->char_start) || (c > font->char_end))
+        c = ' ';
+    c = c - font->char_start;   // c now become index to tables
+    bitmap = font->bitmap + font->char_descriptors[(unsigned char)c].offset;
+    for (j = 0; j < font->height; ++j)
+    {
+        for (i = 0; i < font->char_descriptors[(unsigned char)c].width; ++i)
+        {
+            if (i % 8 == 0)
+            {
+                line = bitmap[(font->char_descriptors[(unsigned char)c].width + 7) / 8 * j + i / 8]; // line data
+            }
+            if (line & 0x80)
+            {
+                ssd1306_draw_pixel(dev, fb, x + i, y + j, foreground);
+            }
+            else
+            {
+                switch (background)
+                {
+                case OLED_COLOR_TRANSPARENT:
+                    // Not drawing for transparent background
+                    break;
+                case OLED_COLOR_WHITE:
+                case OLED_COLOR_BLACK:
+                    ssd1306_draw_pixel(dev, fb, x + i, y + j, background);
+                    break;
+                case OLED_COLOR_INVERT:
+                    // I don't know why I need invert background
+                    break;
+                }
+            }
+            line = line << 1;
+        }
+    }
+    return (font->char_descriptors[(unsigned char)c].width);
+}
+
+uint8_t ssd1306_draw_string(const ssd1306_t *dev, uint8_t *fb, uint8_t x, uint8_t y, char *str, ssd1306_color_t foreground, ssd1306_color_t background)
+{
+    uint8_t t = x;
+
+    if (font == NULL)
+        return 0;
+    if (str == NULL)
+        return 0;
+
+    while (*str)
+    {
+       x += ssd1306_draw_char(dev, fb, x, y, *str, foreground, background);
+       ++str;
+       if (*str)
+           x += font->c;
+
+    }
+    return (x - t);
+}
+
+uint8_t ssd1306_get_font_height()
+{
+    if (font == NULL)
+        return 0;
+    return (font->height);
+}
+
+
+uint8_t ssd1306_get_font_c()
+{
+    if (font == NULL)
+        return 0;
+    return (font->c);
+}
+
+uint8_t ssd1306_measure_string(char *str)
+{
+    uint8_t w = 0;
+    char c;
+
+    if (font == NULL)
+        return 0;
+
+    while (*str)
+    {
+        c = *str;
+        // we always have space in the font set
+        if ((c < font->char_start) || (c > font->char_end))
+            c = ' ';
+        c = c - font->char_start;   // c now become index to tables
+        w += font->char_descriptors[(unsigned char)c].width;
+        ++str;
+       if (*str)
+           w += font->c;
+    }
+    return w;
+}

extras/ssd1306/ssd1306.h

@@ -63,6 +63,16 @@ typedef enum
     SSD1306_ADDR_MODE_PAGE
 } ssd1306_mem_addr_mode_t;
 
+/**
+ * Drawing color
+ */
+typedef enum
+{
+    OLED_COLOR_TRANSPARENT = -1, //!< Transparent (not drawing)
+    OLED_COLOR_BLACK = 0,        //!< Black (pixel off)
+    OLED_COLOR_WHITE = 1,        //!< White (or blue, yellow, pixel on)
+    OLED_COLOR_INVERT = 2,       //!< Invert pixel (XOR)
+} ssd1306_color_t;
 /**
  * Issue a single command on SSD1306.
  * @param dev Pointer to device descriptor
@@ -275,6 +285,186 @@ int ssd1306_set_deseltct_lvl(const ssd1306_t *dev, uint8_t lvl);
  */
 int ssd1306_set_whole_display_lighting(const ssd1306_t *dev, bool light);
 
+/**
+ * Draw one pixel
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x X coordinate
+ * @param y Y coordinate
+ * @param color Color of the pixel
+ * @return Non-zero if error occured
+ */
+int ssd1306_draw_pixel(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, ssd1306_color_t color);
+
+/**
+ * Draw a horizontal line
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x X coordinate or starting (left) point
+ * @param y Y coordinate or starting (left) point
+ * @param w Line width
+ * @param color Color of the line
+ * @return Non-zero if error occured
+ */
+int ssd1306_draw_hline(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, uint8_t w, ssd1306_color_t color);
+
+/**
+ * Draw a vertical line
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x X coordinate or starting (top) point
+ * @param y Y coordinate or starting (top) point
+ * @param h Line height
+ * @param color Color of the line
+ * @return Non-zero if error occured
+ */
+int ssd1306_draw_vline(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, uint8_t h, ssd1306_color_t color);
+
+/**
+ * Draw a line
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x0 First x point coordinate
+ * @param y0 First y point coordinate
+ * @param x1 Second x point coordinate
+ * @param y1 Second y point coordinate
+ * @param color Color of the line
+ * @return Non-zero if error occured
+ */
+int ssd1306_draw_line(const ssd1306_t *dev, uint8_t *fb, int16_t x0, int16_t y0, int16_t x1, int16_t y1, ssd1306_color_t color);
+
+/**
+ * Draw a rectangle
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x X coordinate or starting (top left) point
+ * @param y Y coordinate or starting (top left) point
+ * @param w Rectangle width
+ * @param h Rectangle height
+ * @param color Color of the rectangle border
+ * @return Non-zero if error occured
+ */
+int ssd1306_draw_rectangle(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, uint8_t w, uint8_t h, ssd1306_color_t color);
+
+/**
+ * Draw a filled rectangle
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x X coordinate or starting (top left) point
+ * @param y Y coordinate or starting (top left) point
+ * @param w Rectangle width
+ * @param h Rectangle height
+ * @param color Color of the rectangle
+ * @return Non-zero if error occured
+ */
+int ssd1306_fill_rectangle(const ssd1306_t *dev, uint8_t *fb, int8_t x, int8_t y, uint8_t w, uint8_t h, ssd1306_color_t color);
+
+/**
+ * Draw a circle
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x0 X coordinate or center
+ * @param y0 Y coordinate or center
+ * @param r Radius
+ * @param color Color of the circle border
+ * @return Non-zero if error occured
+ */
+int ssd1306_draw_circle(const ssd1306_t *dev, uint8_t *fb, int8_t x0, int8_t y0, uint8_t r, ssd1306_color_t color);
+
+/**
+ * Draw a filled circle
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x0 X coordinate or center
+ * @param y0 Y coordinate or center
+ * @param r Radius
+ * @param color Color of the circle
+ * @return Non-zero if error occured
+ */
+int ssd1306_fill_circle(const ssd1306_t *dev, uint8_t *fb, int8_t x0, int8_t y0, uint8_t r, ssd1306_color_t color);
+
+/**
+ * Draw a triangle
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x0 First x point coordinate
+ * @param y0 First y point coordinate
+ * @param x1 Second x point coordinate
+ * @param y1 Second y point coordinate
+ * @param x2 third x point coordinate
+ * @param y2 third y point coordinate
+ * @param color Color of the triangle border
+ * @return Non-zero if error occured
+ */
+int ssd1306_draw_triangle(const ssd1306_t *dev, uint8_t *fb, int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, ssd1306_color_t color);
+
+/**
+ * Draw a filled triangle
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x0 First x point coordinate
+ * @param y0 First y point coordinate
+ * @param x1 Second x point coordinate
+ * @param y1 Second y point coordinate
+ * @param x2 third x point coordinate
+ * @param y2 third y point coordinate
+ * @param color Color of the triangle
+ * @return Non-zero if error occured
+ */
+int ssd1306_fill_triangle(const ssd1306_t *dev, uint8_t *fb, int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, ssd1306_color_t color);
+
+/**
+ * Select the font
+ * @param idx id value of the font
+ * @return Non-zero if error occured
+ */
+int ssd1306_select_font(uint8_t idx);
+
+/**
+ * Get the height of current selected font
+ * @return Height of the font (in pixels) or 0 if none font selected
+ */
+uint8_t ssd1306_get_font_height();
+
+/**
+ * Get the "C" value (space between adjacent characters) of current selected font
+ * @return "C" value
+ */
+uint8_t ssd1306_get_font_c();
+
+/**
+ * Draw one character using currently selected font
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x X position of character (top-left corner)
+ * @param y Y position of character (top-left corner)
+ * @param c The character to draw
+ * @param foreground Character color
+ * @param background Background color
+ * @return Width of the character
+ */
+uint8_t ssd1306_draw_char(const ssd1306_t *dev, uint8_t *fb, uint8_t x, uint8_t y, char c, ssd1306_color_t foreground, ssd1306_color_t background);
+
+/**
+ * Draw one character using currently selected font
+ * @param dev Pointer to device descriptor
+ * @param buf Pointer to framebuffer. Framebuffer size = width * height / 8
+ * @param x X position of character (top-left corner)
+ * @param y Y position of character (top-left corner)
+ * @param str         The string to draw
+ * @param foreground Character color
+ * @param background Background color
+ * @return Width of the string (out-of-display pixels also included)
+ */
+uint8_t ssd1306_draw_string(const ssd1306_t *dev, uint8_t *fb, uint8_t x, uint8_t y, char *str, ssd1306_color_t foreground, ssd1306_color_t background);
+
+/**
+ * Measure width of string with current selected font
+ * @param str String to measure
+ * @return Width of the string
+ */
+uint8_t ssd1306_measure_string(char *str);
+
 #ifdef __cplusplus
 extern "C"
 }