;;; ATMega Thread context save/restore
;;;
;;; Tim Walls <tim.walls@snowgoons.com>
;;; Copyright (c) 2022, ALl Rights Reserved
;;;
;;; Use general-purpose-IO-register 1 and 2 to point to the current
;;; thread context data structure
;;;
;;;
;;; Generic implementation that should work for AVR microcontrollers
;;; without the extend addressing registers (RAMP etc.)
;;;
.section .text.context_S
;;; #define the following:
;;; CURRENT_CONTEXT_LO - GPIO register storing low byte of current context pointer
;;; CURRENT_CONTEXT_HI - GPIO register storing hi byte of current context pointer
;;; CONTEXT_FLAGS - GPIO register storing flag bits for context system
;;; SREG - status register address
;;; SPH - stack pointer hi byte address
;;; SPL - stack pointer lo byte address
.global save_thread_context
.type save_thread_context, @function
;;;
;;; Save the current thread context into the datastructure pointed to
;;; by IO registers (CURRENT_CONTEXT_HI,LO)
;;;
;;;
save_thread_context:
;; First stash the Z register
push r31
push r30
;; Also push r29 to use as scratch
push r29
;; Now load context data structure address into Z
in r30, CURRENT_CONTEXT_LO
in r31, CURRENT_CONTEXT_HI
;; Important! We must be sure any instructions before this point don't
;; modify SREG
;; Store the status register in the first byte
in r29, SREG
st Z,r29
;; Now store registers 0 up to 29
std Z+1,r0
std Z+2,r1
std Z+3,r2
std Z+4,r3
std Z+5,r4
std Z+6,r5
std Z+7,r6
std Z+8,r7
std Z+9,r8
std Z+10,r9
std Z+11,r10
std Z+12,r11
std Z+13,r12
std Z+14,r13
std Z+15,r14
std Z+16,r15
std Z+17,r16
std Z+18,r17
std Z+19,r18
std Z+20,r19
std Z+21,r20
std Z+22,r21
std Z+23,r22
std Z+24,r23
std Z+25,r24
std Z+26,r25
std Z+27,r26
std Z+28,r27
std Z+29,r28
;; Store registers 29,30,31 by popping them off the stack
pop r29
std Z+30,r29
pop r29
std Z+31,r29
pop r29
std Z+32,r29
;; OK, at this point we have all the registers stored. Next step
;; is to store the program counter - which just happens to be our
;; return address, also on the stack (in reverse order)
pop r29
std Z+34,r29 ;; pch
pop r29
std Z+33,r29 ;; pcl
;; The stack pointer now is at the top of the stack when execution
;; resumes from the given SP, and that is what we store
in r29,SPL
std Z+35,r29
in r29,SPH
std Z+36,r29
;; We now need to push the return address back onto the stack
ldd r29,Z+33 ;; pcl
push r29
ldd r29,Z+34 ;; pch
push r29
;; Now also restore r29,r30,r31 by first pushing them on the stack,
;; and then popping them off (because once I start restoring them,
;; Z will no longer be valid)
ldd r29,Z+30 ;; r29
push r29
ldd r29,Z+31 ;; r30
push r29
ldd r29,Z+32 ;; r31
push r29
pop r31
pop r30
pop r29
;; It is as if we were never here. Now we can return.
;; We clear bit 0 of the CONTEXT_FLAGS GPIO register to indicate
;; to the caller that we saved the context. This allows the caller
;; to differentiate between return from context saved and return from
;; context restored
cbi CONTEXT_RESTORE_FLAG
ret
;; Restore the context of a thread. The address of the context
;; to restore is expected to be in NEXT_THREAD_CONTEXT
.section .noinit.context_S
.global __NEXT_THREAD_CONTEXT
__NEXT_THREAD_CONTEXT: .space 2
.section .text.context_S
.global restore_thread_context
.type restore_thread_context, @function
restore_thread_context:
cli
;; Clear all our flags
ldi r29,0x00
out CONTEXT_FLAGS,r29
#ifdef HAS_INTERRUPT_CONTROLLER
lds r29, CPUINT_STATUS
sbrc r29,0 ;; If bit 0 is set, we are inside an ISR
sbi CONTEXT_RETI_FLAG ;; We set bit 1 of CONTEXT_FLAGS so we remember
;; OK, so, if bit 1 of context flags is set, we are in an ISR
;; we remember this so we know if we need to execute a reti or a ret
#else
;; If there's no interrupt controller, we can ignore this and just
;; always assume reti will do what we want
sbi CONTEXT_RETI_FLAG
#endif
;; Load address of thread context to restore into Z
lds r30, __NEXT_THREAD_CONTEXT
lds r31, __NEXT_THREAD_CONTEXT+1
;; Ensure the *current* thread context pointer is updated
out CURRENT_CONTEXT_LO,r30
out CURRENT_CONTEXT_HI,r31
;; Load and restore the status register
ld r29,Z
;; We must be careful not to accidentally re-enable interrupts
;; in the middle of our restore routine, so we mask off the top
;; bit - but we also set a reminder that we will need to
;; re-enable them later
sbrc r29,7
sbi CONTEXT_ENABLEINTS_FLAG
cbr r29,0x80 ;; Clear the interrupt-enable bit now,
;; but set a flag to remind us to enable them later
out SREG,r29
;; WARNING, WARNING WILL ROBINSON
;; NONE of the instructions following this point should touch the
;; status register!
;;
;; Safe opcodes: `ldd`,`out`,`push`,`pop`,`sbi`,`sbic`,`reti`,`ret`
;; Restore registers 0 through 28
ldd r0,Z+1
ldd r1,Z+2
ldd r2,Z+3
ldd r3,Z+4
ldd r4,Z+5
ldd r5,Z+6
ldd r6,Z+7
ldd r7,Z+8
ldd r8,Z+9
ldd r9,Z+10
ldd r10,Z+11
ldd r11,Z+12
ldd r12,Z+13
ldd r13,Z+14
ldd r14,Z+15
ldd r15,Z+16
ldd r16,Z+17
ldd r17,Z+18
ldd r18,Z+19
ldd r19,Z+20
ldd r20,Z+21
ldd r21,Z+22
ldd r22,Z+23
ldd r23,Z+24
ldd r24,Z+25
ldd r25,Z+26
ldd r26,Z+27
ldd r27,Z+28
ldd r28,Z+29
;; Skip over 29,30,31 until we can use the stack (after restoring SP)
;; Now restore the stack pointer
ldd r29,Z+35
out SPL,r29
ldd r29,Z+36
out SPH,r29
;; OK, we changed the SP to be the stack we are going to fall back
;; into. We go backward a bit and now restore the PC (by pushing
;; it onto our new stack so we can RET to it), and then the last three
;; registers
ldd r29,Z+33 ;; pcl
push r29 ;; pcl on the stack
ldd r29,Z+34 ;; pch
push r29 ;; pch on the stack
;; The SP is now ready for us to return. We just borrow a few bytes
;; of stack to let us restore our remaining registers before we do it
ldd r29,Z+30 ;; r29
push r29
ldd r29,Z+31 ;; r30
push r29
ldd r29,Z+32 ;; r31
push r29
;; Now we are done with Z, we can restore the last three registers
;; from what we just pushed onto the stack
pop r31
pop r30
pop r29
;; All registers are restored, the return address is on the stack,
;; all we need to do now is set bit 0 of CONTEXT_RESTORE_FLAG - so
;; the caller can identify a return from loaded context from a
;; return from save context - and call return
sbi CONTEXT_RESTORE_FLAG
;; The caller (of save_context actually, rather than restore_context)
;; can now make a decision on whether to re-enable interrupts based
;; one the context flags
ret