From c092db8909f92e3bcc4a8732eb28eebcf07308f1 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Sat, 2 Mar 2019 14:28:06 +0000
Subject: [PATCH] - introduced enterCritical, exitCritial

git-svn-id: http://moon:8086/svn/projects/HendiControl@36 fda53097-d464-4ada-af97-ba876c37ca34
---
 Control/firmware/uart_echo/fifo.c  | 76 ++++++++++++++++++------------
 Control/firmware/uart_echo/fifo.h  | 20 ++++++++
 Control/firmware/uart_echo/main.c  | 10 ++--
 Control/firmware/uart_echo/timer.c |  4 +-
 Control/firmware/uart_echo/uart.c  | 20 ++++++--
 5 files changed, 89 insertions(+), 41 deletions(-)

diff --git a/Control/firmware/uart_echo/fifo.c b/Control/firmware/uart_echo/fifo.c
index 389389d..c5deae1 100644
--- a/Control/firmware/uart_echo/fifo.c
+++ b/Control/firmware/uart_echo/fifo.c
@@ -37,44 +37,62 @@ void fifo_free(Fifo *pObj)
 
 int fifo_push(Fifo *pObj, void *pItem)
 {
-	if (pObj->fill >= pObj->capacity)
+	int success = 0;
+	enterCritical();
+	do 
 	{
-		return 0;
-	}
-	for (size_t i=0; i < pObj->itemSize; i++)
-	{
-		((uint8_t*)pObj->ppData[pObj->wi])[i] = ((uint8_t*)pItem)[i];
-	}
-	pObj->wi++;
-	pObj->fill++;
-	if (pObj->wi == pObj->capacity)
-	{
-		pObj->wi = 0;
-	}
-	return 1;
+		if (pObj->fill >= pObj->capacity)
+		{
+			break;
+		}
+		for (size_t i=0; i < pObj->itemSize; i++)
+		{
+			((uint8_t*)pObj->ppData[pObj->wi])[i] = ((uint8_t*)pItem)[i];
+		}
+		pObj->wi++;
+		pObj->fill++;
+		if (pObj->wi == pObj->capacity)
+		{
+			pObj->wi = 0;
+		}
+		success = 1;
+	} while (0);
+	exitCritial();
+	return success;
 }
 
 int fifo_pop(Fifo *pObj, void *pItem)
 {
-	if (!pObj->fill)
+	int success = 0;
+	enterCritical();
+	do
 	{
-		return 0;
-	}
-	for (size_t i=0; i < pObj->itemSize; i++)
-	{
-		((uint8_t*)pItem)[i] = ((uint8_t*)pObj->ppData[pObj->ri])[i];
-	}
-	pObj->ri++;
-	pObj->fill--;
-	if (pObj->ri == pObj->capacity)
-	{
-		pObj->ri = 0;
-	}
-	return 1;
+		if (!pObj->fill)
+		{
+			break;
+		}
+		for (size_t i=0; i < pObj->itemSize; i++)
+		{
+			((uint8_t*)pItem)[i] = ((uint8_t*)pObj->ppData[pObj->ri])[i];
+		}
+		pObj->ri++;
+		pObj->fill--;
+		if (pObj->ri == pObj->capacity)
+		{
+			pObj->ri = 0;
+		}
+		success = 1;
+	} while(0);
+	exitCritial();
+	return success;
 }
 
 int fifo_isEmpty(Fifo *pObj)
 {
-	return pObj->fill == 0;
+	enterCritical();
+	int isEmpty = (pObj->fill == 0);
+	exitCritial();
+
+	return isEmpty;
 }
 
diff --git a/Control/firmware/uart_echo/fifo.h b/Control/firmware/uart_echo/fifo.h
index 664c287..802463e 100644
--- a/Control/firmware/uart_echo/fifo.h
+++ b/Control/firmware/uart_echo/fifo.h
@@ -10,6 +10,7 @@
 #define FIFO_H_
 
 #include <stdlib.h>
+#include <avr/interrupt.h>
 
 typedef struct _sFifo
 {
@@ -28,4 +29,23 @@ 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;
+
+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 999a3ed..e485167 100755
--- a/Control/firmware/uart_echo/main.c
+++ b/Control/firmware/uart_echo/main.c
@@ -123,10 +123,7 @@ int main(void)
 		Msg_t msg;
 		if (!fifo_isEmpty(&messageFifo))
 		{
-			cli();
 			fifo_pop(&messageFifo, &msg);
-			sei();
-			
 			switch(msg.code)
 			{
 				case Uart:
@@ -135,6 +132,9 @@ int main(void)
 					cmdStr[cmdStrSize] = 0;
 					char c = msg.data;
 						
+					putchar(c);
+					break;
+
 					// Command finished with CR/LF
 					if (c == 0x0A || c == 0x0D)
 					{
@@ -147,7 +147,7 @@ int main(void)
 						if (state == StateNormal || state == StateRemote)
 						{
 							// Power Control
-							if (toupper(cmdStr[0]) == 'P')
+							if (toupper(cmdStr[0]) == 'Z')
 							{
 								uint16_t arg = (uint16_t)strtol(cmdStr+1, (char **)NULL, 10);
 								setPower(arg);
@@ -170,7 +170,7 @@ int main(void)
 					{
 						if (cmdStrSize < (sizeof(cmdStr)-1))
 						{
-							printf("%c", c);
+							putchar(c);
 							cmdStr[cmdStrSize++] = c;
 						}
 					}
diff --git a/Control/firmware/uart_echo/timer.c b/Control/firmware/uart_echo/timer.c
index 27cdce8..0a2985b 100644
--- a/Control/firmware/uart_echo/timer.c
+++ b/Control/firmware/uart_echo/timer.c
@@ -51,9 +51,9 @@ void timer_init(Fifo *pFifo, uint16_t TIMER_RELOAD, TimerClockSel clockSel)
 
 void timer_start(TimerId timerId, uint16_t count)
 {
-	cli();
+	enterCritical();
 	user_timer[timerId].count = count;
 	user_timer[timerId].isRunning = 1;
-	sei();
+	exitCritial();
 }
 
diff --git a/Control/firmware/uart_echo/uart.c b/Control/firmware/uart_echo/uart.c
index cb738a1..8d7983b 100644
--- a/Control/firmware/uart_echo/uart.c
+++ b/Control/firmware/uart_echo/uart.c
@@ -15,9 +15,19 @@ static Fifo *g_pFifo = NULL;
 
 ISR(USART_RX_vect)
 {
-	uint8_t rx_data = UDR0;
-	Msg_t msg = {Uart, rx_data};
-	fifo_push(g_pFifo, &msg);
+	uint8_t status = UCSR0A;
+	
+	if (status & (1 << RXC0))
+	{
+		uint8_t rx_data = UDR0;
+		Msg_t msg = {Uart, rx_data};
+		fifo_push(g_pFifo, &msg);
+	}
+	else
+	{
+		Msg_t msg = {NOP, 0};
+		fifo_push(g_pFifo, &msg);
+	}
 }
 
 void uart_init(Fifo *pFifo, uint16_t BAUD_PRESCALE)
@@ -38,12 +48,12 @@ void uart_init(Fifo *pFifo, uint16_t BAUD_PRESCALE)
 
 void uart_putc(char c)
 {
-	cli();
+	enterCritical();
 	while((UCSR0A & (1<<UDRE0)) == 0)
 	{
 	}
 	UDR0 = c;
-	sei();
+	exitCritial();
 }
 
 int uart_putchar(char c, FILE *stream)