3 files changed, 72 insertions, 89 deletions

diff --git a/apps/action.c b/apps/action.c
index 286d4f39b3..30277dc3a6 100644
--- a/apps/action.c
+++ b/apps/action.c
@@ -181,38 +181,6 @@ static inline int get_next_context(const struct button_mapping *items, int i)
             items[i].action_code;
 }
 
-#if defined(HAVE_GUI_BOOST) && defined(HAVE_ADJUSTABLE_CPU_FREQ)
-
-/* Timeout for gui boost in seconds. */
-#define GUI_BOOST_TIMEOUT (HZ)
-
-/* Helper function which is called to boost / unboost CPU. This function
- * avoids to increase boost_count with each call of gui_boost(). */
-static void gui_boost(bool want_to_boost)
-{
-    static bool boosted = false;
-
-    if (want_to_boost && !boosted)
-    {
-        cpu_boost(true);
-        boosted = true;
-    }
-    else if (!want_to_boost && boosted)
-    {
-        cpu_boost(false);
-        boosted = false;
-    }
-}
-
-/* gui_unboost_callback() is called GUI_BOOST_TIMEOUT seconds after the
- * last wheel scrolling event. */
-static int gui_unboost_callback(struct timeout *tmo)
-{
-    (void)tmo;
-    gui_boost(false);
-    return 0;
-}
-#endif
 
 /*
  * int get_action_worker(int context, int timeout, bool *is_pre_button,
@@ -242,23 +210,7 @@ static int get_action_worker(int context, int timeout, bool *is_pre_button,
 
     send_event(GUI_EVENT_ACTIONUPDATE, NULL);
 
-    if (timeout == TIMEOUT_NOBLOCK)
-        button = button_get(false);
-    else if  (timeout == TIMEOUT_BLOCK)
-        button = button_get(true);
-    else
-        button = button_get_w_tmo(timeout);
-
-#if defined(HAVE_GUI_BOOST) && defined(HAVE_ADJUSTABLE_CPU_FREQ)
-    static struct timeout gui_unboost;
-    /* Boost the CPU in case of wheel scrolling activity in the defined contexts.
-     * Call unboost with a timeout of GUI_BOOST_TIMEOUT. */
-    if (button != BUTTON_NONE)
-    {
-        gui_boost(true);
-        timeout_register(&gui_unboost, gui_unboost_callback, GUI_BOOST_TIMEOUT, 0);
-    }
-#endif
+    button = button_get_w_tmo(timeout);
 
     /* Data from sys events can be pulled with button_get_data
      * multimedia button presses don't go through the action system */
diff --git a/firmware/drivers/button.c b/firmware/drivers/button.c
index d47a486b43..fe9040cef8 100644
--- a/firmware/drivers/button.c
+++ b/firmware/drivers/button.c
@@ -371,21 +371,75 @@ static void button_tick(void)
 }
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+static bool button_boosted = false;
+static long button_unboost_tick;
+#define BUTTON_UNBOOST_TMO HZ
+
 static void button_boost(bool state)
 {
-    static bool boosted = false;
-    
-    if (state && !boosted)
+    if (state)
     {
-        cpu_boost(true);
-        boosted = true;
+        button_unboost_tick = current_tick + BUTTON_UNBOOST_TMO;
+
+        if (!button_boosted)
+        {
+            button_boosted = true;
+            cpu_boost(true);
+        }
     }
-    else if (!state && boosted)
+    else if (!state && button_boosted)
     {
+        button_boosted = false;
         cpu_boost(false);
-        boosted = false;
     }
 }
+
+static void button_queue_wait(struct queue_event *evp, int timeout)
+{
+    /* Loop once after wait time if boosted in order to unboost and wait the
+       full remaining time */
+    do
+    {
+        int ticks = timeout;
+
+        if (ticks == 0) /* TIMEOUT_NOBLOCK */
+            ;
+        else if (ticks > 0)
+        {
+            if (button_boosted && ticks > BUTTON_UNBOOST_TMO)
+                ticks = BUTTON_UNBOOST_TMO;
+
+            timeout -= ticks;
+        }
+        else            /* TIMEOUT_BLOCK (ticks < 0) */
+        {
+            if (button_boosted)
+                ticks = BUTTON_UNBOOST_TMO;
+        }
+
+        queue_wait_w_tmo(&button_queue, evp, ticks);
+        if (evp->id != SYS_TIMEOUT)
+        {
+            /* GUI boost build gets immediate kick, otherwise at least 3
+               messages had to be there */
+        #ifndef HAVE_GUI_BOOST
+            if (queue_count(&button_queue) >= 2)
+        #endif
+                button_boost(true);
+
+            break; 
+        }
+
+        if (button_boosted && TIME_AFTER(current_tick, button_unboost_tick))
+            button_boost(false);
+    }
+    while (timeout);
+}
+#else /* ndef HAVE_ADJUSTABLE_CPU_FREQ */
+static inline void button_queue_wait(struct queue_event *evp, int timeout)
+{
+    queue_wait_w_timeout(&button_queue, evp, timeout);
+}
 #endif /* HAVE_ADJUSTABLE_CPU_FREQ */
 
 int button_queue_count( void )
@@ -396,44 +450,26 @@ int button_queue_count( void )
 long button_get(bool block)
 {
     struct queue_event ev;
-    int pending_count = queue_count(&button_queue);
+    button_queue_wait(&ev, block ? TIMEOUT_BLOCK : TIMEOUT_NOBLOCK);
 
-#ifdef HAVE_ADJUSTABLE_CPU_FREQ
-    /* Control the CPU boost trying to keep queue empty. */
-    if (pending_count == 0)
-        button_boost(false);
-    else if (pending_count > 2)
-        button_boost(true);
-#endif
-    
-    if ( block || pending_count )
-    {
-        queue_wait(&button_queue, &ev);
-        
+    if (ev.id == SYS_TIMEOUT)
+        ev.id = BUTTON_NONE;
+    else
         button_data = ev.data;
-        return ev.id;
-    }
-    
-    return BUTTON_NONE;
+
+    return ev.id;
 }
 
 long button_get_w_tmo(int ticks)
 {
-    struct queue_event ev;
-    
-#ifdef HAVE_ADJUSTABLE_CPU_FREQ
-    /* Be sure to keep boosted state. */
-    if (!queue_empty(&button_queue))
-        return button_get(true);
-    
-    button_boost(false);
-#endif
-    
-    queue_wait_w_tmo(&button_queue, &ev, ticks);
+    struct queue_event ev;    
+    button_queue_wait(&ev, ticks);
+
     if (ev.id == SYS_TIMEOUT)
         ev.id = BUTTON_NONE;
     else
         button_data = ev.data;
+
     return ev.id;
 }
 
diff --git a/firmware/export/config.h b/firmware/export/config.h
index 73464526a6..e7cfc698df 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -1129,11 +1129,6 @@ Lyre prototype 1 */
 #define INCLUDE_TIMEOUT_API
 #endif /* HAVE_USB_CHARGING_ENABLE && HAVE_USBSTACK */
 
-#if defined(HAVE_GUI_BOOST) && defined(HAVE_ADJUSTABLE_CPU_FREQ)
-/* Timeout objects required if GUI boost is enabled */
-#define INCLUDE_TIMEOUT_API
-#endif /* HAVE_GUI_BOOST && HAVE_ADJUSTABLE_CPU_FREQ */
-
 #ifndef SIMULATOR
 #if defined(HAVE_USBSTACK) || (CONFIG_STORAGE & STORAGE_NAND)
 #define STORAGE_GET_INFO