[z180-stamp-cpm3.git] / cbios / misc.180


	global add_hla,div32_16,div32_r
	global ioiniml,ioini1l
	global intinit
	global bufinit
	global cpu_frq

	global fifolst

	extrn ?pmsg
	extrn msg.sm,msg.recv,hwl2phy

	include config.inc
	include	z180reg.inc


;--------------------------------------------------------------------
; add a to hl
;
;    return:
;       hl = hl + a
;       Flags undefined

	cseg	; (common. TODO: check for banked)

add_hla:
	add	a,l
	ld	l,a
	ret	nc
	inc	h
	ret

;--------------------------------------------------------------------
; rounded div 32 by 16 bit
;
;	HLDE:	Dividend (x)
;	BC:	Divisor  (y)
;    return:
;	HLDE: 	Rounded Quotient (q)
;	BC:   	Remainder	       (r)

	dseg	;banked.

div32_r:
	push bc
	srl	b		;y/2
	rr	c
	add	hl,bc		;low x + y/2
	pop	bc
	jr	nc,div_r1
	inc	hl
div_r1:
	;fall thru

;--------------------------------------------------------------------
; Divide 32 bit by 16
;
;    DEHL:	Dividend (x)
;    BC:	Divisor  (y)
;
;  return:
;    DEHL:	Quotient
;    BC:	Reminder

	dseg	; same as above!
div32_16:
	exx			;low
	push	de		;save alternate registers (de,bc)
	push	bc
	exx			;high
	push	hl		;lx
	push	bc		;ly
	ld	bc,0		;bc  = hy = 0
	ld	h,b		;hl  = hr = 0
	ld	l,c
				;de = x, hl = r
	exx			;low
	pop	bc		;bc' = ly
	ex	(sp),hl		;hl' = lx, save alternate hl
	ld	de,0		;de' = lr = 0
	ex	de,hl		;de = x, hl = r
	exx			;high
	ld	a,32		;count
;
; start:
;     de: x	(de: hx, de': lx)
;     bc: y	(bc: hy, bc': ly)
;     hl: 0
;
div_lp:				;do
	exx			;    low
	ex	de,hl		;    x
	add	hl,hl		;    x <<= 1
	exx			;    high
	ex	de,hl		;    x
	adc	hl,hl		;    x <<= 1
	exx			;    low
	ex	de,hl		;    r
	adc	hl,hl		;    r <<= 1
	exx			;    high
	ex	de,hl		;    r
	adc	hl,hl		;    r <<= 1
	exx			;    low
	inc	de		;    x/q += 1
	or	a		;
	sbc	hl,bc		;
	exx			;    high
	sbc	hl,bc		;
	jr	nc,div_no_restore
	exx			;        low
	dec	de		;
	add	hl,bc		;        r += y
	exx			;        high
	adc	hl,bc		;

div_no_restore:			;
	dec	a		;
	jr	nz,div_lp	;while (--count)

; result:
;     de: q	(de: hq, de': lq)
;     hl: r	(hl: hr, hl': lr)

	exx			;low
	ex	de,hl		;hl = lq,  de = lr

	ex	(sp),hl		;lq
	push	de		;lr
	exx			;high
	pop	bc		;bc = lr
	pop	hl		;de = lq

	exx			;low
	pop	bc		;restore alternate registers
	pop	de
	exx			;high
	ret

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

	; output bytes to consecutive portaddresses
	;
	;     hl: table with following structure:
	;        db n, port1, val1, val2,... valn
	;        db m, port1, val1, val2,... valm
	;        ...
	;        db 0 ; Terminate table


	cseg	; (common for now)

ioiniml:
	push	bc
	xor	a
ioml_lp:
	ld	b,(hl)
	inc	hl
	cp	b
	jr	z,ioml_e

	ld	c,(hl)
	inc	hl
	otimr
	jr	ioml_lp
ioml_e:
	pop	bc
	ret

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

	; output bytes to ports
	;
	;     hl: tables of port,value pairs:
	;         db n, port1,val1, port2,val2,... portn,valn
	;         ...
	;         db 0 ; Terminate table

	cseg	; (common for now)

ioini1l:
	push	bc
	jr	io1_nxt
io1_lp:
	ld	c,(hl)		;port address
	inc	hl
	otim
	jr	nz,io1_lp
io1_nxt:
	ld	b,(hl)		;count
	inc	hl
	inc	b
	djnz	io1_lp

	pop	bc
	ret

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

	dseg

intinit:
	ld	hl,ivtab	;
	ld	a,h		;
	ld	i,a		;
	out0	(il),l		;
	im	2

; Let all vectors point to spurious int routines.

	ld	de,sp.int0
	ld	bc,sp.int.len
	ld	a,9
ivt_i1:
	ld	(hl),e
	inc	l
	ld	(hl),d
	inc	l
	ex	de,hl
	add	hl,bc
	ex	de,hl
	dec	a
	jr	nz,ivt_i1
	ret


;--------------------------------------------------------------------
; Spurious interrupt handler

	cseg	; common area
sp.int0:
	ld	a,00h
	jr	sp.i.1
sp.int.len equ	$-sp.int0
	ld	a,01h
	jr	sp.i.1
	ld	a,02h
	jr	sp.i.1
	ld	a,03h
	jr	sp.i.1
	ld	a,04h
	jr	sp.i.1
	ld	a,05h
	jr	sp.i.1
	ld	a,06h
	jr	sp.i.1
	ld	a,07h
	jr	sp.i.1
	ld	a,08h
sp.i.1:
;	out	(80h),a

	add	a,'0'
	ld	(spi$nr),a
	ld	hl,spi$msg
	call	?pmsg
sp.i.2:
	halt
	jr	sp.i.2

spi$msg:
	db	13,10,'Spurious Int: '
spi$nr:	db	'0'
	db	0

;--------------------------------------------------------------------
;
; Get/compute CPU clock
;
;    return:
;	hlde:	CPU frequency (Hz)
;

	dseg

cpu_frq:
	ld	hl,0
	ld	d,h
	ld	e,l
	call	get_timer
	push	de
	push	hl

; delay  ~8ms @ 18.432MHz  --> 147456 clock cycles
; delay ~10ms @ 18.432MHz  --> 184320 clock cycles
;

;	ld	hl,8192		; 147456/18
	ld	hl,10240	; 184320/18
	ld	de,1
	or	a
dly_lp:
	sbc	hl,de		; 10
	jr	nz,dly_lp	; 6/8  -> 18

	pop	hl
	pop	de
	call	get_timer

	ld	bc,10
	call	div32_r
	ld	b,h
	ld	c,l
	ld	de,00119h	;18432000/(2**16)
	ld	hl,04000h	;18432000%(2**16)

	ld	a,b
	or	a
	jr	nz,cpuf_div
	ld	a,c
	cp	2
	jr	c,cpuf_done
cpuf_div:
	call	div32_r
cpuf_done:
	ret

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

get_timer:
	push	de
	push	hl
	ld	hl,1*256 + 3		; d = subcommand, e = command
	push	hl
	ld	hl,0
	add	hl,sp
	ld	b,6
	call	msg.sm

	dec	sp
	ld	hl,0
	add	hl,sp
	ld	b,7			; max receive message len
	call	msg.recv

	pop	bc
	inc	sp
	pop	hl
	pop	de
	ret

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

	cseg

fifolst:
	rept 4
	 dw	0
	 db	0
	endm

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

	dseg

bufinit:
	ld	c,a
	ld	a,(ix+o.id)
	cp	4
	jr	nc,bfi_doit2

	ld	hl,fifolst
	ld	e,a
	ld	d,0
	add	hl,de
	add	hl,de
	add	hl,de

   if 0
	ld	e,(hl)
	inc	hl
	ld	d,(hl)
	dec	hl
	ld	a,e
	or	d
	jr	z,bfi_doit

; TODO: address translation
	push	de
	pop	ix
	ret
   endif

bfi_doit:
	push	ix
	pop	de
; TODO: address translation
	ld	(hl),e
	inc	hl
	ld	(hl),d
	ld	a,(ix+o.id)
	or	a
	jr	nz,bfi_doit2

	ld	hl,fifolst
	ld	(040h),hl
	ld	(040h+2),a

bfi_doit2:
	ld	(ix+o.in_idx),0		;reset pointers
	ld	(ix+o.out_idx),0
	ld	(ix+o.mask),c		;reset "size"

	push	ix			;get phys. address of fifo
	pop	hl
	call	hwl2phy
	ex	de,hl
	ld	c,a
	ld	a,(ix+o.id)		;fifo id
	or	a			;test if fifo 0
	jr	nz,bfi_1

	out	(AVRINT5),a
	ret

bfi_1:
	ld	hl,bfi_msg_end
	ld	(hl),c
	dec	hl
	ld	(hl),d
	dec	hl
	ld	(hl),e
	dec	hl
	ld	(hl),a
	dec	hl
	ld	b,bfi_msg_len
	call	msg.sm
	ret

bfi_msg:
	db	0			;function: 0
	db	0			;subfunc: gets overwritten with buf id
	dw	0			;physical
	db	0			;   address
bfi_msg_len	equ $ - bfi_msg
bfi_msg_end	equ $ - 1

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


	end
Commit	Line	Data
ea5293bb	1
43bb4ff6	2	global add_hla,div32_16,div32_r
ea5293bb L	3	global ioiniml,ioini1l
	4	global intinit
	5	global bufinit
f80331a6	6	global cpu_frq
ea5293bb L	7
	8	global fifolst
	9
	10	extrn ?pmsg
f80331a6	11	extrn msg.sm,msg.recv,hwl2phy
ea5293bb L	12
	13	include config.inc
	14	include z180reg.inc
	15
43bb4ff6 L	16
43bb4ff6 L	17	;--------------------------------------------------------------------
ea5293bb L	18	; add a to hl
	19	;
	20	; return:
	21	; hl = hl + a
	22	; Flags undefined
	23
	24	cseg ; (common. TODO: check for banked)
f80331a6	25
ea5293bb	26	add_hla:
f80331a6 L	27	add a,l
	28	ld l,a
	29	ret nc
	30	inc h
	31	ret
	32
43bb4ff6 L	33	;--------------------------------------------------------------------
	34	; rounded div 32 by 16 bit
	35	;
	36	; HLDE: Dividend (x)
	37	; BC: Divisor (y)
	38	; return:
	39	; HLDE: Rounded Quotient (q)
	40	; BC: Remainder (r)
	41
f4471ef9	42	dseg ;banked.
43bb4ff6 L	43
	44	div32_r:
	45	push bc
	46	srl b ;y/2
	47	rr c
	48	add hl,bc ;low x + y/2
	49	pop bc
	50	jr nc,div_r1
	51	inc hl
	52	div_r1:
	53	;fall thru
	54
f80331a6	55	;--------------------------------------------------------------------
d12d8b38	56	; Divide 32 bit by 16
f80331a6	57	;
43bb4ff6	58	; DEHL: Dividend (x)
d12d8b38 L	59	; BC: Divisor (y)
	60	;
	61	; return:
43bb4ff6	62	; DEHL: Quotient
d12d8b38	63	; BC: Reminder
f80331a6	64
f4471ef9	65	dseg ; same as above!
f80331a6 L	66	div32_16:
	67	exx ;low
	68	push de ;save alternate registers (de,bc)
	69	push bc
	70	exx ;high
43bb4ff6 L	71	push hl ;lx
	72	push bc ;ly
	73	ld bc,0 ;bc = hy = 0
	74	ld h,b ;hl = hr = 0
	75	ld l,c
	76	;de = x, hl = r
f80331a6	77	exx ;low
43bb4ff6 L	78	pop bc ;bc' = ly
	79	ex (sp),hl ;hl' = lx, save alternate hl
	80	ld de,0 ;de' = lr = 0
f80331a6 L	81	ex de,hl ;de = x, hl = r
	82	exx ;high
	83	ld a,32 ;count
d12d8b38	84	;
43bb4ff6 L	85	; start:
	86	; de: x (de: hx, de': lx)
	87	; bc: y (bc: hy, bc': ly)
	88	; hl: 0
	89	;
f80331a6 L	90	div_lp: ;do
	91	exx ; low
	92	ex de,hl ; x
	93	add hl,hl ; x <<= 1
	94	exx ; high
	95	ex de,hl ; x
	96	adc hl,hl ; x <<= 1
	97	exx ; low
	98	ex de,hl ; r
	99	adc hl,hl ; r <<= 1
	100	exx ; high
	101	ex de,hl ; r
	102	adc hl,hl ; r <<= 1
	103	exx ; low
	104	inc de ; x/q += 1
	105	or a ;
	106	sbc hl,bc ;
	107	exx ; high
	108	sbc hl,bc ;
	109	jr nc,div_no_restore
	110	exx ; low
	111	dec de ;
	112	add hl,bc ; r += y
	113	exx ; high
	114	adc hl,bc ;
	115
d12d8b38 L	116	div_no_restore: ;
d12d8b38 L	117	dec a ;
f80331a6 L	118	jr nz,div_lp ;while (--count)
f80331a6 L	119
43bb4ff6 L	120	; result:
	121	; de: q (de: hq, de': lq)
	122	; hl: r (hl: hr, hl': lr)
	123
f80331a6	124	exx ;low
43bb4ff6	125	ex de,hl ;hl = lq, de = lr
f80331a6	126
43bb4ff6 L	127	ex (sp),hl ;lq
43bb4ff6 L	128	push de ;lr
f80331a6	129	exx ;high
43bb4ff6 L	130	pop bc ;bc = lr
43bb4ff6 L	131	pop hl ;de = lq
f80331a6 L	132
	133	exx ;low
	134	pop bc ;restore alternate registers
	135	pop de
	136	exx ;high
ea5293bb L	137	ret
	138
	139	;----------------------------------------------------------------------
	140
	141	; output bytes to consecutive portaddresses
	142	;
	143	; hl: table with following structure:
	144	; db n, port1, val1, val2,... valn
	145	; db m, port1, val1, val2,... valm
	146	; ...
	147	; db 0 ; Terminate table
	148
	149
	150	cseg ; (common for now)
	151
	152	ioiniml:
	153	push bc
	154	xor a
	155	ioml_lp:
	156	ld b,(hl)
	157	inc hl
	158	cp b
	159	jr z,ioml_e
	160
	161	ld c,(hl)
	162	inc hl
	163	otimr
	164	jr ioml_lp
	165	ioml_e:
	166	pop bc
	167	ret
	168
	169	;----------------------------------------------------------------------
	170
	171	; output bytes to ports
	172	;
	173	; hl: tables of port,value pairs:
	174	; db n, port1,val1, port2,val2,... portn,valn
	175	; ...
	176	; db 0 ; Terminate table
	177
	178	cseg ; (common for now)
	179
	180	ioini1l:
	181	push bc
	182	jr io1_nxt
	183	io1_lp:
	184	ld c,(hl) ;port address
	185	inc hl
	186	otim
	187	jr nz,io1_lp
	188	io1_nxt:
	189	ld b,(hl) ;count
	190	inc hl
	191	inc b
	192	djnz io1_lp
	193
	194	pop bc
	195	ret
	196
	197	;----------------------------------------------------------------------
	198
	199	dseg
	200
201	intinit:
202	ld hl,ivtab ;
203	ld a,h ;
204	ld i,a ;
205	out0 (il),l ;
206	im 2
207
208	; Let all vectors point to spurious int routines.
209
210	ld de,sp.int0
211	ld bc,sp.int.len
212	ld a,9
213	ivt_i1:
214	ld (hl),e
215	inc l
216	ld (hl),d
217	inc l
218	ex de,hl
219	add hl,bc
220	ex de,hl
221	dec a
222	jr nz,ivt_i1
223	ret
224
225
226	;--------------------------------------------------------------------
227	; Spurious interrupt handler
228
229	cseg ; common area
230	sp.int0:
231	ld a,00h
232	jr sp.i.1
233	sp.int.len equ $-sp.int0
234	ld a,01h
235	jr sp.i.1
236	ld a,02h
237	jr sp.i.1
238	ld a,03h
239	jr sp.i.1
240	ld a,04h
241	jr sp.i.1
242	ld a,05h
243	jr sp.i.1
244	ld a,06h
245	jr sp.i.1
246	ld a,07h
247	jr sp.i.1
248	ld a,08h
249	sp.i.1:
250	; out (80h),a
251
252	add a,'0'
253	ld (spi$nr),a
254	ld hl,spi$msg
255	call ?pmsg
256	sp.i.2:
257	halt
258	jr sp.i.2
259
260	spi$msg:
261	db 13,10,'Spurious Int: '
262	spi$nr: db '0'
263	db 0
264
f80331a6 L	265	;--------------------------------------------------------------------
	266	;
	267	; Get/compute CPU clock
	268	;
	269	; return:
	270	; hlde: CPU frequency (Hz)
	271	;
	272
	273	dseg
	274
	275	cpu_frq:
8e627f7e L	276	ld hl,0
	277	ld d,h
	278	ld e,l
f80331a6	279	call get_timer
8e627f7e	280	push de
43bb4ff6	281	push hl
f80331a6 L	282
	283	; delay ~8ms @ 18.432MHz --> 147456 clock cycles
	284	; delay ~10ms @ 18.432MHz --> 184320 clock cycles
	285	;
	286
	287	; ld hl,8192 ; 147456/18
	288	ld hl,10240 ; 184320/18
	289	ld de,1
	290	or a
	291	dly_lp:
	292	sbc hl,de ; 10
	293	jr nz,dly_lp ; 6/8 -> 18
	294
8e627f7e	295	pop hl
43bb4ff6	296	pop de
f80331a6 L	297	call get_timer
f80331a6 L	298
f80331a6	299	ld bc,10
43bb4ff6 L	300	call div32_r
	301	ld b,h
	302	ld c,l
	303	ld de,00119h ;18432000/(2**16)
	304	ld hl,04000h ;18432000%(2**16)
f80331a6 L	305
	306	ld a,b
	307	or a
	308	jr nz,cpuf_div
	309	ld a,c
	310	cp 2
	311	jr c,cpuf_done
	312	cpuf_div:
43bb4ff6	313	call div32_r
f80331a6 L	314	cpuf_done:
	315	ret
	316
	317	;--------------------------------------------------------------------
	318
	319	get_timer:
8e627f7e	320	push de
43bb4ff6	321	push hl
8e627f7e L	322	ld hl,1*256 + 3 ; d = subcommand, e = command
	323	push hl
	324	ld hl,0
	325	add hl,sp
	326	ld b,6
f80331a6 L	327	call msg.sm
f80331a6 L	328
8e627f7e L	329	dec sp
	330	ld hl,0
	331	add hl,sp
	332	ld b,7 ; max receive message len
	333	call msg.recv
f80331a6	334
8e627f7e L	335	pop bc
8e627f7e L	336	inc sp
8e627f7e	337	pop hl
43bb4ff6	338	pop de
8e627f7e	339	ret
ea5293bb L	340
	341	;--------------------------------------------------------------------
	342
	343	cseg
	344
	345	fifolst:
	346	rept 4
	347	dw 0
	348	db 0
	349	endm
	350
	351	;--------------------------------------------------------------------
	352
	353	dseg
	354
	355	bufinit:
	356	ld c,a
	357	ld a,(ix+o.id)
	358	cp 4
	359	jr nc,bfi_doit2
	360
	361	ld hl,fifolst
	362	ld e,a
	363	ld d,0
	364	add hl,de
	365	add hl,de
	366	add hl,de
	367
5f701f3a	368	if 0
ea5293bb L	369	ld e,(hl)
	370	inc hl
	371	ld d,(hl)
	372	dec hl
	373	ld a,e
	374	or d
	375	jr z,bfi_doit
	376
	377	; TODO: address translation
	378	push de
	379	pop ix
	380	ret
5f701f3a	381	endif
ea5293bb L	382
	383	bfi_doit:
	384	push ix
	385	pop de
	386	; TODO: address translation
	387	ld (hl),e
	388	inc hl
	389	ld (hl),d
	390	ld a,(ix+o.id)
	391	or a
	392	jr nz,bfi_doit2
	393
	394	ld hl,fifolst
	395	ld (040h),hl
	396	ld (040h+2),a
	397
	398	bfi_doit2:
	399	ld (ix+o.in_idx),0 ;reset pointers
	400	ld (ix+o.out_idx),0
	401	ld (ix+o.mask),c ;reset "size"
	402
	403	push ix ;get phys. address of fifo
	404	pop hl
	405	call hwl2phy
	406	ex de,hl
	407	ld c,a
	408	ld a,(ix+o.id) ;fifo id
	409	or a ;test if fifo 0
	410	jr nz,bfi_1
	411
	412	out (AVRINT5),a
	413	ret
	414
	415	bfi_1:
	416	ld hl,bfi_msg_end
	417	ld (hl),c
	418	dec hl
	419	ld (hl),d
	420	dec hl
	421	ld (hl),e
	422	dec hl
	423	ld (hl),a
	424	dec hl
	425	ld b,bfi_msg_len
	426	call msg.sm
	427	ret
	428
	429	bfi_msg:
	430	db 0 ;function: 0
	431	db 0 ;subfunc: gets overwritten with buf id
	432	dw 0 ;physical
	433	db 0 ; address
	434	bfi_msg_len equ $ - bfi_msg
	435	bfi_msg_end equ $ - 1
	436
	437	;----------------------------------------------------------------------
	438
	439
	440	end