diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -75,13 +75,13 @@ int main(void)
 
     // Init OLED over SPI
     ssd1306_Init();
-    ssd1306_block_write();
+    ssd1306_clearscreen();
 
     // Startup screen 
     ssd1306_DrawString("therm v0.1", 1, 40);
     ssd1306_DrawString("protofusion.org/therm", 3, 0);
     delay(1500);
-    ssd1306_block_write();
+    ssd1306_clearscreen();
 
     // Main loop
     while(1)
@@ -466,7 +466,7 @@ void machine()
 
     if(last_state != state) {
         // Clear screen on state change
-        ssd1306_block_write();
+        ssd1306_clearscreen();
     }
 }
 
diff --git a/ssd1306.c b/ssd1306.c
--- a/ssd1306.c
+++ b/ssd1306.c
@@ -1,54 +1,8 @@
-/*******************************************************************************
-* File Name          : ssd1306.c
-* Author             : lxyppc
-* Version            : V1.0
-* Date               : 10-01-21
-* Description        : ssd1306 operations
-*                      the SSH1101A is compatible with ssd1306
-*******************************************************************************/
-
-/* Includes ------------------------------------------------------------------*/
 #include "stm32l100c_discovery.h"
 #include "bsp.h"
 #include "ssd1306.h"
 
-/* Private typedef -----------------------------------------------------------*/
-/* Private define ------------------------------------------------------------*/
-#define   SSD1306_PAGE_NUMBER           8
-#define   SSD1306_COLUMN_NUMBER         128
-#define   SSD1306_COLUMN_MARGIN_START   2
-#define   SSD1306_COLUMN_MARGIN_END     2
-#define   SSD1306_X_PIXEL   128
-#define   SSD1306_Y_PIXEL   32
-
-/* Private macro -------------------------------------------------------------*/
-#define   ssd1306_Buffer    (_SSD1306_Buffer + SSD1306_COLUMN_MARGIN_START)
-
-/* Private variables ---------------------------------------------------------*/
-static  uint8_t  _SSD1306_Buffer[SSD1306_COLUMN_NUMBER*SSD1306_PAGE_NUMBER + SSD1306_COLUMN_MARGIN_START + SSD1306_COLUMN_MARGIN_END] = {0};
-static  uint8_t  pageIndex = 0;
-static  uint8_t  iS_SSD_On = 0;
-static  uint8_t  pre_on = 0;
-static  uint8_t  curContrast = 0xCC;
-static  uint8_t  lastContrast = 0xCC;
-
-/* Private function prototypes -----------------------------------------------*/
-void  WriteCommand(unsigned char command);
-void  WriteData(unsigned char data);
-void  OnPageTransferDone(void);
-unsigned long ssd1306_OFF(void);
-unsigned long ssd1306_ON(void);
-unsigned char* ssd1306_GetBuffer()
-{
-  return ssd1306_Buffer;
-}
-/*******************************************************************************
-* Function Name  : WriteCommand
-* Description    : Write command to the ssd1306
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
+// Write command to OLED
 void WriteCommand(unsigned char command)
 {
   SSD_A0_Low();
@@ -56,13 +10,7 @@ void WriteCommand(unsigned char command)
   SPI_Wait();
 }
 
-/*******************************************************************************
-* Function Name  : WriteData
-* Description    : Write data to the ssd1306
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
+// Write data to OLED
 void WriteData(unsigned char data)
 {
   SSD_A0_High();
@@ -70,13 +18,7 @@ void WriteData(unsigned char data)
   SPI_Wait();
 }
 
-/*******************************************************************************
-* Function Name  : ssd1306_Init
-* Description    : Initialize the ssd1306
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
+// Initialize OLED
 void ssd1306_Init(void)
 {
 
@@ -114,395 +56,6 @@ void ssd1306_Init(void)
   WriteCommand(0xAF); // display on 
 }
 
-void ssd1306_block_write(void)
-{
-  // Set col start addr to 0
-  WriteCommand(0x21); 
-  WriteCommand(0x00); 
-  // Set col end addr to width - 1
-  WriteCommand(127);
-
-  // Set page addr
-  WriteCommand(0x22);
-  WriteCommand(0x00); // start page addr
-  WriteCommand(4); // end page addr (height / 8)
-
-  // Write data
-  uint32_t i = 0;
-  for(i=0;i<512;i++)
-    WriteData(ssd1306_Buffer[i]);
-}
-
-
-/*******************************************************************************
-* Function Name  : ssd1306_TurnOff
-* Description    : Turn off the ssd1306 controller
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned long ssd1306_TurnOff(void)
-{
-  pre_on = 0;
-  return iS_SSD_On;
-}
-
-/*******************************************************************************
-* Function Name  : ssd1306_TurnOn
-* Description    : Turn off the ssd1306 controller
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned long ssd1306_TurnOn(void)
-{
-  pre_on = 1;
-  return iS_SSD_On;
-}
-
-/*******************************************************************************
-* Function Name  : ssd1306_SetContrast
-* Description    : Set the ssd1306 contrast
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned char ssd1306_SetContrast(unsigned char contrast)
-{
-  curContrast = contrast;
-  return lastContrast;
-}
-
-/*******************************************************************************
-* Function Name  : ssd1306_GetContrast
-* Description    : Get the ssd1306 contrast
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned char ssd1306_GetContrast()
-{
-  return lastContrast;
-}
-
-/*******************************************************************************
-* Function Name  : ssd1306_OFF
-* Description    : Turn off the ssd1306 controller
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned long ssd1306_OFF(void)
-{
-  if(iS_SSD_On){
-#ifdef  DEBUG_UI
-    uint32_t i = 0;
-    for(i=0;i<ssd1306_COLUMN_NUMBER*SSD1306_PAGE_NUMBER;i++){
-      ssd1306_Buffer[i] = 0;
-    }
-#else
-    // Turn off the display
-    WriteCommand(0xae);
-    
-    // Set Charge pump
-    WriteCommand(0x8D); /* Set Charge pump */
-    WriteCommand(0x10); /* 0x14=ON, 0x10=Off */
-#endif
-    iS_SSD_On = 0;
-  }
-  return iS_SSD_On;
-}
-
-
-/*******************************************************************************
-* Function Name  : ssd1306_ON
-* Description    : Turn on the ssd1306 controller
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned long ssd1306_ON(void)
-{
-  if(!iS_SSD_On){
-#ifdef  DEBUG_UI
-#else
-  #ifdef    DEBUG_BOARD
-  #else
-    // Set Charge pump
-    WriteCommand(0x8D); /* Set Charge pump */
-    WriteCommand(0x14); /* 0x14=ON, 0x10=Off */
-  #endif
-    // Turn on the display
-    WriteCommand(0xaf);
-#endif
-    iS_SSD_On = 1;
-  }
-  return iS_SSD_On;
-}
-
-
-/*******************************************************************************
-* Function Name  : ssd1306_IsOn
-* Description    : Check whether the ssd1306 is on or off
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned long ssd1306_IsOn(void)
-{
-  return iS_SSD_On;
-}
-
-/*******************************************************************************
-* Function Name  : OnPageTransferDone
-* Description    : Called when each page transfer done
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-void  OnPageTransferDone(void)
-{
-  if(pageIndex == SSD1306_PAGE_NUMBER){
-    pageIndex = 0;
-    return;
-  }
-#ifdef    DEBUG_BOARD
-  WriteCommand(0xb0 + pageIndex);
-  if(pageIndex == 0){
-    WriteCommand(0x00);
-    WriteCommand(0x10);
-  }
-  SSD_A0_High();
-  DMA1_Channel5->CCR &= ((uint32_t)0xFFFFFFFE);
-  DMA1_Channel5->CNDTR = ssd1306_COLUMN_NUMBER+SSD1306_COLUMN_MARGIN_START + SSD1306_COLUMN_MARGIN_END;
-  DMA1_Channel5->CMAR = (uint32_t)(ssd1306_Buffer+SSD1306_COLUMN_NUMBER*pageIndex - SSD1306_COLUMN_MARGIN_START);
-  DMA_SSD_1306->CCR |= ((uint32_t)0x00000001);
-  pageIndex++;
-#else
-  SSD_A0_High();
-  DMA_SSD_1306->CCR &= ((uint32_t)0xFFFFFFFE);
-  DMA_SSD_1306->CNDTR = SSD1306_COLUMN_NUMBER*SSD1306_PAGE_NUMBER;//+SSD1306_COLUMN_MARGIN_START + SSD1306_COLUMN_MARGIN_END;
-  DMA_SSD_1306->CMAR = (uint32_t)(ssd1306_Buffer);//+SSD1306_COLUMN_NUMBER*pageIndex);
-  //DMA_Cmd(DMA_SSD_1306, ENABLE);
-  DMA_SSD_1306->CCR |= ((uint32_t)0x00000001);
-//  pageIndex++;
-  pageIndex = SSD1306_PAGE_NUMBER;
-#endif
-}
-
-/*******************************************************************************
-* Function Name  : DMA1_Channel5_IRQHandler
-* Description    : This function handles DMA1 Channel 5 interrupt request.
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-void DMA_Handler_SSD_1306(void) //DMA1_Channel5_IRQHandler(void)
-{
-  if(DMA_GetITStatus(DMA1_IT_TC5)){
-    DMA_ClearITPendingBit(DMA1_IT_GL5);
-    OnPageTransferDone();
-  }
-}
-
-/*******************************************************************************
-* Function Name  : StartPageTransfer
-* Description    : Start a new page transfer
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-void StartPageTransfer(void)
-{
-  pageIndex = 0;
-  if(pre_on){
-    if(iS_SSD_On == 0){
-      pre_on = ssd1306_ON();
-    }
-  }else{
-    if(iS_SSD_On){
-      pre_on = ssd1306_OFF();
-    }
-  }
-  if( curContrast != lastContrast ){
-    lastContrast = curContrast;
-    WriteCommand(0x81); /* Set Contrast Control */
-    WriteCommand(lastContrast); /* 0 ~ 255 0x1f*/
-  }
-  
-  if(iS_SSD_On){
-    OnPageTransferDone();
-  }
-}
-
-/*******************************************************************************
-  Display related functions
- ******************************************************************************/
-/*******************************************************************************
-* Function Name  : ssd1306_DrawBlock
-* Description    : Draw a block with specify data to ssd1306
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned long ssd1306_DrawBlock(
-  Pos_t x,
-  Pos_t y,
-  Pos_t cx,
-  Pos_t cy,
-  const unsigned char* data)
-{
-  if(x >= SSD1306_X_PIXEL)return 0;
-  if(y >= SSD1306_Y_PIXEL)return 0;
-  if(x+cx > SSD1306_X_PIXEL ) cx = SSD1306_X_PIXEL - x ;
-  if(y+cy > SSD1306_Y_PIXEL ) cy = SSD1306_Y_PIXEL - y ;
-  unsigned char* pStart = ssd1306_Buffer + (y/8)*SSD1306_COLUMN_NUMBER + x;
-  unsigned char offset1 = y%8;
-  unsigned char offset2 = (cy+y)%8;
-  if(data){
-    unsigned char mask1 = (1<<offset1)-1;
-    unsigned char mask2 = ~((1<<offset2)-1);
-    cy = (cy+offset1)/8;
-   
-    Pos_t i = 0;
-    for(i=0;i<cx;i++){
-      Pos_t j = 0;
-      unsigned short tmp = *pStart & mask1;
-      for(;j<cy;j++){
-        tmp |= (((unsigned short)data[j*cx])<<offset1);
-        *(pStart + j*SSD1306_COLUMN_NUMBER) = tmp;
-        tmp>>=8;
-      }
-      if(offset2){
-        tmp |= ((((unsigned short)data[j*cx])<<offset1) & (~mask2))
-            | *(pStart + j*SSD1306_COLUMN_NUMBER) & mask2;
-        *(pStart + j*SSD1306_COLUMN_NUMBER) = tmp;
-      }
-      data++;
-      pStart++;
-    }
-  }else{
-    unsigned char mask1 = ~((1<<offset1)-1);
-    unsigned char mask2 = ((1<<offset2)-1);
-    cy = (cy+offset1)/8;
-
-    Pos_t i = 0;
-    for(i=0;i<cx;i++){
-      Pos_t j = 1;
-      *pStart ^= mask1;
-      for(;j<cy;j++){
-        *(pStart + j*SSD1306_COLUMN_NUMBER) ^= 0xFF;
-      }
-      if(offset2){
-        *(pStart + j*SSD1306_COLUMN_NUMBER) ^= mask2;
-      }
-      pStart++;
-    }
-  }
-  
-  
-//  if(!offset){
-//    for(Pos_t i=0;i<cx;i++){
-//      unsigned char tmp;
-//      Pos_t j = 0;
-//      for(;j<cy/8;j++){
-//        tmp = data[i+j*cx];
-//        *(pStart + j*ssd1306_COLUMN_NUMBER + i + x) = tmp;
-//      }
-//      if(cy&0x7){
-//        unsigned char mask1 = (1<<(cy&7)) - 1;
-//        unsigned char mask2 = 0xFF - mask1;
-//        tmp = (data[i+j*cx] & mask1)
-//            | (*(pStart + j*ssd1306_COLUMN_NUMBER + i + x) & mask2);
-//        *(pStart + j*ssd1306_COLUMN_NUMBER + i + x) = tmp;
-//      }
-//    }
-//  }else{
-//    for(Pos_t i=0;i<cx;i++){
-//      unsigned short tmp;
-//      for(Pos_t j=0;j<cy/8;j++){
-//        if(j == 0){
-//          // First line
-//          tmp = *(pStart + j*ssd1306_COLUMN_NUMBER + i + x) & mask;
-//        }
-//        tmp |= (((unsigned short)data[i+j*cx])<<offset);
-//        *(pStart + j*ssd1306_COLUMN_NUMBER + i + x) = tmp;
-//        tmp>>=8;
-//        if(j == (cy/8)-1){
-//          // Last line
-//          *(pStart + (j+1)*ssd1306_COLUMN_NUMBER + i + x) &= ~mask;
-//          *(pStart + (j+1)*ssd1306_COLUMN_NUMBER + i + x) |= tmp;
-//        }
-//      }
-//    }
-//  }
-  
-  return 0;
-}
-
-/*******************************************************************************
-* Function Name  : ssd1306_DrawPoint
-* Description    : Draw a point with specify data to ssd1306
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned long ssd1306_DrawPoint(
-  Pos_t x,
-  Pos_t y,
-  Color_t color)
-{
-  if(x >= SSD1306_X_PIXEL)return 0;
-  if(y >= SSD1306_Y_PIXEL)return 0;
-  unsigned char* pStart = ssd1306_Buffer + (y/8)*SSD1306_COLUMN_NUMBER + x;
-  unsigned char mask = 1<<(y%8);
-  if(color){
-    *pStart |= mask;
-  }else{
-    *pStart &= ~mask;
-  }
-  return 0;
-}
-
-/*******************************************************************************
-* Function Name  : ssd1306_ReadPoint
-* Description    : Read a point from ssd1306
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-unsigned long ssd1306_ReadPoint(
-  Pos_t x,
-  Pos_t y)
-{
-  unsigned char* pStart = ssd1306_Buffer + (y/8)*SSD1306_COLUMN_NUMBER + x;
-  unsigned char mask = 1<<(y%8);
-  return *pStart&mask ? 1 : 0;
-}
-
-/*******************************************************************************
-* Function Name  : ssd1306_ClearScreen
-* Description    : Clear the screen contents
-* Input          : None
-* Output         : None
-* Return         : None
-*******************************************************************************/
-void  ssd1306_FillScreen(Color_t color)
-{
-  unsigned char mask = color ? 0xFF : 0;
-  unsigned long i = 0;
-  for(i=0;i<SSD1306_COLUMN_NUMBER*SSD1306_PAGE_NUMBER;i++){
-    ssd1306_Buffer[i] = mask;
-  }
-}
-
-const DeviceProp  ssd1306_Prop =
-{
-  .pfnDrawBlok = ssd1306_DrawBlock,
-  .pfnDrawPoint = ssd1306_DrawPoint,
-  .xPixel = 128,
-  .yPixel = 64,
-};
 
 // Times New Roman font
 const char fontData[240][5] =
@@ -638,20 +191,32 @@ void setStartColumn(unsigned char d)
 
 const uint8_t row[4][32] = { 
 
+	{0x00,0x00,0x01,0x03,0x07,0x0F,0x1E,0x3C,0x3C,0x7C,0x7C,0x7C,0xFC,0xFF,0xFF,0xFC,0xFC,0xFC,0xFC,0xFF,0x7F,0x7F,0x7F,0x3C,0x3C,0x1C,0x0C,0x06,0x03,0x01,0x00,0x00},
 
-{0x00,0x00,0x01,0x03,0x07,0x0F,0x1E,0x3C,0x3C,0x7C,0x7C,0x7C,0xFC,0xFF,0xFF,0xFC,0xFC,0xFC,0xFC,0xFF,0x7F,0x7F,0x7F,0x3C,0x3C,0x1C,0x0C,0x06,0x03,0x01,0x00,0x00},
-
-{0x0F,0x7F,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x3F,0x3F,0x7F,0xFF,0xFF,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x7F,0x0F},
+	{0x0F,0x7F,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x3F,0x3F,0x7F,0xFF,0xFF,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x7F,0x0F},
 
-{0xF0,0xFE,0xFF,0xFF,0xFF,0xC7,0x00,0x00,0x00,0x00,0x87,0xC7,0xC7,0xFF,0xFF,0x00,0x00,0x00,0x00,0x87,0x87,0xC7,0xC3,0x03,0x07,0x07,0x0F,0x7F,0xFF,0xFF,0xFE,0xF0},
+	{0xF0,0xFE,0xFF,0xFF,0xFF,0xC7,0x00,0x00,0x00,0x00,0x87,0xC7,0xC7,0xFF,0xFF,0x00,0x00,0x00,0x00,0x87,0x87,0xC7,0xC3,0x03,0x07,0x07,0x0F,0x7F,0xFF,0xFF,0xFE,0xF0},
 
-{0x00,0x00,0x80,0xC0,0xE0,0xF0,0xF8,0xFC,0xFC,0xFE,0xFE,0xFE,0xFF,0xFF,0xFF,0x1F,0x1F,0x1F,0x1F,0xFF,0xFE,0xFE,0xFE,0xFC,0xFC,0xF8,0xF0,0xE0,0xC0,0x80,0x00,0x00},
-
-
-
+	{0x00,0x00,0x80,0xC0,0xE0,0xF0,0xF8,0xFC,0xFC,0xFE,0xFE,0xFE,0xFF,0xFF,0xFF,0x1F,0x1F,0x1F,0x1F,0xFF,0xFE,0xFE,0xFE,0xFC,0xFC,0xF8,0xF0,0xE0,0xC0,0x80,0x00,0x00},
 
 };
 
+void ssd1306_clearscreen()
+{
+    uint8_t i = 0;
+    uint8_t page = 0;
+    for(page = 0; page<4; page++)
+    {
+        setStartPage(page);
+        setStartColumn(0);
+        for(i = 0; i<128; i++)
+        {
+            WriteData(0x00);
+        }
+    }
+    WriteData(0x00);
+}
+
 void ssd1306_drawlogo()
 {
     uint8_t i = 0;
diff --git a/ssd1306.h b/ssd1306.h
--- a/ssd1306.h
+++ b/ssd1306.h
@@ -1,55 +1,10 @@
 #ifndef   ssd1306_H
 #define   ssd1306_H
 
-typedef unsigned long Pos_t;
-typedef unsigned long Size_t;
-typedef unsigned long Color_t;
-
-typedef   unsigned long  (*pfnDrawBlock_t)(
-  Pos_t x,
-  Pos_t y,
-  Pos_t cx,
-  Pos_t cy,
-  const unsigned char* data);
-typedef   unsigned long  (*pfnDrawPoint_t)(Pos_t x, Pos_t y, Color_t color);
-
-
-typedef unsigned long(*pfnFontDrawChar)(pfnDrawBlock_t DrawBlock,Pos_t x, Pos_t y, unsigned int ch);
-
-
-
-
-typedef  struct _DeviceProp
-{
-  pfnDrawBlock_t    pfnDrawBlok;
-  pfnDrawPoint_t    pfnDrawPoint;
-  Size_t            xPixel;
-  Size_t            yPixel;
-}DeviceProp;
-
-typedef  struct  _Device
-{
-  const DeviceProp*   pDevProp;
-  pfnFontDrawChar     pfnFont;
-  Pos_t               curX;
-  Pos_t               curY;
-}Device;
-
-
-void  ssd1306_Init(void);
-void  StartPageTransfer(void);
-extern  const DeviceProp  ssd1306_Prop;
-unsigned long ssd1306_DrawBlock(Pos_t x, Pos_t y, Pos_t cx, Pos_t cy, const unsigned char* data);
-unsigned long ssd1306_IsOn(void);
-unsigned long ssd1306_TurnOff(void);
-unsigned long ssd1306_TurnOn(void);
-unsigned char ssd1306_SetContrast(unsigned char contrast);
-unsigned char ssd1306_GetContrast();
-unsigned char* ssd1306_GetBuffer();
-unsigned long ssd1306_DrawPoint(Pos_t x, Pos_t y, Color_t color);
-void ssd1306_block_write(void);
+void ssd1306_Init(void);
 void ssd1306_DrawChar(char ascii, unsigned char row, unsigned char xPos);
 void ssd1306_DrawString(const char *dataPtr, unsigned char row, unsigned char xPos);
 void ssd1306_drawlogo();
+void ssd1306_clearscreen();
 
 #endif