From 0998876ef691666e5a7fb59b3f32484fb89e1558 Mon Sep 17 00:00:00 2001
From: Dimitry Kloper <dikloper@cisco.com>
Date: Sat, 26 Dec 2015 21:55:40 +0200
Subject: [PATCH] AVR: Fix interrupt bombing during a context switch

TCB_RESTORE macro has a problem when restoring Status Register
and returning from the function (in up_fullcontextrestore()) as non-atomic action.

If there is some frequently occurring interrupt, chances are that we will
enter the interrupt handler just before ret is called.
The handler may cause a context switch which, when unrolled, will
execute up_fullcontextrestore() function that employs TCB_RESTORE.
It will be interrupted again just before return, leaving part of context
switch content un-popped again, etc... Thus, chances are that the stack will
eventually blow.

Note that this is not some edge condition fix. This bug was discovered when
testing AVR with UART configured to work on 115200 baud rate.
---
 arch/avr/src/avr/excptmacros.h      | 32 ++++++++++++++++++++++++++++-
 arch/avr/src/avr/up_switchcontext.S |  5 +----
 2 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/arch/avr/src/avr/excptmacros.h b/arch/avr/src/avr/excptmacros.h
index be19551c7f..1abc14273d 100644
--- a/arch/avr/src/avr/excptmacros.h
+++ b/arch/avr/src/avr/excptmacros.h
@@ -514,9 +514,22 @@
 
 	ld		r0, x+
 
-	/* Restore the status register (probably re-enabling interrupts) */
+        /* The following control flow split is required to eliminate non-atomic
+           interrupt_enable - return sequence.
+        
+           NOTE: since actual returning is handled by this macro it has been removed
+                 from up_fullcontextrestore function (up_switchcontext.S) 
+        */
 
+        /* if interrupts shall be enabled go to 'restore remaining and reti' code
+           otherwise just do 'restore remaining and ret' */
+        
 	ld		r24, x+
+	bst		r24, SREG_I
+	brts		go_reti
+
+	/* Restore the status register, interrupts are disabled */
+	
 	out		_SFR_IO_ADDR(SREG), r24
 
 	/* Restore r24-r25 - The temporary and IRQ number registers */
@@ -531,6 +544,23 @@
 
 	pop		r27					/* R27 then R26 */
 	pop		r26
+	ret
+
+go_reti:
+        /* restore the Status Register with interrupts disabled
+           and exit with reti (that will set the Interrupt Enable) */
+        
+	andi		r24, ~(1 << SREG_I)	
+	out		_SFR_IO_ADDR(SREG), r24
+
+	ld		r25, x+
+	ld		r24, x+
+
+	pop		r27 
+	pop		r26
+
+	reti
+	
 	.endm
 
 /********************************************************************************************
diff --git a/arch/avr/src/avr/up_switchcontext.S b/arch/avr/src/avr/up_switchcontext.S
index 645d5e17a9..da5f45e617 100755
--- a/arch/avr/src/avr/up_switchcontext.S
+++ b/arch/avr/src/avr/up_switchcontext.S
@@ -130,11 +130,8 @@ up_fullcontextrestore:
 
 	TCB_RESTORE
 
-	/* And "return" to the new task (using ret not reti so that the interrupt
-	 * state that we just restored will be preserved).
-	 */
+    /* Retruning from the function is handled in TCB_RESTORE */
 
-	ret
 	.endfunc
 	.end