diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..e899e24
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.0)
+
+set(CMAKE_TOOLCHAIN_FILE cmake/sdcc-generic.cmake) # path to sdcc-generic.cmake toolchain file.
+set(CMAKE_MODULE_PATH .) # path to cmake files directory.
+#set(STM8_StdPeriph_DIR ../stm8s-sdcc-template/STM8S_StdPeriph_Lib/Libraries/STM8S_StdPeriph_Driver) # path to StdPeriph directory.
+set(STM8_CHIP stm8s103f3) # stm8 chip name, e.g. stm8l152c6 or stm8s105k4
+set(CMAKE_C_COMPILER_WORKS 1)
+
+project(cap-tester C)
+
+add_definitions(-DSTM8S103 -DF_CPU=2000000)
+
+include(../stm8-sdcc-cmake/cmake/sdcc-stm8s.cmake)
+
+message(STATUS CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE: ${CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE})
+
+set(std-periph /home/user/Project/IoT/stm8/stm8s-sdcc-template/STM8S_StdPeriph_Lib/Libraries/STM8S_StdPeriph_Driver)
+
+include_directories(
+  /usr/local/share/sdcc/include
+  /home/user/Project/IoT/stm8/stm8-sdcc-examples
+  /home/user/Project/IoT/stm8/stm8s-sdcc-template/Inc
+  ${std-periph}/inc
+#  /home/user/Project/IoT/stm8/stm8-sdcc-cmake/StdPeriph/STM8S/Libraries/STM8S_StdPeriph_Driver/inc/
+#/home/user/Project/IoT/stm8/stm8-sdcc-cmake/StdPeriph/
+)
+
+#DEFINES = -D$(COMPILER) -D$(MCU) -DUSE_STDPERIPH_DRIVER
+
+#cmake -DCMAKE_TOOLCHAIN_FILE=<path_to_sdcc-generic.cmake> -DCMAKE_MODULE_PATH=<path_to_project_cmake_dir> -DSTM8_CHIP=<chip name> -DSTM8_StdPeriph_DIR=<path to std periph> -G"MinGW Makefiles" <path_to_source_dir>
+find_package(StdPeriph COMPONENTS gpio)
+file(GLOB src
+  main.*
+  analog.*
+  serial.*
+  util.*
+  gpio.*
+  stm8.h
+  cmake/*.cmake
+  )
+add_executable(${PROJECT_NAME} ${src})
+
+#add_custom_target(Resource SOURCES readme.txt)
+
+#Important compiler options for STM8 developers include:
+
+#-c to compile into object files to be linked later
+#--std-c99 for compilation in C99 mode (some C99 features, e.g. variable-length arrays
+# are not yet supported in sdcc though)
+#--opt-code-size for optimization for code size
+#--max-allocs-per-node to select the optimization level. the default value is 3000.
+# Higher values result in more optimized code, longer compiler runtime,
+# and higher memory usage during compilation.
diff --git a/analog.c b/analog.c
new file mode 100644
index 0000000..948fc6e
--- /dev/null
+++ b/analog.c
@@ -0,0 +1,24 @@
+#include "analog.h"
+#include "main.h"
+#include "stm8.h"
+
+void AnalogInit(uint8_t Channel)
+{
+  ADC_CSR = 0;
+  ADC_CR1 = 0;
+  ADC_CR2 = 0;
+  ADC_CR3 = 0;
+  ADC_CSR = Channel; // Select channel 2 (AIN2=PC4)
+  ADC_CR1 |= ADC_CR1_ADON; // ADON
+  ADC_CR2 &= ~ADC_CR2_ALIGN; // Align left
+  delay(1000); // Give little time to be ready for first conversion
+}
+
+uint16_t AnalogRead()
+{
+  ADC_CR1 &= ~ADC_CR1_CONT; // Single conversion mode
+  ADC_CR1 |= ADC_CR1_ADON; // Start conversion
+  do { nop(); } while ((ADC_CSR >> 7) == 0);
+  ADC_CSR &= ~ADC_CSR_EOC; // Clear "End of conversion"-flag
+  return (ADC_DRH << 2) | (ADC_DRL >> 6);  // Left aligned
+}
diff --git a/analog.h b/analog.h
new file mode 100644
index 0000000..8ddbe2a
--- /dev/null
+++ b/analog.h
@@ -0,0 +1,12 @@
+#include <stdint.h>
+
+void AnalogInit(uint8_t Channel);
+uint16_t AnalogRead(); //just adding analog input to pin C4
+
+typedef struct
+{
+  void (*Init)(uint8_t Channel);
+  uint16_t (*Read)(void);
+} TAnalog;
+
+static TAnalog Analog = {.Init = AnalogInit, .Read = AnalogRead};
diff --git a/cmake/sdcc-generic.cmake b/cmake/sdcc-generic.cmake
new file mode 100644
index 0000000..93c20e0
--- /dev/null
+++ b/cmake/sdcc-generic.cmake
@@ -0,0 +1,46 @@
+set(CMAKE_SYSTEM_NAME Generic)
+
+set(CMAKE_C_COMPILER sdcc)
+set(CMAKE_OBJCOPY sdobjcopy CACHE INTERNAL "objcopy tool")
+set(CMAKE_PACKIHX packihx CACHE INTERNAL "packihx tool")
+
+set(CMAKE_STATIC_LIBRARY_PREFIX "")
+set(CMAKE_STATIC_LIBRARY_SUFFIX ".lib")
+set(CMAKE_SHARED_LIBRARY_PREFIX "")
+set(CMAKE_SHARED_LIBRARY_SUFFIX ".lib")
+set(CMAKE_IMPORT_LIBRARY_PREFIX )
+set(CMAKE_IMPORT_LIBRARY_SUFFIX )
+set(CMAKE_EXECUTABLE_SUFFIX ".ihx")
+set(CMAKE_LINK_LIBRARY_SUFFIX ".lib")
+set(CMAKE_DL_LIBS "")
+set(CMAKE_C_OUTPUT_EXTENSION ".rel")
+
+get_filename_component(SDCC_LOCATION "${CMAKE_C_COMPILER}" PATH)
+find_program(SDCCLIB_EXECUTABLE sdcclib PATHS "${SDCC_LOCATION}" NO_DEFAULT_PATH)
+find_program(SDCCLIB_EXECUTABLE sdcclib)
+set(CMAKE_AR "${SDCCLIB_EXECUTABLE}" CACHE FILEPATH "The sdcc librarian" FORCE)
+
+if(NOT DEFINED CMAKE_C_FLAGS_INIT)
+  set(flags "-mstm8 --opt-code-size --std-c11 --print-search-dirs")
+  if(CMAKE_BUILD_TYPE MATCHES "debug")
+    set(flags "${flags} --out-fmt-elf --all-callee-saves --debug --verbose --stack-auto --fverbose-asm  --float-reent --no-peep")
+  endif()
+  set(CMAKE_C_FLAGS_INIT ${flags})
+endif()
+
+if(NOT DEFINED CMAKE_EXE_LINKER_FLAGS_INIT)
+    set (CMAKE_EXE_LINKER_FLAGS_INIT "")
+endif()
+
+#set(CMAKE_C_COMPILE_OBJECT  "<CMAKE_C_COMPILER> <DEFINES> <INCLUDES> <FLAGS> -o <OBJECT> -c <SOURCE>")
+set(CMAKE_C_COMPILE_OBJECT  "<CMAKE_C_COMPILER> <DEFINES> <INCLUDES> <FLAGS> -o -c <SOURCE>")
+set(CMAKE_C_LINK_EXECUTABLE "<CMAKE_C_COMPILER> <FLAGS> <OBJECTS> --out-fmt-ihx -o  <TARGET> <CMAKE_C_LINK_FLAGS> <LINK_FLAGS> <LINK_LIBRARIES>")
+
+set(CMAKE_C_CREATE_STATIC_LIBRARY
+        "\"${CMAKE_COMMAND}\" -E remove <TARGET>"
+        "<CMAKE_AR> -a <TARGET> <LINK_FLAGS> <OBJECTS> ")
+
+set(CMAKE_C_CREATE_SHARED_LIBRARY "")
+set(CMAKE_C_CREATE_MODULE_LIBRARY "")
+
+add_definitions(-D__SDCC__)
diff --git a/gpio.c b/gpio.c
new file mode 100644
index 0000000..e69de29
diff --git a/gpio.h b/gpio.h
new file mode 100644
index 0000000..25896ef
--- /dev/null
+++ b/gpio.h
@@ -0,0 +1,13 @@
+#include "util.h"
+#include "main.h"
+
+typedef enum {In = 0, Out} TPinDirection;
+enum {Float = 0, PullUp};
+enum {OpenDrain = 0, PushPull};
+//void SetPin(unsigned char *PinGroup, unsigned char Pin)
+//{
+//  *(PinGroup + PA_DDR - PA) |= Pin;
+//  PC_DDR |= PIN5;
+//  PC_CR1 |= PIN5;
+//  PC_ODR |= Pin;
+//}
diff --git a/main.c b/main.c
new file mode 100644
index 0000000..722e5f4
--- /dev/null
+++ b/main.c
@@ -0,0 +1,146 @@
+#include "main.h"
+#include "util.h"
+#include "analog.h"
+#include "serial.h"
+#include <stdint.h>
+
+/* Build in LED is in pin B5 (STM8S103 board) or D3 (STM8S003F3 board) */
+#define LED_PORT    PB
+#define LED_PIN     PIN5
+
+//  Setup the system clock to run at 16MHz using the internal oscillator.
+void InitialiseSystemClock()
+{
+  CLK_ICKR = 0;                       //  Reset the Internal Clock Register.
+  CLK_ICKR |= CLK_ICKR_HSIEN;                 //  Enable the HSI.
+  CLK_ECKR = 0;                       //  Disable the external clock.
+  while (!(CLK_ICKR & CLK_ICKR_HSIRDY));       //  Wait for the HSI to be ready for use.
+  CLK_CKDIVR = 0;                     //  Ensure the clocks are running at full speed.
+  CLK_PCKENR1 = 0xff;                 //  Enable all peripheral clocks.
+  CLK_PCKENR2 = 0xff;                 //  Ditto.
+  CLK_CCOR = 0;                       //  Turn off CCO.
+  CLK_HSITRIMR = 0;                   //  Turn off any HSIU trimming.
+  CLK_SWIMCCR = 0;                    //  Set SWIM to run at clock / 2.
+  CLK_SWR = 0xe1;                     //  Use HSI as the clock source.
+  CLK_SWCR = 0;                       //  Reset the clock switch control register.
+  CLK_SWCR |= CLK_SWCR_SWEN;                  //  Enable switching.
+  while (CLK_SWCR & CLK_SWCR_SWBSY);        //  Pause while the clock switch is busy.
+}
+
+void SetPCFloat(unsigned char Pins)
+{
+  PC_DDR &= ~Pins;
+  PC_CR1 &= ~Pins;
+}
+
+void SetPCHigh(uint8_t Pins)
+{
+  PC_DDR |= Pins;
+  PC_CR1 |= Pins;
+  PC_ODR |= Pins;
+}
+
+void SetPCLow(uint8_t Pins)
+{
+  PC_DDR |= Pins;
+  PC_ODR &= ~Pins;
+  PC_CR1 &= ~Pins;
+}
+
+//PC4 - ADC. 150
+//PC5 - 10k
+//PC6 - 100k
+//PC7 - 750
+#define PIN_ADC PC4
+#define PIN_FAST_CHARGE PIN5
+#define PIN_SLOW_CHARGE PIN6
+#define PIN_FAST_DISCHARGE PIN7
+static Bool FastCharge = True;
+static volatile uint32_t MeasureTickCount = 0;
+float Test()
+{
+  const uint16_t ChargeResistors[2] = {100, 10}; // KOhm
+//  CLK_PCKENR1 = 0;  // Disable all Peripheral Clocks - at start
+//  CLK_PCKENR2 = 0;
+  Analog.Init(2);
+  SetPCFloat(PIN_FAST_CHARGE | PIN_SLOW_CHARGE | PIN_FAST_DISCHARGE);
+  if(FastCharge)
+    SetPCHigh(PIN_FAST_CHARGE);
+  else
+    SetPCHigh(PIN_SLOW_CHARGE);
+  static const int LinearChargeTimeCoeff = 648;
+  while(Analog.Read() < LinearChargeTimeCoeff) MeasureTickCount++;
+  float pF = ((float)MeasureTickCount * 7700 / ChargeResistors[FastCharge]);
+  FastCharge = !MeasureTickCount || MeasureTickCount > 1000;
+  SetPCFloat(PIN_FAST_CHARGE | PIN_SLOW_CHARGE);
+  SetPCLow(PIN_FAST_DISCHARGE);
+  while(Analog.Read() > 0);
+  SetPCFloat(PIN7);
+  return pF;
+}
+
+int main()
+{
+  int i;
+  //  InitialiseSystemClock();
+  CLK_CKDIVR = 0; // Set clock to full speed (16 Mhz)
+  Serial.Init();
+  Serial.Write("Started\r\n");
+  //   GPIO setup
+  PORT(LED_PORT, DDR) |= LED_PIN; // PB_DDR |= (1 << 5); // Set pin data direction as output
+  PORT(LED_PORT, CR1) |= LED_PIN; // PB_CR1 |= (1 << 5); // Set pin as "Push-pull"
+  for(i = 0; i < 3; i++)
+  {
+    PORT(LED_PORT, ODR) &= ~LED_PIN; // PB_ODR &= ~(1 << 5);
+    delay(100000L);
+    PORT(LED_PORT, ODR) |= LED_PIN; // PB_ODR |= (1 << 5);
+    delay(100000L);
+  }
+  float pFAverage = 0;
+  const uint32_t OutputPeriod = 40000;
+  uint32_t OutputTickCount = 0;
+  while(1)
+  {
+    float pF;
+    pF = Test();
+    const uint16_t IterationTickCount = 5000;
+    OutputTickCount += MeasureTickCount + IterationTickCount;
+    MeasureTickCount = 0;
+    if(pF != 0.0f)
+    {
+      float MeasureWeight = 0.9f;
+      pFAverage = pF * MeasureWeight + pFAverage * (1 - MeasureWeight);
+    }
+    else
+    {
+      pFAverage = 0;
+      OutputTickCount = 0;
+    }
+    if(OutputTickCount > OutputPeriod)
+    {
+      OutputTickCount = 0;
+      const uint8_t NextMeasureUnit = 100;
+      if(pFAverage >= (float)NextMeasureUnit * 1000)
+      {
+        Serial.WriteFloat(pFAverage / 1000000);
+        Serial.Write(" uF\r\n");
+      }
+      else if(pFAverage >= (float)NextMeasureUnit)
+      {
+        Serial.WriteFloat(pFAverage / 1000);
+        Serial.Write(" nF\r\n");
+      }
+      else
+      {
+        Serial.WriteFloat(pFAverage);
+        Serial.Write(" pF\r\n");
+      }
+    }
+    PORT(LED_PORT, ODR) &= ~LED_PIN; // PB_ODR &= ~(1 << 5);
+    delay(10000L);
+    PORT(LED_PORT, ODR) |= LED_PIN; // PB_ODR |= (1 << 5);
+  }
+  return 0;
+}
+
+
diff --git a/main.h b/main.h
new file mode 100644
index 0000000..640266c
--- /dev/null
+++ b/main.h
@@ -0,0 +1,10 @@
+#pragma once
+#include "stm8.h"
+
+
+/* Simple busy loop delay */
+static void delay(unsigned long count)
+{
+  while(count--)
+    nop();
+}
diff --git a/serial.c b/serial.c
new file mode 100644
index 0000000..c8b8970
--- /dev/null
+++ b/serial.c
@@ -0,0 +1,37 @@
+#include "../stm8-sdcc-examples/stm8.h"
+#include "main.h"
+#include "serial.h"
+#include "util.h"
+
+void SerialInit()
+{
+  // Setup UART1 (TX=D5)
+  UART1_CR2 |= UART_CR2_TEN; // Transmitter enable
+  // UART1_CR2 |= UART_CR2_REN; // Receiver enable
+  UART1_CR3 &= ~(UART_CR3_STOP1 | UART_CR3_STOP2); // 1 stop bit
+  // 9600 baud: UART_DIV = 16000000/9600 ~ 1667 = 0x0683
+  UART1_BRR2 = 0x03; UART1_BRR1 = 0x68; // 0x0683 coded funky way (see ref manual)
+}
+
+int SerialWrite(const char *str)
+{
+  const char *i;
+  for(i = str; *i; i++)
+  {
+    while(!(UART1_SR & UART_SR_TXE)); // !Transmit data register empty
+    UART1_DR = (unsigned char)*i;
+  }
+  return i - str; // Bytes sent
+}
+
+void SerialWriteInt(const uint32_t Value)
+{
+  IntToStr(Value, Num);
+  SerialWrite(Num);
+}
+
+void SerialWriteFloat(const float Value)
+{
+  FloatToStr(Value, Num);
+  SerialWrite(Num);
+}
diff --git a/serial.h b/serial.h
new file mode 100644
index 0000000..afa2e6e
--- /dev/null
+++ b/serial.h
@@ -0,0 +1,25 @@
+#pragma once
+#include <stdint.h>
+
+static char Num[11];
+
+void SerialInit();
+int SerialWrite(const char *str);
+void SerialWriteInt(const uint32_t Value);
+void SerialWriteFloat(const float Value);
+
+typedef struct
+{
+  void (*Init)();
+  int (*Write)(const char *str);
+  void (*WriteInt)(const uint32_t Value);
+  void (*WriteFloat)(const float Value);
+} TSerial;
+
+static TSerial Serial =
+{
+  .Init = SerialInit,
+  .Write = SerialWrite,
+  .WriteInt = SerialWriteInt,
+  .WriteFloat = SerialWriteFloat
+};
diff --git a/stm8.h b/stm8.h
new file mode 100644
index 0000000..3d7608a
--- /dev/null
+++ b/stm8.h
@@ -0,0 +1,399 @@
+/*
+ * Register definitions for STM8S103 (and STM8S003)
+ * Still incomplete.
+ */
+#ifndef _STH8_H
+#define _STH8_H
+
+
+/* Handy macros for GPIO */
+#define CONCAT(a, b)    a##_##b
+#define PORT(a, b)      CONCAT(a , b)
+
+#define PIN0    (1 << 0)
+#define PIN1    (1 << 1)
+#define PIN2    (1 << 2)
+#define PIN3    (1 << 3)
+#define PIN4    (1 << 4)
+#define PIN5    (1 << 5)
+#define PIN6    (1 << 6)
+#define PIN7    (1 << 7)
+
+/* Register addresses */
+#define PReg volatile unsigned char*
+/* Clock */
+#define CLK_CKDIVR	*(PReg)0x50C6
+/* CLOCK */
+#define CLK_ICKR	*(unsigned char*)0x50C0
+#define CLK_ECKR	*(unsigned char*)0x50C1
+#define CLK_CMSR	*(unsigned char*)0x50C3
+#define CLK_SWR 	*(unsigned char*)0x50C4
+#define CLK_SWCR	*(unsigned char*)0x50C5
+//#define CLK_CKDIVR	*(unsigned char*)0x50C6
+#define CLK_PCKENR1	*(unsigned char*)0x50C7
+#define CLK_CSSR	*(unsigned char*)0x50C8
+#define CLK_CCOR	*(unsigned char*)0x50C9
+#define CLK_PCKENR2	*(unsigned char*)0x50CA
+#define CLK_HSITRIMR	*(unsigned char*)0x50CC
+#define CLK_SWIMCCR	*(unsigned char*)0x50CD
+
+#define CLK_ICKR_HSIEN (1<<0)
+#define CLK_ICKR_HSIRDY (1<<1)
+#define CLK_ICKR_FHW (1<<2)
+#define CLK_ICKR_LSIEN (1<<3)
+#define CLK_ICKR_LSIRDY (1<<4)
+#define CLK_ICKR_REGAH (1<<5)
+
+#define CLK_CKDIVR_HSIDIV1 0
+#define CLK_CKDIVR_HSIDIV2 (1<<0)
+#define CLK_CKDIVR_HSIDIV4 (1<<1)
+#define CLK_CKDIVR_HSIDIV8 (1<<0|1<<1)
+
+#define CLK_CKDIVR_CPUDIV1		0
+#define CLK_CKDIVR_CPUDIV2		(1<<0)
+#define CLK_CKDIVR_CPUDIV4		(1<<1)
+#define CLK_CKDIVR_CPUDIV8		(1<<1|1<<0)
+#define CLK_CKDIVR_CPUDIV16		(1<<2)
+#define CLK_CKDIVR_CPUDIV32		(1<<2|1<<0)
+#define CLK_CKDIVR_CPUDIV64		(1<<2|1<<1)
+#define CLK_CKDIVR_CPUDIV128	(1<<2|1<<1|1<<0)
+
+#define CLK_SWCR_SWBSY	(1<<0)
+#define CLK_SWCR_SWEN 	(1<<1)
+#define CLK_SWCR_SWIEN 	(1<<2)
+#define CLK_SWCR_SWIF 	(1<<3)
+
+#define CLK_SWIMCCR_SWIMCLK		(1<<0)
+//#define CLK_SWCR_SWEN        ((uint8_t)0x02) /*!< Switch start/stop */
+
+
+/* GPIO */
+#define PA_ODR *(PReg)0x5000
+#define PA_IDR *(PReg)0x5001
+#define PA_DDR *(PReg)0x5002
+#define PA_CR1 *(PReg)0x5003
+#define PA_CR2 *(PReg)0x5004
+
+#define PB_ODR *(PReg)0x5005
+#define PB_IDR *(PReg)0x5006
+#define PB_DDR *(PReg)0x5007
+#define PB_CR1 *(PReg)0x5008
+#define PB_CR2 *(PReg)0x5009
+
+#define PC_ODR *(PReg)0x500A //data output latch register
+#define PC_IDR *(PReg)0x500B //input pin value register
+#define PC_DDR *(PReg)0x500C //data dir reg: 0-in, 1-out
+#define PC_CR1 *(PReg)0x500D //ctl reg: in:0-float, 1-pull-up; out:0-open drain, 1-push-pull
+#define PC_CR2 *(PReg)0x500E //ctl reg: in:0-int disable, 1-int enable; out: 0-2MHz, 1-10MHz
+
+#define PD_ODR *(PReg)0x500F
+#define PD_IDR *(PReg)0x5010
+#define PD_DDR *(PReg)0x5011
+#define PD_CR1 *(PReg)0x5012
+#define PD_CR2 *(PReg)0x5013
+
+/* UART */
+#define UART1_SR *(PReg)0x5230
+#define UART1_DR *(PReg)0x5231
+#define UART1_BRR1 *(PReg)0x5232
+#define UART1_BRR2 *(PReg)0x5233
+#define UART1_CR1 *(PReg)0x5234
+#define UART1_CR2 *(PReg)0x5235
+#define UART1_CR3 *(PReg)0x5236
+#define UART1_CR4 *(PReg)0x5237
+#define UART1_CR5 *(PReg)0x5238
+#define UART1_GTR *(PReg)0x5239
+#define UART1_PSCR *(PReg)0x523A
+
+#define UART_SR_TXE (1 << 7)
+#define UART_SR_TC (1 << 6)
+#define UART_SR_RXNE (1 << 5)
+#define UART_SR_IDLE (1 << 4)
+#define UART_SR_OR (1 << 3)
+#define UART_SR_NF (1 << 2)
+#define UART_SR_FE (1 << 1)
+#define UART_SR_PE (1 << 0)
+
+#define UART_CR1_R8 (1 << 7)
+#define UART_CR1_T8 (1 << 6)
+#define UART_CR1_UARTD (1 << 5)
+#define UART_CR1_M (1 << 4)
+#define UART_CR1_WAKE (1 << 3)
+#define UART_CR1_PCEN (1 << 2)
+#define UART_CR1_PS (1 << 1)
+#define UART_CR1_PIEN (1 << 0)
+
+#define UART_CR2_TIEN (1 << 7)
+#define UART_CR2_TCIEN (1 << 6)
+#define UART_CR2_RIEN (1 << 5)
+#define UART_CR2_ILIEN (1 << 4)
+#define UART_CR2_TEN (1 << 3)
+#define UART_CR2_REN (1 << 2)
+#define UART_CR2_RWU (1 << 1)
+#define UART_CR2_SBK (1 << 0)
+
+#define UART_CR3_LINEN (1 << 6)
+#define UART_CR3_STOP2 (1 << 5)
+#define UART_CR3_STOP1 (1 << 4)
+#define UART_CR3_CLKEN (1 << 3)
+#define UART_CR3_CPOL (1 << 2)
+#define UART_CR3_CPHA (1 << 1)
+#define UART_CR3_LBCL (1 << 0)
+
+/* Timers */
+#define TIM1_CR1 *(PReg)0x5250
+#define TIM1_CR2 *(PReg)0x5251
+#define TIM1_SMCR *(PReg)0x5252
+#define TIM1_ETR *(PReg)0x5253
+#define TIM1_IER *(PReg)0x5254
+#define TIM1_SR1 *(PReg)0x5255
+#define TIM1_SR2 *(PReg)0x5256
+#define TIM1_EGR *(PReg)0x5257
+#define TIM1_CCMR1 *(PReg)0x5258
+#define TIM1_CCMR2 *(PReg)0x5259
+#define TIM1_CCMR3 *(PReg)0x525A
+#define TIM1_CCMR4 *(PReg)0x525B
+#define TIM1_CCER1 *(PReg)0x525C
+#define TIM1_CCER2 *(PReg)0x525D
+#define TIM1_CNTRH *(PReg)0x525E
+#define TIM1_CNTRL *(PReg)0x525F
+#define TIM1_PSCRH *(PReg)0x5260
+#define TIM1_PSCRL *(PReg)0x5261
+#define TIM1_ARRH *(PReg)0x5262
+#define TIM1_ARRL *(PReg)0x5263
+#define TIM1_RCR *(PReg)0x5264
+#define TIM1_CCR1H *(PReg)0x5265
+#define TIM1_CCR1L *(PReg)0x5266
+#define TIM1_CCR2H *(PReg)0x5267
+#define TIM1_CCR2L *(PReg)0x5268
+#define TIM1_CCR3H *(PReg)0x5269
+#define TIM1_CCR3L *(PReg)0x526A
+#define TIM1_CCR4H *(PReg)0x526B
+#define TIM1_CCR4L *(PReg)0x526C
+#define TIM1_BKR *(PReg)0x526D
+#define TIM1_DTR *(PReg)0x526E
+#define TIM1_OISR *(PReg)0x526F
+
+/* Note these are for STM8S103 and STM8S003
+   STM8S105,104/207/208 are different */
+#define TIM2_CR1 *(PReg)0x5300
+#define TIM2_CR2 *(PReg)0x5301
+#define TIM2_SMCR *(PReg)0x5302
+#define TIM2_IER *(PReg)0x5303
+#define TIM2_SR1 *(PReg)0x5304
+#define TIM2_SR2 *(PReg)0x5305
+#define TIM2_EGR *(PReg)0x5306
+#define TIM2_CCMR1 *(PReg)0x5307
+#define TIM2_CCMR2 *(PReg)0x5308
+#define TIM2_CCMR3 *(PReg)0x5309
+#define TIM2_CCER1 *(PReg)0x530A
+#define TIM2_CCER2 *(PReg)0x530B
+#define TIM2_CNTRH *(PReg)0x530C
+#define TIM2_CNTRL *(PReg)0x530D
+#define TIM2_PSCR *(PReg)0x530E
+#define TIM2_ARRH *(PReg)0x530F
+#define TIM2_ARRL *(PReg)0x5310
+#define TIM2_CCR1H *(PReg)0x5311
+#define TIM2_CCR1L *(PReg)0x5312
+#define TIM2_CCR2H *(PReg)0x5313
+#define TIM2_CCR2L *(PReg)0x5314
+#define TIM2_CCR3H *(PReg)0x5315
+#define TIM2_CCR3L *(PReg)0x5316
+
+/* Note these are for STM8S103 and STM8S003
+   STM8S105,104/207/208 are different */
+#define TIM4_CR1 *(PReg)0x5340
+#define TIM4_CR2 *(PReg)0x5341
+#define TIM4_SMCR *(PReg)0x5342
+#define TIM4_IER *(PReg)0x5343
+#define TIM4_SR *(PReg)0x5344
+#define TIM4_EGR *(PReg)0x5345
+#define TIM4_CNTR *(PReg)0x5346
+#define TIM4_PSCR *(PReg)0x5347
+#define TIM4_ARR *(PReg)0x5348
+
+#define TIM_IER_BIE (1 << 7)
+#define TIM_IER_TIE (1 << 6)
+#define TIM_IER_COMIE (1 << 5)
+#define TIM_IER_CC4IE (1 << 4)
+#define TIM_IER_CC3IE (1 << 3)
+#define TIM_IER_CC2IE (1 << 2)
+#define TIM_IER_CC1IE (1 << 1)
+#define TIM_IER_UIE (1 << 0)
+
+#define TIM_CR1_APRE (1 << 7)
+#define TIM_CR1_CMSH (1 << 6)
+#define TIM_CR1_CMSL (1 << 5)
+#define TIM_CR1_DIR (1 << 4)
+#define TIM_CR1_OPM (1 << 3)
+#define TIM_CR1_URS (1 << 2)
+#define TIM_CR1_UDIS (1 << 1)
+#define TIM_CR1_CEN (1 << 0)
+
+#define TIM_SR1_BIF (1 << 7)
+#define TIM_SR1_TIF (1 << 6)
+#define TIM_SR1_COMIF (1 << 5)
+#define TIM_SR1_CC4IF (1 << 4)
+#define TIM_SR1_CC3IF (1 << 3)
+#define TIM_SR1_CC2IF (1 << 2)
+#define TIM_SR1_CC1IF (1 << 1)
+#define TIM_SR1_UIF (1 << 0)
+
+/* SPI */
+#define SPI_CR1 *(PReg)0x5200
+#define SPI_CR2 *(PReg)0x5201
+#define SPI_ICR *(PReg)0x5202
+#define SPI_SR *(PReg)0x5203
+#define SPI_DR *(PReg)0x5204
+#define SPI_CRCPR *(PReg)0x5205
+#define SPI_RXCRCR *(PReg)0x5206
+#define SPI_TXCRCR *(PReg)0x5207
+
+#define SPI_CR1_LSBFIRST (1 << 7)
+#define SPI_CR1_SPE (1 << 6)
+#define SPI_CR1_BR(br) ((br) << 3)
+#define SPI_CR1_MSTR (1 << 2)
+#define SPI_CR1_CPOL (1 << 1)
+#define SPI_CR1_CPHA (1 << 0)
+
+#define SPI_CR2_BDM (1 << 7)
+#define SPI_CR2_BDOE (1 << 6)
+#define SPI_CR2_CRCEN (1 << 5)
+#define SPI_CR2_CRCNEXT (1 << 4)
+#define SPI_CR2_RXONLY (1 << 2)
+#define SPI_CR2_SSM (1 << 1)
+#define SPI_CR2_SSI (1 << 0)
+
+#define SPI_ICR_TXIE (1 << 7)
+#define SPI_ICR_RXIE (1 << 6)
+#define SPI_ICR_ERRIE (1 << 5)
+#define SPI_ICR_WKIE (1 << 4)
+
+#define SPI_SR_BSY (1 << 7)
+#define SPI_SR_OVR (1 << 6)
+#define SPI_SR_MODF (1 << 5)
+#define SPI_SR_CRCERR (1 << 4)
+#define SPI_SR_WKUP (1 << 3)
+#define SPI_SR_TXE (1 << 1)
+#define SPI_SR_RxNE (1 << 0)
+
+/* I2C */
+#define I2C_CR1 *(PReg)0x5210
+#define I2C_CR2 *(PReg)0x5211
+#define I2C_FREQR *(PReg)0x5212
+#define I2C_OARL *(PReg)0x5213
+#define I2C_OARH *(PReg)0x5214
+#define I2C_DR *(PReg)0x5216
+#define I2C_SR1 *(PReg)0x5217
+#define I2C_SR2 *(PReg)0x5218
+#define I2C_SR3 *(PReg)0x5219
+#define I2C_ITR *(PReg)0x521A
+#define I2C_CCRL *(PReg)0x521B
+#define I2C_CCRH *(PReg)0x521C
+#define I2C_TRISER *(PReg)0x521D
+#define I2C_PECR *(PReg)0x521E
+
+/* ADC */
+#define ADC_DBxR *(PReg)0x53E0
+#define ADC_CSR *(PReg)0x5400
+#define ADC_CR1 *(PReg)0x5401
+#define ADC_CR2 *(PReg)0x5402
+#define ADC_CR3 *(PReg)0x5403
+#define ADC_DRH *(PReg)0x5404
+#define ADC_DRL *(PReg)0x5405
+#define ADC_TDRH *(PReg)0x5406
+#define ADC_TDRL *(PReg)0x5407
+#define ADC_HTRH *(PReg)0x5408
+#define ADC_HTRL *(PReg)0x5409
+#define ADC_LTRH *(PReg)0x540A
+#define ADC_LTRL *(PReg)0x540B
+#define ADC_AWSRH *(PReg)0x540C
+#define ADC_AWSRL *(PReg)0x540D
+#define ADC_AWCRH *(PReg)0x540E
+#define ADC_AWCRL *(PReg)0x540F
+
+#define ADC_CSR_EOC (1 << 7)
+#define ADC_CSR_AWD (1 << 6)
+#define ADC_CSR_EOCIE (1 << 5)
+#define ADC_CSR_AWDIE (1 << 4)
+
+#define ADC_CR1_CONT (1 << 1)
+#define ADC_CR1_ADON (1 << 0)
+
+#define ADC_CR2_EXTTRIG (1 << 6)
+#define ADC_CR2_EXTSEL (1 << 4)
+#define ADC_CR2_ALIGN (1 << 3)
+#define ADC_CR2_SCAN (1 << 1)
+
+
+/* Interrupt commands */
+#define enableInterrupts()    {__asm__("rim\n");}  /* enable interrupts */
+#define disableInterrupts()   {__asm__("sim\n");}  /* disable interrupts */
+#define rim()                 {__asm__("rim\n");}  /* enable interrupts */
+#define sim()                 {__asm__("sim\n");}  /* disable interrupts */
+#define nop()                 {__asm__("nop\n");}  /* No Operation */
+#define trap()                {__asm__("trap\n");} /* Trap (soft IT) */
+#define wfi()                 {__asm__("wfi\n");}  /* Wait For Interrupt */
+#define halt()                {__asm__("halt\n");} /* Halt */
+
+/* Interrupt numbers */
+#define TIM1_OVR_UIF_IRQ 11
+#define TIM2_OVR_UIF_IRQ 13
+#define TIM3_OVR_UIF_IRQ 15
+#define ADC1_EOC_IRQ 22
+#define TIM4_OVR_UIF_IRQ 23
+/*
+Interrupts:
+
+0 TLI
+1 AWU Auto Wake up from Halt
+2 CLK Clock controller
+3 EXTI0 Port A external interrupts
+4 EXTI1 Port B external interrupts
+5 EXTI2 Port C external interrupts
+6 EXTI3 Port D external interrupts
+7 EXTI4 Port E external interrupts
+8 CAN CAN RX interrupt
+9 CAN CAN TX/ER/SC interrupt
+10 SPI End of Transfer
+11 TIM1 Update /Overflow/Underflow/Trigger/Break
+12 TIM1 Capture/Compare
+13 TIM2 Update /Overflow
+14 TIM2 Capture/Compare
+15 TIM3 Update /Overflow
+16 TIM3 Capture/Compare
+17 UART1 Tx complete
+18 UART1 Receive Register DATA FULL
+19 I2C I2C interrupt
+20 UART2/3 Tx complete
+21 UART2/3 Receive Register DATA FULL
+22 ADC End of Conversion
+23 TIM4 Update/Overflow
+24 FLASH EOP/WR_PG_DIS
+
+TLI 0
+AWU 1
+CLK 2
+EXTI_PORTA 3
+EXTI_PORTB 4
+EXTI_PORTC
+EXTI_PORTD
+EXTI_PORTE
+CAN_RX
+CAN_TX
+SPI
+TIM1_UPD_OVF_TRG_BRK
+TIM1_CAP_COM
+TIM2_UPD_OVF_BRK
+TIM2_CAP_COM
+TIM3_UPD_OVF_BRK
+TIM3_CAP_COM
+UART1_TX
+UART1_RX
+I2C 19
+ADC1 22
+TIM4_UPD_OVF 23
+EEPROM_EEC 24
+*/
+
+#endif
diff --git a/stm8s.h b/stm8s.h
new file mode 100644
index 0000000..e7c33a4
--- /dev/null
+++ b/stm8s.h
@@ -0,0 +1,207 @@
+/* GPIO */
+#define PA_ODR *(unsigned char*)0x5000
+#define PA_IDR *(unsigned char*)0x5001
+#define PA_DDR *(unsigned char*)0x5002
+#define PA_CR1 *(unsigned char*)0x5003
+#define PA_CR2 *(unsigned char*)0x5004
+
+#define PB_ODR *(unsigned char*)0x5005
+#define PB_IDR *(unsigned char*)0x5006
+#define PB_DDR *(unsigned char*)0x5007
+#define PB_CR1 *(unsigned char*)0x5008
+#define PB_CR2 *(unsigned char*)0x5009
+
+#define PC_ODR *(unsigned char*)0x500A
+#define PC_IDR *(unsigned char*)0x500B
+#define PC_DDR *(unsigned char*)0x500C
+#define PC_CR1 *(unsigned char*)0x500D
+#define PC_CR2 *(unsigned char*)0x500E
+
+#define PD_ODR *(unsigned char*)0x500F
+#define PD_IDR *(unsigned char*)0x5010
+#define PD_DDR *(unsigned char*)0x5011
+#define PD_CR1 *(unsigned char*)0x5012
+#define PD_CR2 *(unsigned char*)0x5013
+
+#define PE_ODR *(unsigned char*)0x5014
+#define PE_IDR *(unsigned char*)0x5015
+#define PE_DDR *(unsigned char*)0x5016
+#define PE_CR1 *(unsigned char*)0x5017
+#define PE_CR2 *(unsigned char*)0x5018
+
+#define PF_ODR *(unsigned char*)0x5019
+#define PF_IDR *(unsigned char*)0x501A
+#define PF_DDR *(unsigned char*)0x501B
+#define PF_CR1 *(unsigned char*)0x501C
+#define PF_CR2 *(unsigned char*)0x501D
+
+/* CLOCK */
+#define CLK_ICKR	*(unsigned char*)0x50C0
+#define CLK_ECKR	*(unsigned char*)0x50C1
+#define CLK_CMSR	*(unsigned char*)0x50C3
+#define CLK_SWR 	*(unsigned char*)0x50C4
+#define CLK_SWCR	*(unsigned char*)0x50C5
+#define CLK_CKDIVR	*(unsigned char*)0x50C6
+#define CLK_PCKENR1	*(unsigned char*)0x50C7
+#define CLK_CSSR	*(unsigned char*)0x50C8
+#define CLK_CCOR	*(unsigned char*)0x50C9
+#define CLK_PCKENR2	*(unsigned char*)0x50CA
+#define CLK_HSITRIMR	*(unsigned char*)0x50CC
+#define CLK_SWIMCCR	*(unsigned char*)0x50CD
+
+#define CLK_ICKR_HSIEN (1<<0)
+#define CLK_ICKR_HSIRDY (1<<1)
+#define CLK_ICKR_FHW (1<<2)
+#define CLK_ICKR_LSIEN (1<<3)
+#define CLK_ICKR_LSIRDY (1<<4)
+#define CLK_ICKR_REGAH (1<<5)
+
+#define CLK_CKDIVR_HSIDIV1 0
+#define CLK_CKDIVR_HSIDIV2 (1<<0)
+#define CLK_CKDIVR_HSIDIV4 (1<<1)
+#define CLK_CKDIVR_HSIDIV8 (1<<0|1<<1)
+
+#define CLK_CKDIVR_CPUDIV1		0
+#define CLK_CKDIVR_CPUDIV2		(1<<0)
+#define CLK_CKDIVR_CPUDIV4		(1<<1)
+#define CLK_CKDIVR_CPUDIV8		(1<<1|1<<0)
+#define CLK_CKDIVR_CPUDIV16		(1<<2)
+#define CLK_CKDIVR_CPUDIV32		(1<<2|1<<0)
+#define CLK_CKDIVR_CPUDIV64		(1<<2|1<<1)
+#define CLK_CKDIVR_CPUDIV128	(1<<2|1<<1|1<<0)
+
+#define CLK_SWCR_SWBSY	(1<<0)
+#define CLK_SWCR_SWEN 	(1<<1)
+#define CLK_SWCR_SWIEN 	(1<<2)
+#define CLK_SWCR_SWIF 	(1<<3)
+
+#define CLK_SWIMCCR_SWIMCLK		(1<<0)
+
+/* ------------------- USART ------------------- */
+#define USART1_SR *(unsigned char*)0x5230
+#define USART1_DR *(unsigned char*)0x5231
+#define USART1_BRR1 *(unsigned char*)0x5232
+#define USART1_BRR2 *(unsigned char*)0x5233
+#define USART1_CR1 *(unsigned char*)0x5234
+#define USART1_CR2 *(unsigned char*)0x5235
+#define USART1_CR3 *(unsigned char*)0x5236
+#define USART1_CR4 *(unsigned char*)0x5237
+#define USART1_CR5 *(unsigned char*)0x5238
+#define USART1_GTR *(unsigned char*)0x5239
+#define USART1_PSCR *(unsigned char*)0x523A
+
+/* USART_CR1 bits */
+#define USART_CR1_R8 (1 << 7)
+#define USART_CR1_T8 (1 << 6)
+#define USART_CR1_UARTD (1 << 5)
+#define USART_CR1_M (1 << 4)
+#define USART_CR1_WAKE (1 << 3)
+#define USART_CR1_PCEN (1 << 2)
+#define USART_CR1_PS (1 << 1)
+#define USART_CR1_PIEN (1 << 0)
+
+/* USART_CR2 bits */
+#define USART_CR2_TIEN (1 << 7)
+#define USART_CR2_TCIEN (1 << 6)
+#define USART_CR2_RIEN (1 << 5)
+#define USART_CR2_ILIEN (1 << 4)
+#define USART_CR2_TEN (1 << 3)
+#define USART_CR2_REN (1 << 2)
+#define USART_CR2_RWU (1 << 1)
+#define USART_CR2_SBK (1 << 0)
+
+/* USART_CR3 bits */
+#define USART_CR3_LINEN (1 << 6)
+#define USART_CR3_STOP2 (1 << 5)
+#define USART_CR3_STOP1 (1 << 4)
+#define USART_CR3_CLKEN (1 << 3)
+#define USART_CR3_CPOL (1 << 2)
+#define USART_CR3_CPHA (1 << 1)
+#define USART_CR3_LBCL (1 << 0)
+
+/* USART_SR bits */
+#define USART_SR_TXE (1 << 7)
+#define USART_SR_TC (1 << 6)
+#define USART_SR_RXNE (1 << 5)
+#define USART_SR_IDLE (1 << 4)
+#define USART_SR_OR (1 << 3)
+#define USART_SR_NF (1 << 2)
+#define USART_SR_FE (1 << 1)
+#define USART_SR_PE (1 << 0)
+
+
+/* ------------------- TIMERS ------------------- */
+#define TIM1_CR1 *(unsigned char*)0x52B0
+#define TIM1_CR2 *(unsigned char*)0x52B1
+#define TIM1_SMCR *(unsigned char*)0x52B2
+#define TIM1_ETR *(unsigned char*)0x52B3
+#define TIM1_DER *(unsigned char*)0x52B4
+#define TIM1_IER *(unsigned char*)0x52B5
+#define TIM1_SR1 *(unsigned char*)0x52B6
+#define TIM1_SR2 *(unsigned char*)0x52B7
+#define TIM1_EGR *(unsigned char*)0x52B8
+#define TIM1_CCMR1 *(unsigned char*)0x52B9
+#define TIM1_CCMR2 *(unsigned char*)0x52BA
+#define TIM1_CCMR3 *(unsigned char*)0x52BB
+#define TIM1_CCMR4 *(unsigned char*)0x52BC
+#define TIM1_CCER1 *(unsigned char*)0x52BD
+#define TIM1_CCER2 *(unsigned char*)0x52BE
+#define TIM1_CNTRH *(unsigned char*)0x52BF
+#define TIM1_CNTRL *(unsigned char*)0x52C0
+#define TIM1_PSCRH *(unsigned char*)0x52C1
+#define TIM1_PSCRL *(unsigned char*)0x52C2
+#define TIM1_ARRH *(unsigned char*)0x52C3
+#define TIM1_ARRL *(unsigned char*)0x52C4
+#define TIM1_RCR *(unsigned char*)0x52C5
+#define TIM1_CCR1H *(unsigned char*)0x52C6
+#define TIM1_CCR1L *(unsigned char*)0x52C7
+#define TIM1_CCR2H *(unsigned char*)0x52C8
+#define TIM1_CCR2L *(unsigned char*)0x52C9
+#define TIM1_CCR3H *(unsigned char*)0x52CA
+#define TIM1_CCR3L *(unsigned char*)0x52CB
+#define TIM1_CCR4H *(unsigned char*)0x52CC
+#define TIM1_CCR4L *(unsigned char*)0x52CD
+#define TIM1_BKR *(unsigned char*)0x52CE
+#define TIM1_DTR *(unsigned char*)0x52CF
+#define TIM1_OISR *(unsigned char*)0x52D0
+#define TIM1_DCR1 *(unsigned char*)0x52D1
+#define TIM1_DCR2 *(unsigned char*)0x52D2
+#define TIM1_DMA1R *(unsigned char*)0x52D3
+
+/* TIM_IER bits */
+#define TIM_IER_BIE (1 << 7)
+#define TIM_IER_TIE (1 << 6)
+#define TIM_IER_COMIE (1 << 5)
+#define TIM_IER_CC4IE (1 << 4)
+#define TIM_IER_CC3IE (1 << 3)
+#define TIM_IER_CC2IE (1 << 2)
+#define TIM_IER_CC1IE (1 << 1)
+#define TIM_IER_UIE (1 << 0)
+
+/* TIM_CR1 bits */
+#define TIM_CR1_APRE (1 << 7)
+#define TIM_CR1_CMSH (1 << 6)
+#define TIM_CR1_CMSL (1 << 5)
+#define TIM_CR1_DIR (1 << 4)
+#define TIM_CR1_OPM (1 << 3)
+#define TIM_CR1_URS (1 << 2)
+#define TIM_CR1_UDIS (1 << 1)
+#define TIM_CR1_CEN (1 << 0)
+
+/* TIM_SR1 bits */
+#define TIM_SR1_BIF (1 << 7)
+#define TIM_SR1_TIF (1 << 6)
+#define TIM_SR1_COMIF (1 << 5)
+#define TIM_SR1_CC4IF (1 << 4)
+#define TIM_SR1_CC3IF (1 << 3)
+#define TIM_SR1_CC2IF (1 << 2)
+#define TIM_SR1_CC1IF (1 << 1)
+#define TIM_SR1_UIF (1 << 0)
+
+
+
+
+#define __enable_interrupt()    {__asm__("rim\n");}  /* enable interrupts */
+#define __disable_interrupt()   {__asm__("sim\n");}  /* disable interrupts */
+#define rim()                 {__asm__("rim\n");}  /* enable interrupts */
+#define sim()                 {__asm__("sim\n");}  /* disable interrupts */
diff --git a/util.c b/util.c
new file mode 100644
index 0000000..11ffa28
--- /dev/null
+++ b/util.c
@@ -0,0 +1,117 @@
+#include "util.h"
+
+void reverse(char str[], int length)
+{
+  int start = 0;
+  int end = length - 1;
+  while(start < end)
+  {
+    char t;
+    t = *(str + start);
+    *(str + start) = *(str + end);
+    *(str + end) = t;
+    start++;
+    end--;
+  }
+}
+
+unsigned char IntToStr(uint32_t Value, char *buf)
+{
+  unsigned char i = 0;
+  if(!Value)
+  {
+    buf[0] = '0';
+    buf[1] = 0;
+    return 1;
+  }
+  while(Value)
+  {
+    buf[i] = '0' + Value % 10;
+    i++;
+    Value /= 10;
+  }
+  reverse(buf, i);
+  buf[i] = 0;
+  return i;
+}
+
+//union
+//{
+//  float f;
+//  struct
+//  {
+//    unsigned int    mantissa_lo : 16;
+//    unsigned int    mantissa_hi : 7;
+//    unsigned int     exponent : 8;
+//    unsigned int     sign : 1;
+//  };
+//} helper;
+unsigned char FloatToStr(float f, char *Buf)
+{
+  long mantissa, int_part, frac_part;
+  short exp2;
+  typedef union
+  {
+    long L;
+    float F;
+  } TLF;
+  TLF x;
+  char *p;
+  if(f == 0.0f)
+  {
+    Buf[0] = '0';
+    Buf[1] = '.';
+    Buf[2] = '0';
+    Buf[3] = 0;
+    return 0;
+  }
+  x.F = f;
+  exp2 = (unsigned char)(x.L >> 23) - 127;
+  mantissa = (x.L & 0xFFFFFF) | 0x800000;
+  frac_part = 0;
+  int_part = 0;
+  if(exp2 >= 31)
+    return _FTOA_TOO_LARGE;
+  else if(exp2 < -23)
+    return _FTOA_TOO_SMALL;
+  else if(exp2 >= 23)
+    int_part = mantissa << (exp2 - 23);
+  else if(exp2 >= 0)
+  {
+    int_part = mantissa >> (23 - exp2);
+    frac_part = (mantissa << (exp2 + 1)) & 0xFFFFFF;
+  }
+  else /* if (exp2 < 0) */
+    frac_part = (mantissa & 0xFFFFFF) >> -(exp2 + 1);
+  p = Buf;
+  if(x.L < 0)
+    *p++ = '-';
+  if(int_part == 0)
+    *p++ = '0';
+  else
+  {
+    //    ltoa(p, int_part, 10);
+    IntToStr((uint32_t)int_part, p);
+    while(*p)
+      p++;
+  }
+  *p++ = '.';
+  if(frac_part == 0)
+    *p++ = '0';
+  else
+  {
+    unsigned char m, max;
+    max = sizeof(Buf) - (unsigned char)(p - Buf) - 1;
+    if(max > 7) max = 7;
+    for(m = 0; m < max; m++)    // print BCD
+    {
+      frac_part = (frac_part << 3) + (frac_part << 1);      // frac_part *= 10;
+      *p++ = (frac_part >> 24) + '0';
+      frac_part &= 0xFFFFFF;
+    }
+    for(p--; p[0] == '0' && p[-1] != '.'; --p);     // delete ending zeroes
+    p++;
+  }
+  *p = 0;
+  return 0;
+}
diff --git a/util.h b/util.h
new file mode 100644
index 0000000..ce5a0b4
--- /dev/null
+++ b/util.h
@@ -0,0 +1,10 @@
+#include <stdint.h>
+typedef uint8_t Bool;
+#define True 1
+#define False 0
+
+//static void reverse(char str[], int length);
+unsigned char IntToStr(uint32_t Value, char *buf);
+#define _FTOA_TOO_LARGE 1
+#define _FTOA_TOO_SMALL 2
+unsigned char FloatToStr(float f, char *Buf);