!to "threading.prg",cbm
!cpu 6510
;!source "mylib.a"
!macro basic_header .a, .b, .c, .d {
        *= $0801
        !byte <.eol,>.eol,0,0,$9e
        !text .a, .b, .c, .d
.eol:   !byte 0,0,0
}

num_threads = 2
thread_num = $fd		; current thread number

;--------------------------------------------------------------------------
+basic_header "2", "0", "6", "1"

*= $080d

init:	sei
		; set up context switch IRQ
		lda #$35
		sta $01

		lda #<context_switch
		ldx #>context_switch
		sta $fffe
		stx $ffff
		
		lda #0
		sta thread_num

		cli
		jmp thread1

;--------------------------------------------------------------------------
context_switch:
		pha
		txa
		pha
		tya
		pha
		lda $dc0d

		; save current thread
		lda thread_num
		; *8
		asl
		asl
		asl
		tay
		; save A,X,Y
		pla
		sta thread_data+6,y
		pla
		sta thread_data+5,y
		pla
		sta thread_data+4,y
		; save PSW
		pla
		sta thread_data+1,y
		; save PC
		pla
		sta thread_data+2,y
		pla
		sta thread_data+3,y
		; save SP
		tsx
		txa
		sta thread_data,y

		; next thread, wraparound
		ldy thread_num
		iny
		cpy #num_threads
		bne +
		ldy #0
+		sty thread_num

		; *8
		tya
		asl
		asl
		asl
		tay

		; restore thread data
		; stack pointer first
		lda thread_data,y
		tax
		txs
		; push PC, PSW for RTI
		lda thread_data+3,y
		pha
		lda thread_data+2,y
		pha
		lda thread_data+1,y
		pha
		; push registers
		lda thread_data+6,y
		pha
		lda thread_data+5,y
		pha
		lda thread_data+4,y
		pha

		pla
		tay
		pla
		tax
		pla
		rti
	
;--------------------------------------------------------------------------
thread1:
		inc $d021
		ldy #$02
		jsr wait2
		jmp thread1


thread2:
		inc $d020
		ldy #$80
		jsr wait2
		jmp thread2
		
wait2:
-		ldx #0
		dex
		bne *-1
		dey
		bne -
		rts

;--------------------------------------------------------------------------
thread_data:
	!fill 8, 0
	!byte $ff-$40, $22, <thread2, >thread2, 0,0,0,0