From 8e02955c3dce4b512b83ef2fda989577fb8abfad Mon Sep 17 00:00:00 2001
From: Cristiano De Alti <cristiano_dealti@hotmail.com>
Date: Wed, 3 Dec 2014 00:12:43 +0100
Subject: [PATCH] Fix menu. How could it worked before? Also added a macro to
 disable JTAG using inline assembly

---
 platform/avr-ravenlcd/menu.c      | 38 +++++++++++++++++++++----------
 platform/avr-ravenlcd/raven3290.c |  8 +++----
 2 files changed, 30 insertions(+), 16 deletions(-)

diff --git a/platform/avr-ravenlcd/menu.c b/platform/avr-ravenlcd/menu.c
index a5cda40a9..3069d7825 100644
--- a/platform/avr-ravenlcd/menu.c
+++ b/platform/avr-ravenlcd/menu.c
@@ -66,6 +66,27 @@ bool auto_temp=true;
 
 /*---------------------------------------------------------------------------*/
 
+/**
+ *   \brief This will reliably set or clear the JTD bit of the MCUCR register.
+ *
+ *   \param x True to set the JTD bit disabling JTAG.
+*/
+#define jtd_set(x)\
+{\
+    __asm__ __volatile__ (\
+    "in __tmp_reg__,__SREG__" "\n\t"\
+    "cli" "\n\t"\
+    "out %1, %0" "\n\t"\
+    "out __SREG__, __tmp_reg__" "\n\t"\
+    "out %1, %0" "\n\t"\
+    : /* no outputs */\
+    : "r" ((uint8_t)(x ? (1<<JTD) : 0)),\
+    "M" (_SFR_IO_ADDR(MCUCR))\
+    : "r0");\
+} 
+
+/*---------------------------------------------------------------------------*/
+
 /**
  *   \brief This function will convert decimal to ascii.
  *
@@ -322,25 +343,18 @@ menu_stop_ping(void)
 void
 menu_debug_mode(uint8_t *val)
 {
-    uint8_t sreg = SREG;
-    cli();
-    if(*val){
-        /* Disable - Could use inline ASM to meet timing requirements. */
-        MCUCR |= (1 << JTD);
-        MCUCR |= (1 << JTD);
-        /* Needed for timing critical JTD disable. */
+    if(val){
+	jtd_set(true);
         temp_init();
         /* Store setting in EEPROM. */
         eeprom_write_byte(EEPROM_DEBUG_ADDR, 0xFF);
     }
     else{
-        /* Enable - Could use inline ASM to meet timing requirements. */
-        MCUCR &= ~(1 << JTD);
-        MCUCR &= ~(1 << JTD);
+	jtd_set(false);
         /* Store setting in EEPROM. */
         eeprom_write_byte(EEPROM_DEBUG_ADDR, 0x01);
     }
-    SREG = sreg;
+    //SREG = sreg;
 }
 
 /*---------------------------------------------------------------------------*/
@@ -353,7 +367,7 @@ menu_debug_mode(uint8_t *val)
 void
 menu_read_temp(uint8_t *val)
 {
-    if(*val){
+    if(val){
         temp_mode = TEMP_UNIT_CELCIUS;
     }
     else{
diff --git a/platform/avr-ravenlcd/raven3290.c b/platform/avr-ravenlcd/raven3290.c
index d63ebeda9..dc2e9e04a 100644
--- a/platform/avr-ravenlcd/raven3290.c
+++ b/platform/avr-ravenlcd/raven3290.c
@@ -225,14 +225,14 @@ const PROGMEM tmenu_item menu_items[18]  = {
     {menu_text3,   2,  2,  2,  2, 0,                       0                  },
     {menu_text4,   0,  5,  2, 11, 0,                       0                  },
     {menu_text5,   4,  6,  8,  8, 0,                       0                  },
-    {menu_text6,   5,  5,  7,  7, (uint8_t*)1,             menu_read_temp     },
-    {menu_text7,   5,  5,  6,  6, (uint8_t*)0,             menu_read_temp     },
+    {menu_text6,   5,  5,  7,  7, (uint8_t*)0,             menu_read_temp     },
+    {menu_text7,   5,  5,  6,  6, (uint8_t*)1,             menu_read_temp     },
     {menu_text8,   4,  9,  5,  5, 0,                       0                  },
     {menu_text9,   8, 14, 10, 10, (uint8_t*)0,             menu_prepare_temp  },
     {menu_text10,  8, 15,  9,  9, (uint8_t*)1,             menu_prepare_temp  },
     {menu_text11,  0, 12,  4, 16, 0,                       0                  },
-    {menu_text12, 11, 11, 13, 13, (uint8_t*)1,             menu_debug_mode    },
-    {menu_text13, 11, 11, 12, 12, (uint8_t*)0,             menu_debug_mode    },
+    {menu_text12, 11, 11, 13, 13, (uint8_t*)0,             menu_debug_mode    },
+    {menu_text13, 11, 11, 12, 12, (uint8_t*)1,             menu_debug_mode    },
     {menu_text14, 9,  14, 14, 14, 0,                       0                  },
     {menu_text15, 10, 15, 15, 15, 0,                       0                  },
  //   {menu_text16,  0, 16, 11, 17, (uint8_t*)&menu_text16,  menu_run_sleep     },