corelib: revert PUTPIXEL changes

- changes to corelib made sdcard i/o unstable for
  unknown reasons and the performance improvement
  for PUTPIXEL was only about 10%

lib/corelib.s

@@ -706,32 +706,108 @@ CMPWORDS_XT2:
 	.EQU FB_PS	$90C
 	.EQU FB_PD	$910
 	.EQU FB_CTL	$914
-	.EQU FB_SHIFTER $918
+; set a pixel in fb memory
-	.EQU FB_SHIFTCOUNT $91C
+; parameters: x,y - coordinates
-	.EQU FB_SHIFTERM $920
+PUTPIXEL_1BPP:
-	.EQU FB_SHIFTERSP $924
+	; calculate vmem address:
-	.EQU FB_MASKGEN $928
+	OVER ; duplicate x
+	; divide x by 32
+	SHR
+	SHR
+	SHR
+	SHR
+	SHR
+	SWAP
+	; multiply y by words per line
+	SHL 2
+	SHL 2
+	SHL
 
-; draw a single pixel
+	ADD ; add results together for vmem addr
-; args: x, y, color
 
+	DUP
+	LOADCP FB_WA
+	SWAP
+	STOREI	; store to framebuffer write addr register
+	DROP
+	LOADCP FB_RA ; and to framebuffer read addr register
+	SWAP
+	STOREI
+	DROP
+
+	; x is now at top of stack
+	; get bit value from x modulo 32
+	LOADC 31
+	AND
+	SHL 2 ; (x & 31) * 4 = offset into table
+	LOADCP INT_TO_PIX_TABLE
+	ADD
+	LOADI
+
+	LOADCP FB_IO
+	; read old vmem value
+	LOADCP FB_IO
+	LOADI
+	; or in new bit
+	OR
+	; write new value
+	STOREI
+	DROP
+
+	RET
+
+INT_TO_PIX_TABLE:
+	.WORD %10000000_00000000_00000000_00000000
+	.WORD %01000000_00000000_00000000_00000000
+	.WORD %00100000_00000000_00000000_00000000
+	.WORD %00010000_00000000_00000000_00000000
+	.WORD %00001000_00000000_00000000_00000000
+	.WORD %00000100_00000000_00000000_00000000
+	.WORD %00000010_00000000_00000000_00000000
+	.WORD %00000001_00000000_00000000_00000000
+	.WORD %00000000_10000000_00000000_00000000
+	.WORD %00000000_01000000_00000000_00000000
+	.WORD %00000000_00100000_00000000_00000000
+	.WORD %00000000_00010000_00000000_00000000
+	.WORD %00000000_00001000_00000000_00000000
+	.WORD %00000000_00000100_00000000_00000000
+	.WORD %00000000_00000010_00000000_00000000
+	.WORD %00000000_00000001_00000000_00000000
+	.WORD %00000000_00000000_10000000_00000000
+	.WORD %00000000_00000000_01000000_00000000
+	.WORD %00000000_00000000_00100000_00000000
+	.WORD %00000000_00000000_00010000_00000000
+	.WORD %00000000_00000000_00001000_00000000
+	.WORD %00000000_00000000_00000100_00000000
+	.WORD %00000000_00000000_00000010_00000000
+	.WORD %00000000_00000000_00000001_00000000
+	.WORD %00000000_00000000_00000000_10000000
+	.WORD %00000000_00000000_00000000_01000000
+	.WORD %00000000_00000000_00000000_00100000
+	.WORD %00000000_00000000_00000000_00010000
+	.WORD %00000000_00000000_00000000_00001000
+	.WORD %00000000_00000000_00000000_00000100
+	.WORD %00000000_00000000_00000000_00000010
+	.WORD %00000000_00000000_00000000_00000001
+
+PUTMPIXEL:
+	LOADC 1
+; set a pixel in fb memory
+; parameters: x,y,color - coordinates, color value (0-15)
 PUTPIXEL:
 PUTPIXEL_4BPP:
 	.EQU PUTPIXEL_X 0
 	.EQU PUTPIXEL_Y 4
 	.EQU PUTPIXEL_COLOR 8
-	.EQU PUTPIXEL_BPSAV 12
+	.EQU PUTPIXEL_PIXPOS 12
 	.EQU PUTPIXEL_FS 16
 
 	FPADJ -PUTPIXEL_FS
+
 	STORE PUTPIXEL_COLOR
 	STORE PUTPIXEL_Y
 	STORE PUTPIXEL_X
 
-	LOADREG BP
-	STORE PUTPIXEL_BPSAV
-	LOADC 0
-	STOREREG BP
 
 	; calculate vmem address: (x / 8) + (y * 80)
 	LOAD PUTPIXEL_X
@@ -750,36 +826,83 @@ PUTPIXEL_4BPP:
 
 	ADD ; add results together for vmem addr
 
-	DUP
+	LOADCP FB_WA
-	STORE.B FB_WA	; set as write and read addresses
+	OVER
-	STORE.B FB_RA
+	STOREI	; store to framebuffer write addr register
+	DROP
+	LOADCP FB_RA ; and to framebuffer read addr register
+	SWAP	; swap addr and value for STOREI
+	STOREI
+	DROP
+
+	LOAD PUTPIXEL_X
+	;		|0000.0000|0000.0000|0000.0000|0000.1111|
+	LOADC 7
+	AND		; calculate pixel position in word
+	LOADC 7
+	SWAP
+	SUB		; pixpos = 7 - (x & 7)
+	STORE PUTPIXEL_PIXPOS
 
-	; create pixel data from color value in
-	; leftmost pixel data bits (31-28)
-	LOADC 0
 	LOAD PUTPIXEL_COLOR
-	BPLC
+	LOAD PUTPIXEL_PIXPOS
+	SHR		; rcount = pixpos / 2
+ROTLOOP_:
+	DUP		; exit loop if rcount is 0
+	CBRANCH.Z ROTLOOP_END
+	SWAP		; pixel value is now on top of stack
+	BROT		; value = value << 8
+	SWAP		; rcount is now on top of stack
+	DEC 1		; rcount = rcount - 1
+	BRANCH ROTLOOP_
+ROTLOOP_END:
+	DROP		; drop rcount
+	; shifted pixel value is now at top of stack
+	LOAD PUTPIXEL_PIXPOS
+	LOADC 1
+	AND
+	CBRANCH.Z EVEN_PIXPOS
+	SHL 2	; if pixpos is odd, shift by 4 bits
 	SHL 2
-	SHL 2
+EVEN_PIXPOS:
-	STORE.B FB_SHIFTER	; store pixel into shifter
+	LOAD PUTPIXEL_X
+	; get bit value from x modulo 8
+	LOADC 7
+	AND
+	SHL 2 ; (x & 7) * 4 = offset into table
+	LOADCP INT_TO_MASK_TABLE
+	ADD
+	LOADI
 
-	LOAD PUTPIXEL_X 	; use x coord as shift count
+	; read old vmem value
-	STORE.B FB_SHIFTCOUNT	; writing triggers shifting
+	LOADCP FB_IO
+	LOADI
+	; mask bits
+	AND
+	; or in shifted pixel value
+	OR
 
-	LOAD.B FB_SHIFTERM	; get shift result as mask
+	; write new value
-	LOAD.B FB_IO		; get background pixel data
+	LOADCP FB_IO
-	AND			; remove bits for new pixel from bg
+	SWAP
-
+	STOREI
-	LOAD.B FB_SHIFTER	; load shifted pixel
+	DROP
-	OR			; OR in new pixel bits
-	STORE.B FB_IO		; write new pixel data word to vmem
-
-	LOAD PUTPIXEL_BPSAV
-	STOREREG BP
 
 	FPADJ PUTPIXEL_FS
 	RET
 
+	.CPOOL
+
+INT_TO_MASK_TABLE:
+	.WORD %00001111_11111111_11111111_11111111
+	.WORD %11110000_11111111_11111111_11111111
+	.WORD %11111111_00001111_11111111_11111111
+	.WORD %11111111_11110000_11111111_11111111
+	.WORD %11111111_11111111_00001111_11111111
+	.WORD %11111111_11111111_11110000_11111111
+	.WORD %11111111_11111111_11111111_00001111
+	.WORD %11111111_11111111_11111111_11110000
+
 ; draw a line between two points
 ; parameters: x0, y0, x1, y1, color
 	.EQU DL_X0 0