diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index dd61127..3f16df7 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -5,15 +5,16 @@
             "includePath": [
                 "${workspaceFolder}/utility/unity/core",
                 "${workspaceFolder}/utility/unity/fixture",
-                // "${workspaceFolder}/test/oled/helpers/inc",
-                // "${workspaceFolder}/test/oled/",
+                "${workspaceFolder}/test/oled/helpers/inc",
+                "${workspaceFolder}/test/oled/",
                 "${workspaceFolder}/cmd_parser/",
                 "${workspaceFolder}/**"
             ],
             "defines": [
                 "_DEBUG",
                 "UNICODE",
-                "_UNICODE"
+                "_UNICODE",
+                "TEST"
             ],
             "compilerPath": "C:\\Apps\\mingw64\\bin\\gcc.exe",
             "cStandard": "gnu17",
diff --git a/oled/connection.c b/oled/connection.c
new file mode 100644
index 0000000..3c716de
--- /dev/null
+++ b/oled/connection.c
@@ -0,0 +1,12 @@
+
+#include "connection.h"
+
+HAL_StatusTypeDef oled_SendCommand(OLED_HandleTypeDef *hOled, uint8_t *pData, uint16_t Size)
+{
+    return HAL_I2C_Mem_Write(hOled->hi2c, (hOled->DevAddress) << 1, I2C_COMMAND, 1, pData, Size, I2C_TIMEOUT);
+}
+
+HAL_StatusTypeDef oled_SendData(OLED_HandleTypeDef *hOled, uint8_t *pData, uint16_t Size)
+{
+    return HAL_I2C_Mem_Write(hOled->hi2c, (hOled->DevAddress) << 1, I2C_DATA, 1, pData, Size, I2C_TIMEOUT);
+}
diff --git a/oled/connection.h b/oled/connection.h
new file mode 100644
index 0000000..7eadf26
--- /dev/null
+++ b/oled/connection.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "main.h"
+#include "oled.h"
+
+HAL_StatusTypeDef oled_SendCommand(OLED_HandleTypeDef *hOled, uint8_t *pData, uint16_t Size);
+HAL_StatusTypeDef oled_SendData(OLED_HandleTypeDef *hOled, uint8_t *pData, uint16_t Size);
diff --git a/oled/gfx.c b/oled/gfx.c
new file mode 100644
index 0000000..fd2528c
--- /dev/null
+++ b/oled/gfx.c
@@ -0,0 +1,364 @@
+#include "gfx.h"
+
+/**
+ * @brief Set the pixel
+ *
+ * @param hOled Display object
+ * @param x X coordinate
+ * @param y Y coordinate
+ * @param color Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
+ */
+void writePixel(OLED_HandleTypeDef *hOled, uint8_t x, uint8_t y, GFX_Color_t color)
+{
+    if (x > hOled->Width || y > hOled->Height)
+        return;
+
+    switch (color)
+    {
+    case WHITE:
+        hOled->Buffer[(y / 8) * hOled->Width + x] |= (1 << (y % 8));
+        break;
+    case BLACK:
+        hOled->Buffer[(y / 8) * hOled->Width + x] &= ~(1 << (y % 8));
+        break;
+    case INVERSE:
+        hOled->Buffer[(y / 8) * hOled->Width + x] ^= (1 << (y % 8));
+        break;
+    default:
+        break;
+    }
+}
+
+typedef struct
+{
+    uint16_t bitmap_max_idx;
+    uint8_t buf_row_first;
+    uint8_t buf_row_last;
+    uint8_t buf_col_first;
+    uint8_t buf_col_last;
+    uint8_t buf_mask_top;
+    uint8_t buf_mask_bottom;
+    uint8_t bitmap_col;
+    uint8_t bitmap_row_first;
+    uint8_t bitmap_row_last;
+    uint8_t bitmap_shift;
+} buf_bitmap_boundry_t;
+
+static void _getBoundry(OLED_HandleTypeDef *hOled, buf_bitmap_boundry_t *boundry, uint8_t bitmap_width, uint8_t bitmap_height, int8_t pos_x, int8_t pos_y)
+{
+    if (pos_x < 0)
+    {
+        boundry->bitmap_col = pos_x * -1;
+        boundry->buf_col_first = 0;
+    }
+    else
+    {
+        boundry->bitmap_col = 0;
+        boundry->buf_col_first = pos_x;
+    }
+
+    if (pos_y < 0)
+    {
+        boundry->bitmap_shift = 8 + (pos_y % 8);
+        boundry->bitmap_row_first = (pos_y / 8) * (-1) + 1;
+        boundry->buf_row_first = 0;
+        boundry->buf_mask_top = 0;
+    }
+    else
+    {
+        boundry->bitmap_shift = pos_y % 8;
+        boundry->bitmap_row_first = 0;
+        boundry->buf_row_first = pos_y / 8;
+        boundry->buf_mask_top = 0xFF >> (8 - boundry->bitmap_shift);
+    }
+    boundry->buf_mask_bottom = 0xFF << ((pos_y + bitmap_height) % 8);
+    if (boundry->buf_mask_bottom == 0xFF)
+    {
+        boundry->buf_mask_bottom = 0;
+    }
+
+    if ((bitmap_width + pos_x) > hOled->Width)
+    {
+        boundry->buf_col_last = hOled->Width;
+    }
+    else
+    {
+        boundry->buf_col_last = bitmap_width + pos_x;
+    }
+
+    if (bitmap_height + pos_y > hOled->Height)
+    {
+        boundry->buf_row_last = hOled->Height / 8;
+    }
+    else
+    {
+        boundry->buf_row_last = (bitmap_height + pos_y + 7) / 8;
+    }
+
+    boundry->bitmap_row_last = (pos_y + bitmap_height) / 8;
+    boundry->bitmap_max_idx = bitmap_width * ((bitmap_height + 7) / 8);
+}
+
+static inline uint8_t _getBitmapByte(const uint8_t *bitmap, uint16_t index, GFX_Color_t color)
+{
+    switch (color)
+    {
+    case INVERSE:
+        return ~(bitmap[index]);
+    case WHITE:
+        return 0xFF;
+    case BLACK:
+        return 0x00;
+    default:
+        return bitmap[index];
+    }
+}
+/**
+ * @brief A function that writes a bitmap into the buffer at the given position.
+ * 			0,0 -------->x
+ * 			 |
+ * 			 |
+ * 			\ /
+ * 			 y
+ * @param bitmap A pointer to bitmap array.
+ * @param bitmap_width Bitmap witdh in pixels.
+ * @param bitmap_height Bitmap height in pixels.
+ * @param pos_x Position x in the display
+ * @param pos_y Position y in the display
+ * @param color NORMAL (2) normal mode or INVERSE(3) mode for bitmap
+ * 				WHITE (0) or BLACK (1) for fill screen
+ */
+void writeBitmap(OLED_HandleTypeDef *hOled, const uint8_t *bitmap, uint8_t bitmap_width, uint8_t bitmap_height, int8_t pos_x, int8_t pos_y, GFX_Color_t color)
+{
+    if (bitmap_width + pos_x < 0 || bitmap_height + pos_y < 0)
+        return;
+
+    uint16_t tmp_buf16, bitmap_idx, buf_idx;
+    uint8_t tmp_bitmap, bitmap_row;
+
+    buf_bitmap_boundry_t b;
+    _getBoundry(hOled, &b, bitmap_width, bitmap_height, pos_x, pos_y);
+
+    for (uint8_t col = b.buf_col_first; col < b.buf_col_last; col++, b.bitmap_col++)
+    {
+        tmp_buf16 = 0;
+        bitmap_row = b.bitmap_row_first;
+
+        if (b.bitmap_row_first > 0)
+        {
+            tmp_buf16 = _getBitmapByte(bitmap, bitmap_width * (b.bitmap_row_first - 1) + b.bitmap_col, color) >> (8 - b.bitmap_shift);
+        }
+        else
+        {
+            tmp_buf16 = hOled->Buffer[b.buf_row_first * hOled->Width + col] & b.buf_mask_top;
+        }
+
+        for (uint8_t buf_row = b.buf_row_first; buf_row < b.buf_row_last; buf_row++, bitmap_row++)
+        {
+            bitmap_idx = bitmap_width * bitmap_row + b.bitmap_col;
+            buf_idx = buf_row * hOled->Width + col;
+
+            if (bitmap_idx < b.bitmap_max_idx)
+            {
+                tmp_bitmap = _getBitmapByte(bitmap, bitmap_idx, color);
+                tmp_buf16 |= tmp_bitmap << b.bitmap_shift;
+            }
+
+            if (b.bitmap_row_last == buf_row)
+            {
+                hOled->Buffer[buf_idx] = (hOled->Buffer[buf_idx] & b.buf_mask_bottom) | (tmp_buf16 & ~(b.buf_mask_bottom));
+            }
+            else
+            {
+                hOled->Buffer[buf_idx] = (uint8_t)tmp_buf16;
+            }
+            tmp_buf16 = tmp_buf16 >> 8;
+        }
+    }
+}
+
+void writeSlashLine(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, int16_t x1, int16_t y1, GFX_Color_t color)
+{
+    uint8_t steep = _diff(y1, y0) > _diff(x1, x0); // bool
+    if (steep)
+    {
+        _swap_int16_t(x0, y0);
+        _swap_int16_t(x1, y1);
+    }
+
+    if (x0 > x1)
+    {
+        _swap_int16_t(x0, x1);
+        _swap_int16_t(y0, y1);
+    }
+
+    int16_t dx = x1 - x0;
+    int16_t dy = _diff(y1, y0);
+    int16_t err = dx >> 1;
+    int16_t step = (y0 < y1) ? 1 : -1;
+
+    for (; x0 <= x1; x0++)
+    {
+        if (steep)
+        {
+            writePixel(hOled, y0, x0, color);
+        }
+        else
+        {
+            writePixel(hOled, x0, y0, color);
+        }
+        err -= dy;
+        if (err < 0)
+        {
+            err += dx;
+            y0 += step;
+        }
+    }
+}
+
+/**************************************************************************/
+/*!
+  @brief  Write a perfectly vertical line
+  @param  x       Top-most x coordinate
+  @param  y       Top-most y coordinate
+  @param  height       Height in pixels
+  @param  color   Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
+*/
+/**************************************************************************/
+void writeVerticalLine(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t height, GFX_Color_t color)
+{
+    for (int16_t i = y; i < y + height; i++)
+    {
+        writePixel(hOled, x, i, color);
+    }
+}
+
+/**************************************************************************/
+/*!
+  @brief  Write a perfectly horizontal line
+  @param  x       Left-most x coordinate
+  @param  y       Left-most y coordinate
+  @param  width       Width in pixels
+  @param  color   Color of pixel WHITE(0), BLACK(1) or INVERSE(2)
+*/
+/**************************************************************************/
+void writeHorizontalLine(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t width, GFX_Color_t color)
+{
+    for (int16_t i = x; i < x + width; i++)
+    {
+        writePixel(hOled, i, y, color);
+    }
+}
+
+void writeRect(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t width, int16_t height, GFX_Color_t color)
+{
+    writeHorizontalLine(hOled, x, y, width, color);
+    writeHorizontalLine(hOled, x, y + height - 1, width, color);
+    writeVerticalLine(hOled, x, y, height, color);
+    writeVerticalLine(hOled, x + width - 1, y, height, color);
+}
+
+void writeFillRect(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t width, int16_t height, GFX_Color_t color)
+{
+    writeBitmap(hOled, NULL, width, height, x, y, color);
+}
+
+/**************************************************************************/
+/*!
+   @brief    Draw a circle outline
+    @param    x0   Center-point x coordinate
+    @param    y0   Center-point y coordinate
+    @param    radius   Radius of circle
+    @param    color 16-bit 5-6-5 Color to draw with
+*/
+/**************************************************************************/
+void writeCircle(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, uint8_t radius, GFX_Color_t color)
+{
+
+    int16_t f = 1 - radius;
+    int16_t ddF_x = 1;
+    int16_t ddF_y = -2 * radius;
+    int16_t x = 0;
+    int16_t y = radius;
+
+    writePixel(hOled, x0, y0 + radius, color);
+    writePixel(hOled, x0, y0 - radius, color);
+    writePixel(hOled, x0 + radius, y0, color);
+    writePixel(hOled, x0 - radius, y0, color);
+
+    while (x < y)
+    {
+        if (f >= 0)
+        {
+            y--;
+            ddF_y += 2;
+            f += ddF_y;
+        }
+        x++;
+        ddF_x += 2;
+        f += ddF_x;
+
+        writePixel(hOled, x0 + x, y0 + y, color);
+        writePixel(hOled, x0 - x, y0 + y, color);
+        writePixel(hOled, x0 + x, y0 - y, color);
+        writePixel(hOled, x0 - x, y0 - y, color);
+        writePixel(hOled, x0 + y, y0 + x, color);
+        writePixel(hOled, x0 - y, y0 + x, color);
+        writePixel(hOled, x0 + y, y0 - x, color);
+        writePixel(hOled, x0 - y, y0 - x, color);
+    }
+    endWrite();
+}
+
+/**************************************************************************/
+/*!
+    @brief    Quarter-circle drawer, used to do circles and roundrects
+    @param    x0   Center-point x coordinate
+    @param    y0   Center-point y coordinate
+    @param    radius   Radius of circle
+    @param    corner  Mask bit #1 or bit #2 to indicate which quarters of
+   the circle we're doing
+    @param    color 16-bit 5-6-5 Color to draw with
+*/
+/**************************************************************************/
+void writeQuarterCircle(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, uint8_t radius, CIRC_Corners_t corner, GFX_Color_t color)
+{
+    int16_t f = 1 - radius;
+    int16_t ddF_x = 1;
+    int16_t ddF_y = -2 * radius;
+    int16_t x = 0;
+    int16_t y = radius;
+
+    while (x < y)
+    {
+        if (f >= 0)
+        {
+            y--;
+            ddF_y += 2;
+            f += ddF_y;
+        }
+        x++;
+        ddF_x += 2;
+        f += ddF_x;
+        if (corner & BOTTOM_LEFT)
+        {
+            writePixel(hOled, x0 + x, y0 + y, color);
+            writePixel(hOled, x0 + y, y0 + x, color);
+        }
+        if (corner & BOTTOM_RIGHT)
+        {
+            writePixel(hOled, x0 + x, y0 - y, color);
+            writePixel(hOled, x0 + y, y0 - x, color);
+        }
+        if (corner & TOP_LEFT)
+        {
+            writePixel(hOled, x0 - y, y0 + x, color);
+            writePixel(hOled, x0 - x, y0 + y, color);
+        }
+        if (corner & TOP_RIGHT)
+        {
+            writePixel(hOled, x0 - y, y0 - x, color);
+            writePixel(hOled, x0 - x, y0 - y, color);
+        }
+    }
+}
\ No newline at end of file
diff --git a/oled/gfx.h b/oled/gfx.h
new file mode 100644
index 0000000..35dc3c0
--- /dev/null
+++ b/oled/gfx.h
@@ -0,0 +1,39 @@
+#include "oled.h"
+
+#ifndef _swap_int16_t
+#define _swap_int16_t(a, b) \
+    {                       \
+        int16_t t = a;      \
+        a = b;              \
+        b = t;              \
+    }
+#endif
+
+#ifndef _diff
+#define _diff(a, b) ((a > b) ? (a - b) : (b - a))
+#endif
+
+typedef enum
+{
+    WHITE,
+    BLACK,
+    NORMAL,
+    INVERSE
+} GFX_Color_t;
+
+typedef enum
+{
+    TOP_RIGHT = 1,
+    BOTTOM_RIGHT = 2,
+    BOTTOM_LEFT = 4,
+    TOP_LEFT = 8
+} CIRC_Corners_t;
+
+void writePixel(OLED_HandleTypeDef *hOled, uint8_t x, uint8_t y, GFX_Color_t color);
+void writeBitmap(OLED_HandleTypeDef *hOled, const uint8_t *bitmap, uint8_t bitmap_width, uint8_t bitmap_height, int8_t pos_x, int8_t pos_y, GFX_Color_t color);
+void writeSlashLine(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, int16_t x1, int16_t y1, GFX_Color_t color);
+void writeVerticalLine(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t height, GFX_Color_t color);
+void writeHorizontalLine(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t width, GFX_Color_t color);
+void writeRect(OLED_HandleTypeDef *hOled, int16_t x, int16_t y, int16_t width, int16_t height, GFX_Color_t color);
+void writeCircle(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, uint8_t radius, GFX_Color_t color);
+void writeQuarterCircle(OLED_HandleTypeDef *hOled, int16_t x0, int16_t y0, uint8_t radius, CIRC_Corners_t corner, GFX_Color_t color);
\ No newline at end of file
diff --git a/oled/oled.c b/oled/oled.c
new file mode 100644
index 0000000..4e88287
--- /dev/null
+++ b/oled/oled.c
@@ -0,0 +1,102 @@
+#include <stdlib.h>
+// #include <assert.h>
+#include "oled.h"
+#include "ssd1306.h"
+#include "sh1106.h"
+
+HAL_StatusTypeDef oled_Config(OLED_HandleTypeDef *hOled, uint8_t DevAddress, uint8_t Width, uint8_t Height, OLED_DisplayType_t OledType)
+{
+    // static_assert(hOled != NULL, "brak pointera");
+    if (hOled == NULL || Width == 0 || Height == 0 || OledType == UNKNOWN)
+    {
+        return HAL_ERROR;
+    }
+
+    hOled->hi2c = NULL;
+    hOled->DevAddress = DevAddress;
+    hOled->Width = Width;
+    hOled->Height = Height;
+    hOled->OledType = OledType;
+    hOled->BufSize = Width * ((Height + 7) / 8);
+    hOled->Buffer = (uint8_t *)malloc(hOled->BufSize);
+
+    if (hOled->Buffer == NULL)
+    {
+        return HAL_ERROR;
+    }
+
+    return HAL_OK;
+}
+
+void oled_init(OLED_HandleTypeDef *hOled, I2C_HandleTypeDef *hi2c)
+{
+    if (hOled == NULL || hi2c == NULL)
+    {
+        return;
+    }
+
+    if (hOled->Buffer == NULL || hOled->BufSize == 0)
+    {
+        return;
+    }
+
+    hOled->hi2c = hi2c;
+
+    switch (hOled->OledType)
+    {
+    case SSD1306:
+        SSD1306_Init(hOled);
+        break;
+    case SH1106:
+        SH1106_Init(hOled);
+        break;
+
+    default:
+        break;
+    }
+}
+
+void oled_display_all(OLED_HandleTypeDef *hOled)
+{
+    if (hOled == NULL)
+    {
+        return;
+    }
+
+    switch (hOled->OledType)
+    {
+    case SSD1306:
+        SSD1306_display_all(hOled);
+        break;
+    case SH1106:
+        SH1106_display_all(hOled);
+        break;
+
+    default:
+        break;
+    }
+}
+
+OLED_SendStatus_t oled_display_page(OLED_HandleTypeDef *hOled)
+{
+    if (hOled == NULL)
+    {
+        return ERROR;
+    }
+    OLED_SendStatus_t sendStatus;
+    switch (hOled->OledType)
+    {
+    case SSD1306:
+        sendStatus = SSD1306_display_page(hOled);
+        break;
+    case SH1106:
+        sendStatus = SH1106_display_page(hOled);
+        break;
+
+    default:
+        break;
+    }
+
+    return sendStatus;
+}
+
diff --git a/oled/oled.h b/oled/oled.h
new file mode 100644
index 0000000..17fce82
--- /dev/null
+++ b/oled/oled.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include "main.h"
+
+#define I2C_COMMAND 0x00
+#define I2C_DATA 0x40
+#define I2C_TIMEOUT 100
+
+typedef enum
+{
+    UNKNOWN,
+    SSD1306,
+    SH1106
+} OLED_DisplayType_t;
+
+typedef enum{
+    ERROR,
+    SENDPAGE,
+    SENDALL
+} OLED_SendStatus_t;
+
+typedef struct
+{
+    I2C_HandleTypeDef *hi2c;
+    OLED_DisplayType_t OledType;
+    uint8_t DevAddress;
+    uint8_t Width;
+    uint8_t Height;
+    uint8_t *Buffer;
+    uint16_t BufSize;
+
+} OLED_HandleTypeDef;
+
+HAL_StatusTypeDef oled_Config(OLED_HandleTypeDef *hOled, uint8_t DevAddress, uint8_t Width, uint8_t Height, OLED_DisplayType_t OledType);
+void oled_init(OLED_HandleTypeDef *hOled, I2C_HandleTypeDef *hi2c);
+void oled_display_all(OLED_HandleTypeDef *hOled);
+OLED_SendStatus_t oled_display_page(OLED_HandleTypeDef *hOled);
\ No newline at end of file
diff --git a/oled/sh1106/sh1106.c b/oled/sh1106/sh1106.c
new file mode 100644
index 0000000..b7c1669
--- /dev/null
+++ b/oled/sh1106/sh1106.c
@@ -0,0 +1,62 @@
+#include "oled.h"
+#include "sh1106.h"
+#include "connection.h"
+
+void SH1106_Init(OLED_HandleTypeDef *hOled)
+{
+    uint8_t display = SH1106_DISPLAYOFF;
+    oled_SendCommand(hOled, &display, 1);
+
+    uint8_t config[] = {
+        SH1106_LOWCOLADDR | 0x00,                      // 0x00h (0000 XXXX)
+        SH1106_HIGHCOLADDR | 0x00,                     // 0x10h (0001 XXXX)
+        SH1106_PUMPVOLTAGE | 0x02,                     // 0x32h (0011 00XX)
+        SH1106_LINEADDRESS | 0x00,                     // 0x40h (01XX XXXX)
+        SH1106_CONTRASTMODE, SH1106_CONTRASTVALUE,     //       (1000 0001)  // 0x80h (0x00h - 0xFFh) (XXXX XXXX)
+        SH1106_SEGMENTREMAP | 0x00,                    // 0xA0h (1010 000X) right(0), left(1)
+        SH1106_ENTIREDISPALY | 0x00,                   // 0xA4h (1010 010X) normal(0), entire(1)
+        SH1106_NORMALORREVERS | 0x00,                  // 0xA6h (1010 011X) normal(0), reverse(1)
+        SH1106_MULTIPLEXMODE, SH1106_MILTIPLEXVALUE,   //       (1010 1000)  // 0x3Fh (00XX XXXX) (0x00h - 0x3Fh)
+        SH1106_DCCONTROLMODE, SH1106_DCCONTROLVALON,   //       (1010 1101)  // 0x8Bh (1000 101X) ON(1), OFF(0)
+        SH1106_PAGEADDRESS | 0x00,                     // 0xB0h (1011 XXXX)
+        SH1106_OUTPUTSCANDIR | 0x00,                   // 0xC0h (1100 X000) Scan from COM0 to COM [N- 1] (0x00) or Scan from COM [N-1] to COM0 (0x08)
+        SH1106_OFFSETMODE, SH1106_OFFSETVALUE,         //       (1101 0011)  // 0x00h (00XX XXXX) COM0-63
+        SH1106_DIVRATIOFREQM, SH1106_DIVRATIOFREQV,    //       (1101 0101)  // 0x50h (FFFF DDDD) Frequency, Divide
+        SH1106_CHARGEMODE, SH1106_CHARGEVALUE,         //       (1101 1001)  // 0x22h (DDDD PPPP) Dis-charge Period, Pre-charge Period
+        SH1106_COMPADSCONFMODE, SH1106_COMPADSCONFVAL, //       (1101 1010)  // 0x12h (000X 0010)
+        SH1106_VCOMDESMODE, SH1106_VCOMDESVALUE        //       (1101 1101)  // 0x35h (XXXX XXXX) VCOM (Beta x Vref)
+    };
+    oled_SendCommand(hOled, config, sizeof(config));
+
+    display = SH1106_DISPLAYON;
+    oled_SendCommand(hOled, &display, 1);
+}
+
+void SH1106_display_all(OLED_HandleTypeDef *hOled)
+{
+    while (SH1106_display_page(hOled) != SENDALL);
+}
+
+OLED_SendStatus_t SH1106_display_page(OLED_HandleTypeDef *hOled)
+{
+    static uint8_t page = 0;
+
+    uint8_t config[] = {
+        SH1106_PAGEADDRESS | page,
+        SH1106_LOWCOLADDR | 0x00,
+        SH1106_HIGHCOLADDR | 0x00,
+    };
+    oled_SendCommand(hOled, config, sizeof(config));
+
+    oled_SendData(hOled, hOled->Buffer + (page * hOled->Width), hOled->Width);
+
+    if (++page > 7)
+    {
+        page = 0;
+        return SENDALL;
+    }
+    else
+    {
+        return SENDPAGE;
+    }
+}
\ No newline at end of file
diff --git a/oled/sh1106/sh1106.h b/oled/sh1106/sh1106.h
new file mode 100644
index 0000000..af39efd
--- /dev/null
+++ b/oled/sh1106/sh1106.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#define SH1106_LOWCOLADDR      0x00    // 0x00h (0000 XXXX)
+#define SH1106_HIGHCOLADDR     0x10    // 0x10h (0001 XXXX)
+#define SH1106_PUMPVOLTAGE     0x30    // 0x32h (0011 00XX)
+#define SH1106_LINEADDRESS     0x40    // 0x40h (01XX XXXX)
+#define SH1106_CONTRASTMODE    0x81    //       (1000 0001)
+#define SH1106_CONTRASTVALUE   0x80    // 0x80h (0x00h - 0xFFh) (XXXX XXXX)
+#define SH1106_SEGMENTREMAP    0xA0    // 0xA0h (1010 000X) right(0), left(1)
+#define SH1106_ENTIREDISPALY   0xA4    // 0xA4h (1010 010X) normal(0), entire(1)
+#define SH1106_NORMALORREVERS  0xA6    // 0xA6h (1010 011X) normal(0), reverse(1)
+#define SH1106_MULTIPLEXMODE   0xA8    //       (1010 1000)
+#define SH1106_MILTIPLEXVALUE  0x3F    // 0x3Fh (00XX XXXX) (0x00h - 0x3Fh)
+#define SH1106_DCCONTROLMODE   0xAD    //       (1010 1101)
+#define SH1106_DCCONTROLVALON  0x8B    // 0x8Bh (1000 101X) ON(1), OFF(0)
+#define SH1106_DISPLAYOFF      0xAE    // 0xAEh (1010 111X) ON(1), OFF(0)
+#define SH1106_DISPLAYON       0xAF    // 0xAEh (1010 111X) ON(1), OFF(0)
+#define SH1106_PAGEADDRESS     0xB0    // 0xB0h (1011 XXXX)
+#define SH1106_OUTPUTSCANDIR   0xC0    // 0xC0h (1100 X000) Scan from COM0 to COM [N- 1] (0) or Scan from COM [N-1] to COM0 (1).
+#define SH1106_OFFSETMODE      0xD3    //       (1101 0011)
+#define SH1106_OFFSETVALUE     0x00    // 0x00h (00XX XXXX) COM0-63
+#define SH1106_DIVRATIOFREQM   0xD5    //       (1101 0101)
+#define SH1106_DIVRATIOFREQV   0x50    // 0x50h (FFFF DDDD) Frequency, Divide
+#define SH1106_CHARGEMODE      0xD9    //       (1101 1001)
+#define SH1106_CHARGEVALUE     0x22    // 0x22h (DDDD PPPP) Dis-charge Period, Pre-charge Period
+#define SH1106_COMPADSCONFMODE 0xDA    //       (1101 1010)
+#define SH1106_COMPADSCONFVAL  0x12    // 0x12h (000X 0010)
+#define SH1106_VCOMDESMODE     0xDD    //       (1101 1101)
+#define SH1106_VCOMDESVALUE    0x35    // 0x35h (XXXX XXXX) VCOM (Beta x Vref)
+
+void SH1106_Init(OLED_HandleTypeDef *hOled);
+void SH1106_display_all(OLED_HandleTypeDef *hOled);
+OLED_SendStatus_t SH1106_display_page(OLED_HandleTypeDef *hOled);
\ No newline at end of file
diff --git a/oled/ssd1306/ssd1306.c b/oled/ssd1306/ssd1306.c
new file mode 100644
index 0000000..e7defff
--- /dev/null
+++ b/oled/ssd1306/ssd1306.c
@@ -0,0 +1,81 @@
+
+#include "ssd1306.h"
+#include "connection.h"
+
+void SSD1306_Init(OLED_HandleTypeDef *hOled)
+{
+	uint8_t comPins = 0x02;
+	uint8_t contrast = 0x8F;
+
+	if ((hOled->Width == 128) && (hOled->Height == 32)) {
+		comPins = 0x02;
+		contrast = 0x0F;
+	} else if ((hOled->Width == 128) && (hOled->Height == 64)) {
+		comPins = 0x12;
+		contrast = 0xCF;
+	} else if ((hOled->Width == 96) && (hOled->Height == 16)) {
+		comPins = 0x02; // ada x12
+		contrast = 0xAF;
+	}
+
+    uint8_t display = SSD1306_DISPLAYOFF;
+    oled_SendCommand(hOled, &display, 1);
+
+    uint8_t config[] = {
+        SSD1306_SETMULTIPLEX, hOled->Height -1,
+        SSD1306_SETDISPLAYOFFSET, 0x00,
+        SSD1306_SETSTARTLINE | 0x00,
+        SSD1306_SEGREMAP | 0x01,
+        SSD1306_COMSCANDEC,
+        SSD1306_SETCOMPINS, comPins,
+        SSD1306_SETCONTRAST, contrast,
+        SSD1306_DISPLAYALLON_RESUME, 
+        SSD1306_NORMALDISPLAY,
+        SSD1306_SETDISPLAYCLOCKDIV, 0x80,
+        SSD1306_CHARGEPUMP, 0x14, 
+        SSD1306_MEMORYMODE, 0x00,
+        SSD1306_DEACTIVATE_SCROLL,
+        SSD1306_SETPRECHARGE, 0xF1,
+        SSD1306_SETVCOMDETECT, 0x40
+    };
+
+    oled_SendCommand(hOled, config, sizeof(config));
+
+    display = SSD1306_DISPLAYOFF;
+    oled_SendCommand(hOled, &display, 1);
+}
+
+void SSD1306_display_all(OLED_HandleTypeDef *hOled)
+{
+
+    uint8_t config[] = {
+        SSD1306_PAGEADDR, 0x00, 0x07,                   //cmd, start_page, end_page
+        SSD1306_COLUMNADDR, 0x00, hOled->Width - 1      //cmd, start_col, end_col
+    };
+	oled_SendCommand(hOled, config, sizeof(config));
+
+    oled_SendData(hOled, hOled->Buffer, hOled->BufSize);
+}
+
+OLED_SendStatus_t SSD1306_display_page(OLED_HandleTypeDef *hOled)
+{
+	static uint8_t page = 0;
+
+    uint8_t config[] = {
+        SSD1306_PAGEADDR, page, page,
+        SSD1306_COLUMNADDR, 0x00, hOled->Width - 1
+    };
+	oled_SendCommand(hOled, config, sizeof(config));
+
+	oled_SendData(hOled, hOled->Buffer + (page * hOled->Width), hOled->Width);
+
+	if (++page > 7)
+	{
+		page = 0;
+		return SENDALL;
+	}
+	else
+	{
+		return SENDPAGE;
+	}
+}
\ No newline at end of file
diff --git a/oled/ssd1306/ssd1306.h b/oled/ssd1306/ssd1306.h
new file mode 100644
index 0000000..ddf2c0b
--- /dev/null
+++ b/oled/ssd1306/ssd1306.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include "oled.h"
+
+#define SSD1306_MEMORYMODE 0x20          ///< See datasheet
+#define SSD1306_COLUMNADDR 0x21          ///< See datasheet
+#define SSD1306_PAGEADDR 0x22            ///< See datasheet
+#define SSD1306_SETCONTRAST 0x81         ///< See datasheet
+#define SSD1306_CHARGEPUMP 0x8D          ///< See datasheet
+#define SSD1306_SEGREMAP 0xA0            ///< See datasheet
+#define SSD1306_DISPLAYALLON_RESUME 0xA4 ///< See datasheet
+#define SSD1306_DISPLAYALLON 0xA5        ///< Not currently used
+#define SSD1306_NORMALDISPLAY 0xA6       ///< See datasheet
+#define SSD1306_INVERTDISPLAY 0xA7       ///< See datasheet
+#define SSD1306_SETMULTIPLEX 0xA8        ///< See datasheet
+#define SSD1306_DISPLAYOFF 0xAE          ///< See datasheet
+#define SSD1306_DISPLAYON 0xAF           ///< See datasheet
+#define SSD1306_COMSCANINC 0xC0          ///< Not currently used
+#define SSD1306_COMSCANDEC 0xC8          ///< See datasheet
+#define SSD1306_SETDISPLAYOFFSET 0xD3    ///< See datasheet
+#define SSD1306_SETDISPLAYCLOCKDIV 0xD5  ///< See datasheet
+#define SSD1306_SETPRECHARGE 0xD9        ///< See datasheet
+#define SSD1306_SETCOMPINS 0xDA          ///< See datasheet
+#define SSD1306_SETVCOMDETECT 0xDB       ///< See datasheet
+
+#define SSD1306_SETLOWCOLUMN 0x00  ///< Not currently used
+#define SSD1306_SETHIGHCOLUMN 0x10 ///< Not currently used
+#define SSD1306_SETSTARTLINE 0x40  ///< See datasheet
+
+#define SSD1306_EXTERNALVCC 0x01  ///< External display voltage source
+#define SSD1306_SWITCHCAPVCC 0x02 ///< Gen. display voltage from 3.3V
+
+#define SSD1306_RIGHT_HORIZONTAL_SCROLL 0x26              ///< Init rt scroll
+#define SSD1306_LEFT_HORIZONTAL_SCROLL 0x27               ///< Init left scroll
+#define SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL 0x29 ///< Init diag scroll
+#define SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL 0x2A  ///< Init diag scroll
+#define SSD1306_DEACTIVATE_SCROLL 0x2E                    ///< Stop scroll
+#define SSD1306_ACTIVATE_SCROLL 0x2F                      ///< Start scroll
+#define SSD1306_SET_VERTICAL_SCROLL_AREA 0xA3             ///< Set scroll range
+
+void SSD1306_Init(OLED_HandleTypeDef *hOled);
+void SSD1306_display_all(OLED_HandleTypeDef *hOled);
+OLED_SendStatus_t SSD1306_display_page(OLED_HandleTypeDef *hOled);
\ No newline at end of file
diff --git a/test/oled/CMakeLists.txt b/test/oled/CMakeLists.txt
index 0af9b85..2e8dfd9 100644
--- a/test/oled/CMakeLists.txt
+++ b/test/oled/CMakeLists.txt
@@ -4,14 +4,22 @@ set(TEST_NAME oled)
 
 set(INCLUDE_DIRS
     ../../oled
+    ../../oled/ssd1306
+    ../../oled/sh1106
     helpers/inc
 )
 
 set(SRCS
+    ../../oled/oled.c
+    ../../oled/connection.c
+    ../../oled/ssd1306/ssd1306.c
+    ../../oled/sh1106/sh1106.c
 
+    helpers/src/mock_i2c.c
+    oled_test.c
 )
 
 add_definitions(-DTEST)
 add_executable(${TEST_NAME} ${SRCS})
 
-target_include_directories(${TEST_NAME} PUBLIC ${INCLUDE_DIR})
\ No newline at end of file
+target_include_directories(${TEST_NAME} PUBLIC ${INCLUDE_DIRS})
\ No newline at end of file
diff --git a/test/oled/helpers/inc/main.h b/test/oled/helpers/inc/main.h
new file mode 100644
index 0000000..0ad3b41
--- /dev/null
+++ b/test/oled/helpers/inc/main.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <stdint.h>
+
+typedef struct 
+{
+    uint8_t dummy;
+}I2C_HandleTypeDef;
+
+typedef enum 
+{
+  HAL_OK       = 0x00U,
+  HAL_ERROR    = 0x01U,
+  HAL_BUSY     = 0x02U,
+  HAL_TIMEOUT  = 0x03
+} HAL_StatusTypeDef;
+
+HAL_StatusTypeDef HAL_I2C_Mem_Write(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size, uint32_t Timeout);
diff --git a/test/oled/helpers/src/mock_i2c.c b/test/oled/helpers/src/mock_i2c.c
new file mode 100644
index 0000000..b75c552
--- /dev/null
+++ b/test/oled/helpers/src/mock_i2c.c
@@ -0,0 +1,14 @@
+#include "main.h"
+
+
+HAL_StatusTypeDef HAL_I2C_Mem_Write(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size, uint32_t Timeout)
+{
+    (void)hi2c;
+    (void)DevAddress;
+    (void)MemAddress;
+    (void)MemAddSize;
+    (void)pData;
+    (void)Size;
+    (void)Timeout;
+    return HAL_OK;
+}
\ No newline at end of file
diff --git a/test/oled/oled_test.c b/test/oled/oled_test.c
new file mode 100644
index 0000000..70c6569
--- /dev/null
+++ b/test/oled/oled_test.c
@@ -0,0 +1,50 @@
+#include "stdlib.h"
+#include "unity.h"
+#include "oled.h"
+#include "string.h"
+
+OLED_HandleTypeDef* display;
+
+void setUp(void)
+{
+    // uint32_t size = sizeof(OLED_HandleTypeDef);
+    // printf("size in B: %u\n", size);
+    display = (OLED_HandleTypeDef*)malloc(sizeof(OLED_HandleTypeDef));
+}
+
+void tearDown(void)
+{
+    free(display);
+}
+
+void test_configWithCorecctArgs(void)
+{
+    HAL_StatusTypeDef status = oled_Config(display, 0x0F, 128, 64, SH1106);
+
+    TEST_ASSERT_EQUAL(status, HAL_OK);
+    TEST_ASSERT_NULL(display->hi2c);
+    TEST_ASSERT_EQUAL(display->OledType, SH1106);
+    TEST_ASSERT_EQUAL_HEX8(display->DevAddress, 0x0F);
+    TEST_ASSERT_EQUAL(display->Width, 128);
+    TEST_ASSERT_EQUAL(display->Height, 64);
+    TEST_ASSERT_EQUAL(display->BufSize, 128*64/8);
+    TEST_ASSERT_NOT_NULL(display->Buffer);
+}
+
+void test_configWithoutPointerToDisplay(void)
+{
+    HAL_StatusTypeDef status = oled_Config(NULL, 0x0F, 128, 64, SH1106);
+
+    TEST_ASSERT_EQUAL(status, HAL_ERROR);
+}
+
+
+
+int main(void)
+{
+    UNITY_BEGIN();
+    RUN_TEST(test_configWithCorecctArgs);
+    RUN_TEST(test_configWithoutPointerToDisplay);
+
+    return UNITY_END();
+}
\ No newline at end of file