xtensa port: add function xt_update_clock_frequency

Introduce new function xt_update_clock_frequency and configuration macro
configUSE_VARIABLE_FREQUENCY that enables it.

Signed-off-by: Max Filippov <[email protected]>

include/portable.h

@@ -191,6 +191,20 @@ void vPortEndScheduler( void ) PRIVILEGED_FUNCTION;
 	void vPortStoreTaskMPUSettings( xMPU_SETTINGS *xMPUSettings, const struct xMEMORY_REGION * const xRegions, StackType_t *pxBottomOfStack, uint32_t ulStackDepth ) PRIVILEGED_FUNCTION;
 #endif
 
+/*
+ * Update CPU core/timer frequency.
+ *
+ * This is implemented when configUSE_TICKLESS_IDLE is set to 1.
+ * XT_CLOCK_FREQ or xtbsp_clock_freq_hz must return new clock frequency
+ * when this function is called.
+ *
+ * This function should be called as soon as the core frequency has changed.
+ * It is safe to call it from an interrupt service routine.
+ * It will update deadline of the internal tick timer and cache new core
+ * frequency.
+ */
+void xt_update_clock_frequency(void);
+
 #ifdef __cplusplus
 }
 #endif

portable/XCC/Xtensa/port.c

@@ -149,23 +149,24 @@ static void xt_tick_handler( void )
     while ( diff > xt_tick_cycles );
 }
 
-//-----------------------------------------------------------------------------
-// Tick timer init. Install interrupt handler, set up first tick, and
-// enable timer interrupt.
-//-----------------------------------------------------------------------------
-static void xt_tick_timer_init( void )
+static void update_xt_tick_cycles( void )
 {
     // Compute the number of cycles per tick.
     #ifdef XT_CLOCK_FREQ
     xt_tick_cycles = ( XT_CLOCK_FREQ / XT_TICK_PER_SEC );
-    #else
-    #ifdef XT_BOARD
+    #elif defined(XT_BOARD)
     xt_tick_cycles = xtbsp_clock_freq_hz() / XT_TICK_PER_SEC;
     #else
     #error "No way to obtain processor clock frequency"
     #endif
-    #endif
-
+}
+//-----------------------------------------------------------------------------
+// Tick timer init. Install interrupt handler, set up first tick, and
+// enable timer interrupt.
+//-----------------------------------------------------------------------------
+static void xt_tick_timer_init( void )
+{
+    update_xt_tick_cycles();
     xMaxSuppressedTicks = 0xFFFFFFFFU / xt_tick_cycles;
     xt_set_interrupt_handler( XT_TIMER_INTNUM, (xt_handler) xt_tick_handler, 0 );
     xt_set_ccompare( XT_TIMER_INDEX, xthal_get_ccount() + xt_tick_cycles );
@@ -422,3 +423,94 @@ void vPortExitCritical( void )
   vPortResetPrivilege( xRunningPrivileged );
 }
 #endif
+
+#if ( configUSE_VARIABLE_FREQUENCY != 0 )
+static void update_tick_remainder( uint32_t now )
+{
+    uint32_t old_ccompare;
+    uint32_t old_tick_cycles;
+
+    old_ccompare = xt_get_ccompare( XT_TIMER_INDEX );
+    old_tick_cycles = xt_tick_cycles;
+    update_xt_tick_cycles();
+
+    // If tick deadline has not been reached yet correct number of remaining
+    // cycles, otherwise timer interrupt must be pending, just service it.
+
+    if ( old_ccompare - now < old_tick_cycles )
+    {
+        uint32_t new_ccompare = now +
+            (uint64_t)(old_ccompare - now) * xt_tick_cycles / old_tick_cycles;
+
+        xt_set_ccompare( XT_TIMER_INDEX, new_ccompare );
+        now = xt_get_ccount();
+        if ( new_ccompare - now > xt_tick_cycles )
+            xt_tick_handler();
+    }
+}
+
+#if ( configUSE_TICKLESS_IDLE != 0 )
+void xt_update_clock_frequency( void )
+{
+    uint32_t ps;
+    uint32_t skip_tick;
+    uint32_t now;
+
+    ps = XT_RSIL( XCHAL_NUM_INTLEVELS );
+
+    now = xt_get_ccount();
+    skip_tick = xt_skip_tick;
+
+    if ( skip_tick )
+    {
+        uint32_t ccompare = xt_get_ccompare( XT_TIMER_INDEX );
+
+        // If there's more than a tick period from now to the timer
+        // deadline try to move deadline to the next possible tick.
+        // Otherwise update tick count for the passed ticks, but don't
+        // change the deadline.
+
+        if ( ccompare - now > xt_tick_cycles &&
+             ccompare - now <= INT32_MAX )
+        {
+            uint32_t first_blocked_tick = ccompare - xt_tick_cycles * skip_tick;
+            uint32_t prev_tick = first_blocked_tick - xt_tick_cycles;
+            uint32_t actual_cycles = now - prev_tick;
+            uint32_t ticks = actual_cycles / xt_tick_cycles;
+            uint32_t diff;
+
+            ccompare = first_blocked_tick + ticks * xt_tick_cycles;
+
+            do
+            {
+                vTaskStepTick( ticks );
+                xt_tick_count += ticks;
+                xt_set_ccompare( XT_TIMER_INDEX, ccompare );
+                diff = xt_get_ccount() - ccompare;
+                ccompare += xt_tick_cycles;
+                ticks = 1;
+
+            } while ( diff <= INT32_MAX );
+        }
+        else
+        {
+            vTaskStepTick( skip_tick );
+            xt_tick_count += skip_tick;
+        }
+        xt_skip_tick = 0;
+    }
+    update_tick_remainder( now );
+
+    XT_WSR_PS( ps );
+}
+#else
+void xt_update_clock_frequency( void )
+{
+    uint32_t ps;
+
+    ps = XT_RSIL( XCHAL_NUM_INTLEVELS );
+    update_tick_remainder( xt_get_ccount() );
+    XT_WSR_PS( ps );
+}
+#endif
+#endif