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


	global add_hla,div32_16
	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

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

	cseg	; common area
div32_16:
	exx			;low
	push	de		;save alternate registers (de,bc)
	push	bc
	exx			;high
	push	de		;low x
	push	bc		;low y
	ld	bc,0		;bc  = high y = 0
	ld	d,b		;de  = high r = 0
	ld	e,c
	ex	de,hl		;de = x, hl = r
	exx			;low
	pop	bc		;bc' = low y
	ex	(sp),hl		;hl' = low x, save alternate hl
	ld	de,0		;de' = low r = 0
	ex	de,hl		;de = x, hl = r
	exx			;high
	ld	a,32		;count
;
; divide x/y
;     x: hl
;     y: de
;   result:
;     x/y: hl
;     rem: de
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)

	ex	de,hl		; q
	exx			;low
	ex	de,hl		; q

	ex	(sp),hl		;low q
	push	de		;low r
	exx			;high
	pop	bc		;bc = r
	pop	de		;de = low q

	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	hl
	push	de

; 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	de
	pop	hl
	call	get_timer

;	ld	hl,4		; round
	ld	hl,5		; round
	add	hl,de
	ex	de,hl
	ld	hl,0
	ld	bc,10
	call	div32_16
	ld	b,d
	ld	c,e
	ld	hl,00119h	;18432000/(2**16)
	ld	de,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_16
cpuf_done:
	ret

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

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