From 1e0ed7fb062f5c3dd277e0d7b9efd71f48d8d034 Mon Sep 17 00:00:00 2001
From: Alan Cox <alan@linux.intel.com>
Date: Wed, 13 Dec 2017 19:13:00 +0000
Subject: [PATCH] 6502: Fix longmp

This is ugly. For the 6502/6509/65C02 and friends we need to just deal with the
8bit S register. For the 65C816 we have to change the low 8bits of S only, while
keeping interrupts off as we might otherwise get swapped out and change S mid
calculation.

Not always my favourite processor for multi-tasking..
---
 Library/libs/Makefile.6502  |   2 +-
 Library/libs/longjmp_6502.s | 115 ++++++++++++++++++++++++++++++++++++
 2 files changed, 116 insertions(+), 1 deletion(-)
 create mode 100644 Library/libs/longjmp_6502.s

diff --git a/Library/libs/Makefile.6502 b/Library/libs/Makefile.6502
index 7a90b73f..142481c2 100644
--- a/Library/libs/Makefile.6502
+++ b/Library/libs/Makefile.6502
@@ -8,7 +8,7 @@ ASM_OPT = -o
 LINKER_OPT = -o
 SRC_CRT0 = crt0_6502.s crt0nostdio_6502.s
 OBJ_CRT0 = $(SRC_CRT0:.s=.o)
-SRC_ASM =
+SRC_ASM = longjmp_6502.s
 OBJ_ASM = $(SRC_ASM:.s=.o)
 #
 #	Library routines. cc65 has its own library which has some overlap.
diff --git a/Library/libs/longjmp_6502.s b/Library/libs/longjmp_6502.s
new file mode 100644
index 00000000..688a1416
--- /dev/null
+++ b/Library/libs/longjmp_6502.s
@@ -0,0 +1,115 @@
+;
+; 1998-06-06, Ullrich von Bassewitz
+; 2015-09-11, Greg King
+;
+; void __fastcall__ longjmp (jmp_buf buf, int retval);
+;
+; Modified for the Fuzix 65C816 setup
+;
+
+        .export         _longjmp
+        .import         popax
+        .importzp       sp, ptr1, ptr2
+	.importzp	tmp1,tmp2
+
+	.p816
+	.a8
+	.i8
+
+_longjmp:
+        sta     ptr2            ; Save retval
+        stx     ptr2+1
+        ora     ptr2+1          ; Check for 0
+        bne     @L1
+        inc     ptr2            ; 0 is illegal, according to the standard ...
+                                ; ... and, must be replaced by 1
+@L1:    jsr     popax           ; get buf
+        sta     ptr1
+        stx     ptr1+1
+        ldy     #0
+
+; Get the old parameter stack
+
+        lda     (ptr1),y
+        iny
+        sta     sp
+        lda     (ptr1),y
+        iny
+        sta     sp+1
+
+;
+; Detection code based on an idea by Jody Bruchon
+;
+; Party time we have two cases
+;
+; 6502/65C02/65WD02 etc		- S is 8bit
+; 65C816			- S is 16bit and we must merge the saved low
+;				  8 with the current high 8 with IRQs off
+;				  due to the sucky CPU design
+;
+	lda #$00
+	inc			; 65C02 or later will add one
+	cmp #$01
+	bmi is_8bit		; 6502so skip
+;
+;	We can now safely play with xba to see if it's an 816.
+; 
+	xba			; 65c02 the xba's do nothing		
+	dec			; so a goes to 0
+	xba			; while 65C816 keeps it as one
+	cmp #$01
+	bmi is_8bit
+;
+;	16bit mode. Please ensure your barf bucket is to hand
+;
+	sei			; Interrupts off
+	rep #$30
+	.a16
+	.i16
+	tsx			; 16 bit stack pointer into A
+	txa
+	sep #$20
+	.a8
+        lda     (ptr1),y	; Restore low 8bits of SP only
+	rep #$20
+	.a16
+
+        iny			; Move down struct
+
+	tax			; Restore stack pointer 16bits
+	txs
+
+	sep #$30
+	.a8
+	.i8
+
+	cli			; Interrupts back on
+	bra pop_out
+
+;
+; 8bit is nice and simple
+;
+is_8bit:	
+; Get the old stack pointer
+
+        lda     (ptr1),y
+        iny
+        tax
+
+        txs
+
+pop_out:
+
+; Get the return address and push it on the stack
+
+        lda     (ptr1),y
+        iny
+        pha
+        lda     (ptr1),y
+        pha
+
+; Load the return value and return to the caller
+
+        lda     ptr2
+        ldx     ptr2+1
+        rts
-- 
2.34.1