4 files changed, 57 insertions, 42 deletions

diff --git a/tmk_core/protocol/arm_atsam/i2c_master.c b/tmk_core/protocol/arm_atsam/i2c_master.c
index 4f5a79e89..ece9ee5db 100644
--- a/tmk_core/protocol/arm_atsam/i2c_master.c
+++ b/tmk_core/protocol/arm_atsam/i2c_master.c
@@ -261,8 +261,9 @@ uint8_t I2C3733_Init_Control(void)
 {
     DBGC(DC_I2C3733_INIT_CONTROL_BEGIN);
 
-    srdata.bit.SDB_N = 1;
-    SPI_WriteSRData();
+    //Hardware state shutdown on boot
+    //USB state machine will enable driver when communication is ready
+    I2C3733_Control_Set(0);
 
     CLK_delay_ms(1);
 
diff --git a/tmk_core/protocol/arm_atsam/main_arm_atsam.c b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
index d3dc272ee..13034a05d 100644
--- a/tmk_core/protocol/arm_atsam/main_arm_atsam.c
+++ b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
@@ -31,6 +31,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //From keyboard's directory
 #include "config_led.h"
 
+uint8_t g_usb_state = USB_FSMSTATUS_FSMSTATE_OFF_Val;   //Saved USB state from hardware value to detect changes
+
 void main_subtasks(void);
 uint8_t keyboard_leds(void);
 void send_keyboard(report_keyboard_t *report);
@@ -62,12 +64,6 @@ void send_keyboard(report_keyboard_t *report)
 {
     uint32_t irqflags;
 
-    if (usb_state == USB_STATE_POWERDOWN)
-    {
-        udc_remotewakeup();
-        return;
-    }
-
 #ifdef NKRO_ENABLE
     if (!keymap_config.nkro)
     {
@@ -161,41 +157,56 @@ void send_consumer(uint16_t data)
 #endif //EXTRAKEY_ENABLE
 }
 
-uint8_t g_drvid;
-uint8_t g_usb_sleeping = 0;
-
 void main_subtask_usb_state(void)
 {
-    if (usb_state == USB_STATE_POWERDOWN)
+    static uint32_t fsmstate_on_delay = 0;                          //Delay timer to be sure USB is actually operating before bringing up hardware
+    uint8_t fsmstate_now = USB->DEVICE.FSMSTATUS.reg;               //Current state from hardware register
+
+    if (fsmstate_now == USB_FSMSTATUS_FSMSTATE_SUSPEND_Val)         //If USB SUSPENDED
     {
-        if (!g_usb_sleeping)
+        fsmstate_on_delay = 0;                                      //Clear ON delay timer
+
+        if (g_usb_state != USB_FSMSTATUS_FSMSTATE_SUSPEND_Val)      //If previously not SUSPENDED
         {
-            g_usb_sleeping = 1;
-            if (led_enabled)
-            {
-                for (g_drvid = 0; g_drvid < ISSI3733_DRIVER_COUNT; g_drvid++)
-                {
-                    I2C3733_Control_Set(0);
-                }
-            }
+            suspend_power_down();                                   //Run suspend routine
+            g_usb_state = fsmstate_now;                             //Save current USB state
         }
     }
-    else if (g_usb_sleeping)
+    else if (fsmstate_now == USB_FSMSTATUS_FSMSTATE_SLEEP_Val)      //Else if USB SLEEPING
     {
-        g_usb_sleeping = 0;
-        if (led_enabled)
+        fsmstate_on_delay = 0;                                      //Clear ON delay timer
+
+        if (g_usb_state != USB_FSMSTATUS_FSMSTATE_SLEEP_Val)        //If previously not SLEEPING
         {
-            for (g_drvid = 0; g_drvid < ISSI3733_DRIVER_COUNT; g_drvid++)
+            suspend_power_down();                                   //Run suspend routine
+            g_usb_state = fsmstate_now;                             //Save current USB state
+        }
+    }
+    else if (fsmstate_now == USB_FSMSTATUS_FSMSTATE_ON_Val)         //Else if USB ON
+    {
+        if (g_usb_state != USB_FSMSTATUS_FSMSTATE_ON_Val)           //If previously not ON
+        {
+            if (fsmstate_on_delay == 0)                             //If ON delay timer is cleared
             {
-                I2C3733_Control_Set(1);
+                fsmstate_on_delay = CLK_get_ms() + 250;             //Set ON delay timer
+            }
+            else if (CLK_get_ms() > fsmstate_on_delay)              //Else if ON delay timer is active and timed out
+            {
+                suspend_wakeup_init();                              //Run wakeup routine
+                g_usb_state = fsmstate_now;                         //Save current USB state
             }
         }
     }
+    else                                                            //Else if USB is in a state not being tracked
+    {
+        fsmstate_on_delay = 0;                                      //Clear ON delay timer
+    }
 }
 
 void main_subtask_led(void)
 {
-    if (g_usb_sleeping) return;
+    if (g_usb_state != USB_FSMSTATUS_FSMSTATE_ON_Val) return; //Only run LED tasks if USB is operating
+
     led_matrix_task();
 }
 
@@ -275,8 +286,8 @@ int main(void)
 
     i2c_led_q_init();
 
-    for (g_drvid = 0; g_drvid < ISSI3733_DRIVER_COUNT; g_drvid++)
-        I2C_LED_Q_ONOFF(g_drvid); //Queue data
+    for (uint8_t drvid = 0; drvid < ISSI3733_DRIVER_COUNT; drvid++)
+        I2C_LED_Q_ONOFF(drvid); //Queue data
 
     keyboard_setup();
 
@@ -294,10 +305,21 @@ int main(void)
 
     while (1)
     {
-        keyboard_task();
-
         main_subtasks(); //Note these tasks will also be run while waiting for USB keyboard polling intervals
 
+        if (g_usb_state == USB_FSMSTATUS_FSMSTATE_SUSPEND_Val || g_usb_state == USB_FSMSTATUS_FSMSTATE_SLEEP_Val)
+        {
+            if (suspend_wakeup_condition())
+            {
+                udc_remotewakeup(); //Send remote wakeup signal
+                wait_ms(50);
+            }
+
+            continue;
+        }
+
+        keyboard_task();
+
 #ifdef CONSOLE_ENABLE
         if (CLK_get_ms() > next_print)
         {
diff --git a/tmk_core/protocol/arm_atsam/usb/ui.c b/tmk_core/protocol/arm_atsam/usb/ui.c
index 031678b64..70a619109 100644
--- a/tmk_core/protocol/arm_atsam/usb/ui.c
+++ b/tmk_core/protocol/arm_atsam/usb/ui.c
@@ -52,8 +52,6 @@
 #include "samd51j18a.h"
 #include "ui.h"
 
-volatile uint8_t usb_state;
-
 //! Sequence process running each \c SEQUENCE_PERIOD ms
 #define SEQUENCE_PERIOD 150
 
@@ -72,12 +70,12 @@ static void ui_wakeup_handler(void)
 
 void ui_init(void)
 {
-    usb_state = USB_STATE_POWERUP;
+
 }
 
 void ui_powerdown(void)
 {
-    usb_state = USB_STATE_POWERDOWN;
+
 }
 
 void ui_wakeup_enable(void)
@@ -92,7 +90,7 @@ void ui_wakeup_disable(void)
 
 void ui_wakeup(void)
 {
-    usb_state = USB_STATE_POWERUP;
+
 }
 
 void ui_process(uint16_t framenumber)
diff --git a/tmk_core/protocol/arm_atsam/usb/ui.h b/tmk_core/protocol/arm_atsam/usb/ui.h
index 3d899e669..d1c767d45 100644
--- a/tmk_core/protocol/arm_atsam/usb/ui.h
+++ b/tmk_core/protocol/arm_atsam/usb/ui.h
@@ -47,12 +47,6 @@
 #ifndef _UI_H_
 #define _UI_H_
 
-extern volatile uint8_t usb_state;
-
-#define USB_STATE_UNKNOWN 0
-#define USB_STATE_POWERDOWN 1
-#define USB_STATE_POWERUP 2
-
 //! \brief Initializes the user interface
 void ui_init(void);