From 6e984a8b5e34ba181b0893a929af569a1faef2b6 Mon Sep 17 00:00:00 2001
From: patrickmt <40182064+patrickmt@users.noreply.github.com>
Date: Tue, 18 Dec 2018 15:21:25 -0500
Subject: [PATCH] Update to arm_atsam wait and timer routines

Microsecond (us) delays are now handled by a busy wait loop according to MCU frequency. This replaces the system counter method which had an overhead of around 12us.
TC5 device and supporting routines removed as it was the old us delay counter.
wait_ms is now properly a macro to CLK_delay_ms.
wait_us is now properly a macro to CLK_delay_us.
Removed CLK_get_us as it has no use.
All calls to CLK_get_ms() have been replaced by timer_read64() with corrected typing.
All calls to CLK_delay_ms() have been replaced by wait_ms().
All calls to CLK_delay_us() have been replaced by wait_us() and timings verified or updated as needed after review on scope.
Corrected typing of variables using 64bit ms timer readings if needed.
---
 keyboards/massdrop/alt/matrix.c               | 12 +--
 keyboards/massdrop/ctrl/matrix.c              | 12 +--
 tmk_core/common/arm_atsam/timer.c             | 20 +----
 tmk_core/common/wait.h                        |  4 +
 .../protocol/arm_atsam/arm_atsam_protocol.h   |  2 +
 tmk_core/protocol/arm_atsam/clks.c            | 90 +++++--------------
 tmk_core/protocol/arm_atsam/clks.h            |  5 +-
 tmk_core/protocol/arm_atsam/i2c_master.c      |  4 +-
 tmk_core/protocol/arm_atsam/led_matrix.c      |  4 +-
 tmk_core/protocol/arm_atsam/main_arm_atsam.c  | 18 ++--
 tmk_core/protocol/arm_atsam/usb/udi_cdc.c     |  8 +-
 tmk_core/protocol/arm_atsam/usb/usb2422.c     | 15 ++--
 12 files changed, 61 insertions(+), 133 deletions(-)

diff --git a/keyboards/massdrop/alt/matrix.c b/keyboards/massdrop/alt/matrix.c
index 892d38791c..472479d303 100644
--- a/keyboards/massdrop/alt/matrix.c
+++ b/keyboards/massdrop/alt/matrix.c
@@ -79,8 +79,6 @@ void matrix_init(void)
     matrix_init_quantum();
 }
 
-#define MATRIX_SCAN_DELAY 10   //Delay after setting a col to output (in us)
-
 uint64_t mdebouncing = 0;
 uint8_t matrix_scan(void)
 {
@@ -89,9 +87,7 @@ uint8_t matrix_scan(void)
     uint8_t col;
     uint32_t scans[2]; //PA PB
 
-    if (CLK_get_ms() < mdebouncing) return 1; //mdebouncing == 0 when no debouncing active
-
-    //DBG_1_OFF; //Profiling scans
+    if (timer_read64() < mdebouncing) return 1; //mdebouncing == 0 when no debouncing active
 
     memset(mlatest, 0, MATRIX_ROWS * sizeof(matrix_row_t)); //Zero the result buffer
 
@@ -99,7 +95,7 @@ uint8_t matrix_scan(void)
     {
         PORT->Group[col_ports[col]].OUTSET.reg = 1 << col_pins[col]; //Set col output
 
-        CLK_delay_us(MATRIX_SCAN_DELAY); //Delay for output
+        wait_us(1); //Delay for output
 
         scans[PA] = PORT->Group[PA].IN.reg & row_masks[PA]; //Read PA row pins data
         scans[PB] = PORT->Group[PB].IN.reg & row_masks[PB]; //Read PB row pins data
@@ -132,11 +128,9 @@ uint8_t matrix_scan(void)
     else
     {
         //Begin or extend debounce on change
-        mdebouncing = CLK_get_ms() + DEBOUNCING_DELAY;
+        mdebouncing = timer_read64() + DEBOUNCING_DELAY;
     }
 
-    //DBG_1_ON; //Profiling scans
-
     matrix_scan_quantum();
 
     return 1;
diff --git a/keyboards/massdrop/ctrl/matrix.c b/keyboards/massdrop/ctrl/matrix.c
index 3580577dc1..5f1741e58a 100644
--- a/keyboards/massdrop/ctrl/matrix.c
+++ b/keyboards/massdrop/ctrl/matrix.c
@@ -79,8 +79,6 @@ void matrix_init(void)
     matrix_init_quantum();
 }
 
-#define MATRIX_SCAN_DELAY 10   //Delay after setting a col to output (in us)
-
 uint64_t mdebouncing = 0;
 uint8_t matrix_scan(void)
 {
@@ -89,9 +87,7 @@ uint8_t matrix_scan(void)
     uint8_t col;
     uint32_t scans[2]; //PA PB
 
-    if (CLK_get_ms() < mdebouncing) return 1; //mdebouncing == 0 when no debouncing active
-
-    //DBG_1_OFF; //Profiling scans
+    if (timer_read64() < mdebouncing) return 1; //mdebouncing == 0 when no debouncing active
 
     memset(mlatest, 0, MATRIX_ROWS * sizeof(matrix_row_t)); //Zero the result buffer
 
@@ -99,7 +95,7 @@ uint8_t matrix_scan(void)
     {
         PORT->Group[col_ports[col]].OUTSET.reg = 1 << col_pins[col]; //Set col output
 
-        CLK_delay_us(MATRIX_SCAN_DELAY); //Delay for output
+        wait_us(1); //Delay for output
 
         scans[PA] = PORT->Group[PA].IN.reg & row_masks[PA]; //Read PA row pins data
         scans[PB] = PORT->Group[PB].IN.reg & row_masks[PB]; //Read PB row pins data
@@ -132,11 +128,9 @@ uint8_t matrix_scan(void)
     else
     {
         //Begin or extend debounce on change
-        mdebouncing = CLK_get_ms() + DEBOUNCING_DELAY;
+        mdebouncing = timer_read64() + DEBOUNCING_DELAY;
     }
 
-    //DBG_1_ON; //Profiling scans
-
     matrix_scan_quantum();
 
     return 1;
diff --git a/tmk_core/common/arm_atsam/timer.c b/tmk_core/common/arm_atsam/timer.c
index bcfe5002c3..6c3905e308 100644
--- a/tmk_core/common/arm_atsam/timer.c
+++ b/tmk_core/common/arm_atsam/timer.c
@@ -9,7 +9,7 @@ void set_time(uint64_t tset)
 
 void timer_init(void)
 {
-    ms_clk = 0;
+    timer_clear();
 }
 
 uint16_t timer_read(void)
@@ -37,23 +37,7 @@ uint32_t timer_elapsed32(uint32_t tlast)
     return TIMER_DIFF_32(timer_read32(), tlast);
 }
 
-uint32_t timer_elapsed64(uint32_t tlast)
-{
-    uint64_t tnow = timer_read64();
-    return (tnow >= tlast ? tnow - tlast : UINT64_MAX - tlast + tnow);
-}
-
 void timer_clear(void)
 {
-    ms_clk = 0;
-}
-
-void wait_ms(uint64_t msec)
-{
-    CLK_delay_ms(msec);
-}
-
-void wait_us(uint16_t usec)
-{
-    CLK_delay_us(usec);
+    set_time(0);
 }
diff --git a/tmk_core/common/wait.h b/tmk_core/common/wait.h
index a7cded9420..a77840bcef 100644
--- a/tmk_core/common/wait.h
+++ b/tmk_core/common/wait.h
@@ -15,6 +15,10 @@ extern "C" {
 #   include "ch.h"
 #   define wait_ms(ms) chThdSleepMilliseconds(ms)
 #   define wait_us(us) chThdSleepMicroseconds(us)
+#elif defined PROTOCOL_ARM_ATSAM
+#   include "clks.h"
+#   define wait_ms(ms) CLK_delay_ms(ms)
+#   define wait_us(us) CLK_delay_us(us)
 #elif defined(__arm__)
 #   include "wait_api.h"
 #else  // Unit tests
diff --git a/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h b/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
index 2ba0991749..928af8c7e1 100644
--- a/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
+++ b/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
@@ -21,8 +21,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "samd51j18a.h"
 #include "md_bootloader.h"
 
+#include "timer.h"
 #include "d51_util.h"
 #include "clks.h"
+#include "wait.h"
 #include "adc.h"
 #include "i2c_master.h"
 #include "spi.h"
diff --git a/tmk_core/protocol/arm_atsam/clks.c b/tmk_core/protocol/arm_atsam/clks.c
index 8768d0a99e..1ff318e59b 100644
--- a/tmk_core/protocol/arm_atsam/clks.c
+++ b/tmk_core/protocol/arm_atsam/clks.c
@@ -21,8 +21,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 volatile clk_t system_clks;
 volatile uint64_t ms_clk;
-
-volatile uint8_t us_delay_done;
+uint32_t usec_delay_mult;
+#define USEC_DELAY_LOOP_CYCLES 3 //Sum of instruction cycles in us delay loop
 
 const uint32_t sercom_apbbase[] = {(uint32_t)SERCOM0,(uint32_t)SERCOM1,(uint32_t)SERCOM2,(uint32_t)SERCOM3,(uint32_t)SERCOM4,(uint32_t)SERCOM5};
 const uint8_t sercom_pchan[] = {7, 8, 23, 24, 34, 35};
@@ -73,6 +73,9 @@ void CLK_oscctrl_init(void)
 
     system_clks.freq_gclk[0] = system_clks.freq_dpll[0];
 
+    usec_delay_mult = system_clks.freq_gclk[0] / (USEC_DELAY_LOOP_CYCLES * 1000000);
+    if (usec_delay_mult < 1) usec_delay_mult = 1; //Never allow a multiplier of zero
+
     DBGC(DC_CLK_OSC_INIT_COMPLETE);
 }
 
@@ -158,23 +161,11 @@ void TC4_Handler()
     }
 }
 
-void TC5_Handler()
-{
-    if (TC5->COUNT16.INTFLAG.bit.MC0)
-    {
-        TC5->COUNT16.INTFLAG.reg = TC_INTENCLR_MC0;
-        us_delay_done = 1;
-        TC5->COUNT16.CTRLA.bit.ENABLE = 0;
-        while (TC5->COUNT16.SYNCBUSY.bit.ENABLE) {}
-    }
-}
-
 uint32_t CLK_enable_timebase(void)
 {
     Gclk *pgclk = GCLK;
     Mclk *pmclk = MCLK;
     Tc *ptc4 = TC4;
-    Tc *ptc5 = TC5;
     Tc *ptc0 = TC0;
     Evsys *pevsys = EVSYS;
 
@@ -189,11 +180,6 @@ uint32_t CLK_enable_timebase(void)
     pgclk->PCHCTRL[TC4_GCLK_ID].bit.GEN = GEN_TC45;
     pgclk->PCHCTRL[TC4_GCLK_ID].bit.CHEN = 1;
 
-    //unmask TC5 sourcegclk2 to TC5
-    pmclk->APBCMASK.bit.TC5_ = 1;
-    pgclk->PCHCTRL[TC5_GCLK_ID].bit.GEN = GEN_TC45;
-    pgclk->PCHCTRL[TC5_GCLK_ID].bit.CHEN = 1;
-
     //configure TC4
     DBGC(DC_CLK_ENABLE_TIMEBASE_TC4_BEGIN);
     ptc4->COUNT16.CTRLA.bit.ENABLE = 0;
@@ -220,30 +206,6 @@ uint32_t CLK_enable_timebase(void)
 
     DBGC(DC_CLK_ENABLE_TIMEBASE_TC4_COMPLETE);
 
-    //configure TC5
-    DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_BEGIN);
-    ptc5->COUNT16.CTRLA.bit.ENABLE = 0;
-    while (ptc5->COUNT16.SYNCBUSY.bit.ENABLE) { DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_SYNC_DISABLE); }
-    ptc5->COUNT16.CTRLA.bit.SWRST = 1;
-    while (ptc5->COUNT16.SYNCBUSY.bit.SWRST) { DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_SYNC_SWRST_1); }
-    while (ptc5->COUNT16.CTRLA.bit.SWRST) { DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_SYNC_SWRST_2); }
-
-    //CTRLA defaults
-    //CTRLB as default, counting up
-    ptc5->COUNT16.CTRLBCLR.reg = 5;
-    while (ptc5->COUNT16.SYNCBUSY.bit.CTRLB) { DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_SYNC_CLTRB); }
-    //ptc5->COUNT16.DBGCTRL.bit.DBGRUN = 1;
-
-    //wave mode
-    ptc5->COUNT16.WAVE.bit.WAVEGEN = 1; //MFRQ match frequency mode, toggle each CC match
-    //generate event for next stage
-    ptc5->COUNT16.EVCTRL.bit.MCEO0 = 1;
-
-    NVIC_EnableIRQ(TC5_IRQn);
-    ptc5->COUNT16.INTENSET.bit.MC0 = 1;
-
-    DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_COMPLETE);
-
     //unmask TC0,1, sourcegclk2 to TC0,1
     pmclk->APBAMASK.bit.TC0_ = 1;
     pgclk->PCHCTRL[TC0_GCLK_ID].bit.GEN = GEN_TC45;
@@ -289,37 +251,27 @@ uint32_t CLK_enable_timebase(void)
     return 0;
 }
 
-uint32_t CLK_get_ms(void)
+void CLK_delay_us(uint32_t usec)
 {
-    return ms_clk;
-}
-
-void CLK_delay_us(uint16_t usec)
-{
-    us_delay_done = 0;
-
-    if (TC5->COUNT16.CTRLA.bit.ENABLE)
-    {
-        TC5->COUNT16.CTRLA.bit.ENABLE = 0;
-        while (TC5->COUNT16.SYNCBUSY.bit.ENABLE) {}
-    }
-
-    if (usec < 10) usec = 0;
-    else usec -= 10;
-
-    TC5->COUNT16.CC[0].reg = usec;
-    while (TC5->COUNT16.SYNCBUSY.bit.CC0) {}
-
-    TC5->COUNT16.CTRLA.bit.ENABLE = 1;
-    while (TC5->COUNT16.SYNCBUSY.bit.ENABLE) {}
-
-    while (!us_delay_done) {}
+    asm (
+        "CBZ R0, return\n\t"        //If usec == 0, branch to return label
+    );
+    asm (
+        "MULS R0, %0\n\t"           //Multiply R0(usec) by usec_delay_mult and store in R0
+        ".balign 16\n\t"            //Ensure loop is aligned for fastest performance
+        "loop: SUBS R0, #1\n\t"     //Subtract 1 from R0 and update flags (1 cycle)
+        "BNE loop\n\t"              //Branch if non-zero to loop label (2 cycles)  NOTE: USEC_DELAY_LOOP_CYCLES is the sum of loop cycles
+        "return:\n\t"               //Return label
+        : //No output registers
+        : "r" (usec_delay_mult)     //For %0
+    );
+    //Note: BX LR generated
 }
 
 void CLK_delay_ms(uint64_t msec)
 {
-    msec += CLK_get_ms();
-    while (msec > CLK_get_ms()) {}
+    msec += timer_read64();
+    while (msec > timer_read64()) {}
 }
 
 void clk_enable_sercom_apbmask(int sercomn)
diff --git a/tmk_core/protocol/arm_atsam/clks.h b/tmk_core/protocol/arm_atsam/clks.h
index 96819bfdd0..1b01a1764e 100644
--- a/tmk_core/protocol/arm_atsam/clks.h
+++ b/tmk_core/protocol/arm_atsam/clks.h
@@ -77,9 +77,8 @@ void CLK_oscctrl_init(void);
 void CLK_reset_time(void);
 uint32_t CLK_set_gclk_freq(uint8_t gclkn, uint32_t freq);
 uint32_t CLK_enable_timebase(void);
-uint32_t CLK_get_ms(void);
-uint64_t CLK_get_us(void);
-void CLK_delay_us(uint16_t usec);
+uint64_t timer_read64(void);
+void CLK_delay_us(uint32_t usec);
 void CLK_delay_ms(uint64_t msec);
 
 uint32_t CLK_set_spi_freq(uint8_t sercomn, uint32_t freq);
diff --git a/tmk_core/protocol/arm_atsam/i2c_master.c b/tmk_core/protocol/arm_atsam/i2c_master.c
index f608a79cc9..d91a851f37 100644
--- a/tmk_core/protocol/arm_atsam/i2c_master.c
+++ b/tmk_core/protocol/arm_atsam/i2c_master.c
@@ -265,12 +265,12 @@ uint8_t I2C3733_Init_Control(void)
     //USB state machine will enable driver when communication is ready
     I2C3733_Control_Set(0);
 
-    CLK_delay_ms(1);
+    wait_ms(1);
 
     sr_exp_data.bit.IRST = 0;
     SR_EXP_WriteData();
 
-    CLK_delay_ms(1);
+    wait_ms(1);
 
     DBGC(DC_I2C3733_INIT_CONTROL_COMPLETE);
 
diff --git a/tmk_core/protocol/arm_atsam/led_matrix.c b/tmk_core/protocol/arm_atsam/led_matrix.c
index e914fc80ea..9ef7393a25 100644
--- a/tmk_core/protocol/arm_atsam/led_matrix.c
+++ b/tmk_core/protocol/arm_atsam/led_matrix.c
@@ -494,11 +494,11 @@ void led_matrix_task(void)
     if (led_enabled)
     {
         //If an update may run and frame processing has completed
-        if (CLK_get_ms() >= led_next_run && led_cur == lede)
+        if (timer_read64() >= led_next_run && led_cur == lede)
         {
             uint8_t drvid;
 
-            led_next_run = CLK_get_ms() + LED_UPDATE_RATE;  //Set next frame update time
+            led_next_run = timer_read64() + LED_UPDATE_RATE;  //Set next frame update time
 
             //NOTE: GCR does not need to be timed with LED processing, but there is really no harm
             if (gcr_actual != gcr_actual_last)
diff --git a/tmk_core/protocol/arm_atsam/main_arm_atsam.c b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
index 2bda7d7c7b..eaad66e9fc 100644
--- a/tmk_core/protocol/arm_atsam/main_arm_atsam.c
+++ b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
@@ -159,7 +159,7 @@ void send_consumer(uint16_t data)
 
 void main_subtask_usb_state(void)
 {
-    static uint32_t fsmstate_on_delay = 0;                          //Delay timer to be sure USB is actually operating before bringing up hardware
+    static uint64_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
@@ -188,9 +188,9 @@ void main_subtask_usb_state(void)
         {
             if (fsmstate_on_delay == 0)                             //If ON delay timer is cleared
             {
-                fsmstate_on_delay = CLK_get_ms() + 250;             //Set ON delay timer
+                fsmstate_on_delay = timer_read64() + 250;             //Set ON delay timer
             }
-            else if (CLK_get_ms() > fsmstate_on_delay)              //Else if ON delay timer is active and timed out
+            else if (timer_read64() > 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
@@ -214,9 +214,9 @@ void main_subtask_power_check(void)
 {
     static uint64_t next_5v_checkup = 0;
 
-    if (CLK_get_ms() > next_5v_checkup)
+    if (timer_read64() > next_5v_checkup)
     {
-        next_5v_checkup = CLK_get_ms() + 5;
+        next_5v_checkup = timer_read64() + 5;
 
         v_5v = adc_get(ADC_5V);
         v_5v_avg = 0.9 * v_5v_avg + 0.1 * v_5v;
@@ -229,9 +229,9 @@ void main_subtask_usb_extra_device(void)
 {
     static uint64_t next_usb_checkup = 0;
 
-    if (CLK_get_ms() > next_usb_checkup)
+    if (timer_read64() > next_usb_checkup)
     {
-        next_usb_checkup = CLK_get_ms() + 10;
+        next_usb_checkup = timer_read64() + 10;
 
         USB_HandleExtraDevice();
     }
@@ -325,9 +325,9 @@ int main(void)
         keyboard_task();
 
 #ifdef CONSOLE_ENABLE
-        if (CLK_get_ms() > next_print)
+        if (timer_read64() > next_print)
         {
-            next_print = CLK_get_ms() + 250;
+            next_print = timer_read64() + 250;
             //Add any debug information here that you want to see very often
             //dprintf("5v=%u 5vu=%u dlow=%u dhi=%u gca=%u gcd=%u\r\n", v_5v, v_5v_avg, v_5v_avg - V5_LOW, v_5v_avg - V5_HIGH, gcr_actual, gcr_desired);
         }
diff --git a/tmk_core/protocol/arm_atsam/usb/udi_cdc.c b/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
index 5f3c289e81..ffe3526db5 100644
--- a/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
+++ b/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
@@ -1227,9 +1227,9 @@ uint32_t cdc_tx_send_time_next;
 
 void CDC_send(void)
 {
-    while (CLK_get_ms() < cdc_tx_send_time_next);
+    while (timer_read64() < cdc_tx_send_time_next);
     udi_cdc_tx_send(0);
-    cdc_tx_send_time_next = CLK_get_ms() + CDC_SEND_INTERVAL;
+    cdc_tx_send_time_next = timer_read64() + CDC_SEND_INTERVAL;
 }
 
 uint32_t CDC_print(char *printbuf)
@@ -1238,7 +1238,7 @@ uint32_t CDC_print(char *printbuf)
     char *buf = printbuf;
     char c;
 
-    if (CLK_get_ms() < 5000) return 0;
+    if (timer_read64() < 5000) return 0;
 
     while ((c = *buf++) != 0 && !(count >= MAX_PRINT))
     {
@@ -1339,7 +1339,7 @@ void CDC_init(void)
     inbuf.count = 0;
     inbuf.lastcount = 0;
     printbuf[0] = 0;
-    cdc_tx_send_time_next = CLK_get_ms() + CDC_SEND_INTERVAL;
+    cdc_tx_send_time_next = timer_read64() + CDC_SEND_INTERVAL;
 }
 
 #else //CDC line 62
diff --git a/tmk_core/protocol/arm_atsam/usb/usb2422.c b/tmk_core/protocol/arm_atsam/usb/usb2422.c
index ac19bf4ea0..d6e1922429 100644
--- a/tmk_core/protocol/arm_atsam/usb/usb2422.c
+++ b/tmk_core/protocol/arm_atsam/usb/usb2422.c
@@ -64,7 +64,7 @@ void USB_write2422_block(void)
         i2c0_transmit(USB2422_ADDR, dest, 34, 50000);
         SERCOM0->I2CM.CTRLB.bit.CMD = 0x03;
         while (SERCOM0->I2CM.SYNCBUSY.bit.SYSOP) { DBGC(DC_USB_WRITE2422_BLOCK_SYNC_SYSOP); }
-        CLK_delay_us(100);
+        wait_us(100);
     }
 
     DBGC(DC_USB_WRITE2422_BLOCK_COMPLETE);
@@ -135,7 +135,7 @@ void USB2422_init(void)
     sr_exp_data.bit.HUB_RESET_N = 1; //reset high
     SR_EXP_WriteData();
 
-    CLK_delay_us(100);
+    wait_us(100);
 
 #ifndef MD_BOOTLOADER
 
@@ -154,10 +154,9 @@ void USB_reset(void)
     //pulse reset for at least 1 usec
     sr_exp_data.bit.HUB_RESET_N = 0; //reset low
     SR_EXP_WriteData();
-    CLK_delay_us(1);
+    wait_us(2);
     sr_exp_data.bit.HUB_RESET_N = 1; //reset high to run
     SR_EXP_WriteData();
-    CLK_delay_us(1);
 
     DBGC(DC_USB_RESET_COMPLETE);
 }
@@ -247,7 +246,7 @@ void USB_set_host_by_voltage(void)
 
     SR_EXP_WriteData();
 
-    CLK_delay_ms(250);
+    wait_ms(250);
 
     while ((v_5v = adc_get(ADC_5V)) < ADC_5V_START_LEVEL) { DBGC(DC_USB_SET_HOST_5V_LOW_WAITING); }
 
@@ -313,11 +312,11 @@ uint8_t USB2422_Port_Detect_Init(void)
 
     USB_set_host_by_voltage();
 
-    port_detect_retry_ms = CLK_get_ms() + PORT_DETECT_RETRY_INTERVAL;
+    port_detect_retry_ms = timer_read64() + PORT_DETECT_RETRY_INTERVAL;
 
     while (!USB_active())
     {
-        tmod = CLK_get_ms() % PORT_DETECT_RETRY_INTERVAL;
+        tmod = timer_read64() % PORT_DETECT_RETRY_INTERVAL;
 
         if (v_con_1 > v_con_2) //Values updated from USB_set_host_by_voltage();
         {
@@ -333,7 +332,7 @@ uint8_t USB2422_Port_Detect_Init(void)
             else { DBG_LED_OFF; }
         }
 
-        if (CLK_get_ms() > port_detect_retry_ms)
+        if (timer_read64() > port_detect_retry_ms)
         {
             DBGC(DC_PORT_DETECT_INIT_FAILED);
             return 0;