diff --git a/Main/Main.c b/Main/Main.c
index 0d7245c..5d2aac8 100644
--- a/Main/Main.c
+++ b/Main/Main.c
@@ -17,17 +17,60 @@
  * this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <avr/io.h>
-#include <util/delay.h>
+#/*
+ * This file is part of the µOS++ distribution.
+ *   (https://github.com/micro-os-plus)
+ * Copyright (c) 2014 Liviu Ionescu.
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or
+ * sell copies of the Software, and to permit persons to whom
+ * the Software is furnished to do so, subject to the following
+ * conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+// ----------------------------------------------------------------------------
+
+#include <stdio.h>
+#include <stdlib.h>
 #include <nokia1661_lcd_driver.h>
 #include <lcd_font5x7.h>
-#include <stdio.h>
+// ----- main() ---------------------------------------------------------------
 
-int main(void)
-{
-	nlcdInit();
+// Sample pragmas to cope with warnings. Please note the related line at
+// the end of this function, used to pop the compiler diagnostics status.
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#pragma GCC diagnostic ignored "-Wmissing-declarations"
+#pragma GCC diagnostic ignored "-Wreturn-type"
 
+#if defined(LCD_STM_HW)
+void delay_ms(uint32_t __IO delay)
+{
+	delay *=((SystemCoreClock/1000000)/5)*643;
+	while(delay--);
+}
+#endif
 
+int
+main(int argc, char* argv[])
+{
+	nlcdInit();
 
 	nlcdSetBackgroundColor(LCD_VGA_RED);
 	nlcdClear();
@@ -88,10 +131,13 @@ int main(void)
 
 	for(int i=0;i<32;i++)
 		nlcdPixel(64+31,64+i,LCD_VGA_GREEN);
+  // Infinite loop
+  while (1)
+  {
 
-	while(1)
-	{
-
-	}
-	return 0;
+  }
 }
+
+#pragma GCC diagnostic pop
+
+// ----------------------------------------------------------------------------
diff --git a/NokiaTFTLib/lcd_font5x7.h b/NokiaTFTLib/lcd_font5x7.h
index 796506c..35985bf 100644
--- a/NokiaTFTLib/lcd_font5x7.h
+++ b/NokiaTFTLib/lcd_font5x7.h
@@ -1,7 +1,7 @@
 #ifndef _LCD_FONT_H
 #define _LCD_FONT_H
+#include "nokia1661_Hw.h"
 
-#include <avr/pgmspace.h>
 
 /*
 	font Latin +  Cyrillic 5x7 dots
diff --git a/NokiaTFTLib/nokia1661_Hw.h b/NokiaTFTLib/nokia1661_Hw.h
new file mode 100644
index 0000000..02eb4ba
--- /dev/null
+++ b/NokiaTFTLib/nokia1661_Hw.h
@@ -0,0 +1,122 @@
+/*
+ * nokia1661_Hw.h
+ *
+ *  Created on: Sep 11, 2019
+ *      Author: zeus
+ */
+
+#ifndef NOKIA1661_HW_H_
+#define NOKIA1661_HW_H_
+
+#define LCD_AVR_HW		1
+//#define LCD_STM_HW		1
+
+#if defined(LCD_AVR_HW) && defined(LCD_STM_HW)
+#error Only Select One Hardware AVR/STM
+#endif
+#if !defined(LCD_AVR_HW) && !defined(LCD_STM_HW)
+#error You need to define Hardware Type for LCD usage
+#endif
+
+#ifdef LCD_AVR_HW
+
+#define LCD_PORT		PORTB
+#define LCD_PIN			PINB
+#define LCD_DDR			DDRB
+
+#define LCD_RST			PINB1
+#define LCD_CS			PINB2
+#define LCD_SDA			PINB3
+#define LCD_CLK			PINB5
+
+#include <avr/io.h>
+#include <util/delay.h>
+#include <avr/pgmspace.h>
+
+#define SBI(port,bit) 	asm("sbi %0, %1" : : "I" (_SFR_IO_ADDR(port)), "I" (bit))
+#define CBI(port,bit) 	asm("cbi %0, %1" : : "I" (_SFR_IO_ADDR(port)), "I" (bit))
+#define LCD_RSDA()		(LCD_PIN&(1 << LCD_SDA))
+#define LCD_GPIO_Enable()
+#define _sda_In()		LCD_DDR &=~(1<<LCD_SDA)
+#define _sda_Out()		LCD_DDR |= (1<<LCD_SDA)
+
+#define _rst_set()	SBI(LCD_PORT,LCD_RST)
+#define _rst_clr()	CBI(LCD_PORT,LCD_RST)
+#define _cs_set()	SBI(LCD_PORT,LCD_CS)
+#define _cs_clr()	CBI(LCD_PORT,LCD_CS)
+#define _sda_set()	SBI(LCD_PORT,LCD_SDA)
+#define _sda_clr()	CBI(LCD_PORT,LCD_SDA)
+#define _clk_set()	SBI(LCD_PORT,LCD_CLK)
+#define _clk_clr()	CBI(LCD_PORT,LCD_CLK)
+#define _Init_GPIO()	LCD_DDR |= (1 << LCD_RST) | (1 << LCD_CS) | (1 << LCD_SDA) | (1 << LCD_CLK)
+
+#elif defined(LCD_STM_HW)
+#include <stm32f10x.h>
+#include <string.h>
+
+#define LCD_PORT			GPIOA
+#define LCD_GPIO_Enable()	RCC->APB2ENR |= (1<<2)
+
+#define LCD_RST				7
+#define LCD_CS				6
+#define LCD_SDA				5
+#define LCD_CLK				4
+
+#define LCD_RSDA()			(LCD_PORT->IDR&(1<<LCD_SDA))
+#define SBI(port,bit) 		port->BSRR = (1<<bit)
+#define CBI(port,bit) 		port->BRR  = (1<<bit)
+
+#define memcpy_P			memcpy
+#define PROGMEM
+#define PSTR(x)				(x)
+#define pgm_read_byte(x)	*(x)
+#define _delay_ms(x)		delay_ms(x) /*Define this Function in External Source*/
+
+#define _rst_set()	SBI(LCD_PORT,LCD_RST)
+#define _rst_clr()	CBI(LCD_PORT,LCD_RST)
+#define _cs_set()	SBI(LCD_PORT,LCD_CS)
+#define _cs_clr()	CBI(LCD_PORT,LCD_CS)
+#define _sda_set()	SBI(LCD_PORT,LCD_SDA)
+#define _sda_clr()	CBI(LCD_PORT,LCD_SDA)
+#define _clk_set()	SBI(LCD_PORT,LCD_CLK)
+#define _clk_clr()	CBI(LCD_PORT,LCD_CLK)
+
+#if(LCD_RST<8)
+#define _rst_Out()	LCD_PORT->CRL &=~(0xF<<(4*LCD_RST));\
+					LCD_PORT->CRL |= (0x3<<(4*LCD_RST));
+#else
+#define _rst_Out()	LCD_PORT->CRH &=~(0xF<<(4*LCD_RST));\
+					LCD_PORT->CRH |= (0x3<<(4*LCD_RST));
+#endif
+#if(LCD_CS<8)
+#define _cs_Out()	LCD_PORT->CRL &=~(0xF<<(4*LCD_CS));\
+					LCD_PORT->CRL |= (0x3<<(4*LCD_CS));
+#else
+#define _cs_Out()	LCD_PORT->CRH &=~(0xF<<(4*LCD_CS));\
+					LCD_PORT->CRH |= (0x3<<(4*LCD_CS));
+#endif
+#if(LCD_SDA<8)
+#define _sda_Out()	LCD_PORT->CRL &=~(0xF<<(4*LCD_SDA));\
+					LCD_PORT->CRL |= (0x3<<(4*LCD_SDA));
+#define _sda_In()	LCD_PORT->CRL &=~(0xF<<(4*LCD_SDA));\
+					LCD_PORT->CRL |= (0x8<<(4*LCD_SDA));
+#else
+#define _sda_Out()	LCD_PORT->CRH &=~(0xF<<(4*LCD_SDA));\
+					LCD_PORT->CRH |= (0x3<<(4*LCD_SDA));
+#define _sda_In()	LCD_PORT->CRH &=~(0xF<<(4*LCD_SDA));\
+					LCD_PORT->CRH |= (0x8<<(4*LCD_SDA));
+#endif
+#if(LCD_CLK<8)
+#define _clk_Out()	LCD_PORT->CRL &=~(0xF<<(4*LCD_CLK));\
+					LCD_PORT->CRL |= (0x3<<(4*LCD_CLK));
+#else
+#define _clk_Out()	LCD_PORT->CRH &=~(0xF<<(4*LCD_CLK));\
+					LCD_PORT->CRH |= (0x3<<(4*LCD_CLK));
+#endif
+
+#define _Init_GPIO() { _rst_Out(); _cs_Out(); _sda_Out(); _clk_Out();}
+
+#endif
+
+
+#endif /* NOKIA1661_HW_H_ */
diff --git a/NokiaTFTLib/nokia1661_lcd_driver.c b/NokiaTFTLib/nokia1661_lcd_driver.c
index d6dcfdb..65f6a4a 100644
--- a/NokiaTFTLib/nokia1661_lcd_driver.c
+++ b/NokiaTFTLib/nokia1661_lcd_driver.c
@@ -18,27 +18,11 @@
  */
 
 #include "spfd54124b.h"
-#include <util/delay.h>
-#include <avr/pgmspace.h>
 #include "nokia1661_lcd_driver.h"
 
 
 struct N1616Data	_lcd_data;
 
-#define SBI(port,bit) 	asm("sbi %0, %1" : : "I" (_SFR_IO_ADDR(port)), "I" (bit))
-#define CBI(port,bit) 	asm("cbi %0, %1" : : "I" (_SFR_IO_ADDR(port)), "I" (bit))
-
-/*---------------------------------------------------------------------------------------------------------------
-* lcd pin functions
-*/
-#define LCD_PIN_FUNC(a, b) \
-	void _ ## a ## _set() { SBI(LCD_PORT,(LCD_ ## b)); } \
-	void _ ## a ## _clr() { CBI(LCD_PORT,(LCD_ ## b)); }
-
-LCD_PIN_FUNC(rst, RST)
-LCD_PIN_FUNC(cs, CS)
-LCD_PIN_FUNC(sda, SDA)
-LCD_PIN_FUNC(clk, CLK)
 
 /*----------------------------------------------------------------------------------------------------------------
 * hardware functions
@@ -66,23 +50,14 @@ uint8_t __nlcdRead(void)
 {
 	uint8_t data = 0;
 	/*Read Bits*/
-	_clk_set();	_clk_clr(); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0;
-	_clk_set();	_clk_clr(); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0;
-	_clk_set();	_clk_clr(); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0;
-	_clk_set();	_clk_clr(); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0;
-	_clk_set();	_clk_clr(); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0;
-	_clk_set();	_clk_clr(); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0;
-	_clk_set();	_clk_clr(); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0;
-	_clk_set();	_clk_clr(); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0;
-
-//	_clk_clr();_delay_us(100); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0; _clk_set();
-//	_clk_clr();_delay_us(100); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0; _clk_set();
-//	_clk_clr();_delay_us(100); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0; _clk_set();
-//	_clk_clr();_delay_us(100); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0; _clk_set();
-//	_clk_clr();_delay_us(100); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0; _clk_set();
-//	_clk_clr();_delay_us(100); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0; _clk_set();
-//	_clk_clr();_delay_us(100); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0; _clk_set();
-//	_clk_clr();_delay_us(100); data<<=1; data|=((LCD_PIN&(1 << LCD_SDA))) ? 1:0; _clk_set();
+	_clk_set();	_clk_clr(); data<<=1; data|=(LCD_RSDA()) ? 1:0;
+	_clk_set();	_clk_clr(); data<<=1; data|=(LCD_RSDA()) ? 1:0;
+	_clk_set();	_clk_clr(); data<<=1; data|=(LCD_RSDA()) ? 1:0;
+	_clk_set();	_clk_clr(); data<<=1; data|=(LCD_RSDA()) ? 1:0;
+	_clk_set();	_clk_clr(); data<<=1; data|=(LCD_RSDA()) ? 1:0;
+	_clk_set();	_clk_clr(); data<<=1; data|=(LCD_RSDA()) ? 1:0;
+	_clk_set();	_clk_clr(); data<<=1; data|=(LCD_RSDA()) ? 1:0;
+	_clk_set();	_clk_clr(); data<<=1; data|=(LCD_RSDA()) ? 1:0;
 
 	return data;
 }
@@ -104,7 +79,7 @@ void _nlcdRead(uint8_t Reg,uint8_t *Readbuffer,uint8_t NRead)
 	if(data & ShiftBit[8]) _sda_set(); else _sda_clr();_clk_set();_clk_clr();
 
 	/*Now Input Data Line And PullUp It*/
-	LCD_DDR &= ~(1 << LCD_SDA);
+	_sda_In();
 	_sda_set();
 
 	if(NRead==1)
@@ -124,7 +99,7 @@ void _nlcdRead(uint8_t Reg,uint8_t *Readbuffer,uint8_t NRead)
 	_cs_set();
 
 	/*Make Data Line As Output Again*/
-	LCD_DDR |= (1 << LCD_SDA);
+	_sda_Out();
 }
 /*----------------------------------------------------------------------------------------------------------------
 * private functions
@@ -181,10 +156,15 @@ const uint16_t _lcd_init_list[] =
 
 void nlcdInit()
 {
+	LCD_GPIO_Enable();
+
 	// write 1 to rst and cs
-	LCD_PORT |= (1 << LCD_RST) | (1 << LCD_CS) | (1 << LCD_SDA);
+	_rst_set();
+	_cs_set();
+	_sda_set();
+
 	// init lcd pins
-	LCD_DDR |= (1 << LCD_RST) | (1 << LCD_CS) | (1 << LCD_SDA) | (1 << LCD_CLK);
+	_Init_GPIO();
 
 	_rst_clr();
 	_delay_ms(500);
@@ -232,7 +212,7 @@ void nlcdCharXY(uint8_t x, uint8_t y, rgb_color16bit color, char c)
 
 void nlcdChar(rgb_color16bit color, char c)
 {
-	if(c > nlcdFontLastChar()) return;
+	//if(c > nlcdFontLastChar()) return;
 
 
 	_nlcdSetWindow(_lcd_data.charx, _lcd_data.chary, _lcd_data.charw, _lcd_data.charh);
diff --git a/NokiaTFTLib/nokia1661_lcd_driver.h b/NokiaTFTLib/nokia1661_lcd_driver.h
index 5a79fae..6eb7a51 100644
--- a/NokiaTFTLib/nokia1661_lcd_driver.h
+++ b/NokiaTFTLib/nokia1661_lcd_driver.h
@@ -19,23 +19,8 @@
 
 #ifndef _NOKIA1616_H
 #define _NOKIA1616_H
+#include <nokia1661_Hw.h>
 
-// definign LCD connection
-#define LCD_PORT		PORTB
-#define LCD_PIN			PINB
-#define LCD_DDR			DDRB
-
-#define LCD_RST			PINB1
-#define LCD_CS			PINB2
-#define LCD_SDA			PINB3
-#define LCD_CLK			PINB5
-
-
-#if !defined(LCD_PORT) || !defined(LCD_DDR) || !defined(LCD_RST) || !defined(LCD_CS) || !defined(LCD_SDA) || !defined(LCD_CLK)
-#error You need to define ports & pins for LCD usage
-#endif
-
-#include <avr/io.h>
 
 struct font_header
 {