diff --git a/Control/firmware/uart_echo/fifo.c b/Control/firmware/uart_echo/fifo.c
index c5deae1..96819d0 100644
--- a/Control/firmware/uart_echo/fifo.c
+++ b/Control/firmware/uart_echo/fifo.c
@@ -8,6 +8,26 @@
 #include <stdint.h>
 #include "fifo.h"
 
+static volatile size_t criticalCount = 0;
+
+void enterCritical()
+{
+	cli();
+	criticalCount++;
+}
+
+void exitCritial()
+{
+	if (criticalCount)
+	{
+		criticalCount--;
+		if (!criticalCount)
+		{
+			sei();
+		}
+	}
+}
+
 void fifo_init(Fifo *pObj, size_t capacity, size_t itemSize, const char *pName)
 {
 	pObj->wi = 0;
@@ -15,11 +35,11 @@ void fifo_init(Fifo *pObj, size_t capacity, size_t itemSize, const char *pName)
 	pObj->fill = 0;
 	pObj->capacity = capacity;
 	pObj->itemSize = itemSize;
-	pObj->ppData = (void**)malloc(capacity);
+	pObj->ppData = (uint8_t**)malloc(capacity);
 	
 	for (int i=0; i < capacity; i++)
 	{
-		pObj->ppData[i] = (void*)malloc(itemSize);
+		pObj->ppData[i] = (uint8_t*)malloc(itemSize);
 	}
 	pObj->pName = pName;
 }
@@ -47,7 +67,7 @@ int fifo_push(Fifo *pObj, void *pItem)
 		}
 		for (size_t i=0; i < pObj->itemSize; i++)
 		{
-			((uint8_t*)pObj->ppData[pObj->wi])[i] = ((uint8_t*)pItem)[i];
+			pObj->ppData[pObj->wi][i] = ((uint8_t*)pItem)[i];
 		}
 		pObj->wi++;
 		pObj->fill++;
@@ -73,7 +93,7 @@ int fifo_pop(Fifo *pObj, void *pItem)
 		}
 		for (size_t i=0; i < pObj->itemSize; i++)
 		{
-			((uint8_t*)pItem)[i] = ((uint8_t*)pObj->ppData[pObj->ri])[i];
+			((uint8_t*)pItem)[i] = pObj->ppData[pObj->ri][i];
 		}
 		pObj->ri++;
 		pObj->fill--;
diff --git a/Control/firmware/uart_echo/fifo.h b/Control/firmware/uart_echo/fifo.h
index 802463e..2197833 100644
--- a/Control/firmware/uart_echo/fifo.h
+++ b/Control/firmware/uart_echo/fifo.h
@@ -14,7 +14,7 @@
 
 typedef struct _sFifo
 {
-	void **ppData;
+	uint8_t **ppData;
 	size_t capacity;
 	size_t itemSize;
 	size_t volatile fill;
@@ -29,23 +29,7 @@ int fifo_push(Fifo *pObj, void *pItem);
 int fifo_pop(Fifo *pObj, void *pItem);
 int fifo_isEmpty(Fifo *pObj);
 
-static volatile size_t criticalCount = 0;
+void enterCritical();
+void exitCritial();
 
-static inline void enterCritical()
-{
-	cli();
-	criticalCount++;
-}
-
-static inline void exitCritial()
-{
-	if (criticalCount)
-	{
-		criticalCount--;
-		if (!criticalCount)
-		{
-			sei();
-		}
-	}
-}
 #endif /* FIFO_H_ */
\ No newline at end of file
diff --git a/Control/firmware/uart_echo/main.c b/Control/firmware/uart_echo/main.c
index ebd5f73..fa5898b 100755
--- a/Control/firmware/uart_echo/main.c
+++ b/Control/firmware/uart_echo/main.c
@@ -39,11 +39,6 @@
 
 #define TIMER_SW_DELAY_MS(dly)	((dly*TIMER_IRQ_CLOCK)/1000UL)
 
-static uint8_t ledState = 0;
-static int16_t adc_last = 0;
-static int sw_in_last = 0;
-static Fifo messageFifo;
-
 FILE uart_file = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);
 
 typedef enum _eSwitch
@@ -83,6 +78,12 @@ typedef enum _eState_t
 
 int main(void)
 {
+	uint8_t ledState = 0;
+	int16_t adc_last = 0;
+	int sw_in_last = 0;
+	size_t cmdStrSize = 0;
+	Fifo messageFifo;
+	
 	cli();
     wdt_reset();
     wdt_disable();
@@ -109,7 +110,6 @@ int main(void)
 	i2c_send(0x60, I2C_WRITE, dac_cmd, res);
 
 	char cmdStr[16];
-	size_t cmdStrSize = 0;
 	PRINT_PROMPT;
 	
 	// Start Timer