From db234a6cc109f704dd2f6d2476173aba7ba7b713 Mon Sep 17 00:00:00 2001
From: Phillip Stevens <phillip.stevens@gmail.com>
Date: Tue, 14 Apr 2020 14:27:33 +1000
Subject: [PATCH] ATmegaxxxx - add megaAVR 0-Series support

---
 portable/GCC/ATmegaxxxx/port.c      | 95 +++++++----------------------
 portable/GCC/ATmegaxxxx/portmacro.h |  2 -
 2 files changed, 23 insertions(+), 74 deletions(-)

diff --git a/portable/GCC/ATmegaxxxx/port.c b/portable/GCC/ATmegaxxxx/port.c
index 47dc58d2b..d65cfedc2 100644
--- a/portable/GCC/ATmegaxxxx/port.c
+++ b/portable/GCC/ATmegaxxxx/port.c
@@ -27,6 +27,8 @@
 
 
 #include <stdlib.h>
+
+#include <avr/io.h>
 #include <avr/interrupt.h>
 #include <avr/wdt.h>
 
@@ -524,17 +526,6 @@ static void prvSetupTimerInterrupt( void );
 StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters )
 {
 uint16_t usAddress;
-
-    /* Place a few bytes of known values on the bottom of the stack.
-    This is just useful for debugging. */
-
-    *pxTopOfStack = 0x11;
-    pxTopOfStack--;
-    *pxTopOfStack = 0x22;
-    pxTopOfStack--;
-    *pxTopOfStack = 0x33;
-    pxTopOfStack--;
-
     /* Simulate how the stack would look after a call to vPortYield() generated by
     the compiler. */
 
@@ -590,51 +581,9 @@ uint16_t usAddress;
 
     /* Now the remaining registers. The compiler expects R1 to be 0. */
     *pxTopOfStack = ( StackType_t ) 0x00;    /* R1 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x02;    /* R2 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x03;    /* R3 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x04;    /* R4 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x05;    /* R5 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x06;    /* R6 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x07;    /* R7 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x08;    /* R8 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x09;    /* R9 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x10;    /* R10 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x11;    /* R11 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x12;    /* R12 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x13;    /* R13 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x14;    /* R14 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x15;    /* R15 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x16;    /* R16 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x17;    /* R17 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x18;    /* R18 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x19;    /* R19 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x20;    /* R20 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x21;    /* R21 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x22;    /* R22 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x23;    /* R23 */
-    pxTopOfStack--;
+
+    /* Leave R2 - R23 untouched */
+    pxTopOfStack -= 23;
 
     /* Place the parameter on the stack in the expected location. */
     usAddress = ( uint16_t ) pvParameters;
@@ -643,20 +592,9 @@ uint16_t usAddress;
 
     usAddress >>= 8;
     *pxTopOfStack = ( StackType_t ) ( usAddress & ( uint16_t ) 0x00ff );
-    pxTopOfStack--;
 
-    *pxTopOfStack = ( StackType_t ) 0x26;    /* R26 X */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x27;    /* R27 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x28;    /* R28 Y */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x29;    /* R29 */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x30;    /* R30 Z */
-    pxTopOfStack--;
-    *pxTopOfStack = ( StackType_t ) 0x031;   /* R31 */
-    pxTopOfStack--;
+    /* Leave register R26 - R31 untouched */
+    pxTopOfStack -= 7;
 
     return pxTopOfStack;
 }
@@ -709,6 +647,21 @@ void vPortYield( void )
 }
 /*-----------------------------------------------------------*/
 
+/*
+ * Manual context switch callable from ISRs. The first thing we do is save
+ * the registers so we can use a naked attribute.
+ */
+void vPortYieldFromISR(void) __attribute__ ( ( hot, flatten, naked ) );
+void vPortYieldFromISR(void)
+{
+    portSAVE_CONTEXT();
+    vTaskSwitchContext();
+    portRESTORE_CONTEXT();
+
+    __asm__ __volatile__ ( "reti" );
+}
+/*-----------------------------------------------------------*/
+
 /*
  * Context switch function used by the tick. This must be identical to
  * vPortYield() from the call to vTaskSwitchContext() onwards. The only
@@ -725,7 +678,7 @@ void vPortYieldFromTick( void )
     }
     portRESTORE_CONTEXT();
 
-    __asm__ __volatile__ ( "ret" );
+    __asm__ __volatile__ ( "reti" );
 }
 /*-----------------------------------------------------------*/
 
@@ -776,7 +729,6 @@ uint8_t ucLowByte;
     ucLowByte = portTIMSK;
     ucLowByte |= portCOMPARE_MATCH_A_INTERRUPT_ENABLE;
     portTIMSK = ucLowByte;
-
 }
 
 #endif
@@ -799,7 +751,6 @@ uint8_t ucLowByte;
     ISR(portSCHEDULER_ISR)
     {
         vPortYieldFromTick();
-        __asm__ __volatile__ ( "reti" );
     }
 #else
 
diff --git a/portable/GCC/ATmegaxxxx/portmacro.h b/portable/GCC/ATmegaxxxx/portmacro.h
index 9a520d509..a6f26f515 100644
--- a/portable/GCC/ATmegaxxxx/portmacro.h
+++ b/portable/GCC/ATmegaxxxx/portmacro.h
@@ -45,7 +45,6 @@ extern "C" {
 #include <avr/wdt.h>
 
 /* Type definitions. */
-
 #define portCHAR                    char
 #define portFLOAT                   float
 #define portDOUBLE                  double
@@ -152,4 +151,3 @@ extern void vPortYield( void )      __attribute__ ( ( naked ) );
 #endif
 
 #endif /* PORTMACRO_H */
-