;/****************************************************************************
;[ schedule_cortexm4.s ] - scheduling for Cortex-M4
;****************************************************************************/

    PRESERVE8
    AREA |CODE$$cortexm4_schedule|, CODE, READONLY
    ALIGN

EXTENDED_SVC_CODE           EQU 0x00001500
SIZE_OF_SSYCB               EQU          4

EXC_RETURN_CHECK_SP         EQU          4
STACKED_REG_PC              EQU         24
OFFSET_OF_IMMEDIATE_DATA    EQU         -2
EXC_RETURN_PSP              EQU 0xFFFFFFFD

UTK_SIZE_OF_TOP_PRIORITY    EQU          4
UTK_SIZE_OF_QUEUE           EQU          8

;USE_DBGSPT                  EQU          1

    IF      :DEF:USE_DBGSPT
TCB_SP_OFFSET               EQU        124
    ELSE
TCB_SP_OFFSET               EQU        116
    ENDIF

    EXPORT knl_svc_handler
    EXPORT kcpu_schedule_cortexm4

    IMPORT disint
    IMPORT enaint
    IMPORT knl_ready_queue
    IMPORT knl_tcb_table
    IMPORT knl_ctxtsk
    IMPORT knl_dispatch_disabled
    IMPORT kcpu_sleep_cortexm4
    IMPORT knl_svc_ientry
    IMPORT knl_ssycb_table
    IMPORT knl_intvec
    IMPORT knl_taskindp

    INCLUDE schedule_cortexm4.inc
    INCLUDE utk_svc_code.inc

knl_svc_handler
    tst lr, #EXC_RETURN_CHECK_SP
    ite eq
    mrseq r0, msp
    mrsne r0, psp

    ; get the immediate data from the instruction
    ldr r1, [r0, #STACKED_REG_PC]
    ldrb r1, [r1, #OFFSET_OF_IMMEDIATE_DATA]

    cmp r1, #SWI_SVC
    beq knl_svc_handler_call_svc
    cmp r1, #SWI_RETINT
    beq knl_svc_handler_retint
    cmp r1, #SWI_DEBUG
    beq knl_svc_handler_call_dbg
    cmp r1, #SWI_DISPATCH_FROM_TKMAIN
    beq knl_svc_handler_dsp_from_tkmain
    bne knl_svc_handler_other

knl_svc_handler_call_svc
    and r0, ip, #EXTENDED_SVC_CODE
    cmp r0, #EXTENDED_SVC_CODE
    beq call_extended_svc
    ; uTK svc not supported
    bx lr

call_extended_svc
    mrs r0, psp
    mov r1, #4
    add r0, r1
    mov r1, ip
    str r1, [r0]

    mrs r0, psp
    mov r1, #24
    add r0, r0, r1
    ldr r1, =knl_svc_ientry
    str r1, [r0]

    bx lr    

knl_svc_handler_retint
    bx lr

knl_svc_handler_call_dbg
    ; not supported
    bx lr

knl_svc_handler_dsp_from_tkmain
    mov lr, #EXC_RETURN_PSP

    ldr r3, _knl_ready_queue
    ldr r1, [r3, #0]
    mov r2, #UTK_SIZE_OF_QUEUE
    mul r1, r1, r2
    add r1, r1, #UTK_SIZE_OF_TOP_PRIORITY

    add r3, r3, r1
    ldr r3, [r3, #0]

    ldr r1, _knl_ctxtsk
    str r3, [r1]

    ldr r0, _knl_ctxtsk
    ldr r0, [r0]
    add r0, #TCB_SP_OFFSET
    ldr r0, [r0]
    add r0, #32
    msr psp, r0
    bx lr

knl_svc_handler_other
; Y.ishioka
;    sub r1, #120
    mov r0, #4
    mul r0, r0, r1
    ldr r2, _knl_intvec
    add r2, r0
    ldr r1, [r2]

    ; Enter task independent part
    ldr r3, _knl_taskindp
    ldr r2,[r3]
    add r2, #1
    str r2, [r3, #0]

	mov r3, #4
	sdiv r0, r3
; Y.ishioka
;	add r0, #120
    blx r1        
    mov lr, #EXC_RETURN_PSP

    ; Leave task independent part
    ldr r3, _knl_taskindp
    ldr r2,[r3]
    sub r2, #1
    str r2, [r3, #0]

    bx lr

kcpu_schedule_cortexm4

    ; disable interrupt.
    push {r0, lr}
    ldr r2, =disint
    blx r2
    pop {r0, lr}

    ; is dispatch disable ?
    ldr r0, _knl_dispatch_disabled
    ldrb r1, [r0]
    cmp r1, #1
    beq kcpu_schedule_cortexm4_skip_preemption


    ; check current tsk.
    ldr r0, _knl_ctxtsk
    ldr r1, [r0]
    cmp r1, #0
    beq kcpu_schedule_cortexm4_skip

    ; save current task context
    mrs r0, psp
    stmfd r0!, {r4-r11}
    add r1, #TCB_SP_OFFSET
    str r0, [r1, #0]
kcpu_schedule_cortexm4_skip

    ; clear knl_ctxtsk
    ldr r0, _knl_ctxtsk
    ldr r3, =0
    str r3, [r0]

kcpu_cortexm4_wait_start
    ; set top priority TCB to current context
    ldr r3, _knl_ready_queue
    ldr r1, [r3, #0]
    cmp r1, #0x20
    bne kcpu_cortexm4_wait_end

    push {lr}
    ldr r1, =kcpu_sleep_cortexm4
    blx r1
    pop {lr}
    b kcpu_cortexm4_wait_start 

kcpu_cortexm4_wait_end
    mov r0, #UTK_SIZE_OF_QUEUE
    mul r1, r1, r0
    add r1, r1, #UTK_SIZE_OF_TOP_PRIORITY
    add r3, r3, r1
    ldr r3, [r3, #0]
    ldr r2, _knl_ctxtsk
    str r3, [r2, #0]

    mov r0, r3
    ldr r0, [r0, #TCB_SP_OFFSET]
    msr psp, r0
    ldmfd r0!, {r4-r11}
    msr psp, r0

    ; enable interrupts with low priority
kcpu_schedule_cortexm4_skip_preemption
    push {r0, lr}
    mov r0, #0
    ldr r1, =enaint
    blx r1
    pop {r0, lr}

    bx lr

    ALIGN 4

_knl_ready_queue        DCD knl_ready_queue
_knl_ctxtsk             DCD knl_ctxtsk
_knl_dispatch_disabled  DCD knl_dispatch_disabled
_knl_ssycb_table        DCD knl_ssycb_table
_knl_intvec             DCD knl_intvec
_knl_taskindp           DCD knl_taskindp


    END
