1 files changed, 21 insertions, 69 deletions

diff --git a/tmk_core/common/chibios/wait.c b/tmk_core/common/chibios/wait.c
index c6270fd95e..56fd6ffcec 100644
--- a/tmk_core/common/chibios/wait.c
+++ b/tmk_core/common/chibios/wait.c
@@ -14,76 +14,28 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef __OPTIMIZE__
-#    pragma message "Compiler optimizations disabled; wait_cpuclock() won't work as designed"
-#endif
+#include <ch.h>
+#include <hal.h>
 
-#define CLOCK_DELAY_NOP8 "nop\n\t nop\n\t nop\n\t nop\n\t   nop\n\t nop\n\t nop\n\t nop\n\t"
+#include "_wait.h"
 
-__attribute__((always_inline)) static inline void wait_cpuclock(unsigned int n) { /* n: 1..135 */
-    /* The argument n must be a constant expression.
-     * That way, compiler optimization will remove unnecessary code. */
-    if (n < 1) {
-        return;
-    }
-    if (n > 8) {
-        unsigned int n8 = n / 8;
-        n               = n - n8 * 8;
-        switch (n8) {
-            case 16:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 15:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 14:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 13:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 12:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 11:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 10:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 9:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 8:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 7:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 6:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 5:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 4:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 3:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 2:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 1:
-                asm volatile(CLOCK_DELAY_NOP8::: "memory");
-            case 0:
-                break;
-        }
+#ifdef WAIT_US_TIMER
+void wait_us(uint16_t duration) {
+    static const GPTConfig gpt_cfg = {1000000, NULL, 0, 0}; /* 1MHz timer, no callback */
+
+    if (duration == 0) {
+        duration = 1;
     }
-    switch (n) {
-        case 8:
-            asm volatile("nop" ::: "memory");
-        case 7:
-            asm volatile("nop" ::: "memory");
-        case 6:
-            asm volatile("nop" ::: "memory");
-        case 5:
-            asm volatile("nop" ::: "memory");
-        case 4:
-            asm volatile("nop" ::: "memory");
-        case 3:
-            asm volatile("nop" ::: "memory");
-        case 2:
-            asm volatile("nop" ::: "memory");
-        case 1:
-            asm volatile("nop" ::: "memory");
-        case 0:
-            break;
+
+    /*
+     * Only use this timer on the main thread;
+     * other threads need to use their own timer.
+     */
+    if (chThdGetSelfX() == &ch.mainthread && duration < (1ULL << (sizeof(gptcnt_t) * 8))) {
+        gptStart(&WAIT_US_TIMER, &gpt_cfg);
+        gptPolledDelay(&WAIT_US_TIMER, duration);
+    } else {
+        chThdSleepMicroseconds(duration);
     }
-}
\ No newline at end of file
+}
+#endif