;/***************************************************************************
; * Copyright (c) 2024 Microsoft Corporation
; *
; * This program and the accompanying materials are made available under the
; * terms of the MIT License which is available at
; * https://opensource.org/licenses/MIT.
; *
; * SPDX-License-Identifier: MIT
; **************************************************************************/
;
;
;/**************************************************************************/
;/**************************************************************************/
;/**                                                                       */
;/** ThreadX Component                                                     */
;/**                                                                       */
;/**   Thread                                                              */
;/**                                                                       */
;/**************************************************************************/
;/**************************************************************************/
;
;
;#define TX_SOURCE_CODE
;
;
;/* Include necessary system files.  */
;
;#include "tx_api.h"
;#include "tx_thread.h"
;#include "tx_timer.h"
;
;
    .global     __tx_thread_execute_ptr
    .global     __tx_thread_current_ptr
    .global     __tx_timer_time_slice
#if (TX_LOW_POWER == 1)
    .global     _tx_low_power_enter
    .global     _tx_low_power_exit
    .global     __tx_thread_preempt_disable
#endif
;
    .text

;/**************************************************************************/
;/*                                                                        */
;/*  FUNCTION                                               RELEASE        */
;/*                                                                        */
;/*    _tx_thread_schedule                                  RXv3/GNURX     */
;/*                                                           6.1.10       */
;/*  AUTHOR                                                                */
;/*                                                                        */
;/*    William E. Lamie, Microsoft Corporation                             */
;/*                                                                        */
;/*  DESCRIPTION                                                           */
;/*                                                                        */
;/*    This function waits for a thread control block pointer to appear in */
;/*    the _tx_thread_execute_ptr variable.  Once a thread pointer appears */
;/*    in the variable, the corresponding thread is resumed.               */
;/*                                                                        */
;/*  INPUT                                                                 */
;/*                                                                        */
;/*    None                                                                */
;/*                                                                        */
;/*  OUTPUT                                                                */
;/*                                                                        */
;/*    None                                                                */
;/*                                                                        */
;/*  CALLS                                                                 */
;/*                                                                        */
;/*    None                                                                */
;/*                                                                        */
;/*  CALLED BY                                                             */
;/*                                                                        */
;/*    _tx_initialize_kernel_enter          ThreadX entry function         */
;/*    _tx_thread_system_return             Return to system from thread   */
;/*    _tx_thread_context_restore           Restore thread's context       */
;/*                                                                        */
;/**************************************************************************/
;VOID   _tx_thread_schedule(VOID)
;{
    .global __tx_thread_schedule
__tx_thread_schedule:
;
;
;    /* Wait for a thread to execute.  */
;    do
;    {
    MOV.L    #__tx_thread_execute_ptr, R1        ; Address of thread to executer ptr
__tx_thread_schedule_loop:
    SETPSW I                                     ; Enable interrupts
    CLRPSW I                                     ; Disable interrupts
    MOV.L    [R1],R2                             ; Pickup next thread to execute
    CMP      #0,R2                               ; Is it NULL?
    BNE      __tx_thread_thread_ready            ; Not NULL, schedule the thread
                                                 ; Idle system - no thread is ready
#if (TX_LOW_POWER == 1)
    MOV.L    #__tx_thread_preempt_disable, R1    ; Load prempt disable flag.
    MOV.L    [R1], R2
    ADD	     #1, R2                              ; Disable preemption while enter/exit
    MOV.L    R2, [R1]
    BSR      _tx_low_power_enter                 ; Possibly enter low power mode
#endif

#if (TX_ENABLE_WAIT == 1)
    WAIT                                         ; Wait for interrupt
#endif

#if (TX_LOW_POWER == 1)
    CLRPSW I                                     ; Disable interrupts (because WAIT enables interrupts)
    BSR      _tx_low_power_exit                  ; Possibly exit low power mode
    MOV.L    #__tx_thread_preempt_disable, R1    ; Load prempt disable flag.
    MOV.L    [R1], R2
    SUB	     #1, R2				 ; Enable preemption
    MOV.L    R2, [R1]
    MOV.L    #__tx_thread_execute_ptr, R1        ; Address of thread to executer ptr
#endif

    BRA      __tx_thread_schedule_loop           ; Idle system, keep checking

__tx_thread_thread_ready:
;
;    }
;    while(_tx_thread_execute_ptr == TX_NULL);
;
;    /* Yes! We have a thread to execute. Note that interrupts are locked out at this point.  */
;
;    /* Setup the current thread pointer.  */
;    _tx_thread_current_ptr =  _tx_thread_execute_ptr;
;
    MOV.L    #__tx_thread_current_ptr, R3
    MOV.L    R2,[R3]                            ; Setup current thread pointer
;
;    /* Increment the run count for this thread.  */
;    _tx_thread_current_ptr -> tx_thread_run_count++;
;
    MOV.L    4[R2],R3                           ; Pickup run count
    ADD      #1,R3                              ; Increment run counter
    MOV.L    R3,4[R2]                           ; Store it back in control block
;
;    /* Setup time-slice, if present.  */
;    _tx_timer_time_slice =  _tx_thread_current_ptr -> tx_thread_time_slice;
;
    MOV.L    24[R2],R3                          ; Pickup thread time-slice
    MOV.L    #__tx_timer_time_slice,R4          ; Pickup pointer to time-slice
    MOV.L    R3, [R4]                           ; Setup time-slice
;
;    /* Switch to the thread's stack.  */
;    SP =  _tx_thread_execute_ptr -> tx_thread_stack_ptr;
    SETPSW U                                    ; User stack mode
    MOV.L   8[R2],R0                            ; Pickup stack pointer

#if (__RX_DFPU_INSNS__ == 1)
    MOV.L   144[R2], R1                         ; Get tx_thread_fpu_enable.
    CMP     #0, R1
    BEQ     __tx_thread_schedule_fpu_skip

    DPOPM.L DPSW-DECNT                          ; Restore FPU register bank if tx_thread_fpu_enable is not 0.
    DPOPM.D DR0-DR15

__tx_thread_schedule_fpu_skip:
#endif

    POPM    R1-R3                               ; Restore accumulators.
    MVTACLO R3, A0
    MVTACHI R2, A0
    MVTACGU R1, A0
    POPM    R1-R3
    MVTACLO R3, A1
    MVTACHI R2, A1
    MVTACGU R1, A1

    POPM   R6-R13                               ; Recover interrupt stack frame
    POPC   FPSW
    POPM   R14-R15
    POPM   R3-R5
    POPM   R1-R2
    RTE                                         ; Return to point of interrupt, this restores PC and PSW

;
;}


.global __tx_thread_context_save
.global __tx_thread_context_restore


; Software triggered interrupt used to perform context switches.
; The priority of this interrupt is set to the lowest priority within
; tx_initialize_low_level() and triggered by ThreadX when calling
; _tx_thread_system_return().
.global $tableentry$27$.rvectors
$tableentry$27$.rvectors:

    PUSHM R1-R2

    BSR __tx_thread_context_save

    BRA __tx_thread_context_restore


; Enable saving of DFPU registers for the current thread.
; If DPFU op are disabled do nothing.
.GLB _tx_thread_fpu_enable
_tx_thread_fpu_enable:
#if (__RX_DFPU_INSNS__ == 1)
    PUSHM    R1-R4
    MVFC     PSW, R2                            ; Save PSW to R2
    CLRPSW   I                                  ; Lockout interrupts

    MOV.L    #__tx_thread_current_ptr, R4
    MOV.L    [R4], R1                           ; Fetch current thread pointer

    MOV.L    #1, R3
    MOV.L    R3, 144[R1]                        ; Set tx_thread_fpu_enable to 1.

__tx_thread_fpu_enable_exit:
    MVTC     R2, PSW                            ; Restore interrupt status
    POPM     R1-R4
#endif
    RTS


; Disable saving of DFPU registers for the current thread.
; If DPFU op are disabled do nothing.
.GLB _tx_thread_fpu_disable
_tx_thread_fpu_disable:
#if (__RX_DFPU_INSNS__ == 1)
    PUSHM    R1-R4
    MVFC     PSW, R2                            ; Save PSW to R2
    CLRPSW   I                                  ; Lockout interrupts

    MOV.L    #__tx_thread_current_ptr, R4
    MOV.L    [R4], R1                           ; Fetch current thread pointer

    MOV.L    #1, R3
    MOV.L    R3, 144[R1]                        ; Set tx_thread_fpu_enable to 1.

__tx_thread_fpu_disable_exit:
    MVTC     R2, PSW                            ; Restore interrupt status
    POPM     R1-R4
#endif
    RTS

    .end



