diff --git a/src/debug.cpp b/src/debug.cpp
index 15c213c..bfaee33 100644
--- a/src/debug.cpp
+++ b/src/debug.cpp
@@ -1,21 +1,24 @@
 #include "debug.h"
+#include <intrinsics.h>
+#include <msp430g2553.h>
+#include <stdint.h>
 
 
 Debug::Debug() {
-    P1DIR |= BIT0 | BIT1 | BIT2 | BIT3;
+//    P1DIR |= BIT0 | BIT1 | BIT2 | BIT3;
 }
 
 void Debug::write(uint16_t v) {
-  P1OUT |= BIT1;
-  for (uint8_t i = 0; i < 16; i++) {
-    P1OUT |= BIT3;
-    uint16_t x = v >> i;
-    if ((x & 1) == 1) {
-      P1OUT |= BIT2;
-    } else {
-      P1OUT &= ~BIT2;
-    }
-    P1OUT &= ~BIT3;
-  }
-  P1OUT &= ~BIT1;
-}
\ No newline at end of file
+//  P1OUT |= BIT1;
+//  for (uint8_t i = 0; i < 16; i++) {
+//    P1OUT |= BIT3;
+//    uint16_t x = v >> i;
+//    if ((x & 1) == 1) {
+//      P1OUT |= BIT2;
+//    } else {
+//      P1OUT &= ~BIT2;
+//    }
+//    P1OUT &= ~BIT3;
+//  }
+//  P1OUT &= ~BIT1;
+}
diff --git a/src/debug.h b/src/debug.h
index 3e88eac..743b197 100644
--- a/src/debug.h
+++ b/src/debug.h
@@ -5,4 +5,4 @@ class Debug {
 public:
   Debug();
   void write(uint16_t v);
-}
+};
diff --git a/src/display.cpp b/src/display.cpp
index 883ceb9..ac5a2e4 100644
--- a/src/display.cpp
+++ b/src/display.cpp
@@ -1,15 +1,11 @@
-#include <Arduino.h>
-#include <Streaming.h>
 
 #include "mySpi.h"
 #include "display.h"
 
-#include "hardware.h"
-
 
 
 typedef struct display_s {
-  int16_t old_value;
+	int16_t old_value;
 } display_t;
 
 display_t displays[NUM_OF_DISP_UNITS];
@@ -17,161 +13,159 @@ display_t displays[NUM_OF_DISP_UNITS];
 uint8_t old_disable = 247; // illegal value
 
 void dispInit() {
-  pinMode(DISP_CS_PIN, OUTPUT);
 
+	spiXfer16(0x09ff); // decode mode
+	spiXfer16(0x0a01); // intensity
 
-  spiXfer16(DISP_CS_PIN, 0x09ff); // decode mode
-  spiXfer16(DISP_CS_PIN, 0x0a01); // intensity
+	spiXfer16(0x0b03); // scan limit
 
-  spiXfer16(DISP_CS_PIN, 0x0b03); // scan limit
+	spiXfer16(0x0c01); // normal op
 
-  spiXfer16(DISP_CS_PIN, 0x0c01); // normal op
+	spiXfer16(0x0f00); // led test: normal
 
-  spiXfer16(DISP_CS_PIN, 0x0f00); // led test: normal
-
-  for (uint8_t i = 0; i < NUM_OF_DISP_UNITS; i++) {
-    displays[i].old_value = 11111; // illegal value
-  }
+	for (uint8_t i = 0; i < NUM_OF_DISP_UNITS; i++) {
+		displays[i].old_value = 11111; // illegal value
+	}
 }
 
 void dispDisable(uint8_t disable) {
-  if (disable != old_disable) {
-    old_disable = disable;
-    if (disable) {
-      spiXfer16(DISP_CS_PIN, 0x0c00);
-    } else {
-      spiXfer16(DISP_CS_PIN, 0x0c01);
-    }
-  }
+	if (disable != old_disable) {
+		old_disable = disable;
+		if (disable) {
+			spiXfer16(0x0c00);
+		} else {
+			spiXfer16(0x0c01);
+		}
+	}
 }
 
 void dispSetError() {
-  spiXfer16(DISP_CS_PIN, 0x010f);
-  spiXfer16(DISP_CS_PIN, 0x020f);
-  spiXfer16(DISP_CS_PIN, 0x030f);
-  spiXfer16(DISP_CS_PIN, 0x040b);
+	spiXfer16(0x010f);
+	spiXfer16(0x020f);
+	spiXfer16(0x030f);
+	spiXfer16(0x040b);
 }
 
 #define MAX_DIGIT 4
 
 void dispSetDigit(uint8_t digit, uint8_t value) {
-  if ((digit > MAX_DIGIT) || (digit == 0) || (value > 9)) {
-    dispSetError();
-  } else {
-    uint16_t v = (((uint16_t) digit) << 8) | value;
-    spiXfer16(DISP_CS_PIN, v);
-  }
+	if ((digit > MAX_DIGIT) || (digit == 0) || (value > 9)) {
+		dispSetError();
+	} else {
+		uint16_t v = (((uint16_t) digit) << 8) | value;
+		spiXfer16(v);
+	}
 }
 
 
-void dispSetFloat(float value) {
-  if ((value < -99) || (value > 999)) {
-    dispSetError();
-  } else {
-    uint8_t neg = (value < 0) ? 1 : 0;
-    value = abs(value);
-    uint16_t preComma = (uint16_t)value;
-    uint16_t postComma = round((value - (float)preComma) * 10);
-    if (postComma == 10) {
-      postComma = 0;
-      preComma += 1;
-    }
-    if ((preComma == 0) && (postComma == 0)) {
-      neg = false;
-    }
-
-    uint8_t digit1 = preComma / 100;
-    uint8_t remVal = preComma - (digit1 * 100);
-    uint8_t digit2 = remVal / 10;
-    remVal = remVal - (digit2 * 10);
-    uint8_t digit3 = remVal;
-
-    if (digit1 == 0) {
-      if (neg) {
-        digit1 = 0x0a;
-      } else {
-        digit1 = 0x0f;
-      }
-    }
-
-    if (digit1 > 0x09 && digit2 == 0) {
-      if (neg) {
-        digit2 = 0x0a;
-        digit1 = 0x0f;
-      } else {
-        digit2 = 0x0f;
-      }
-    }
-
-    digit3 |= 0x80;
-
-    spiXfer16(DISP_CS_PIN, 0x0100 | digit1);
-    spiXfer16(DISP_CS_PIN, 0x0200 | digit2);
-    spiXfer16(DISP_CS_PIN, 0x0300 | digit3);
-
-    spiXfer16(DISP_CS_PIN, 0x0400 | postComma);
-
-
-  }
+void dispSetFloat(uint8_t unit, float value) {
+	//  if ((value < -99) || (value > 999)) {
+	//    dispSetError();
+	//  } else {
+	//    uint8_t neg = (value < 0) ? 1 : 0;
+	//    value = abs(value);
+	//    uint16_t preComma = (uint16_t)value;
+	//    uint16_t postComma = round((value - (float)preComma) * 10);
+	//    if (postComma == 10) {
+	//      postComma = 0;
+	//      preComma += 1;
+	//    }
+	//    if ((preComma == 0) && (postComma == 0)) {
+	//      neg = false;
+	//    }
+	//
+	//    uint8_t digit1 = preComma / 100;
+	//    uint8_t remVal = preComma - (digit1 * 100);
+	//    uint8_t digit2 = remVal / 10;
+	//    remVal = remVal - (digit2 * 10);
+	//    uint8_t digit3 = remVal;
+	//
+	//    if (digit1 == 0) {
+	//      if (neg) {
+	//        digit1 = 0x0a;
+	//      } else {
+	//        digit1 = 0x0f;
+	//      }
+	//    }
+	//
+	//    if (digit1 > 0x09 && digit2 == 0) {
+	//      if (neg) {
+	//        digit2 = 0x0a;
+	//        digit1 = 0x0f;
+	//      } else {
+	//        digit2 = 0x0f;
+	//      }
+	//    }
+	//
+	//    digit3 |= 0x80;
+	//
+	//    spiXfer16(0x0100 | digit1);
+	//    spiXfer16(0x0200 | digit2);
+	//    spiXfer16(0x0300 | digit3);
+	//
+	//    spiXfer16(0x0400 | postComma);
+	//
+	//
+	//  }
 }
 
 void dispSet(uint8_t unit, int16_t value) {
-  if (displays[unit].old_value != value) {
-    displays[unit].old_value = value;
+	if (displays[unit].old_value != value) {
+		displays[unit].old_value = value;
 
-    if ((value < -999) || (value > 9999)) {
-      dispSetError();
-    } else {
-      uint8_t neg = (value < 0) ? 1 : 0;
-      value = abs(value);
+		if ((value < -999) || (value > 9999)) {
+			dispSetError();
+		} else {
+			uint8_t neg = (value < 0) ? 1 : 0;
+			// value = abs(value);
 
-      uint8_t digit0 = value / 1000;
-      uint16_t remVal = value - (digit0 * 1000);
-      uint8_t digit1 = remVal / 100;
-      remVal = remVal - (digit1 * 100);
-      uint8_t digit2 = remVal / 10;
-      remVal = remVal - (digit2 * 10);
-      uint8_t digit3 = remVal;
+			uint8_t digit0 = value / 1000;
+			uint16_t remVal = value - (digit0 * 1000);
+			uint8_t digit1 = remVal / 100;
+			remVal = remVal - (digit1 * 100);
+			uint8_t digit2 = remVal / 10;
+			remVal = remVal - (digit2 * 10);
+			uint8_t digit3 = remVal;
 
-      if (digit0 == 0) {
-        if (neg) {
-          digit0 = 0x0a;
-        } else {
-          digit0 = 0x0f;
-        }
-      }
+			if (digit0 == 0) {
+				if (neg) {
+					digit0 = 0x0a;
+				} else {
+					digit0 = 0x0f;
+				}
+			}
 
-      if (digit0 > 0x09 && digit1 == 0) {
-        if (neg) {
-          digit1 = 0x0a;
-          digit0 = 0x0f;
-        } else {
-          digit1 = 0x0f;
-        }
-      }
+			if (digit0 > 0x09 && digit1 == 0) {
+				if (neg) {
+					digit1 = 0x0a;
+					digit0 = 0x0f;
+				} else {
+					digit1 = 0x0f;
+				}
+			}
 
-      if (digit0 > 0x09 && digit1 > 0x09 && digit2 == 0) {
-        if (neg) {
-          digit2 = 0x0a;
-          digit1 = 0x0f;
-        } else {
-          digit2 = 0x0f;
-        }
-      }
+			if (digit0 > 0x09 && digit1 > 0x09 && digit2 == 0) {
+				if (neg) {
+					digit2 = 0x0a;
+					digit1 = 0x0f;
+				} else {
+					digit2 = 0x0f;
+				}
+			}
 
-      if (unit == 0) {
-        spiXfer16(DISP_CS_PIN, 0x0100 | digit0);
-        spiXfer16(DISP_CS_PIN, 0x0200 | digit1);
-        spiXfer16(DISP_CS_PIN, 0x0300 | digit2);
-        spiXfer16(DISP_CS_PIN, 0x0400 | digit3);
-      } else {
-        spiXfer16(DISP_CS_PIN, 0x0500 | digit0);
-        spiXfer16(DISP_CS_PIN, 0x0600 | digit1);
-        spiXfer16(DISP_CS_PIN, 0x0700 | digit2);
-        spiXfer16(DISP_CS_PIN, 0x0800 | digit3);
-      }
-    }
-  }
+			if (unit == 0) {
+				spiXfer16(0x0100 | digit0);
+				spiXfer16(0x0200 | digit1);
+				spiXfer16(0x0300 | digit2);
+				spiXfer16(0x0400 | digit3);
+			} else {
+				spiXfer16(0x0500 | digit0);
+				spiXfer16(0x0600 | digit1);
+				spiXfer16(0x0700 | digit2);
+				spiXfer16(0x0800 | digit3);
+			}
+		}
+	}
 }
 
 
diff --git a/src/display.h b/src/display.h
index d4001c0..63aac9b 100644
--- a/src/display.h
+++ b/src/display.h
@@ -4,7 +4,7 @@
 #define DISPLAY_H_
 
 
-#define NUM_OF_DISP_UNITS 1
+#define NUM_OF_DISP_UNITS 2
 
 
 
@@ -13,7 +13,7 @@ void dispSetError();
 void dispSetDigit(uint8_t digit, uint8_t value);
 
 void dispSet(uint8_t unit, int16_t value);
-void dispSetFloat(float value);
+void dispSetFloat(uint8_t unit, float value);
 
 void dispDisable(uint8_t disable);
 
diff --git a/src/main.cpp b/src/main.cpp
index 974738a..a982ca4 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -11,31 +11,45 @@
 
 #include "pwm.h"
 #include "hmi.h"
+#include "mySpi.h"
+#include "display.h"
 
 
 
 
 void init() {
-  // disable watchdog
-  WDTCTL = WDTPW | WDTHOLD;
+	// disable watchdog
+	WDTCTL = WDTPW | WDTHOLD;
 
-  // highest possible system clock
-  DCOCTL = DCO0 | DCO1 | DCO2;
-  BCSCTL1 = XT2OFF | RSEL0 | RSEL1 | RSEL2 | RSEL3;
-  BCSCTL2 = 0;
-  BCSCTL3 = 0;
+	// highest possible system clock
+	DCOCTL = DCO0 | DCO1 | DCO2;
+	BCSCTL1 = XT2OFF | RSEL0 | RSEL1 | RSEL2 | RSEL3;
+	BCSCTL2 = 0;
+	BCSCTL3 = 0;
 }
 
 int main() {
-  init();
-  pwmInit();
-  hmiInit();
+	init();
+	//pwmInit();
+	//hmiInit();
+	spiInit();
+	dispInit();
 
-  __enable_interrupt();
 
-  while (1) {
-    hmiExec();
-  }
+
+	__enable_interrupt();
+
+	uint32_t c = 0;
+	uint16_t d = 0;
+	while (1) {
+		// hmiExec();
+		c++;
+		if (c > 1000000) {
+			c = 0;
+			d++;
+			dispSet(0, d);
+		}
+	}
 }
 
 
diff --git a/src/mySpi.cpp b/src/mySpi.cpp
index e1a1cce..d67a939 100644
--- a/src/mySpi.cpp
+++ b/src/mySpi.cpp
@@ -1,40 +1,58 @@
 #include "mySpi.h"
-#include <SPI.h>
+#include <intrinsics.h>
+#include <msp430g2553.h>
+#include <stdint.h>
+
+
+const uint8_t SPI_CS = BIT1;
+const uint8_t SPI_OUT = BIT0;
+const uint8_t SPI_CLK = BIT2;
 
 void spiInit() {
-  SPI.begin();
-
-  SPI.setBitOrder(MSBFIRST);
-  SPI.setClockDivider(SPI_CLOCK_DIV4);
-  SPI.setDataMode(SPI_MODE0);
+	P1DIR |= SPI_OUT | SPI_CS | SPI_CLK;
+	P1OUT |= SPI_CS;
+	P1OUT &= ~SPI_CLK;
 }
 
-static void _spiSetCS(uint8_t cs) {
-  digitalWrite(cs, LOW);
+void spiSetCS() {
+	P1OUT &= ~SPI_CS;
 }
 
-static void _spiReleaseCS(uint8_t cs) {
-  digitalWrite(cs, HIGH);
+void spiReleaseCS() {
+	P1OUT |= SPI_CS;
+	P1OUT &= ~SPI_CLK;
 }
 
-static uint8_t __spiXfer(uint8_t o) {
-  return SPI.transfer(o);
+uint8_t spiXfer(uint8_t o) {
+	for (int8_t i = 7; i >= 0; i--) {
+		P1OUT &= ~SPI_CLK;
+		uint8_t v = (o >> i) & 1;
+		if (v == 1) {
+			P1OUT |= SPI_OUT;
+		} else {
+			P1OUT &= ~SPI_OUT;
+		}
+		P1OUT |= SPI_CLK;
+	}
+
+	return 0;
 }
 
-uint8_t spiXfer16(uint8_t cs, uint16_t o) {
-  _spiSetCS(cs);
-  __spiXfer((o >> 8) & 0x0ff);
-  uint8_t res = __spiXfer(o & 0x0ff);
-  _spiReleaseCS(cs);
+uint8_t spiXfer16(uint16_t o) {
+  spiSetCS();
+  spiXfer((o >> 8) & 0x0ff);
+  uint8_t res = 0;
+  res = spiXfer(o & 0x0ff);
+  spiReleaseCS();
   return res;
 }
 
-uint16_t spiXferD16(uint8_t cs, uint16_t o) {
-  _spiSetCS(cs);
-  uint16_t res = __spiXfer((o >> 8) & 0x0ff);
+uint16_t spiXferD16(uint16_t o) {
+  spiSetCS();
+  uint16_t res = spiXfer((o >> 8) & 0x0ff);
   res <<= 8;
-  res |= __spiXfer(o && 0x0ff);
-  _spiReleaseCS(cs);
+  res |= spiXfer(o && 0x0ff);
+  spiReleaseCS();
   return res;
 }
 
diff --git a/src/mySpi.h b/src/mySpi.h
index 6635df3..470d37c 100644
--- a/src/mySpi.h
+++ b/src/mySpi.h
@@ -12,7 +12,11 @@
 
 
 void spiInit();
-uint8_t spiXfer16(uint8_t cs, uint16_t o);
-uint16_t spiXferD16(uint8_t cs, uint16_t o);
+uint8_t spiXfer16(uint16_t o);
+uint16_t spiXferD16(uint16_t o);
+
+void spiSetCS();
+void spiReleaseCS();
+uint8_t spiXfer(uint8_t o);
 
 #endif /* MY_SPI_H_ */