.DATA

.CODE
ALIGN 16



READ_REGS PROC
    ; STORE REGISTERS TO STRUCTURE 
    ; (rcx - pointer to struct)
    mov [rcx],    rax     ; Store rax (Frame)
    mov [rcx+8],  rbx     ; Store rbx
    mov [rcx+24], rbp     ; Store rbp
    mov [rcx+32], rsi     ; Store rsi
    mov [rcx+40], rdi     ; Store rdi
    mov [rcx+48], r12     ; Store r12
    mov [rcx+56], r13     ; Store r13
    mov [rcx+64], r14     ; Store r14
    mov [rcx+72], r15     ; Store r15
     
     
    ; STORE RSP
    mov r11,            rsp     ; Store rsp to r11 (in order to not change actual rsp, because it will use in return)
	add r11,            8       ; when this func was called, rsp was decreased. So increased it back
	mov [rcx+16],       r11     ; store rsp to REGS struct
	mov [rcx+256+8],    r11     ; store rsp to pointer to stack var

	ret
READ_REGS ENDP

;-------------------------------------------------------------------------

RESTORE_CONTEXT_AND_JUMP PROC
    ; SET POINTER TO STACK
    mov         rax,                        [rcx]           ; rcx = &hmcu = &pStackPtr_Origin
    mov         qword ptr [rcx+118h],       rax             ; pStackPtr_Current [rcx+118h] = pStackPtr_Origin [rcx]


    STACK_FILLING: ; code copied from disassembled code
    cmp         dword ptr [rcx+110h],       0               ; compare is Stack_Size[rcx+110h] to the zero
    jl          REGISTERS_FILLING                           ; if StackSize less then zero - exit the while                                      
; SAVE VARIABLES OF STRUCTURE IN REGISTERS                                                                               
    movsxd          rdx,              dword ptr [rcx+110h]      ; rdx = Stack_Size          [rcx+110h]
    mov             rbx,              qword ptr [rcx+118h]      ; rbx = pStackPtr_Current   [rcx+118h]
    lea             r10,                        [rcx+120h]      ; r10 = &Stack_Buff         [rcx+120h]
    mov             rax,              qword ptr [r10+rdx*8]     ; rax = Stack_Buff[n]       [r10+rdx*8] = [&Stack_Buff+Stack_Size*8]    
; SAVE FROM STACK_BUFF TO THE REAL STACK   
    mov   qword ptr [rbx],                      rax             ; *pStackPtr_Current = Stack_Buff[n]
; DECREMENT INDEX AND POINTER
    sub             rbx,                        8               ; pStackPtr_Current--
    mov   qword ptr [rcx+118h],                 rbx             ; pStackPtr_Current = (new)pStackPtr_Current
    dec             rdx                                         ; Stack_Size--
    mov   qword ptr [rcx+110h],                 rdx             ; Stack_Size = (new)Stack_Size

    jmp         STACK_FILLING                                   ; back to comparing
                                                             
                                                             

    REGISTERS_FILLING:    
    ; SET REGISTERS FROM STRUCTURE     
    add rcx, 16             ; (set rcx as pointer to struct REGS)

    mov rax, [rcx]          ; Set rax (Frame)
    mov rbx, [rcx+8]        ; Set rbx
    mov rsp, [rcx+16]       ; Set rsp
    mov rbp, [rcx+24]       ; Set â rbp
    mov rsi, [rcx+32]       ; Set rsi
    mov rdi, [rcx+40]       ; Set rdi
    mov r12, [rcx+48]       ; Set r12
    mov r13, [rcx+56]       ; Set r13
    mov r14, [rcx+64]       ; Set r14
    mov r15, [rcx+72]       ; Set r15
    
	jmp qword ptr [rcx+80]  ; go to MCU app

	ret
RESTORE_CONTEXT_AND_JUMP ENDP

;-------------------------------------------------------------------------

GET_RSP_ORIGIN PROC
    ; STORE RSP
    mov r11,            rsp     ; Store rsp to r11 (in order to not change actual rsp, because it will use in return)
	add r11,            8       ; when this func was called, rsp was decreased. So increased it back
	mov [rcx],          r11     ; store origin rsp to pStackPtr

	ret
GET_RSP_ORIGIN ENDP
	

END