xref: /openbmc/linux/arch/m68k/ifpsp060/src/isp.S (revision ad61dd30)
1~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2MOTOROLA MICROPROCESSOR & MEMORY TECHNOLOGY GROUP
3M68000 Hi-Performance Microprocessor Division
4M68060 Software Package
5Production Release P1.00 -- October 10, 1994
6
7M68060 Software Package Copyright © 1993, 1994 Motorola Inc.  All rights reserved.
8
9THE SOFTWARE is provided on an "AS IS" basis and without warranty.
10To the maximum extent permitted by applicable law,
11MOTOROLA DISCLAIMS ALL WARRANTIES WHETHER EXPRESS OR IMPLIED,
12INCLUDING IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
13and any warranty against infringement with regard to the SOFTWARE
14(INCLUDING ANY MODIFIED VERSIONS THEREOF) and any accompanying written materials.
15
16To the maximum extent permitted by applicable law,
17IN NO EVENT SHALL MOTOROLA BE LIABLE FOR ANY DAMAGES WHATSOEVER
18(INCLUDING WITHOUT LIMITATION, DAMAGES FOR LOSS OF BUSINESS PROFITS,
19BUSINESS INTERRUPTION, LOSS OF BUSINESS INFORMATION, OR OTHER PECUNIARY LOSS)
20ARISING OF THE USE OR INABILITY TO USE THE SOFTWARE.
21Motorola assumes no responsibility for the maintenance and support of the SOFTWARE.
22
23You are hereby granted a copyright license to use, modify, and distribute the SOFTWARE
24so long as this entire notice is retained without alteration in any modified and/or
25redistributed versions, and that such modified versions are clearly identified as such.
26No licenses are granted by implication, estoppel or otherwise under any patents
27or trademarks of Motorola, Inc.
28~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
29# ireal.s:
30#	This file is appended to the top of the 060ISP package
31# and contains the entry points into the package. The user, in
32# effect, branches to one of the branch table entries located
33# after _060ISP_TABLE.
34#	Also, subroutine stubs exist in this file (_isp_done for
35# example) that are referenced by the ISP package itself in order
36# to call a given routine. The stub routine actually performs the
37# callout. The ISP code does a "bsr" to the stub routine. This
38# extra layer of hierarchy adds a slight performance penalty but
39# it makes the ISP code easier to read and more mainatinable.
40#
41
42set	_off_chk,	0x00
43set	_off_divbyzero,	0x04
44set	_off_trace,	0x08
45set	_off_access,	0x0c
46set	_off_done,	0x10
47
48set	_off_cas,	0x14
49set	_off_cas2,	0x18
50set	_off_lock,	0x1c
51set	_off_unlock,	0x20
52
53set	_off_imr,	0x40
54set	_off_dmr,	0x44
55set	_off_dmw,	0x48
56set	_off_irw,	0x4c
57set	_off_irl,	0x50
58set	_off_drb,	0x54
59set	_off_drw,	0x58
60set	_off_drl,	0x5c
61set	_off_dwb,	0x60
62set	_off_dww,	0x64
63set	_off_dwl,	0x68
64
65_060ISP_TABLE:
66
67# Here's the table of ENTRY POINTS for those linking the package.
68	bra.l		_isp_unimp
69	short		0x0000
70
71	bra.l		_isp_cas
72	short		0x0000
73
74	bra.l		_isp_cas2
75	short		0x0000
76
77	bra.l		_isp_cas_finish
78	short		0x0000
79
80	bra.l		_isp_cas2_finish
81	short		0x0000
82
83	bra.l		_isp_cas_inrange
84	short		0x0000
85
86	bra.l		_isp_cas_terminate
87	short		0x0000
88
89	bra.l		_isp_cas_restart
90	short		0x0000
91
92	space		64
93
94#############################################################
95
96	global		_real_chk
97_real_chk:
98	mov.l		%d0,-(%sp)
99	mov.l		(_060ISP_TABLE-0x80+_off_chk,%pc),%d0
100	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
101	mov.l		0x4(%sp),%d0
102	rtd		&0x4
103
104	global		_real_divbyzero
105_real_divbyzero:
106	mov.l		%d0,-(%sp)
107	mov.l		(_060ISP_TABLE-0x80+_off_divbyzero,%pc),%d0
108	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
109	mov.l		0x4(%sp),%d0
110	rtd		&0x4
111
112	global		_real_trace
113_real_trace:
114	mov.l		%d0,-(%sp)
115	mov.l		(_060ISP_TABLE-0x80+_off_trace,%pc),%d0
116	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
117	mov.l		0x4(%sp),%d0
118	rtd		&0x4
119
120	global		_real_access
121_real_access:
122	mov.l		%d0,-(%sp)
123	mov.l		(_060ISP_TABLE-0x80+_off_access,%pc),%d0
124	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
125	mov.l		0x4(%sp),%d0
126	rtd		&0x4
127
128	global		_isp_done
129_isp_done:
130	mov.l		%d0,-(%sp)
131	mov.l		(_060ISP_TABLE-0x80+_off_done,%pc),%d0
132	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
133	mov.l		0x4(%sp),%d0
134	rtd		&0x4
135
136#######################################
137
138	global		_real_cas
139_real_cas:
140	mov.l		%d0,-(%sp)
141	mov.l		(_060ISP_TABLE-0x80+_off_cas,%pc),%d0
142	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
143	mov.l		0x4(%sp),%d0
144	rtd		&0x4
145
146	global		_real_cas2
147_real_cas2:
148	mov.l		%d0,-(%sp)
149	mov.l		(_060ISP_TABLE-0x80+_off_cas2,%pc),%d0
150	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
151	mov.l		0x4(%sp),%d0
152	rtd		&0x4
153
154	global		_real_lock_page
155_real_lock_page:
156	mov.l		%d0,-(%sp)
157	mov.l		(_060ISP_TABLE-0x80+_off_lock,%pc),%d0
158	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
159	mov.l		0x4(%sp),%d0
160	rtd		&0x4
161
162	global		_real_unlock_page
163_real_unlock_page:
164	mov.l		%d0,-(%sp)
165	mov.l		(_060ISP_TABLE-0x80+_off_unlock,%pc),%d0
166	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
167	mov.l		0x4(%sp),%d0
168	rtd		&0x4
169
170#######################################
171
172	global		_imem_read
173_imem_read:
174	mov.l		%d0,-(%sp)
175	mov.l		(_060ISP_TABLE-0x80+_off_imr,%pc),%d0
176	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
177	mov.l		0x4(%sp),%d0
178	rtd		&0x4
179
180	global		_dmem_read
181_dmem_read:
182	mov.l		%d0,-(%sp)
183	mov.l		(_060ISP_TABLE-0x80+_off_dmr,%pc),%d0
184	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
185	mov.l		0x4(%sp),%d0
186	rtd		&0x4
187
188	global		_dmem_write
189_dmem_write:
190	mov.l		%d0,-(%sp)
191	mov.l		(_060ISP_TABLE-0x80+_off_dmw,%pc),%d0
192	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
193	mov.l		0x4(%sp),%d0
194	rtd		&0x4
195
196	global		_imem_read_word
197_imem_read_word:
198	mov.l		%d0,-(%sp)
199	mov.l		(_060ISP_TABLE-0x80+_off_irw,%pc),%d0
200	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
201	mov.l		0x4(%sp),%d0
202	rtd		&0x4
203
204	global		_imem_read_long
205_imem_read_long:
206	mov.l		%d0,-(%sp)
207	mov.l		(_060ISP_TABLE-0x80+_off_irl,%pc),%d0
208	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
209	mov.l		0x4(%sp),%d0
210	rtd		&0x4
211
212	global		_dmem_read_byte
213_dmem_read_byte:
214	mov.l		%d0,-(%sp)
215	mov.l		(_060ISP_TABLE-0x80+_off_drb,%pc),%d0
216	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
217	mov.l		0x4(%sp),%d0
218	rtd		&0x4
219
220	global		_dmem_read_word
221_dmem_read_word:
222	mov.l		%d0,-(%sp)
223	mov.l		(_060ISP_TABLE-0x80+_off_drw,%pc),%d0
224	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
225	mov.l		0x4(%sp),%d0
226	rtd		&0x4
227
228	global		_dmem_read_long
229_dmem_read_long:
230	mov.l		%d0,-(%sp)
231	mov.l		(_060ISP_TABLE-0x80+_off_drl,%pc),%d0
232	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
233	mov.l		0x4(%sp),%d0
234	rtd		&0x4
235
236	global		_dmem_write_byte
237_dmem_write_byte:
238	mov.l		%d0,-(%sp)
239	mov.l		(_060ISP_TABLE-0x80+_off_dwb,%pc),%d0
240	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
241	mov.l		0x4(%sp),%d0
242	rtd		&0x4
243
244	global		_dmem_write_word
245_dmem_write_word:
246	mov.l		%d0,-(%sp)
247	mov.l		(_060ISP_TABLE-0x80+_off_dww,%pc),%d0
248	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
249	mov.l		0x4(%sp),%d0
250	rtd		&0x4
251
252	global		_dmem_write_long
253_dmem_write_long:
254	mov.l		%d0,-(%sp)
255	mov.l		(_060ISP_TABLE-0x80+_off_dwl,%pc),%d0
256	pea.l		(_060ISP_TABLE-0x80,%pc,%d0)
257	mov.l		0x4(%sp),%d0
258	rtd		&0x4
259
260#
261# This file contains a set of define statements for constants
262# in oreder to promote readability within the core code itself.
263#
264
265set LOCAL_SIZE,		96			# stack frame size(bytes)
266set LV,			-LOCAL_SIZE		# stack offset
267
268set EXC_ISR,		0x4			# stack status register
269set EXC_IPC,		0x6			# stack pc
270set EXC_IVOFF,		0xa			# stacked vector offset
271
272set EXC_AREGS,		LV+64			# offset of all address regs
273set EXC_DREGS,		LV+32			# offset of all data regs
274
275set EXC_A7,		EXC_AREGS+(7*4)		# offset of a7
276set EXC_A6,		EXC_AREGS+(6*4)		# offset of a6
277set EXC_A5,		EXC_AREGS+(5*4)		# offset of a5
278set EXC_A4,		EXC_AREGS+(4*4)		# offset of a4
279set EXC_A3,		EXC_AREGS+(3*4)		# offset of a3
280set EXC_A2,		EXC_AREGS+(2*4)		# offset of a2
281set EXC_A1,		EXC_AREGS+(1*4)		# offset of a1
282set EXC_A0,		EXC_AREGS+(0*4)		# offset of a0
283set EXC_D7,		EXC_DREGS+(7*4)		# offset of d7
284set EXC_D6,		EXC_DREGS+(6*4)		# offset of d6
285set EXC_D5,		EXC_DREGS+(5*4)		# offset of d5
286set EXC_D4,		EXC_DREGS+(4*4)		# offset of d4
287set EXC_D3,		EXC_DREGS+(3*4)		# offset of d3
288set EXC_D2,		EXC_DREGS+(2*4)		# offset of d2
289set EXC_D1,		EXC_DREGS+(1*4)		# offset of d1
290set EXC_D0,		EXC_DREGS+(0*4)		# offset of d0
291
292set EXC_TEMP,		LV+16			# offset of temp stack space
293
294set EXC_SAVVAL,		LV+12			# offset of old areg value
295set EXC_SAVREG,		LV+11			# offset of old areg index
296
297set SPCOND_FLG,		LV+10			# offset of spc condition flg
298
299set EXC_CC,		LV+8			# offset of cc register
300set EXC_EXTWPTR,	LV+4			# offset of current PC
301set EXC_EXTWORD,	LV+2			# offset of current ext opword
302set EXC_OPWORD,		LV+0			# offset of current opword
303
304###########################
305# SPecial CONDition FLaGs #
306###########################
307set mia7_flg,		0x04			# (a7)+ flag
308set mda7_flg,		0x08			# -(a7) flag
309set ichk_flg,		0x10			# chk exception flag
310set idbyz_flg,		0x20			# divbyzero flag
311set restore_flg,	0x40			# restore -(an)+ flag
312set immed_flg,		0x80			# immediate data flag
313
314set mia7_bit,		0x2			# (a7)+ bit
315set mda7_bit,		0x3			# -(a7) bit
316set ichk_bit,		0x4			# chk exception bit
317set idbyz_bit,		0x5			# divbyzero bit
318set restore_bit,	0x6			# restore -(a7)+ bit
319set immed_bit,		0x7			# immediate data bit
320
321#########
322# Misc. #
323#########
324set BYTE,		1			# len(byte) == 1 byte
325set WORD,		2			# len(word) == 2 bytes
326set LONG,		4			# len(longword) == 4 bytes
327
328#########################################################################
329# XDEF ****************************************************************	#
330#	_isp_unimp(): 060ISP entry point for Unimplemented Instruction	#
331#									#
332#	This handler should be the first code executed upon taking the	#
333#	"Unimplemented Integer Instruction" exception in an operating	#
334#	system.								#
335#									#
336# XREF ****************************************************************	#
337#	_imem_read_{word,long}() - read instruction word/longword	#
338#	_mul64() - emulate 64-bit multiply				#
339#	_div64() - emulate 64-bit divide				#
340#	_moveperipheral() - emulate "movep"				#
341#	_compandset() - emulate misaligned "cas"			#
342#	_compandset2() - emulate "cas2"					#
343#	_chk2_cmp2() - emulate "cmp2" and "chk2"			#
344#	_isp_done() - "callout" for normal final exit			#
345#	_real_trace() - "callout" for Trace exception			#
346#	_real_chk() - "callout" for Chk exception			#
347#	_real_divbyzero() - "callout" for DZ exception			#
348#	_real_access() - "callout" for access error exception		#
349#									#
350# INPUT ***************************************************************	#
351#	- The system stack contains the Unimp Int Instr stack frame	#
352#									#
353# OUTPUT **************************************************************	#
354#	If Trace exception:						#
355#	- The system stack changed to contain Trace exc stack frame	#
356#	If Chk exception:						#
357#	- The system stack changed to contain Chk exc stack frame	#
358#	If DZ exception:						#
359#	- The system stack changed to contain DZ exc stack frame	#
360#	If access error exception:					#
361#	- The system stack changed to contain access err exc stk frame	#
362#	Else:								#
363#	- Results saved as appropriate					#
364#									#
365# ALGORITHM ***********************************************************	#
366#	This handler fetches the first instruction longword from	#
367# memory and decodes it to determine which of the unimplemented		#
368# integer instructions caused this exception. This handler then calls	#
369# one of _mul64(), _div64(), _moveperipheral(), _compandset(),		#
370# _compandset2(), or _chk2_cmp2() as appropriate.			#
371#	Some of these instructions, by their nature, may produce other	#
372# types of exceptions. "div" can produce a divide-by-zero exception,	#
373# and "chk2" can cause a "Chk" exception. In both cases, the current	#
374# exception stack frame must be converted to an exception stack frame	#
375# of the correct exception type and an exit must be made through	#
376# _real_divbyzero() or _real_chk() as appropriate. In addition, all	#
377# instructions may be executing while Trace is enabled. If so, then	#
378# a Trace exception stack frame must be created and an exit made	#
379# through _real_trace().						#
380#	Meanwhile, if any read or write to memory using the		#
381# _mem_{read,write}() "callout"s returns a failing value, then an	#
382# access error frame must be created and an exit made through		#
383# _real_access().							#
384#	If none of these occur, then a normal exit is made through	#
385# _isp_done().								#
386#									#
387#	This handler, upon entry, saves almost all user-visible		#
388# address and data registers to the stack. Although this may seem to	#
389# cause excess memory traffic, it was found that due to having to	#
390# access these register files for things like data retrieval and <ea>	#
391# calculations, it was more efficient to have them on the stack where	#
392# they could be accessed by indexing rather than to make subroutine	#
393# calls to retrieve a register of a particular index.			#
394#									#
395#########################################################################
396
397	global		_isp_unimp
398_isp_unimp:
399	link.w		%a6,&-LOCAL_SIZE	# create room for stack frame
400
401	movm.l		&0x3fff,EXC_DREGS(%a6)	# store d0-d7/a0-a5
402	mov.l		(%a6),EXC_A6(%a6)	# store a6
403
404	btst		&0x5,EXC_ISR(%a6)	# from s or u mode?
405	bne.b		uieh_s			# supervisor mode
406uieh_u:
407	mov.l		%usp,%a0		# fetch user stack pointer
408	mov.l		%a0,EXC_A7(%a6)		# store a7
409	bra.b		uieh_cont
410uieh_s:
411	lea		0xc(%a6),%a0
412	mov.l		%a0,EXC_A7(%a6)		# store corrected sp
413
414###############################################################################
415
416uieh_cont:
417	clr.b		SPCOND_FLG(%a6)		# clear "special case" flag
418
419	mov.w		EXC_ISR(%a6),EXC_CC(%a6) # store cc copy on stack
420	mov.l		EXC_IPC(%a6),EXC_EXTWPTR(%a6) # store extwptr on stack
421
422#
423# fetch the opword and first extension word pointed to by the stacked pc
424# and store them to the stack for now
425#
426	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
427	addq.l		&0x4,EXC_EXTWPTR(%a6)	# incr instruction ptr
428	bsr.l		_imem_read_long		# fetch opword & extword
429	mov.l		%d0,EXC_OPWORD(%a6)	# store extword on stack
430
431
432#########################################################################
433# muls.l	0100 1100 00 |<ea>|	0*** 1100 0000 0***		#
434# mulu.l	0100 1100 00 |<ea>|	0*** 0100 0000 0***		#
435#									#
436# divs.l	0100 1100 01 |<ea>|	0*** 1100 0000 0***		#
437# divu.l	0100 1100 01 |<ea>|	0*** 0100 0000 0***		#
438#									#
439# movep.w m2r	0000 ***1 00 001***	| <displacement>  |		#
440# movep.l m2r	0000 ***1 01 001***	| <displacement>  |		#
441# movep.w r2m	0000 ***1 10 001***	| <displacement>  |		#
442# movep.l r2m	0000 ***1 11 001***	| <displacement>  |		#
443#									#
444# cas.w		0000 1100 11 |<ea>|	0000 000* **00 0***		#
445# cas.l		0000 1110 11 |<ea>|	0000 000* **00 0***		#
446#									#
447# cas2.w	0000 1100 11 111100	**** 000* **00 0***		#
448#					**** 000* **00 0***		#
449# cas2.l	0000 1110 11 111100	**** 000* **00 0***		#
450#					**** 000* **00 0***		#
451#									#
452# chk2.b	0000 0000 11 |<ea>|	**** 1000 0000 0000		#
453# chk2.w	0000 0010 11 |<ea>|	**** 1000 0000 0000		#
454# chk2.l	0000 0100 11 |<ea>|	**** 1000 0000 0000		#
455#									#
456# cmp2.b	0000 0000 11 |<ea>|	**** 0000 0000 0000		#
457# cmp2.w	0000 0010 11 |<ea>|	**** 0000 0000 0000		#
458# cmp2.l	0000 0100 11 |<ea>|	**** 0000 0000 0000		#
459#########################################################################
460
461#
462# using bit 14 of the operation word, separate into 2 groups:
463# (group1) mul64, div64
464# (group2) movep, chk2, cmp2, cas2, cas
465#
466	btst		&0x1e,%d0		# group1 or group2
467	beq.b		uieh_group2		# go handle group2
468
469#
470# now, w/ group1, make mul64's decode the fastest since it will
471# most likely be used the most.
472#
473uieh_group1:
474	btst		&0x16,%d0		# test for div64
475	bne.b		uieh_div64		# go handle div64
476
477uieh_mul64:
478# mul64() may use ()+ addressing and may, therefore, alter a7
479
480	bsr.l		_mul64			# _mul64()
481
482	btst		&0x5,EXC_ISR(%a6)	# supervisor mode?
483	beq.w		uieh_done
484	btst		&mia7_bit,SPCOND_FLG(%a6) # was a7 changed?
485	beq.w		uieh_done		# no
486	btst		&0x7,EXC_ISR(%a6)	# is trace enabled?
487	bne.w		uieh_trace_a7		# yes
488	bra.w		uieh_a7			# no
489
490uieh_div64:
491# div64() may use ()+ addressing and may, therefore, alter a7.
492# div64() may take a divide by zero exception.
493
494	bsr.l		_div64			# _div64()
495
496# here, we sort out all of the special cases that may have happened.
497	btst		&mia7_bit,SPCOND_FLG(%a6) # was a7 changed?
498	bne.b		uieh_div64_a7		# yes
499uieh_div64_dbyz:
500	btst		&idbyz_bit,SPCOND_FLG(%a6) # did divide-by-zero occur?
501	bne.w		uieh_divbyzero		# yes
502	bra.w		uieh_done		# no
503uieh_div64_a7:
504	btst		&0x5,EXC_ISR(%a6)	# supervisor mode?
505	beq.b		uieh_div64_dbyz		# no
506# here, a7 has been incremented by 4 bytes in supervisor mode. we still
507# may have the following 3 cases:
508#	(i)	(a7)+
509#	(ii)	(a7)+; trace
510#	(iii)	(a7)+; divide-by-zero
511#
512	btst		&idbyz_bit,SPCOND_FLG(%a6) # did divide-by-zero occur?
513	bne.w		uieh_divbyzero_a7	# yes
514	tst.b		EXC_ISR(%a6)		# no; is trace enabled?
515	bmi.w		uieh_trace_a7		# yes
516	bra.w		uieh_a7			# no
517
518#
519# now, w/ group2, make movep's decode the fastest since it will
520# most likely be used the most.
521#
522uieh_group2:
523	btst		&0x18,%d0		# test for not movep
524	beq.b		uieh_not_movep
525
526
527	bsr.l		_moveperipheral		# _movep()
528	bra.w		uieh_done
529
530uieh_not_movep:
531	btst		&0x1b,%d0		# test for chk2,cmp2
532	beq.b		uieh_chk2cmp2		# go handle chk2,cmp2
533
534	swap		%d0			# put opword in lo word
535	cmpi.b		%d0,&0xfc		# test for cas2
536	beq.b		uieh_cas2		# go handle cas2
537
538uieh_cas:
539
540	bsr.l		_compandset		# _cas()
541
542# the cases of "cas Dc,Du,(a7)+" and "cas Dc,Du,-(a7)" used from supervisor
543# mode are simply not considered valid and therefore are not handled.
544
545	bra.w		uieh_done
546
547uieh_cas2:
548
549	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
550	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
551	bsr.l		_imem_read_word		# read extension word
552
553	tst.l		%d1			# ifetch error?
554	bne.w		isp_iacc		# yes
555
556	bsr.l		_compandset2		# _cas2()
557	bra.w		uieh_done
558
559uieh_chk2cmp2:
560# chk2 may take a chk exception
561
562	bsr.l		_chk2_cmp2		# _chk2_cmp2()
563
564# here we check to see if a chk trap should be taken
565	cmpi.b		SPCOND_FLG(%a6),&ichk_flg
566	bne.w		uieh_done
567	bra.b		uieh_chk_trap
568
569###########################################################################
570
571#
572# the required emulation has been completed. now, clean up the necessary stack
573# info and prepare for rte
574#
575uieh_done:
576	mov.b		EXC_CC+1(%a6),EXC_ISR+1(%a6) # insert new ccodes
577
578# if exception occurred in user mode, then we have to restore a7 in case it
579# changed. we don't have to update a7  for supervisor mose because that case
580# doesn't flow through here
581	btst		&0x5,EXC_ISR(%a6)	# user or supervisor?
582	bne.b		uieh_finish		# supervisor
583
584	mov.l		EXC_A7(%a6),%a0		# fetch user stack pointer
585	mov.l		%a0,%usp		# restore it
586
587uieh_finish:
588	movm.l		EXC_DREGS(%a6),&0x3fff	# restore d0-d7/a0-a5
589
590	btst		&0x7,EXC_ISR(%a6)	# is trace mode on?
591	bne.b		uieh_trace		# yes;go handle trace mode
592
593	mov.l		EXC_EXTWPTR(%a6),EXC_IPC(%a6) # new pc on stack frame
594	mov.l		EXC_A6(%a6),(%a6)	# prepare new a6 for unlink
595	unlk		%a6			# unlink stack frame
596	bra.l		_isp_done
597
598#
599# The instruction that was just emulated was also being traced. The trace
600# trap for this instruction will be lost unless we jump to the trace handler.
601# So, here we create a Trace Exception format number two exception stack
602# frame from the Unimplemented Integer Intruction Exception stack frame
603# format number zero and jump to the user supplied hook "_real_trace()".
604#
605#		   UIEH FRAME		   TRACE FRAME
606#		*****************	*****************
607#		* 0x0 *  0x0f4	*	*    Current	*
608#		*****************	*      PC	*
609#		*    Current	*	*****************
610#		*      PC	*	* 0x2 *  0x024	*
611#		*****************	*****************
612#		*      SR	*	*     Next	*
613#		*****************	*      PC	*
614#	      ->*     Old	*	*****************
615#  from link -->*      A6	*	*      SR	*
616#	        *****************	*****************
617#	       /*      A7	*	*      New	* <-- for final unlink
618#	      / *		*	*      A6	*
619# link frame <  *****************	*****************
620#	      \ ~		~	~		~
621#	       \*****************	*****************
622#
623uieh_trace:
624	mov.l		EXC_A6(%a6),-0x4(%a6)
625	mov.w		EXC_ISR(%a6),0x0(%a6)
626	mov.l		EXC_IPC(%a6),0x8(%a6)
627	mov.l		EXC_EXTWPTR(%a6),0x2(%a6)
628	mov.w		&0x2024,0x6(%a6)
629	sub.l		&0x4,%a6
630	unlk		%a6
631	bra.l		_real_trace
632
633#
634#	   UIEH FRAME		    CHK FRAME
635#	*****************	*****************
636#	* 0x0 *  0x0f4	*	*    Current	*
637#	*****************	*      PC	*
638#	*    Current	*	*****************
639#	*      PC	*	* 0x2 *  0x018	*
640#	*****************	*****************
641#	*      SR	*	*     Next	*
642#	*****************	*      PC	*
643#	    (4 words)		*****************
644#				*      SR	*
645#				*****************
646#				    (6 words)
647#
648# the chk2 instruction should take a chk trap. so, here we must create a
649# chk stack frame from an unimplemented integer instruction exception frame
650# and jump to the user supplied entry point "_real_chk()".
651#
652uieh_chk_trap:
653	mov.b		EXC_CC+1(%a6),EXC_ISR+1(%a6) # insert new ccodes
654	movm.l		EXC_DREGS(%a6),&0x3fff	# restore d0-d7/a0-a5
655
656	mov.w		EXC_ISR(%a6),(%a6)	# put new SR on stack
657	mov.l		EXC_IPC(%a6),0x8(%a6)	# put "Current PC" on stack
658	mov.l		EXC_EXTWPTR(%a6),0x2(%a6) # put "Next PC" on stack
659	mov.w		&0x2018,0x6(%a6)	# put Vector Offset on stack
660
661	mov.l		EXC_A6(%a6),%a6		# restore a6
662	add.l		&LOCAL_SIZE,%sp		# clear stack frame
663
664	bra.l		_real_chk
665
666#
667#	   UIEH FRAME		 DIVBYZERO FRAME
668#	*****************	*****************
669#	* 0x0 *  0x0f4	*	*    Current	*
670#	*****************	*      PC	*
671#	*    Current	*	*****************
672#	*      PC	*	* 0x2 *  0x014	*
673#	*****************	*****************
674#	*      SR	*	*     Next	*
675#	*****************	*      PC	*
676#	    (4 words)		*****************
677#				*      SR	*
678#				*****************
679#				    (6 words)
680#
681# the divide instruction should take an integer divide by zero trap. so, here
682# we must create a divbyzero stack frame from an unimplemented integer
683# instruction exception frame and jump to the user supplied entry point
684# "_real_divbyzero()".
685#
686uieh_divbyzero:
687	mov.b		EXC_CC+1(%a6),EXC_ISR+1(%a6) # insert new ccodes
688	movm.l		EXC_DREGS(%a6),&0x3fff	# restore d0-d7/a0-a5
689
690	mov.w		EXC_ISR(%a6),(%a6)	# put new SR on stack
691	mov.l		EXC_IPC(%a6),0x8(%a6)	# put "Current PC" on stack
692	mov.l		EXC_EXTWPTR(%a6),0x2(%a6) # put "Next PC" on stack
693	mov.w		&0x2014,0x6(%a6)	# put Vector Offset on stack
694
695	mov.l		EXC_A6(%a6),%a6		# restore a6
696	add.l		&LOCAL_SIZE,%sp		# clear stack frame
697
698	bra.l		_real_divbyzero
699
700#
701#				 DIVBYZERO FRAME
702#				*****************
703#				*    Current	*
704#	   UIEH FRAME		*      PC	*
705#	*****************	*****************
706#	* 0x0 *  0x0f4	*	* 0x2 * 0x014	*
707#	*****************	*****************
708#	*    Current	*	*     Next	*
709#	*      PC	*	*      PC	*
710#	*****************	*****************
711#	*      SR	*	*      SR	*
712#	*****************	*****************
713#	    (4 words)		    (6 words)
714#
715# the divide instruction should take an integer divide by zero trap. so, here
716# we must create a divbyzero stack frame from an unimplemented integer
717# instruction exception frame and jump to the user supplied entry point
718# "_real_divbyzero()".
719#
720# However, we must also deal with the fact that (a7)+ was used from supervisor
721# mode, thereby shifting the stack frame up 4 bytes.
722#
723uieh_divbyzero_a7:
724	mov.b		EXC_CC+1(%a6),EXC_ISR+1(%a6) # insert new ccodes
725	movm.l		EXC_DREGS(%a6),&0x3fff	# restore d0-d7/a0-a5
726
727	mov.l		EXC_IPC(%a6),0xc(%a6)	# put "Current PC" on stack
728	mov.w		&0x2014,0xa(%a6)	# put Vector Offset on stack
729	mov.l		EXC_EXTWPTR(%a6),0x6(%a6) # put "Next PC" on stack
730
731	mov.l		EXC_A6(%a6),%a6		# restore a6
732	add.l		&4+LOCAL_SIZE,%sp	# clear stack frame
733
734	bra.l		_real_divbyzero
735
736#
737#				   TRACE FRAME
738#				*****************
739#				*    Current	*
740#	   UIEH FRAME		*      PC	*
741#	*****************	*****************
742#	* 0x0 *  0x0f4	*	* 0x2 * 0x024	*
743#	*****************	*****************
744#	*    Current	*	*     Next	*
745#	*      PC	*	*      PC	*
746#	*****************	*****************
747#	*      SR	*	*      SR	*
748#	*****************	*****************
749#	    (4 words)		    (6 words)
750#
751#
752# The instruction that was just emulated was also being traced. The trace
753# trap for this instruction will be lost unless we jump to the trace handler.
754# So, here we create a Trace Exception format number two exception stack
755# frame from the Unimplemented Integer Intruction Exception stack frame
756# format number zero and jump to the user supplied hook "_real_trace()".
757#
758# However, we must also deal with the fact that (a7)+ was used from supervisor
759# mode, thereby shifting the stack frame up 4 bytes.
760#
761uieh_trace_a7:
762	mov.b		EXC_CC+1(%a6),EXC_ISR+1(%a6) # insert new ccodes
763	movm.l		EXC_DREGS(%a6),&0x3fff	# restore d0-d7/a0-a5
764
765	mov.l		EXC_IPC(%a6),0xc(%a6)	# put "Current PC" on stack
766	mov.w		&0x2024,0xa(%a6)	# put Vector Offset on stack
767	mov.l		EXC_EXTWPTR(%a6),0x6(%a6) # put "Next PC" on stack
768
769	mov.l		EXC_A6(%a6),%a6		# restore a6
770	add.l		&4+LOCAL_SIZE,%sp	# clear stack frame
771
772	bra.l		_real_trace
773
774#
775#				   UIEH FRAME
776#				*****************
777#				* 0x0 * 0x0f4	*
778#	   UIEH FRAME		*****************
779#	*****************	*     Next	*
780#	* 0x0 *  0x0f4	*	*      PC	*
781#	*****************	*****************
782#	*    Current	*	*      SR	*
783#	*      PC	*	*****************
784#	*****************	    (4 words)
785#	*      SR	*
786#	*****************
787#	    (4 words)
788uieh_a7:
789	mov.b		EXC_CC+1(%a6),EXC_ISR+1(%a6) # insert new ccodes
790	movm.l		EXC_DREGS(%a6),&0x3fff	# restore d0-d7/a0-a5
791
792	mov.w		&0x00f4,0xe(%a6)	# put Vector Offset on stack
793	mov.l		EXC_EXTWPTR(%a6),0xa(%a6) # put "Next PC" on stack
794	mov.w		EXC_ISR(%a6),0x8(%a6)	# put SR on stack
795
796	mov.l		EXC_A6(%a6),%a6		# restore a6
797	add.l		&8+LOCAL_SIZE,%sp	# clear stack frame
798	bra.l		_isp_done
799
800##########
801
802# this is the exit point if a data read or write fails.
803# a0 = failing address
804# d0 = fslw
805isp_dacc:
806	mov.l		%a0,(%a6)		# save address
807	mov.l		%d0,-0x4(%a6)		# save partial fslw
808
809	lea		-64(%a6),%sp
810	movm.l		(%sp)+,&0x7fff		# restore d0-d7/a0-a6
811
812	mov.l		0xc(%sp),-(%sp)		# move voff,hi(pc)
813	mov.l		0x4(%sp),0x10(%sp)	# store fslw
814	mov.l		0xc(%sp),0x4(%sp)	# store sr,lo(pc)
815	mov.l		0x8(%sp),0xc(%sp)	# store address
816	mov.l		(%sp)+,0x4(%sp)		# store voff,hi(pc)
817	mov.w		&0x4008,0x6(%sp)	# store new voff
818
819	bra.b		isp_acc_exit
820
821# this is the exit point if an instruction word read fails.
822# FSLW:
823#	misaligned = true
824#	read = true
825#	size = word
826#	instruction = true
827#	software emulation error = true
828isp_iacc:
829	movm.l		EXC_DREGS(%a6),&0x3fff	# restore d0-d7/a0-a5
830	unlk		%a6			# unlink frame
831	sub.w		&0x8,%sp		# make room for acc frame
832	mov.l		0x8(%sp),(%sp)		# store sr,lo(pc)
833	mov.w		0xc(%sp),0x4(%sp)	# store hi(pc)
834	mov.w		&0x4008,0x6(%sp)	# store new voff
835	mov.l		0x2(%sp),0x8(%sp)	# store address (=pc)
836	mov.l		&0x09428001,0xc(%sp)	# store fslw
837
838isp_acc_exit:
839	btst		&0x5,(%sp)		# user or supervisor?
840	beq.b		isp_acc_exit2		# user
841	bset		&0x2,0xd(%sp)		# set supervisor TM bit
842isp_acc_exit2:
843	bra.l		_real_access
844
845# if the addressing mode was (an)+ or -(an), the address register must
846# be restored to its pre-exception value before entering _real_access.
847isp_restore:
848	cmpi.b		SPCOND_FLG(%a6),&restore_flg # do we need a restore?
849	bne.b		isp_restore_done	# no
850	clr.l		%d0
851	mov.b		EXC_SAVREG(%a6),%d0	# regno to restore
852	mov.l		EXC_SAVVAL(%a6),(EXC_AREGS,%a6,%d0.l*4) # restore value
853isp_restore_done:
854	rts
855
856#########################################################################
857# XDEF ****************************************************************	#
858#	_calc_ea(): routine to calculate effective address		#
859#									#
860# XREF ****************************************************************	#
861#	_imem_read_word() - read instruction word			#
862#	_imem_read_long() - read instruction longword			#
863#	_dmem_read_long() - read data longword (for memory indirect)	#
864#	isp_iacc() - handle instruction access error exception		#
865#	isp_dacc() - handle data access error exception			#
866#									#
867# INPUT ***************************************************************	#
868#	d0 = number of bytes related to effective address (w,l)		#
869#									#
870# OUTPUT **************************************************************	#
871#	If exiting through isp_dacc...					#
872#		a0 = failing address					#
873#		d0 = FSLW						#
874#	elsif exiting though isp_iacc...				#
875#		none							#
876#	else								#
877#		a0 = effective address					#
878#									#
879# ALGORITHM ***********************************************************	#
880#	The effective address type is decoded from the opword residing	#
881# on the stack. A jump table is used to vector to a routine for the	#
882# appropriate mode. Since none of the emulated integer instructions	#
883# uses byte-sized operands, only handle word and long operations.	#
884#									#
885#	Dn,An	- shouldn't enter here					#
886#	(An)	- fetch An value from stack				#
887#	-(An)	- fetch An value from stack; return decr value;		#
888#		  place decr value on stack; store old value in case of	#
889#		  future access error; if -(a7), set mda7_flg in	#
890#		  SPCOND_FLG						#
891#	(An)+	- fetch An value from stack; return value;		#
892#		  place incr value on stack; store old value in case of	#
893#		  future access error; if (a7)+, set mia7_flg in	#
894#		  SPCOND_FLG						#
895#	(d16,An) - fetch An value from stack; read d16 using		#
896#		  _imem_read_word(); fetch may fail -> branch to	#
897#		  isp_iacc()						#
898#	(xxx).w,(xxx).l - use _imem_read_{word,long}() to fetch		#
899#		  address; fetch may fail				#
900#	#<data> - return address of immediate value; set immed_flg	#
901#		  in SPCOND_FLG						#
902#	(d16,PC) - fetch stacked PC value; read d16 using		#
903#		  _imem_read_word(); fetch may fail -> branch to	#
904#		  isp_iacc()						#
905#	everything else - read needed displacements as appropriate w/	#
906#		  _imem_read_{word,long}(); read may fail; if memory	#
907#		  indirect, read indirect address using			#
908#		  _dmem_read_long() which may also fail			#
909#									#
910#########################################################################
911
912	global		_calc_ea
913_calc_ea:
914	mov.l		%d0,%a0			# move # bytes to a0
915
916# MODE and REG are taken from the EXC_OPWORD.
917	mov.w		EXC_OPWORD(%a6),%d0	# fetch opcode word
918	mov.w		%d0,%d1			# make a copy
919
920	andi.w		&0x3f,%d0		# extract mode field
921	andi.l		&0x7,%d1		# extract reg  field
922
923# jump to the corresponding function for each {MODE,REG} pair.
924	mov.w		(tbl_ea_mode.b,%pc,%d0.w*2), %d0 # fetch jmp distance
925	jmp		(tbl_ea_mode.b,%pc,%d0.w*1) # jmp to correct ea mode
926
927	swbeg		&64
928tbl_ea_mode:
929	short		tbl_ea_mode	-	tbl_ea_mode
930	short		tbl_ea_mode	-	tbl_ea_mode
931	short		tbl_ea_mode	-	tbl_ea_mode
932	short		tbl_ea_mode	-	tbl_ea_mode
933	short		tbl_ea_mode	-	tbl_ea_mode
934	short		tbl_ea_mode	-	tbl_ea_mode
935	short		tbl_ea_mode	-	tbl_ea_mode
936	short		tbl_ea_mode	-	tbl_ea_mode
937
938	short		tbl_ea_mode	-	tbl_ea_mode
939	short		tbl_ea_mode	-	tbl_ea_mode
940	short		tbl_ea_mode	-	tbl_ea_mode
941	short		tbl_ea_mode	-	tbl_ea_mode
942	short		tbl_ea_mode	-	tbl_ea_mode
943	short		tbl_ea_mode	-	tbl_ea_mode
944	short		tbl_ea_mode	-	tbl_ea_mode
945	short		tbl_ea_mode	-	tbl_ea_mode
946
947	short		addr_ind_a0	-	tbl_ea_mode
948	short		addr_ind_a1	-	tbl_ea_mode
949	short		addr_ind_a2	-	tbl_ea_mode
950	short		addr_ind_a3	-	tbl_ea_mode
951	short		addr_ind_a4	-	tbl_ea_mode
952	short		addr_ind_a5	-	tbl_ea_mode
953	short		addr_ind_a6	-	tbl_ea_mode
954	short		addr_ind_a7	-	tbl_ea_mode
955
956	short		addr_ind_p_a0	-	tbl_ea_mode
957	short		addr_ind_p_a1	-	tbl_ea_mode
958	short		addr_ind_p_a2	-	tbl_ea_mode
959	short		addr_ind_p_a3	-	tbl_ea_mode
960	short		addr_ind_p_a4	-	tbl_ea_mode
961	short		addr_ind_p_a5	-	tbl_ea_mode
962	short		addr_ind_p_a6	-	tbl_ea_mode
963	short		addr_ind_p_a7	-	tbl_ea_mode
964
965	short		addr_ind_m_a0		-	tbl_ea_mode
966	short		addr_ind_m_a1		-	tbl_ea_mode
967	short		addr_ind_m_a2		-	tbl_ea_mode
968	short		addr_ind_m_a3		-	tbl_ea_mode
969	short		addr_ind_m_a4		-	tbl_ea_mode
970	short		addr_ind_m_a5		-	tbl_ea_mode
971	short		addr_ind_m_a6		-	tbl_ea_mode
972	short		addr_ind_m_a7		-	tbl_ea_mode
973
974	short		addr_ind_disp_a0	-	tbl_ea_mode
975	short		addr_ind_disp_a1	-	tbl_ea_mode
976	short		addr_ind_disp_a2	-	tbl_ea_mode
977	short		addr_ind_disp_a3	-	tbl_ea_mode
978	short		addr_ind_disp_a4	-	tbl_ea_mode
979	short		addr_ind_disp_a5	-	tbl_ea_mode
980	short		addr_ind_disp_a6	-	tbl_ea_mode
981	short		addr_ind_disp_a7	-	tbl_ea_mode
982
983	short		_addr_ind_ext		-	tbl_ea_mode
984	short		_addr_ind_ext		-	tbl_ea_mode
985	short		_addr_ind_ext		-	tbl_ea_mode
986	short		_addr_ind_ext		-	tbl_ea_mode
987	short		_addr_ind_ext		-	tbl_ea_mode
988	short		_addr_ind_ext		-	tbl_ea_mode
989	short		_addr_ind_ext		-	tbl_ea_mode
990	short		_addr_ind_ext		-	tbl_ea_mode
991
992	short		abs_short		-	tbl_ea_mode
993	short		abs_long		-	tbl_ea_mode
994	short		pc_ind			-	tbl_ea_mode
995	short		pc_ind_ext		-	tbl_ea_mode
996	short		immediate		-	tbl_ea_mode
997	short		tbl_ea_mode		-	tbl_ea_mode
998	short		tbl_ea_mode		-	tbl_ea_mode
999	short		tbl_ea_mode		-	tbl_ea_mode
1000
1001###################################
1002# Address register indirect: (An) #
1003###################################
1004addr_ind_a0:
1005	mov.l		EXC_A0(%a6),%a0		# Get current a0
1006	rts
1007
1008addr_ind_a1:
1009	mov.l		EXC_A1(%a6),%a0		# Get current a1
1010	rts
1011
1012addr_ind_a2:
1013	mov.l		EXC_A2(%a6),%a0		# Get current a2
1014	rts
1015
1016addr_ind_a3:
1017	mov.l		EXC_A3(%a6),%a0		# Get current a3
1018	rts
1019
1020addr_ind_a4:
1021	mov.l		EXC_A4(%a6),%a0		# Get current a4
1022	rts
1023
1024addr_ind_a5:
1025	mov.l		EXC_A5(%a6),%a0		# Get current a5
1026	rts
1027
1028addr_ind_a6:
1029	mov.l		EXC_A6(%a6),%a0		# Get current a6
1030	rts
1031
1032addr_ind_a7:
1033	mov.l		EXC_A7(%a6),%a0		# Get current a7
1034	rts
1035
1036#####################################################
1037# Address register indirect w/ postincrement: (An)+ #
1038#####################################################
1039addr_ind_p_a0:
1040	mov.l		%a0,%d0			# copy no. bytes
1041	mov.l		EXC_A0(%a6),%a0		# load current value
1042	add.l		%a0,%d0			# increment
1043	mov.l		%d0,EXC_A0(%a6)		# save incremented value
1044
1045	mov.l		%a0,EXC_SAVVAL(%a6)	# save in case of access error
1046	mov.b		&0x0,EXC_SAVREG(%a6)	# save regno, too
1047	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1048	rts
1049
1050addr_ind_p_a1:
1051	mov.l		%a0,%d0			# copy no. bytes
1052	mov.l		EXC_A1(%a6),%a0		# load current value
1053	add.l		%a0,%d0			# increment
1054	mov.l		%d0,EXC_A1(%a6)		# save incremented value
1055
1056	mov.l		%a0,EXC_SAVVAL(%a6)	# save in case of access error
1057	mov.b		&0x1,EXC_SAVREG(%a6)	# save regno, too
1058	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1059	rts
1060
1061addr_ind_p_a2:
1062	mov.l		%a0,%d0			# copy no. bytes
1063	mov.l		EXC_A2(%a6),%a0		# load current value
1064	add.l		%a0,%d0			# increment
1065	mov.l		%d0,EXC_A2(%a6)		# save incremented value
1066
1067	mov.l		%a0,EXC_SAVVAL(%a6)	# save in case of access error
1068	mov.b		&0x2,EXC_SAVREG(%a6)	# save regno, too
1069	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1070	rts
1071
1072addr_ind_p_a3:
1073	mov.l		%a0,%d0			# copy no. bytes
1074	mov.l		EXC_A3(%a6),%a0		# load current value
1075	add.l		%a0,%d0			# increment
1076	mov.l		%d0,EXC_A3(%a6)		# save incremented value
1077
1078	mov.l		%a0,EXC_SAVVAL(%a6)	# save in case of access error
1079	mov.b		&0x3,EXC_SAVREG(%a6)	# save regno, too
1080	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1081	rts
1082
1083addr_ind_p_a4:
1084	mov.l		%a0,%d0			# copy no. bytes
1085	mov.l		EXC_A4(%a6),%a0		# load current value
1086	add.l		%a0,%d0			# increment
1087	mov.l		%d0,EXC_A4(%a6)		# save incremented value
1088
1089	mov.l		%a0,EXC_SAVVAL(%a6)	# save in case of access error
1090	mov.b		&0x4,EXC_SAVREG(%a6)	# save regno, too
1091	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1092	rts
1093
1094addr_ind_p_a5:
1095	mov.l		%a0,%d0			# copy no. bytes
1096	mov.l		EXC_A5(%a6),%a0		# load current value
1097	add.l		%a0,%d0			# increment
1098	mov.l		%d0,EXC_A5(%a6)		# save incremented value
1099
1100	mov.l		%a0,EXC_SAVVAL(%a6)	# save in case of access error
1101	mov.b		&0x5,EXC_SAVREG(%a6)	# save regno, too
1102	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1103	rts
1104
1105addr_ind_p_a6:
1106	mov.l		%a0,%d0			# copy no. bytes
1107	mov.l		EXC_A6(%a6),%a0		# load current value
1108	add.l		%a0,%d0			# increment
1109	mov.l		%d0,EXC_A6(%a6)		# save incremented value
1110
1111	mov.l		%a0,EXC_SAVVAL(%a6)	# save in case of access error
1112	mov.b		&0x6,EXC_SAVREG(%a6)	# save regno, too
1113	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1114	rts
1115
1116addr_ind_p_a7:
1117	mov.b		&mia7_flg,SPCOND_FLG(%a6) # set "special case" flag
1118
1119	mov.l		%a0,%d0			# copy no. bytes
1120	mov.l		EXC_A7(%a6),%a0		# load current value
1121	add.l		%a0,%d0			# increment
1122	mov.l		%d0,EXC_A7(%a6)		# save incremented value
1123	rts
1124
1125####################################################
1126# Address register indirect w/ predecrement: -(An) #
1127####################################################
1128addr_ind_m_a0:
1129	mov.l		EXC_A0(%a6),%d0		# Get current a0
1130	mov.l		%d0,EXC_SAVVAL(%a6)	# save in case of access error
1131	sub.l		%a0,%d0			# Decrement
1132	mov.l		%d0,EXC_A0(%a6)		# Save decr value
1133	mov.l		%d0,%a0
1134
1135	mov.b		&0x0,EXC_SAVREG(%a6)	# save regno, too
1136	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1137	rts
1138
1139addr_ind_m_a1:
1140	mov.l		EXC_A1(%a6),%d0		# Get current a1
1141	mov.l		%d0,EXC_SAVVAL(%a6)	# save in case of access error
1142	sub.l		%a0,%d0			# Decrement
1143	mov.l		%d0,EXC_A1(%a6)		# Save decr value
1144	mov.l		%d0,%a0
1145
1146	mov.b		&0x1,EXC_SAVREG(%a6)	# save regno, too
1147	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1148	rts
1149
1150addr_ind_m_a2:
1151	mov.l		EXC_A2(%a6),%d0		# Get current a2
1152	mov.l		%d0,EXC_SAVVAL(%a6)	# save in case of access error
1153	sub.l		%a0,%d0			# Decrement
1154	mov.l		%d0,EXC_A2(%a6)		# Save decr value
1155	mov.l		%d0,%a0
1156
1157	mov.b		&0x2,EXC_SAVREG(%a6)	# save regno, too
1158	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1159	rts
1160
1161addr_ind_m_a3:
1162	mov.l		EXC_A3(%a6),%d0		# Get current a3
1163	mov.l		%d0,EXC_SAVVAL(%a6)	# save in case of access error
1164	sub.l		%a0,%d0			# Decrement
1165	mov.l		%d0,EXC_A3(%a6)		# Save decr value
1166	mov.l		%d0,%a0
1167
1168	mov.b		&0x3,EXC_SAVREG(%a6)	# save regno, too
1169	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1170	rts
1171
1172addr_ind_m_a4:
1173	mov.l		EXC_A4(%a6),%d0		# Get current a4
1174	mov.l		%d0,EXC_SAVVAL(%a6)	# save in case of access error
1175	sub.l		%a0,%d0			# Decrement
1176	mov.l		%d0,EXC_A4(%a6)		# Save decr value
1177	mov.l		%d0,%a0
1178
1179	mov.b		&0x4,EXC_SAVREG(%a6)	# save regno, too
1180	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1181	rts
1182
1183addr_ind_m_a5:
1184	mov.l		EXC_A5(%a6),%d0		# Get current a5
1185	mov.l		%d0,EXC_SAVVAL(%a6)	# save in case of access error
1186	sub.l		%a0,%d0			# Decrement
1187	mov.l		%d0,EXC_A5(%a6)		# Save decr value
1188	mov.l		%d0,%a0
1189
1190	mov.b		&0x5,EXC_SAVREG(%a6)	# save regno, too
1191	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1192	rts
1193
1194addr_ind_m_a6:
1195	mov.l		EXC_A6(%a6),%d0		# Get current a6
1196	mov.l		%d0,EXC_SAVVAL(%a6)	# save in case of access error
1197	sub.l		%a0,%d0			# Decrement
1198	mov.l		%d0,EXC_A6(%a6)		# Save decr value
1199	mov.l		%d0,%a0
1200
1201	mov.b		&0x6,EXC_SAVREG(%a6)	# save regno, too
1202	mov.b		&restore_flg,SPCOND_FLG(%a6) # set flag
1203	rts
1204
1205addr_ind_m_a7:
1206	mov.b		&mda7_flg,SPCOND_FLG(%a6) # set "special case" flag
1207
1208	mov.l		EXC_A7(%a6),%d0		# Get current a7
1209	sub.l		%a0,%d0			# Decrement
1210	mov.l		%d0,EXC_A7(%a6)		# Save decr value
1211	mov.l		%d0,%a0
1212	rts
1213
1214########################################################
1215# Address register indirect w/ displacement: (d16, An) #
1216########################################################
1217addr_ind_disp_a0:
1218	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1219	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1220	bsr.l		_imem_read_word
1221
1222	tst.l		%d1			# ifetch error?
1223	bne.l		isp_iacc		# yes
1224
1225	mov.w		%d0,%a0			# sign extend displacement
1226	add.l		EXC_A0(%a6),%a0		# a0 + d16
1227	rts
1228
1229addr_ind_disp_a1:
1230	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1231	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1232	bsr.l		_imem_read_word
1233
1234	tst.l		%d1			# ifetch error?
1235	bne.l		isp_iacc		# yes
1236
1237	mov.w		%d0,%a0			# sign extend displacement
1238	add.l		EXC_A1(%a6),%a0		# a1 + d16
1239	rts
1240
1241addr_ind_disp_a2:
1242	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1243	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1244	bsr.l		_imem_read_word
1245
1246	tst.l		%d1			# ifetch error?
1247	bne.l		isp_iacc		# yes
1248
1249	mov.w		%d0,%a0			# sign extend displacement
1250	add.l		EXC_A2(%a6),%a0		# a2 + d16
1251	rts
1252
1253addr_ind_disp_a3:
1254	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1255	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1256	bsr.l		_imem_read_word
1257
1258	tst.l		%d1			# ifetch error?
1259	bne.l		isp_iacc		# yes
1260
1261	mov.w		%d0,%a0			# sign extend displacement
1262	add.l		EXC_A3(%a6),%a0		# a3 + d16
1263	rts
1264
1265addr_ind_disp_a4:
1266	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1267	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1268	bsr.l		_imem_read_word
1269
1270	tst.l		%d1			# ifetch error?
1271	bne.l		isp_iacc		# yes
1272
1273	mov.w		%d0,%a0			# sign extend displacement
1274	add.l		EXC_A4(%a6),%a0		# a4 + d16
1275	rts
1276
1277addr_ind_disp_a5:
1278	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1279	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1280	bsr.l		_imem_read_word
1281
1282	tst.l		%d1			# ifetch error?
1283	bne.l		isp_iacc		# yes
1284
1285	mov.w		%d0,%a0			# sign extend displacement
1286	add.l		EXC_A5(%a6),%a0		# a5 + d16
1287	rts
1288
1289addr_ind_disp_a6:
1290	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1291	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1292	bsr.l		_imem_read_word
1293
1294	tst.l		%d1			# ifetch error?
1295	bne.l		isp_iacc		# yes
1296
1297	mov.w		%d0,%a0			# sign extend displacement
1298	add.l		EXC_A6(%a6),%a0		# a6 + d16
1299	rts
1300
1301addr_ind_disp_a7:
1302	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1303	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1304	bsr.l		_imem_read_word
1305
1306	tst.l		%d1			# ifetch error?
1307	bne.l		isp_iacc		# yes
1308
1309	mov.w		%d0,%a0			# sign extend displacement
1310	add.l		EXC_A7(%a6),%a0		# a7 + d16
1311	rts
1312
1313########################################################################
1314# Address register indirect w/ index(8-bit displacement): (dn, An, Xn) #
1315#    "       "         "    w/   "  (base displacement): (bd, An, Xn)  #
1316# Memory indirect postindexed: ([bd, An], Xn, od)		       #
1317# Memory indirect preindexed: ([bd, An, Xn], od)		       #
1318########################################################################
1319_addr_ind_ext:
1320	mov.l		%d1,-(%sp)
1321
1322	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1323	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1324	bsr.l		_imem_read_word		# fetch extword in d0
1325
1326	tst.l		%d1			# ifetch error?
1327	bne.l		isp_iacc		# yes
1328
1329	mov.l		(%sp)+,%d1
1330
1331	mov.l		(EXC_AREGS,%a6,%d1.w*4),%a0 # put base in a0
1332
1333	btst		&0x8,%d0
1334	beq.b		addr_ind_index_8bit	# for ext word or not?
1335
1336	movm.l		&0x3c00,-(%sp)		# save d2-d5
1337
1338	mov.l		%d0,%d5			# put extword in d5
1339	mov.l		%a0,%d3			# put base in d3
1340
1341	bra.l		calc_mem_ind		# calc memory indirect
1342
1343addr_ind_index_8bit:
1344	mov.l		%d2,-(%sp)		# save old d2
1345
1346	mov.l		%d0,%d1
1347	rol.w		&0x4,%d1
1348	andi.w		&0xf,%d1		# extract index regno
1349
1350	mov.l		(EXC_DREGS,%a6,%d1.w*4),%d1 # fetch index reg value
1351
1352	btst		&0xb,%d0		# is it word or long?
1353	bne.b		aii8_long
1354	ext.l		%d1			# sign extend word index
1355aii8_long:
1356	mov.l		%d0,%d2
1357	rol.w		&0x7,%d2
1358	andi.l		&0x3,%d2		# extract scale value
1359
1360	lsl.l		%d2,%d1			# shift index by scale
1361
1362	extb.l		%d0			# sign extend displacement
1363	add.l		%d1,%d0			# index + disp
1364	add.l		%d0,%a0			# An + (index + disp)
1365
1366	mov.l		(%sp)+,%d2		# restore old d2
1367	rts
1368
1369######################
1370# Immediate: #<data> #
1371#########################################################################
1372# word, long: <ea> of the data is the current extension word		#
1373#	pointer value. new extension word pointer is simply the old	#
1374#	plus the number of bytes in the data type(2 or 4).		#
1375#########################################################################
1376immediate:
1377	mov.b		&immed_flg,SPCOND_FLG(%a6) # set immediate flag
1378
1379	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch extension word ptr
1380	rts
1381
1382###########################
1383# Absolute short: (XXX).W #
1384###########################
1385abs_short:
1386	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1387	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1388	bsr.l		_imem_read_word		# fetch short address
1389
1390	tst.l		%d1			# ifetch error?
1391	bne.l		isp_iacc		# yes
1392
1393	mov.w		%d0,%a0			# return <ea> in a0
1394	rts
1395
1396##########################
1397# Absolute long: (XXX).L #
1398##########################
1399abs_long:
1400	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1401	addq.l		&0x4,EXC_EXTWPTR(%a6)	# incr instruction ptr
1402	bsr.l		_imem_read_long		# fetch long address
1403
1404	tst.l		%d1			# ifetch error?
1405	bne.l		isp_iacc		# yes
1406
1407	mov.l		%d0,%a0			# return <ea> in a0
1408	rts
1409
1410#######################################################
1411# Program counter indirect w/ displacement: (d16, PC) #
1412#######################################################
1413pc_ind:
1414	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1415	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1416	bsr.l		_imem_read_word		# fetch word displacement
1417
1418	tst.l		%d1			# ifetch error?
1419	bne.l		isp_iacc		# yes
1420
1421	mov.w		%d0,%a0			# sign extend displacement
1422
1423	add.l		EXC_EXTWPTR(%a6),%a0	# pc + d16
1424
1425# _imem_read_word() increased the extwptr by 2. need to adjust here.
1426	subq.l		&0x2,%a0		# adjust <ea>
1427
1428	rts
1429
1430##########################################################
1431# PC indirect w/ index(8-bit displacement): (d8, PC, An) #
1432# "     "     w/   "  (base displacement): (bd, PC, An)  #
1433# PC memory indirect postindexed: ([bd, PC], Xn, od)     #
1434# PC memory indirect preindexed: ([bd, PC, Xn], od)      #
1435##########################################################
1436pc_ind_ext:
1437	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1438	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1439	bsr.l		_imem_read_word		# fetch ext word
1440
1441	tst.l		%d1			# ifetch error?
1442	bne.l		isp_iacc		# yes
1443
1444	mov.l		EXC_EXTWPTR(%a6),%a0	# put base in a0
1445	subq.l		&0x2,%a0		# adjust base
1446
1447	btst		&0x8,%d0		# is disp only 8 bits?
1448	beq.b		pc_ind_index_8bit	# yes
1449
1450# the indexed addressing mode uses a base displacement of size
1451# word or long
1452	movm.l		&0x3c00,-(%sp)		# save d2-d5
1453
1454	mov.l		%d0,%d5			# put extword in d5
1455	mov.l		%a0,%d3			# put base in d3
1456
1457	bra.l		calc_mem_ind		# calc memory indirect
1458
1459pc_ind_index_8bit:
1460	mov.l		%d2,-(%sp)		# create a temp register
1461
1462	mov.l		%d0,%d1			# make extword copy
1463	rol.w		&0x4,%d1		# rotate reg num into place
1464	andi.w		&0xf,%d1		# extract register number
1465
1466	mov.l		(EXC_DREGS,%a6,%d1.w*4),%d1 # fetch index reg value
1467
1468	btst		&0xb,%d0		# is index word or long?
1469	bne.b		pii8_long		# long
1470	ext.l		%d1			# sign extend word index
1471pii8_long:
1472	mov.l		%d0,%d2			# make extword copy
1473	rol.w		&0x7,%d2		# rotate scale value into place
1474	andi.l		&0x3,%d2		# extract scale value
1475
1476	lsl.l		%d2,%d1			# shift index by scale
1477
1478	extb.l		%d0			# sign extend displacement
1479	add.l		%d1,%d0			# index + disp
1480	add.l		%d0,%a0			# An + (index + disp)
1481
1482	mov.l		(%sp)+,%d2		# restore temp register
1483
1484	rts
1485
1486# a5 = exc_extwptr	(global to uaeh)
1487# a4 = exc_opword	(global to uaeh)
1488# a3 = exc_dregs	(global to uaeh)
1489
1490# d2 = index		(internal "     "    )
1491# d3 = base		(internal "     "    )
1492# d4 = od		(internal "     "    )
1493# d5 = extword		(internal "     "    )
1494calc_mem_ind:
1495	btst		&0x6,%d5		# is the index suppressed?
1496	beq.b		calc_index
1497	clr.l		%d2			# yes, so index = 0
1498	bra.b		base_supp_ck
1499calc_index:
1500	bfextu		%d5{&16:&4},%d2
1501	mov.l		(EXC_DREGS,%a6,%d2.w*4),%d2
1502	btst		&0xb,%d5		# is index word or long?
1503	bne.b		no_ext
1504	ext.l		%d2
1505no_ext:
1506	bfextu		%d5{&21:&2},%d0
1507	lsl.l		%d0,%d2
1508base_supp_ck:
1509	btst		&0x7,%d5		# is the bd suppressed?
1510	beq.b		no_base_sup
1511	clr.l		%d3
1512no_base_sup:
1513	bfextu		%d5{&26:&2},%d0	# get bd size
1514#	beq.l		_error			# if (size == 0) it's reserved
1515	cmpi.b		%d0,&2
1516	blt.b		no_bd
1517	beq.b		get_word_bd
1518
1519	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1520	addq.l		&0x4,EXC_EXTWPTR(%a6)	# incr instruction ptr
1521	bsr.l		_imem_read_long
1522
1523	tst.l		%d1			# ifetch error?
1524	bne.l		isp_iacc		# yes
1525
1526	bra.b		chk_ind
1527get_word_bd:
1528	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1529	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1530	bsr.l		_imem_read_word
1531
1532	tst.l		%d1			# ifetch error?
1533	bne.l		isp_iacc		# yes
1534
1535	ext.l		%d0			# sign extend bd
1536
1537chk_ind:
1538	add.l		%d0,%d3			# base += bd
1539no_bd:
1540	bfextu		%d5{&30:&2},%d0		# is od suppressed?
1541	beq.w		aii_bd
1542	cmpi.b		%d0,&0x2
1543	blt.b		null_od
1544	beq.b		word_od
1545
1546	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1547	addq.l		&0x4,EXC_EXTWPTR(%a6)	# incr instruction ptr
1548	bsr.l		_imem_read_long
1549
1550	tst.l		%d1			# ifetch error?
1551	bne.l		isp_iacc		# yes
1552
1553	bra.b		add_them
1554
1555word_od:
1556	mov.l		EXC_EXTWPTR(%a6),%a0	# fetch instruction addr
1557	addq.l		&0x2,EXC_EXTWPTR(%a6)	# incr instruction ptr
1558	bsr.l		_imem_read_word
1559
1560	tst.l		%d1			# ifetch error?
1561	bne.l		isp_iacc		# yes
1562
1563	ext.l		%d0			# sign extend od
1564	bra.b		add_them
1565
1566null_od:
1567	clr.l		%d0
1568add_them:
1569	mov.l		%d0,%d4
1570	btst		&0x2,%d5		# pre or post indexing?
1571	beq.b		pre_indexed
1572
1573	mov.l		%d3,%a0
1574	bsr.l		_dmem_read_long
1575
1576	tst.l		%d1			# dfetch error?
1577	bne.b		calc_ea_err		# yes
1578
1579	add.l		%d2,%d0			# <ea> += index
1580	add.l		%d4,%d0			# <ea> += od
1581	bra.b		done_ea
1582
1583pre_indexed:
1584	add.l		%d2,%d3			# preindexing
1585	mov.l		%d3,%a0
1586	bsr.l		_dmem_read_long
1587
1588	tst.l		%d1			# ifetch error?
1589	bne.b		calc_ea_err		# yes
1590
1591	add.l		%d4,%d0			# ea += od
1592	bra.b		done_ea
1593
1594aii_bd:
1595	add.l		%d2,%d3			# ea = (base + bd) + index
1596	mov.l		%d3,%d0
1597done_ea:
1598	mov.l		%d0,%a0
1599
1600	movm.l		(%sp)+,&0x003c		# restore d2-d5
1601	rts
1602
1603# if dmem_read_long() returns a fail message in d1, the package
1604# must create an access error frame. here, we pass a skeleton fslw
1605# and the failing address to the routine that creates the new frame.
1606# FSLW:
1607#	read = true
1608#	size = longword
1609#	TM = data
1610#	software emulation error = true
1611calc_ea_err:
1612	mov.l		%d3,%a0			# pass failing address
1613	mov.l		&0x01010001,%d0		# pass fslw
1614	bra.l		isp_dacc
1615
1616#########################################################################
1617# XDEF **************************************************************** #
1618#	_moveperipheral(): routine to emulate movep instruction		#
1619#									#
1620# XREF **************************************************************** #
1621#	_dmem_read_byte() - read byte from memory			#
1622#	_dmem_write_byte() - write byte to memory			#
1623#	isp_dacc() - handle data access error exception			#
1624#									#
1625# INPUT *************************************************************** #
1626#	none								#
1627#									#
1628# OUTPUT ************************************************************** #
1629#	If exiting through isp_dacc...					#
1630#		a0 = failing address					#
1631#		d0 = FSLW						#
1632#	else								#
1633#		none							#
1634#									#
1635# ALGORITHM ***********************************************************	#
1636#	Decode the movep instruction words stored at EXC_OPWORD and	#
1637# either read or write the required bytes from/to memory. Use the	#
1638# _dmem_{read,write}_byte() routines. If one of the memory routines	#
1639# returns a failing value, we must pass the failing address and	a FSLW	#
1640# to the _isp_dacc() routine.						#
1641#	Since this instruction is used to access peripherals, make sure	#
1642# to only access the required bytes.					#
1643#									#
1644#########################################################################
1645
1646###########################
1647# movep.(w,l)	Dx,(d,Ay) #
1648# movep.(w,l)	(d,Ay),Dx #
1649###########################
1650	global		_moveperipheral
1651_moveperipheral:
1652	mov.w		EXC_OPWORD(%a6),%d1	# fetch the opcode word
1653
1654	mov.b		%d1,%d0
1655	and.w		&0x7,%d0		# extract Ay from opcode word
1656
1657	mov.l		(EXC_AREGS,%a6,%d0.w*4),%a0 # fetch ay
1658
1659	add.w		EXC_EXTWORD(%a6),%a0	# add: an + sgn_ext(disp)
1660
1661	btst		&0x7,%d1		# (reg 2 mem) or (mem 2 reg)
1662	beq.w		mem2reg
1663
1664# reg2mem: fetch dx, then write it to memory
1665reg2mem:
1666	mov.w		%d1,%d0
1667	rol.w		&0x7,%d0
1668	and.w		&0x7,%d0		# extract Dx from opcode word
1669
1670	mov.l		(EXC_DREGS,%a6,%d0.w*4), %d0 # fetch dx
1671
1672	btst		&0x6,%d1		# word or long operation?
1673	beq.b		r2mwtrans
1674
1675# a0 = dst addr
1676# d0 = Dx
1677r2mltrans:
1678	mov.l		%d0,%d2			# store data
1679	mov.l		%a0,%a2			# store addr
1680	rol.l		&0x8,%d2
1681	mov.l		%d2,%d0
1682
1683	bsr.l		_dmem_write_byte	# os  : write hi
1684
1685	tst.l		%d1			# dfetch error?
1686	bne.w		movp_write_err		# yes
1687
1688	add.w		&0x2,%a2		# incr addr
1689	mov.l		%a2,%a0
1690	rol.l		&0x8,%d2
1691	mov.l		%d2,%d0
1692
1693	bsr.l		_dmem_write_byte	# os  : write lo
1694
1695	tst.l		%d1			# dfetch error?
1696	bne.w		movp_write_err		# yes
1697
1698	add.w		&0x2,%a2		# incr addr
1699	mov.l		%a2,%a0
1700	rol.l		&0x8,%d2
1701	mov.l		%d2,%d0
1702
1703	bsr.l		_dmem_write_byte	# os  : write lo
1704
1705	tst.l		%d1			# dfetch error?
1706	bne.w		movp_write_err		# yes
1707
1708	add.w		&0x2,%a2		# incr addr
1709	mov.l		%a2,%a0
1710	rol.l		&0x8,%d2
1711	mov.l		%d2,%d0
1712
1713	bsr.l		_dmem_write_byte	# os  : write lo
1714
1715	tst.l		%d1			# dfetch error?
1716	bne.w		movp_write_err		# yes
1717
1718	rts
1719
1720# a0 = dst addr
1721# d0 = Dx
1722r2mwtrans:
1723	mov.l		%d0,%d2			# store data
1724	mov.l		%a0,%a2			# store addr
1725	lsr.w		&0x8,%d0
1726
1727	bsr.l		_dmem_write_byte	# os  : write hi
1728
1729	tst.l		%d1			# dfetch error?
1730	bne.w		movp_write_err		# yes
1731
1732	add.w		&0x2,%a2
1733	mov.l		%a2,%a0
1734	mov.l		%d2,%d0
1735
1736	bsr.l		_dmem_write_byte	# os  : write lo
1737
1738	tst.l		%d1			# dfetch error?
1739	bne.w		movp_write_err		# yes
1740
1741	rts
1742
1743# mem2reg: read bytes from memory.
1744# determines the dest register, and then writes the bytes into it.
1745mem2reg:
1746	btst		&0x6,%d1		# word or long operation?
1747	beq.b		m2rwtrans
1748
1749# a0 = dst addr
1750m2rltrans:
1751	mov.l		%a0,%a2			# store addr
1752
1753	bsr.l		_dmem_read_byte		# read first byte
1754
1755	tst.l		%d1			# dfetch error?
1756	bne.w		movp_read_err		# yes
1757
1758	mov.l		%d0,%d2
1759
1760	add.w		&0x2,%a2		# incr addr by 2 bytes
1761	mov.l		%a2,%a0
1762
1763	bsr.l		_dmem_read_byte		# read second byte
1764
1765	tst.l		%d1			# dfetch error?
1766	bne.w		movp_read_err		# yes
1767
1768	lsl.w		&0x8,%d2
1769	mov.b		%d0,%d2			# append bytes
1770
1771	add.w		&0x2,%a2		# incr addr by 2 bytes
1772	mov.l		%a2,%a0
1773
1774	bsr.l		_dmem_read_byte		# read second byte
1775
1776	tst.l		%d1			# dfetch error?
1777	bne.w		movp_read_err		# yes
1778
1779	lsl.l		&0x8,%d2
1780	mov.b		%d0,%d2			# append bytes
1781
1782	add.w		&0x2,%a2		# incr addr by 2 bytes
1783	mov.l		%a2,%a0
1784
1785	bsr.l		_dmem_read_byte		# read second byte
1786
1787	tst.l		%d1			# dfetch error?
1788	bne.w		movp_read_err		# yes
1789
1790	lsl.l		&0x8,%d2
1791	mov.b		%d0,%d2			# append bytes
1792
1793	mov.b		EXC_OPWORD(%a6),%d1
1794	lsr.b		&0x1,%d1
1795	and.w		&0x7,%d1		# extract Dx from opcode word
1796
1797	mov.l		%d2,(EXC_DREGS,%a6,%d1.w*4) # store dx
1798
1799	rts
1800
1801# a0 = dst addr
1802m2rwtrans:
1803	mov.l		%a0,%a2			# store addr
1804
1805	bsr.l		_dmem_read_byte		# read first byte
1806
1807	tst.l		%d1			# dfetch error?
1808	bne.w		movp_read_err		# yes
1809
1810	mov.l		%d0,%d2
1811
1812	add.w		&0x2,%a2		# incr addr by 2 bytes
1813	mov.l		%a2,%a0
1814
1815	bsr.l		_dmem_read_byte		# read second byte
1816
1817	tst.l		%d1			# dfetch error?
1818	bne.w		movp_read_err		# yes
1819
1820	lsl.w		&0x8,%d2
1821	mov.b		%d0,%d2			# append bytes
1822
1823	mov.b		EXC_OPWORD(%a6),%d1
1824	lsr.b		&0x1,%d1
1825	and.w		&0x7,%d1		# extract Dx from opcode word
1826
1827	mov.w		%d2,(EXC_DREGS+2,%a6,%d1.w*4) # store dx
1828
1829	rts
1830
1831# if dmem_{read,write}_byte() returns a fail message in d1, the package
1832# must create an access error frame. here, we pass a skeleton fslw
1833# and the failing address to the routine that creates the new frame.
1834# FSLW:
1835#	write = true
1836#	size = byte
1837#	TM = data
1838#	software emulation error = true
1839movp_write_err:
1840	mov.l		%a2,%a0			# pass failing address
1841	mov.l		&0x00a10001,%d0		# pass fslw
1842	bra.l		isp_dacc
1843
1844# FSLW:
1845#	read = true
1846#	size = byte
1847#	TM = data
1848#	software emulation error = true
1849movp_read_err:
1850	mov.l		%a2,%a0			# pass failing address
1851	mov.l		&0x01210001,%d0		# pass fslw
1852	bra.l		isp_dacc
1853
1854#########################################################################
1855# XDEF ****************************************************************	#
1856#	_chk2_cmp2(): routine to emulate chk2/cmp2 instructions		#
1857#									#
1858# XREF ****************************************************************	#
1859#	_calc_ea(): calculate effective address				#
1860#	_dmem_read_long(): read operands				#
1861#	_dmem_read_word(): read operands				#
1862#	isp_dacc(): handle data access error exception			#
1863#									#
1864# INPUT ***************************************************************	#
1865#	none								#
1866#									#
1867# OUTPUT **************************************************************	#
1868#	If exiting through isp_dacc...					#
1869#		a0 = failing address					#
1870#		d0 = FSLW						#
1871#	else								#
1872#		none							#
1873#									#
1874# ALGORITHM ***********************************************************	#
1875#	First, calculate the effective address, then fetch the byte,	#
1876# word, or longword sized operands. Then, in the interest of		#
1877# simplicity, all operands are converted to longword size whether the	#
1878# operation is byte, word, or long. The bounds are sign extended	#
1879# accordingly. If Rn is a data register, Rn is also sign extended. If	#
1880# Rn is an address register, it need not be sign extended since the	#
1881# full register is always used.						#
1882#	The comparisons are made and the condition codes calculated.	#
1883# If the instruction is chk2 and the Rn value is out-of-bounds, set	#
1884# the ichk_flg in SPCOND_FLG.						#
1885#	If the memory fetch returns a failing value, pass the failing	#
1886# address and FSLW to the isp_dacc() routine.				#
1887#									#
1888#########################################################################
1889
1890	global		_chk2_cmp2
1891_chk2_cmp2:
1892
1893# passing size parameter doesn't matter since chk2 & cmp2 can't do
1894# either predecrement, postincrement, or immediate.
1895	bsr.l		_calc_ea		# calculate <ea>
1896
1897	mov.b		EXC_EXTWORD(%a6), %d0	# fetch hi extension word
1898	rol.b		&0x4, %d0		# rotate reg bits into lo
1899	and.w		&0xf, %d0		# extract reg bits
1900
1901	mov.l		(EXC_DREGS,%a6,%d0.w*4), %d2 # get regval
1902
1903	cmpi.b		EXC_OPWORD(%a6), &0x2	# what size is operation?
1904	blt.b		chk2_cmp2_byte		# size == byte
1905	beq.b		chk2_cmp2_word		# size == word
1906
1907# the bounds are longword size. call routine to read the lower
1908# bound into d0 and the higher bound into d1.
1909chk2_cmp2_long:
1910	mov.l		%a0,%a2			# save copy of <ea>
1911	bsr.l		_dmem_read_long		# fetch long lower bound
1912
1913	tst.l		%d1			# dfetch error?
1914	bne.w		chk2_cmp2_err_l		# yes
1915
1916	mov.l		%d0,%d3			# save long lower bound
1917	addq.l		&0x4,%a2
1918	mov.l		%a2,%a0			# pass <ea> of long upper bound
1919	bsr.l		_dmem_read_long		# fetch long upper bound
1920
1921	tst.l		%d1			# dfetch error?
1922	bne.w		chk2_cmp2_err_l		# yes
1923
1924	mov.l		%d0,%d1			# long upper bound in d1
1925	mov.l		%d3,%d0			# long lower bound in d0
1926	bra.w		chk2_cmp2_compare	# go do the compare emulation
1927
1928# the bounds are word size. fetch them in one subroutine call by
1929# reading a longword. sign extend both. if it's a data operation,
1930# sign extend Rn to long, also.
1931chk2_cmp2_word:
1932	mov.l		%a0,%a2
1933	bsr.l		_dmem_read_long		# fetch 2 word bounds
1934
1935	tst.l		%d1			# dfetch error?
1936	bne.w		chk2_cmp2_err_l		# yes
1937
1938	mov.w		%d0, %d1		# place hi in %d1
1939	swap		%d0			# place lo in %d0
1940
1941	ext.l		%d0			# sign extend lo bnd
1942	ext.l		%d1			# sign extend hi bnd
1943
1944	btst		&0x7, EXC_EXTWORD(%a6)	# address compare?
1945	bne.w		chk2_cmp2_compare	# yes; don't sign extend
1946
1947# operation is a data register compare.
1948# sign extend word to long so we can do simple longword compares.
1949	ext.l		%d2			# sign extend data word
1950	bra.w		chk2_cmp2_compare	# go emulate compare
1951
1952# the bounds are byte size. fetch them in one subroutine call by
1953# reading a word. sign extend both. if it's a data operation,
1954# sign extend Rn to long, also.
1955chk2_cmp2_byte:
1956	mov.l		%a0,%a2
1957	bsr.l		_dmem_read_word		# fetch 2 byte bounds
1958
1959	tst.l		%d1			# dfetch error?
1960	bne.w		chk2_cmp2_err_w		# yes
1961
1962	mov.b		%d0, %d1		# place hi in %d1
1963	lsr.w		&0x8, %d0		# place lo in %d0
1964
1965	extb.l		%d0			# sign extend lo bnd
1966	extb.l		%d1			# sign extend hi bnd
1967
1968	btst		&0x7, EXC_EXTWORD(%a6)	# address compare?
1969	bne.b		chk2_cmp2_compare	# yes; don't sign extend
1970
1971# operation is a data register compare.
1972# sign extend byte to long so we can do simple longword compares.
1973	extb.l		%d2			# sign extend data byte
1974
1975#
1976# To set the ccodes correctly:
1977#	(1) save 'Z' bit from (Rn - lo)
1978#	(2) save 'Z' and 'N' bits from ((hi - lo) - (Rn - hi))
1979#	(3) keep 'X', 'N', and 'V' from before instruction
1980#	(4) combine ccodes
1981#
1982chk2_cmp2_compare:
1983	sub.l		%d0, %d2		# (Rn - lo)
1984	mov.w		%cc, %d3		# fetch resulting ccodes
1985	andi.b		&0x4, %d3		# keep 'Z' bit
1986	sub.l		%d0, %d1		# (hi - lo)
1987	cmp.l		%d1,%d2			# ((hi - lo) - (Rn - hi))
1988
1989	mov.w		%cc, %d4		# fetch resulting ccodes
1990	or.b		%d4, %d3		# combine w/ earlier ccodes
1991	andi.b		&0x5, %d3		# keep 'Z' and 'N'
1992
1993	mov.w		EXC_CC(%a6), %d4	# fetch old ccodes
1994	andi.b		&0x1a, %d4		# keep 'X','N','V' bits
1995	or.b		%d3, %d4		# insert new ccodes
1996	mov.w		%d4, EXC_CC(%a6)	# save new ccodes
1997
1998	btst		&0x3, EXC_EXTWORD(%a6)	# separate chk2,cmp2
1999	bne.b		chk2_finish		# it's a chk2
2000
2001	rts
2002
2003# this code handles the only difference between chk2 and cmp2. chk2 would
2004# have trapped out if the value was out of bounds. we check this by seeing
2005# if the 'N' bit was set by the operation.
2006chk2_finish:
2007	btst		&0x0, %d4		# is 'N' bit set?
2008	bne.b		chk2_trap		# yes;chk2 should trap
2009	rts
2010chk2_trap:
2011	mov.b		&ichk_flg,SPCOND_FLG(%a6) # set "special case" flag
2012	rts
2013
2014# if dmem_read_{long,word}() returns a fail message in d1, the package
2015# must create an access error frame. here, we pass a skeleton fslw
2016# and the failing address to the routine that creates the new frame.
2017# FSLW:
2018#	read = true
2019#	size = longword
2020#	TM = data
2021#	software emulation error = true
2022chk2_cmp2_err_l:
2023	mov.l		%a2,%a0			# pass failing address
2024	mov.l		&0x01010001,%d0		# pass fslw
2025	bra.l		isp_dacc
2026
2027# FSLW:
2028#	read = true
2029#	size = word
2030#	TM = data
2031#	software emulation error = true
2032chk2_cmp2_err_w:
2033	mov.l		%a2,%a0			# pass failing address
2034	mov.l		&0x01410001,%d0		# pass fslw
2035	bra.l		isp_dacc
2036
2037#########################################################################
2038# XDEF ****************************************************************	#
2039#	_div64(): routine to emulate div{u,s}.l <ea>,Dr:Dq		#
2040#							64/32->32r:32q	#
2041#									#
2042# XREF ****************************************************************	#
2043#	_calc_ea() - calculate effective address			#
2044#	isp_iacc() - handle instruction access error exception		#
2045#	isp_dacc() - handle data access error exception			#
2046#	isp_restore() - restore An on access error w/ -() or ()+	#
2047#									#
2048# INPUT ***************************************************************	#
2049#	none								#
2050#									#
2051# OUTPUT **************************************************************	#
2052#	If exiting through isp_dacc...					#
2053#		a0 = failing address					#
2054#		d0 = FSLW						#
2055#	else								#
2056#		none							#
2057#									#
2058# ALGORITHM ***********************************************************	#
2059#	First, decode the operand location. If it's in Dn, fetch from	#
2060# the stack. If it's in memory, use _calc_ea() to calculate the		#
2061# effective address. Use _dmem_read_long() to fetch at that address.	#
2062# Unless the operand is immediate data. Then use _imem_read_long().	#
2063# Send failures to isp_dacc() or isp_iacc() as appropriate.		#
2064#	If the operands are signed, make them unsigned and save	the	#
2065# sign info for later. Separate out special cases like divide-by-zero	#
2066# or 32-bit divides if possible. Else, use a special math algorithm	#
2067# to calculate the result.						#
2068#	Restore sign info if signed instruction. Set the condition	#
2069# codes. Set idbyz_flg in SPCOND_FLG if divisor was zero. Store the	#
2070# quotient and remainder in the appropriate data registers on the stack.#
2071#									#
2072#########################################################################
2073
2074set	NDIVISOR,	EXC_TEMP+0x0
2075set	NDIVIDEND,	EXC_TEMP+0x1
2076set	NDRSAVE,	EXC_TEMP+0x2
2077set	NDQSAVE,	EXC_TEMP+0x4
2078set	DDSECOND,	EXC_TEMP+0x6
2079set	DDQUOTIENT,	EXC_TEMP+0x8
2080set	DDNORMAL,	EXC_TEMP+0xc
2081
2082	global		_div64
2083#############
2084# div(u,s)l #
2085#############
2086_div64:
2087	mov.b		EXC_OPWORD+1(%a6), %d0
2088	andi.b		&0x38, %d0		# extract src mode
2089
2090	bne.w		dcontrolmodel_s		# %dn dest or control mode?
2091
2092	mov.b		EXC_OPWORD+1(%a6), %d0	# extract Dn from opcode
2093	andi.w		&0x7, %d0
2094	mov.l		(EXC_DREGS,%a6,%d0.w*4), %d7 # fetch divisor from register
2095
2096dgotsrcl:
2097	beq.w		div64eq0		# divisor is = 0!!!
2098
2099	mov.b		EXC_EXTWORD+1(%a6), %d0	# extract Dr from extword
2100	mov.b		EXC_EXTWORD(%a6), %d1	# extract Dq from extword
2101	and.w		&0x7, %d0
2102	lsr.b		&0x4, %d1
2103	and.w		&0x7, %d1
2104	mov.w		%d0, NDRSAVE(%a6)	# save Dr for later
2105	mov.w		%d1, NDQSAVE(%a6)	# save Dq for later
2106
2107# fetch %dr and %dq directly off stack since all regs are saved there
2108	mov.l		(EXC_DREGS,%a6,%d0.w*4), %d5 # get dividend hi
2109	mov.l		(EXC_DREGS,%a6,%d1.w*4), %d6 # get dividend lo
2110
2111# separate signed and unsigned divide
2112	btst		&0x3, EXC_EXTWORD(%a6)	# signed or unsigned?
2113	beq.b		dspecialcases		# use positive divide
2114
2115# save the sign of the divisor
2116# make divisor unsigned if it's negative
2117	tst.l		%d7			# chk sign of divisor
2118	slt		NDIVISOR(%a6)		# save sign of divisor
2119	bpl.b		dsgndividend
2120	neg.l		%d7			# complement negative divisor
2121
2122# save the sign of the dividend
2123# make dividend unsigned if it's negative
2124dsgndividend:
2125	tst.l		%d5			# chk sign of hi(dividend)
2126	slt		NDIVIDEND(%a6)		# save sign of dividend
2127	bpl.b		dspecialcases
2128
2129	mov.w		&0x0, %cc		# clear 'X' cc bit
2130	negx.l		%d6			# complement signed dividend
2131	negx.l		%d5
2132
2133# extract some special cases:
2134#	- is (dividend == 0) ?
2135#	- is (hi(dividend) == 0 && (divisor <= lo(dividend))) ? (32-bit div)
2136dspecialcases:
2137	tst.l		%d5			# is (hi(dividend) == 0)
2138	bne.b		dnormaldivide		# no, so try it the long way
2139
2140	tst.l		%d6			# is (lo(dividend) == 0), too
2141	beq.w		ddone			# yes, so (dividend == 0)
2142
2143	cmp.l		%d7,%d6			# is (divisor <= lo(dividend))
2144	bls.b		d32bitdivide		# yes, so use 32 bit divide
2145
2146	exg		%d5,%d6			# q = 0, r = dividend
2147	bra.w		divfinish		# can't divide, we're done.
2148
2149d32bitdivide:
2150	tdivu.l		%d7, %d5:%d6		# it's only a 32/32 bit div!
2151
2152	bra.b		divfinish
2153
2154dnormaldivide:
2155# last special case:
2156#	- is hi(dividend) >= divisor ? if yes, then overflow
2157	cmp.l		%d7,%d5
2158	bls.b		ddovf			# answer won't fit in 32 bits
2159
2160# perform the divide algorithm:
2161	bsr.l		dclassical		# do int divide
2162
2163# separate into signed and unsigned finishes.
2164divfinish:
2165	btst		&0x3, EXC_EXTWORD(%a6)	# do divs, divu separately
2166	beq.b		ddone			# divu has no processing!!!
2167
2168# it was a divs.l, so ccode setting is a little more complicated...
2169	tst.b		NDIVIDEND(%a6)		# remainder has same sign
2170	beq.b		dcc			# as dividend.
2171	neg.l		%d5			# sgn(rem) = sgn(dividend)
2172dcc:
2173	mov.b		NDIVISOR(%a6), %d0
2174	eor.b		%d0, NDIVIDEND(%a6)	# chk if quotient is negative
2175	beq.b		dqpos			# branch to quot positive
2176
2177# 0x80000000 is the largest number representable as a 32-bit negative
2178# number. the negative of 0x80000000 is 0x80000000.
2179	cmpi.l		%d6, &0x80000000	# will (-quot) fit in 32 bits?
2180	bhi.b		ddovf
2181
2182	neg.l		%d6			# make (-quot) 2's comp
2183
2184	bra.b		ddone
2185
2186dqpos:
2187	btst		&0x1f, %d6		# will (+quot) fit in 32 bits?
2188	bne.b		ddovf
2189
2190ddone:
2191# at this point, result is normal so ccodes are set based on result.
2192	mov.w		EXC_CC(%a6), %cc
2193	tst.l		%d6			# set %ccode bits
2194	mov.w		%cc, EXC_CC(%a6)
2195
2196	mov.w		NDRSAVE(%a6), %d0	# get Dr off stack
2197	mov.w		NDQSAVE(%a6), %d1	# get Dq off stack
2198
2199# if the register numbers are the same, only the quotient gets saved.
2200# so, if we always save the quotient second, we save ourselves a cmp&beq
2201	mov.l		%d5, (EXC_DREGS,%a6,%d0.w*4) # save remainder
2202	mov.l		%d6, (EXC_DREGS,%a6,%d1.w*4) # save quotient
2203
2204	rts
2205
2206ddovf:
2207	bset		&0x1, EXC_CC+1(%a6)	# 'V' set on overflow
2208	bclr		&0x0, EXC_CC+1(%a6)	# 'C' cleared on overflow
2209
2210	rts
2211
2212div64eq0:
2213	andi.b		&0x1e, EXC_CC+1(%a6)	# clear 'C' bit on divbyzero
2214	ori.b		&idbyz_flg,SPCOND_FLG(%a6) # set "special case" flag
2215	rts
2216
2217###########################################################################
2218#########################################################################
2219# This routine uses the 'classical' Algorithm D from Donald Knuth's	#
2220# Art of Computer Programming, vol II, Seminumerical Algorithms.	#
2221# For this implementation b=2**16, and the target is U1U2U3U4/V1V2,	#
2222# where U,V are words of the quadword dividend and longword divisor,	#
2223# and U1, V1 are the most significant words.				#
2224#									#
2225# The most sig. longword of the 64 bit dividend must be in %d5, least	#
2226# in %d6. The divisor must be in the variable ddivisor, and the		#
2227# signed/unsigned flag ddusign must be set (0=unsigned,1=signed).	#
2228# The quotient is returned in %d6, remainder in %d5, unless the		#
2229# v (overflow) bit is set in the saved %ccr. If overflow, the dividend	#
2230# is unchanged.								#
2231#########################################################################
2232dclassical:
2233# if the divisor msw is 0, use simpler algorithm then the full blown
2234# one at ddknuth:
2235
2236	cmpi.l		%d7, &0xffff
2237	bhi.b		ddknuth			# go use D. Knuth algorithm
2238
2239# Since the divisor is only a word (and larger than the mslw of the dividend),
2240# a simpler algorithm may be used :
2241# In the general case, four quotient words would be created by
2242# dividing the divisor word into each dividend word. In this case,
2243# the first two quotient words must be zero, or overflow would occur.
2244# Since we already checked this case above, we can treat the most significant
2245# longword of the dividend as (0) remainder (see Knuth) and merely complete
2246# the last two divisions to get a quotient longword and word remainder:
2247
2248	clr.l		%d1
2249	swap		%d5			# same as r*b if previous step rqd
2250	swap		%d6			# get u3 to lsw position
2251	mov.w		%d6, %d5		# rb + u3
2252
2253	divu.w		%d7, %d5
2254
2255	mov.w		%d5, %d1		# first quotient word
2256	swap		%d6			# get u4
2257	mov.w		%d6, %d5		# rb + u4
2258
2259	divu.w		%d7, %d5
2260
2261	swap		%d1
2262	mov.w		%d5, %d1		# 2nd quotient 'digit'
2263	clr.w		%d5
2264	swap		%d5			# now remainder
2265	mov.l		%d1, %d6		# and quotient
2266
2267	rts
2268
2269ddknuth:
2270# In this algorithm, the divisor is treated as a 2 digit (word) number
2271# which is divided into a 3 digit (word) dividend to get one quotient
2272# digit (word). After subtraction, the dividend is shifted and the
2273# process repeated. Before beginning, the divisor and quotient are
2274# 'normalized' so that the process of estimating the quotient digit
2275# will yield verifiably correct results..
2276
2277	clr.l		DDNORMAL(%a6)		# count of shifts for normalization
2278	clr.b		DDSECOND(%a6)		# clear flag for quotient digits
2279	clr.l		%d1			# %d1 will hold trial quotient
2280ddnchk:
2281	btst		&31, %d7		# must we normalize? first word of
2282	bne.b		ddnormalized		# divisor (V1) must be >= 65536/2
2283	addq.l		&0x1, DDNORMAL(%a6)	# count normalization shifts
2284	lsl.l		&0x1, %d7		# shift the divisor
2285	lsl.l		&0x1, %d6		# shift u4,u3 with overflow to u2
2286	roxl.l		&0x1, %d5		# shift u1,u2
2287	bra.w		ddnchk
2288ddnormalized:
2289
2290# Now calculate an estimate of the quotient words (msw first, then lsw).
2291# The comments use subscripts for the first quotient digit determination.
2292	mov.l		%d7, %d3		# divisor
2293	mov.l		%d5, %d2		# dividend mslw
2294	swap		%d2
2295	swap		%d3
2296	cmp.w		%d2, %d3		# V1 = U1 ?
2297	bne.b		ddqcalc1
2298	mov.w		&0xffff, %d1		# use max trial quotient word
2299	bra.b		ddadj0
2300ddqcalc1:
2301	mov.l		%d5, %d1
2302
2303	divu.w		%d3, %d1		# use quotient of mslw/msw
2304
2305	andi.l		&0x0000ffff, %d1	# zero any remainder
2306ddadj0:
2307
2308# now test the trial quotient and adjust. This step plus the
2309# normalization assures (according to Knuth) that the trial
2310# quotient will be at worst 1 too large.
2311	mov.l		%d6, -(%sp)
2312	clr.w		%d6			# word u3 left
2313	swap		%d6			# in lsw position
2314ddadj1: mov.l		%d7, %d3
2315	mov.l		%d1, %d2
2316	mulu.w		%d7, %d2		# V2q
2317	swap		%d3
2318	mulu.w		%d1, %d3		# V1q
2319	mov.l		%d5, %d4		# U1U2
2320	sub.l		%d3, %d4		# U1U2 - V1q
2321
2322	swap		%d4
2323
2324	mov.w		%d4,%d0
2325	mov.w		%d6,%d4			# insert lower word (U3)
2326
2327	tst.w		%d0			# is upper word set?
2328	bne.w		ddadjd1
2329
2330#	add.l		%d6, %d4		# (U1U2 - V1q) + U3
2331
2332	cmp.l		%d2, %d4
2333	bls.b		ddadjd1			# is V2q > (U1U2-V1q) + U3 ?
2334	subq.l		&0x1, %d1		# yes, decrement and recheck
2335	bra.b		ddadj1
2336ddadjd1:
2337# now test the word by multiplying it by the divisor (V1V2) and comparing
2338# the 3 digit (word) result with the current dividend words
2339	mov.l		%d5, -(%sp)		# save %d5 (%d6 already saved)
2340	mov.l		%d1, %d6
2341	swap		%d6			# shift answer to ms 3 words
2342	mov.l		%d7, %d5
2343	bsr.l		dmm2
2344	mov.l		%d5, %d2		# now %d2,%d3 are trial*divisor
2345	mov.l		%d6, %d3
2346	mov.l		(%sp)+, %d5		# restore dividend
2347	mov.l		(%sp)+, %d6
2348	sub.l		%d3, %d6
2349	subx.l		%d2, %d5		# subtract double precision
2350	bcc		dd2nd			# no carry, do next quotient digit
2351	subq.l		&0x1, %d1		# q is one too large
2352# need to add back divisor longword to current ms 3 digits of dividend
2353# - according to Knuth, this is done only 2 out of 65536 times for random
2354# divisor, dividend selection.
2355	clr.l		%d2
2356	mov.l		%d7, %d3
2357	swap		%d3
2358	clr.w		%d3			# %d3 now ls word of divisor
2359	add.l		%d3, %d6		# aligned with 3rd word of dividend
2360	addx.l		%d2, %d5
2361	mov.l		%d7, %d3
2362	clr.w		%d3			# %d3 now ms word of divisor
2363	swap		%d3			# aligned with 2nd word of dividend
2364	add.l		%d3, %d5
2365dd2nd:
2366	tst.b		DDSECOND(%a6)		# both q words done?
2367	bne.b		ddremain
2368# first quotient digit now correct. store digit and shift the
2369# (subtracted) dividend
2370	mov.w		%d1, DDQUOTIENT(%a6)
2371	clr.l		%d1
2372	swap		%d5
2373	swap		%d6
2374	mov.w		%d6, %d5
2375	clr.w		%d6
2376	st		DDSECOND(%a6)		# second digit
2377	bra.w		ddnormalized
2378ddremain:
2379# add 2nd word to quotient, get the remainder.
2380	mov.w		%d1, DDQUOTIENT+2(%a6)
2381# shift down one word/digit to renormalize remainder.
2382	mov.w		%d5, %d6
2383	swap		%d6
2384	swap		%d5
2385	mov.l		DDNORMAL(%a6), %d7	# get norm shift count
2386	beq.b		ddrn
2387	subq.l		&0x1, %d7		# set for loop count
2388ddnlp:
2389	lsr.l		&0x1, %d5		# shift into %d6
2390	roxr.l		&0x1, %d6
2391	dbf		%d7, ddnlp
2392ddrn:
2393	mov.l		%d6, %d5		# remainder
2394	mov.l		DDQUOTIENT(%a6), %d6	# quotient
2395
2396	rts
2397dmm2:
2398# factors for the 32X32->64 multiplication are in %d5 and %d6.
2399# returns 64 bit result in %d5 (hi) %d6(lo).
2400# destroys %d2,%d3,%d4.
2401
2402# multiply hi,lo words of each factor to get 4 intermediate products
2403	mov.l		%d6, %d2
2404	mov.l		%d6, %d3
2405	mov.l		%d5, %d4
2406	swap		%d3
2407	swap		%d4
2408	mulu.w		%d5, %d6		# %d6 <- lsw*lsw
2409	mulu.w		%d3, %d5		# %d5 <- msw-dest*lsw-source
2410	mulu.w		%d4, %d2		# %d2 <- msw-source*lsw-dest
2411	mulu.w		%d4, %d3		# %d3 <- msw*msw
2412# now use swap and addx to consolidate to two longwords
2413	clr.l		%d4
2414	swap		%d6
2415	add.w		%d5, %d6		# add msw of l*l to lsw of m*l product
2416	addx.w		%d4, %d3		# add any carry to m*m product
2417	add.w		%d2, %d6		# add in lsw of other m*l product
2418	addx.w		%d4, %d3		# add any carry to m*m product
2419	swap		%d6			# %d6 is low 32 bits of final product
2420	clr.w		%d5
2421	clr.w		%d2			# lsw of two mixed products used,
2422	swap		%d5			# now use msws of longwords
2423	swap		%d2
2424	add.l		%d2, %d5
2425	add.l		%d3, %d5		# %d5 now ms 32 bits of final product
2426	rts
2427
2428##########
2429dcontrolmodel_s:
2430	movq.l		&LONG,%d0
2431	bsr.l		_calc_ea		# calc <ea>
2432
2433	cmpi.b		SPCOND_FLG(%a6),&immed_flg # immediate addressing mode?
2434	beq.b		dimmed			# yes
2435
2436	mov.l		%a0,%a2
2437	bsr.l		_dmem_read_long		# fetch divisor from <ea>
2438
2439	tst.l		%d1			# dfetch error?
2440	bne.b		div64_err		# yes
2441
2442	mov.l		%d0, %d7
2443	bra.w		dgotsrcl
2444
2445# we have to split out immediate data here because it must be read using
2446# imem_read() instead of dmem_read(). this becomes especially important
2447# if the fetch runs into some deadly fault.
2448dimmed:
2449	addq.l		&0x4,EXC_EXTWPTR(%a6)
2450	bsr.l		_imem_read_long		# read immediate value
2451
2452	tst.l		%d1			# ifetch error?
2453	bne.l		isp_iacc		# yes
2454
2455	mov.l		%d0,%d7
2456	bra.w		dgotsrcl
2457
2458##########
2459
2460# if dmem_read_long() returns a fail message in d1, the package
2461# must create an access error frame. here, we pass a skeleton fslw
2462# and the failing address to the routine that creates the new frame.
2463# also, we call isp_restore in case the effective addressing mode was
2464# (an)+ or -(an) in which case the previous "an" value must be restored.
2465# FSLW:
2466#	read = true
2467#	size = longword
2468#	TM = data
2469#	software emulation error = true
2470div64_err:
2471	bsr.l		isp_restore		# restore addr reg
2472	mov.l		%a2,%a0			# pass failing address
2473	mov.l		&0x01010001,%d0		# pass fslw
2474	bra.l		isp_dacc
2475
2476#########################################################################
2477# XDEF ****************************************************************	#
2478#	_mul64(): routine to emulate mul{u,s}.l <ea>,Dh:Dl 32x32->64	#
2479#									#
2480# XREF ****************************************************************	#
2481#	_calc_ea() - calculate effective address			#
2482#	isp_iacc() - handle instruction access error exception		#
2483#	isp_dacc() - handle data access error exception			#
2484#	isp_restore() - restore An on access error w/ -() or ()+	#
2485#									#
2486# INPUT ***************************************************************	#
2487#	none								#
2488#									#
2489# OUTPUT **************************************************************	#
2490#	If exiting through isp_dacc...					#
2491#		a0 = failing address					#
2492#		d0 = FSLW						#
2493#	else								#
2494#		none							#
2495#									#
2496# ALGORITHM ***********************************************************	#
2497#	First, decode the operand location. If it's in Dn, fetch from	#
2498# the stack. If it's in memory, use _calc_ea() to calculate the		#
2499# effective address. Use _dmem_read_long() to fetch at that address.	#
2500# Unless the operand is immediate data. Then use _imem_read_long().	#
2501# Send failures to isp_dacc() or isp_iacc() as appropriate.		#
2502#	If the operands are signed, make them unsigned and save the	#
2503# sign info for later. Perform the multiplication using 16x16->32	#
2504# unsigned multiplies and "add" instructions. Store the high and low	#
2505# portions of the result in the appropriate data registers on the	#
2506# stack. Calculate the condition codes, also.				#
2507#									#
2508#########################################################################
2509
2510#############
2511# mul(u,s)l #
2512#############
2513	global		_mul64
2514_mul64:
2515	mov.b		EXC_OPWORD+1(%a6), %d0	# extract src {mode,reg}
2516	cmpi.b		%d0, &0x7		# is src mode Dn or other?
2517	bgt.w		mul64_memop		# src is in memory
2518
2519# multiplier operand in the data register file.
2520# must extract the register number and fetch the operand from the stack.
2521mul64_regop:
2522	andi.w		&0x7, %d0		# extract Dn
2523	mov.l		(EXC_DREGS,%a6,%d0.w*4), %d3 # fetch multiplier
2524
2525# multiplier is in %d3. now, extract Dl and Dh fields and fetch the
2526# multiplicand from the data register specified by Dl.
2527mul64_multiplicand:
2528	mov.w		EXC_EXTWORD(%a6), %d2	# fetch ext word
2529	clr.w		%d1			# clear Dh reg
2530	mov.b		%d2, %d1		# grab Dh
2531	rol.w		&0x4, %d2		# align Dl byte
2532	andi.w		&0x7, %d2		# extract Dl
2533
2534	mov.l		(EXC_DREGS,%a6,%d2.w*4), %d4 # get multiplicand
2535
2536# check for the case of "zero" result early
2537	tst.l		%d4			# test multiplicand
2538	beq.w		mul64_zero		# handle zero separately
2539	tst.l		%d3			# test multiplier
2540	beq.w		mul64_zero		# handle zero separately
2541
2542# multiplier is in %d3 and multiplicand is in %d4.
2543# if the operation is to be signed, then the operands are converted
2544# to unsigned and the result sign is saved for the end.
2545	clr.b		EXC_TEMP(%a6)		# clear temp space
2546	btst		&0x3, EXC_EXTWORD(%a6)	# signed or unsigned?
2547	beq.b		mul64_alg		# unsigned; skip sgn calc
2548
2549	tst.l		%d3			# is multiplier negative?
2550	bge.b		mul64_chk_md_sgn	# no
2551	neg.l		%d3			# make multiplier positive
2552	ori.b		&0x1, EXC_TEMP(%a6)	# save multiplier sgn
2553
2554# the result sign is the exclusive or of the operand sign bits.
2555mul64_chk_md_sgn:
2556	tst.l		%d4			# is multiplicand negative?
2557	bge.b		mul64_alg		# no
2558	neg.l		%d4			# make multiplicand positive
2559	eori.b		&0x1, EXC_TEMP(%a6)	# calculate correct sign
2560
2561#########################################################################
2562#	63			   32				0	#
2563#	----------------------------					#
2564#	| hi(mplier) * hi(mplicand)|					#
2565#	----------------------------					#
2566#		     -----------------------------			#
2567#		     | hi(mplier) * lo(mplicand) |			#
2568#		     -----------------------------			#
2569#		     -----------------------------			#
2570#		     | lo(mplier) * hi(mplicand) |			#
2571#		     -----------------------------			#
2572#	  |			   -----------------------------	#
2573#	--|--			   | lo(mplier) * lo(mplicand) |	#
2574#	  |			   -----------------------------	#
2575#	========================================================	#
2576#	--------------------------------------------------------	#
2577#	|	hi(result)	   |	    lo(result)         |	#
2578#	--------------------------------------------------------	#
2579#########################################################################
2580mul64_alg:
2581# load temp registers with operands
2582	mov.l		%d3, %d5		# mr in %d5
2583	mov.l		%d3, %d6		# mr in %d6
2584	mov.l		%d4, %d7		# md in %d7
2585	swap		%d6			# hi(mr) in lo %d6
2586	swap		%d7			# hi(md) in lo %d7
2587
2588# complete necessary multiplies:
2589	mulu.w		%d4, %d3		# [1] lo(mr) * lo(md)
2590	mulu.w		%d6, %d4		# [2] hi(mr) * lo(md)
2591	mulu.w		%d7, %d5		# [3] lo(mr) * hi(md)
2592	mulu.w		%d7, %d6		# [4] hi(mr) * hi(md)
2593
2594# add lo portions of [2],[3] to hi portion of [1].
2595# add carries produced from these adds to [4].
2596# lo([1]) is the final lo 16 bits of the result.
2597	clr.l		%d7			# load %d7 w/ zero value
2598	swap		%d3			# hi([1]) <==> lo([1])
2599	add.w		%d4, %d3		# hi([1]) + lo([2])
2600	addx.l		%d7, %d6		#    [4]  + carry
2601	add.w		%d5, %d3		# hi([1]) + lo([3])
2602	addx.l		%d7, %d6		#    [4]  + carry
2603	swap		%d3			# lo([1]) <==> hi([1])
2604
2605# lo portions of [2],[3] have been added in to final result.
2606# now, clear lo, put hi in lo reg, and add to [4]
2607	clr.w		%d4			# clear lo([2])
2608	clr.w		%d5			# clear hi([3])
2609	swap		%d4			# hi([2]) in lo %d4
2610	swap		%d5			# hi([3]) in lo %d5
2611	add.l		%d5, %d4		#    [4]  + hi([2])
2612	add.l		%d6, %d4		#    [4]  + hi([3])
2613
2614# unsigned result is now in {%d4,%d3}
2615	tst.b		EXC_TEMP(%a6)		# should result be signed?
2616	beq.b		mul64_done		# no
2617
2618# result should be a signed negative number.
2619# compute 2's complement of the unsigned number:
2620#   -negate all bits and add 1
2621mul64_neg:
2622	not.l		%d3			# negate lo(result) bits
2623	not.l		%d4			# negate hi(result) bits
2624	addq.l		&1, %d3			# add 1 to lo(result)
2625	addx.l		%d7, %d4		# add carry to hi(result)
2626
2627# the result is saved to the register file.
2628# for '040 compatibility, if Dl == Dh then only the hi(result) is
2629# saved. so, saving hi after lo accomplishes this without need to
2630# check Dl,Dh equality.
2631mul64_done:
2632	mov.l		%d3, (EXC_DREGS,%a6,%d2.w*4) # save lo(result)
2633	mov.w		&0x0, %cc
2634	mov.l		%d4, (EXC_DREGS,%a6,%d1.w*4) # save hi(result)
2635
2636# now, grab the condition codes. only one that can be set is 'N'.
2637# 'N' CAN be set if the operation is unsigned if bit 63 is set.
2638	mov.w		%cc, %d7		# fetch %ccr to see if 'N' set
2639	andi.b		&0x8, %d7		# extract 'N' bit
2640
2641mul64_ccode_set:
2642	mov.b		EXC_CC+1(%a6), %d6	# fetch previous %ccr
2643	andi.b		&0x10, %d6		# all but 'X' bit changes
2644
2645	or.b		%d7, %d6		# group 'X' and 'N'
2646	mov.b		%d6, EXC_CC+1(%a6)	# save new %ccr
2647
2648	rts
2649
2650# one or both of the operands is zero so the result is also zero.
2651# save the zero result to the register file and set the 'Z' ccode bit.
2652mul64_zero:
2653	clr.l		(EXC_DREGS,%a6,%d2.w*4) # save lo(result)
2654	clr.l		(EXC_DREGS,%a6,%d1.w*4) # save hi(result)
2655
2656	movq.l		&0x4, %d7		# set 'Z' ccode bit
2657	bra.b		mul64_ccode_set		# finish ccode set
2658
2659##########
2660
2661# multiplier operand is in memory at the effective address.
2662# must calculate the <ea> and go fetch the 32-bit operand.
2663mul64_memop:
2664	movq.l		&LONG, %d0		# pass # of bytes
2665	bsr.l		_calc_ea		# calculate <ea>
2666
2667	cmpi.b		SPCOND_FLG(%a6),&immed_flg # immediate addressing mode?
2668	beq.b		mul64_immed		# yes
2669
2670	mov.l		%a0,%a2
2671	bsr.l		_dmem_read_long		# fetch src from addr (%a0)
2672
2673	tst.l		%d1			# dfetch error?
2674	bne.w		mul64_err		# yes
2675
2676	mov.l		%d0, %d3		# store multiplier in %d3
2677
2678	bra.w		mul64_multiplicand
2679
2680# we have to split out immediate data here because it must be read using
2681# imem_read() instead of dmem_read(). this becomes especially important
2682# if the fetch runs into some deadly fault.
2683mul64_immed:
2684	addq.l		&0x4,EXC_EXTWPTR(%a6)
2685	bsr.l		_imem_read_long		# read immediate value
2686
2687	tst.l		%d1			# ifetch error?
2688	bne.l		isp_iacc		# yes
2689
2690	mov.l		%d0,%d3
2691	bra.w		mul64_multiplicand
2692
2693##########
2694
2695# if dmem_read_long() returns a fail message in d1, the package
2696# must create an access error frame. here, we pass a skeleton fslw
2697# and the failing address to the routine that creates the new frame.
2698# also, we call isp_restore in case the effective addressing mode was
2699# (an)+ or -(an) in which case the previous "an" value must be restored.
2700# FSLW:
2701#	read = true
2702#	size = longword
2703#	TM = data
2704#	software emulation error = true
2705mul64_err:
2706	bsr.l		isp_restore		# restore addr reg
2707	mov.l		%a2,%a0			# pass failing address
2708	mov.l		&0x01010001,%d0		# pass fslw
2709	bra.l		isp_dacc
2710
2711#########################################################################
2712# XDEF ****************************************************************	#
2713#	_compandset2(): routine to emulate cas2()			#
2714#			(internal to package)				#
2715#									#
2716#	_isp_cas2_finish(): store ccodes, store compare regs		#
2717#			    (external to package)			#
2718#									#
2719# XREF ****************************************************************	#
2720#	_real_lock_page() - "callout" to lock op's page from page-outs	#
2721#	_cas_terminate2() - access error exit				#
2722#	_real_cas2() - "callout" to core cas2 emulation code		#
2723#	_real_unlock_page() - "callout" to unlock page			#
2724#									#
2725# INPUT ***************************************************************	#
2726# _compandset2():							#
2727#	d0 = instruction extension word					#
2728#									#
2729# _isp_cas2_finish():							#
2730#	see cas2 core emulation code					#
2731#									#
2732# OUTPUT **************************************************************	#
2733# _compandset2():							#
2734#	see cas2 core emulation code					#
2735#									#
2736# _isp_cas_finish():							#
2737#	None (register file or memroy changed as appropriate)		#
2738#									#
2739# ALGORITHM ***********************************************************	#
2740# compandset2():							#
2741#	Decode the instruction and fetch the appropriate Update and	#
2742# Compare operands. Then call the "callout" _real_lock_page() for each	#
2743# memory operand address so that the operating system can keep these	#
2744# pages from being paged out. If either _real_lock_page() fails, exit	#
2745# through _cas_terminate2(). Don't forget to unlock the 1st locked page	#
2746# using _real_unlock_paged() if the 2nd lock-page fails.		#
2747# Finally, branch to the core cas2 emulation code by calling the	#
2748# "callout" _real_cas2().						#
2749#									#
2750# _isp_cas2_finish():							#
2751#	Re-perform the comparison so we can determine the condition	#
2752# codes which were too much trouble to keep around during the locked	#
2753# emulation. Then unlock each operands page by calling the "callout"	#
2754# _real_unlock_page().							#
2755#									#
2756#########################################################################
2757
2758set ADDR1,	EXC_TEMP+0xc
2759set ADDR2,	EXC_TEMP+0x0
2760set DC2,	EXC_TEMP+0xa
2761set DC1,	EXC_TEMP+0x8
2762
2763	global		_compandset2
2764_compandset2:
2765	mov.l		%d0,EXC_TEMP+0x4(%a6)		# store for possible restart
2766	mov.l		%d0,%d1			# extension word in d0
2767
2768	rol.w		&0x4,%d0
2769	andi.w		&0xf,%d0		# extract Rn2
2770	mov.l		(EXC_DREGS,%a6,%d0.w*4),%a1 # fetch ADDR2
2771	mov.l		%a1,ADDR2(%a6)
2772
2773	mov.l		%d1,%d0
2774
2775	lsr.w		&0x6,%d1
2776	andi.w		&0x7,%d1		# extract Du2
2777	mov.l		(EXC_DREGS,%a6,%d1.w*4),%d5 # fetch Update2 Op
2778
2779	andi.w		&0x7,%d0		# extract Dc2
2780	mov.l		(EXC_DREGS,%a6,%d0.w*4),%d3 # fetch Compare2 Op
2781	mov.w		%d0,DC2(%a6)
2782
2783	mov.w		EXC_EXTWORD(%a6),%d0
2784	mov.l		%d0,%d1
2785
2786	rol.w		&0x4,%d0
2787	andi.w		&0xf,%d0		# extract Rn1
2788	mov.l		(EXC_DREGS,%a6,%d0.w*4),%a0 # fetch ADDR1
2789	mov.l		%a0,ADDR1(%a6)
2790
2791	mov.l		%d1,%d0
2792
2793	lsr.w		&0x6,%d1
2794	andi.w		&0x7,%d1		# extract Du1
2795	mov.l		(EXC_DREGS,%a6,%d1.w*4),%d4 # fetch Update1 Op
2796
2797	andi.w		&0x7,%d0		# extract Dc1
2798	mov.l		(EXC_DREGS,%a6,%d0.w*4),%d2 # fetch Compare1 Op
2799	mov.w		%d0,DC1(%a6)
2800
2801	btst		&0x1,EXC_OPWORD(%a6)	# word or long?
2802	sne		%d7
2803
2804	btst		&0x5,EXC_ISR(%a6)	# user or supervisor?
2805	sne		%d6
2806
2807	mov.l		%a0,%a2
2808	mov.l		%a1,%a3
2809
2810	mov.l		%d7,%d1			# pass size
2811	mov.l		%d6,%d0			# pass mode
2812	bsr.l		_real_lock_page		# lock page
2813	mov.l		%a2,%a0
2814	tst.l		%d0			# error?
2815	bne.l		_cas_terminate2		# yes
2816
2817	mov.l		%d7,%d1			# pass size
2818	mov.l		%d6,%d0			# pass mode
2819	mov.l		%a3,%a0			# pass addr
2820	bsr.l		_real_lock_page		# lock page
2821	mov.l		%a3,%a0
2822	tst.l		%d0			# error?
2823	bne.b		cas_preterm		# yes
2824
2825	mov.l		%a2,%a0
2826	mov.l		%a3,%a1
2827
2828	bra.l		_real_cas2
2829
2830# if the 2nd lock attempt fails, then we must still unlock the
2831# first page(s).
2832cas_preterm:
2833	mov.l		%d0,-(%sp)		# save FSLW
2834	mov.l		%d7,%d1			# pass size
2835	mov.l		%d6,%d0			# pass mode
2836	mov.l		%a2,%a0			# pass ADDR1
2837	bsr.l		_real_unlock_page	# unlock first page(s)
2838	mov.l		(%sp)+,%d0		# restore FSLW
2839	mov.l		%a3,%a0			# pass failing addr
2840	bra.l		_cas_terminate2
2841
2842#############################################################
2843
2844	global		_isp_cas2_finish
2845_isp_cas2_finish:
2846	btst		&0x1,EXC_OPWORD(%a6)
2847	bne.b		cas2_finish_l
2848
2849	mov.w		EXC_CC(%a6),%cc		# load old ccodes
2850	cmp.w		%d0,%d2
2851	bne.b		cas2_finish_w_save
2852	cmp.w		%d1,%d3
2853cas2_finish_w_save:
2854	mov.w		%cc,EXC_CC(%a6)		# save new ccodes
2855
2856	tst.b		%d4			# update compare reg?
2857	bne.b		cas2_finish_w_done	# no
2858
2859	mov.w		DC2(%a6),%d3		# fetch Dc2
2860	mov.w		%d1,(2+EXC_DREGS,%a6,%d3.w*4) # store new Compare2 Op
2861
2862	mov.w		DC1(%a6),%d2		# fetch Dc1
2863	mov.w		%d0,(2+EXC_DREGS,%a6,%d2.w*4) # store new Compare1 Op
2864
2865cas2_finish_w_done:
2866	btst		&0x5,EXC_ISR(%a6)
2867	sne		%d2
2868	mov.l		%d2,%d0			# pass mode
2869	sf		%d1			# pass size
2870	mov.l		ADDR1(%a6),%a0		# pass ADDR1
2871	bsr.l		_real_unlock_page	# unlock page
2872
2873	mov.l		%d2,%d0			# pass mode
2874	sf		%d1			# pass size
2875	mov.l		ADDR2(%a6),%a0		# pass ADDR2
2876	bsr.l		_real_unlock_page	# unlock page
2877	rts
2878
2879cas2_finish_l:
2880	mov.w		EXC_CC(%a6),%cc		# load old ccodes
2881	cmp.l		%d0,%d2
2882	bne.b		cas2_finish_l_save
2883	cmp.l		%d1,%d3
2884cas2_finish_l_save:
2885	mov.w		%cc,EXC_CC(%a6)		# save new ccodes
2886
2887	tst.b		%d4			# update compare reg?
2888	bne.b		cas2_finish_l_done	# no
2889
2890	mov.w		DC2(%a6),%d3		# fetch Dc2
2891	mov.l		%d1,(EXC_DREGS,%a6,%d3.w*4) # store new Compare2 Op
2892
2893	mov.w		DC1(%a6),%d2		# fetch Dc1
2894	mov.l		%d0,(EXC_DREGS,%a6,%d2.w*4) # store new Compare1 Op
2895
2896cas2_finish_l_done:
2897	btst		&0x5,EXC_ISR(%a6)
2898	sne		%d2
2899	mov.l		%d2,%d0			# pass mode
2900	st		%d1			# pass size
2901	mov.l		ADDR1(%a6),%a0		# pass ADDR1
2902	bsr.l		_real_unlock_page	# unlock page
2903
2904	mov.l		%d2,%d0			# pass mode
2905	st		%d1			# pass size
2906	mov.l		ADDR2(%a6),%a0		# pass ADDR2
2907	bsr.l		_real_unlock_page	# unlock page
2908	rts
2909
2910########
2911	global		cr_cas2
2912cr_cas2:
2913	mov.l		EXC_TEMP+0x4(%a6),%d0
2914	bra.w		_compandset2
2915
2916#########################################################################
2917# XDEF ****************************************************************	#
2918#	_compandset(): routine to emulate cas w/ misaligned <ea>	#
2919#		       (internal to package)				#
2920#	_isp_cas_finish(): routine called when cas emulation completes	#
2921#			   (external and internal to package)		#
2922#	_isp_cas_restart(): restart cas emulation after a fault		#
2923#			    (external to package)			#
2924#	_isp_cas_terminate(): create access error stack frame on fault	#
2925#			      (external and internal to package)	#
2926#	_isp_cas_inrange(): checks whether instr addess is within range	#
2927#			    of core cas/cas2emulation code		#
2928#			    (external to package)			#
2929#									#
2930# XREF ****************************************************************	#
2931#	_calc_ea(): calculate effective address				#
2932#									#
2933# INPUT ***************************************************************	#
2934# compandset():								#
2935#	none								#
2936# _isp_cas_restart():							#
2937#	d6 = previous sfc/dfc						#
2938# _isp_cas_finish():							#
2939# _isp_cas_terminate():							#
2940#	a0 = failing address						#
2941#	d0 = FSLW							#
2942#	d6 = previous sfc/dfc						#
2943# _isp_cas_inrange():							#
2944#	a0 = instruction address to be checked				#
2945#									#
2946# OUTPUT **************************************************************	#
2947# compandset():								#
2948#		none							#
2949# _isp_cas_restart():							#
2950#	a0 = effective address						#
2951#	d7 = word or longword flag					#
2952# _isp_cas_finish():							#
2953#	a0 = effective address						#
2954# _isp_cas_terminate():							#
2955#	initial register set before emulation exception			#
2956# _isp_cas_inrange():							#
2957#	d0 = 0 => in range; -1 => out of range				#
2958#									#
2959# ALGORITHM ***********************************************************	#
2960#									#
2961# compandset():								#
2962#	First, calculate the effective address. Then, decode the	#
2963# instruction word and fetch the "compare" (DC) and "update" (Du)	#
2964# operands.								#
2965#	Next, call the external routine _real_lock_page() so that the	#
2966# operating system can keep this page from being paged out while we're	#
2967# in this routine. If this call fails, jump to _cas_terminate2().	#
2968#	The routine then branches to _real_cas(). This external routine	#
2969# that actually emulates cas can be supplied by the external os or	#
2970# made to point directly back into the 060ISP which has a routine for	#
2971# this purpose.								#
2972#									#
2973# _isp_cas_finish():							#
2974#	Either way, after emulation, the package is re-entered at	#
2975# _isp_cas_finish(). This routine re-compares the operands in order to	#
2976# set the condition codes. Finally, these routines will call		#
2977# _real_unlock_page() in order to unlock the pages that were previously	#
2978# locked.								#
2979#									#
2980# _isp_cas_restart():							#
2981#	This routine can be entered from an access error handler where	#
2982# the emulation sequence should be re-started from the beginning.	#
2983#									#
2984# _isp_cas_terminate():							#
2985#	This routine can be entered from an access error handler where	#
2986# an emulation operand access failed and the operating system would	#
2987# like an access error stack frame created instead of the current	#
2988# unimplemented integer instruction frame.				#
2989#	Also, the package enters here if a call to _real_lock_page()	#
2990# fails.								#
2991#									#
2992# _isp_cas_inrange():							#
2993#	Checks to see whether the instruction address passed to it in	#
2994# a0 is within the software package cas/cas2 emulation routines. This	#
2995# can be helpful for an operating system to determine whether an access	#
2996# error during emulation was due to a cas/cas2 emulation access.	#
2997#									#
2998#########################################################################
2999
3000set DC,		EXC_TEMP+0x8
3001set ADDR,	EXC_TEMP+0x4
3002
3003	global		_compandset
3004_compandset:
3005	btst		&0x1,EXC_OPWORD(%a6)	# word or long operation?
3006	bne.b		compandsetl		# long
3007
3008compandsetw:
3009	movq.l		&0x2,%d0		# size = 2 bytes
3010	bsr.l		_calc_ea		# a0 = calculated <ea>
3011	mov.l		%a0,ADDR(%a6)		# save <ea> for possible restart
3012	sf		%d7			# clear d7 for word size
3013	bra.b		compandsetfetch
3014
3015compandsetl:
3016	movq.l		&0x4,%d0		# size = 4 bytes
3017	bsr.l		_calc_ea		# a0 = calculated <ea>
3018	mov.l		%a0,ADDR(%a6)		# save <ea> for possible restart
3019	st		%d7			# set d7 for longword size
3020
3021compandsetfetch:
3022	mov.w		EXC_EXTWORD(%a6),%d0	# fetch cas extension word
3023	mov.l		%d0,%d1			# make a copy
3024
3025	lsr.w		&0x6,%d0
3026	andi.w		&0x7,%d0		# extract Du
3027	mov.l		(EXC_DREGS,%a6,%d0.w*4),%d2 # get update operand
3028
3029	andi.w		&0x7,%d1		# extract Dc
3030	mov.l		(EXC_DREGS,%a6,%d1.w*4),%d4 # get compare operand
3031	mov.w		%d1,DC(%a6)		# save Dc
3032
3033	btst		&0x5,EXC_ISR(%a6)	# which mode for exception?
3034	sne		%d6			# set on supervisor mode
3035
3036	mov.l		%a0,%a2			# save temporarily
3037	mov.l		%d7,%d1			# pass size
3038	mov.l		%d6,%d0			# pass mode
3039	bsr.l		_real_lock_page		# lock page
3040	tst.l		%d0			# did error occur?
3041	bne.w		_cas_terminate2		# yes, clean up the mess
3042	mov.l		%a2,%a0			# pass addr in a0
3043
3044	bra.l		_real_cas
3045
3046########
3047	global		_isp_cas_finish
3048_isp_cas_finish:
3049	btst		&0x1,EXC_OPWORD(%a6)
3050	bne.b		cas_finish_l
3051
3052# just do the compare again since it's faster than saving the ccodes
3053# from the locked routine...
3054cas_finish_w:
3055	mov.w		EXC_CC(%a6),%cc		# restore cc
3056	cmp.w		%d0,%d4			# do word compare
3057	mov.w		%cc,EXC_CC(%a6)		# save cc
3058
3059	tst.b		%d1			# update compare reg?
3060	bne.b		cas_finish_w_done	# no
3061
3062	mov.w		DC(%a6),%d3
3063	mov.w		%d0,(EXC_DREGS+2,%a6,%d3.w*4) # Dc = destination
3064
3065cas_finish_w_done:
3066	mov.l		ADDR(%a6),%a0		# pass addr
3067	sf		%d1			# pass size
3068	btst		&0x5,EXC_ISR(%a6)
3069	sne		%d0			# pass mode
3070	bsr.l		_real_unlock_page	# unlock page
3071	rts
3072
3073# just do the compare again since it's faster than saving the ccodes
3074# from the locked routine...
3075cas_finish_l:
3076	mov.w		EXC_CC(%a6),%cc		# restore cc
3077	cmp.l		%d0,%d4			# do longword compare
3078	mov.w		%cc,EXC_CC(%a6)		# save cc
3079
3080	tst.b		%d1			# update compare reg?
3081	bne.b		cas_finish_l_done	# no
3082
3083	mov.w		DC(%a6),%d3
3084	mov.l		%d0,(EXC_DREGS,%a6,%d3.w*4) # Dc = destination
3085
3086cas_finish_l_done:
3087	mov.l		ADDR(%a6),%a0		# pass addr
3088	st		%d1			# pass size
3089	btst		&0x5,EXC_ISR(%a6)
3090	sne		%d0			# pass mode
3091	bsr.l		_real_unlock_page	# unlock page
3092	rts
3093
3094########
3095
3096	global		_isp_cas_restart
3097_isp_cas_restart:
3098	mov.l		%d6,%sfc		# restore previous sfc
3099	mov.l		%d6,%dfc		# restore previous dfc
3100
3101	cmpi.b		EXC_OPWORD+1(%a6),&0xfc	# cas or cas2?
3102	beq.l		cr_cas2			# cas2
3103cr_cas:
3104	mov.l		ADDR(%a6),%a0		# load <ea>
3105	btst		&0x1,EXC_OPWORD(%a6)	# word or long operation?
3106	sne		%d7			# set d7 accordingly
3107	bra.w		compandsetfetch
3108
3109########
3110
3111# At this stage, it would be nice if d0 held the FSLW.
3112	global		_isp_cas_terminate
3113_isp_cas_terminate:
3114	mov.l		%d6,%sfc		# restore previous sfc
3115	mov.l		%d6,%dfc		# restore previous dfc
3116
3117	global		_cas_terminate2
3118_cas_terminate2:
3119	mov.l		%a0,%a2			# copy failing addr to a2
3120
3121	mov.l		%d0,-(%sp)
3122	bsr.l		isp_restore		# restore An (if ()+ or -())
3123	mov.l		(%sp)+,%d0
3124
3125	addq.l		&0x4,%sp		# remove sub return addr
3126	subq.l		&0x8,%sp		# make room for bigger stack
3127	subq.l		&0x8,%a6		# shift frame ptr down, too
3128	mov.l		&26,%d1			# want to move 51 longwords
3129	lea		0x8(%sp),%a0		# get address of old stack
3130	lea		0x0(%sp),%a1		# get address of new stack
3131cas_term_cont:
3132	mov.l		(%a0)+,(%a1)+		# move a longword
3133	dbra.w		%d1,cas_term_cont	# keep going
3134
3135	mov.w		&0x4008,EXC_IVOFF(%a6)	# put new stk fmt, voff
3136	mov.l		%a2,EXC_IVOFF+0x2(%a6)	# put faulting addr on stack
3137	mov.l		%d0,EXC_IVOFF+0x6(%a6)	# put FSLW on stack
3138	movm.l		EXC_DREGS(%a6),&0x3fff	# restore user regs
3139	unlk		%a6			# unlink stack frame
3140	bra.l		_real_access
3141
3142########
3143
3144	global		_isp_cas_inrange
3145_isp_cas_inrange:
3146	clr.l		%d0			# clear return result
3147	lea		_CASHI(%pc),%a1		# load end of CAS core code
3148	cmp.l		%a1,%a0			# is PC in range?
3149	blt.b		cin_no			# no
3150	lea		_CASLO(%pc),%a1		# load begin of CAS core code
3151	cmp.l		%a0,%a1			# is PC in range?
3152	blt.b		cin_no			# no
3153	rts					# yes; return d0 = 0
3154cin_no:
3155	mov.l		&-0x1,%d0		# out of range; return d0 = -1
3156	rts
3157
3158#################################################################
3159#################################################################
3160#################################################################
3161# This is the start of the cas and cas2 "core" emulation code.	#
3162# This is the section that may need to be replaced by the host	#
3163# OS if it is too operating system-specific.			#
3164# Please refer to the package documentation to see how to	#
3165# "replace" this section, if necessary.				#
3166#################################################################
3167#################################################################
3168#################################################################
3169
3170#       ######      ##      ######     ####
3171#       #	   #  #     #         #    #
3172#	#	  ######    ######        #
3173#	#	  #    #         #      #
3174#       ######    #    #    ######    ######
3175
3176#########################################################################
3177# XDEF ****************************************************************	#
3178#	_isp_cas2(): "core" emulation code for the cas2 instruction	#
3179#									#
3180# XREF ****************************************************************	#
3181#	_isp_cas2_finish() - only exit point for this emulation code;	#
3182#			     do clean-up; calculate ccodes; store	#
3183#			     Compare Ops if appropriate.		#
3184#									#
3185# INPUT ***************************************************************	#
3186#	*see chart below*						#
3187#									#
3188# OUTPUT **************************************************************	#
3189#	*see chart below*						#
3190#									#
3191# ALGORITHM ***********************************************************	#
3192#	(1) Make several copies of the effective address.		#
3193#	(2) Save current SR; Then mask off all maskable interrupts.	#
3194#	(3) Save current SFC/DFC (ASSUMED TO BE EQUAL!!!); Then set	#
3195#	    according to whether exception occurred in user or		#
3196#	    supervisor mode.						#
3197#	(4) Use "plpaw" instruction to pre-load ATC with effective	#
3198#	    address pages(s). THIS SHOULD NOT FAULT!!! The relevant	#
3199#	    page(s) should have already been made resident prior to	#
3200#	    entering this routine.					#
3201#	(5) Push the operand lines from the cache w/ "cpushl".		#
3202#	    In the 68040, this was done within the locked region. In	#
3203#	    the 68060, it is done outside of the locked region.		#
3204#	(6) Use "plpar" instruction to do a re-load of ATC entries for	#
3205#	    ADDR1 since ADDR2 entries may have pushed ADDR1 out of the	#
3206#	    ATC.							#
3207#	(7) Pre-fetch the core emulation instructions by executing	#
3208#	    one branch within each physical line (16 bytes) of the code	#
3209#	    before actually executing the code.				#
3210#	(8) Load the BUSCR w/ the bus lock value.			#
3211#	(9) Fetch the source operands using "moves".			#
3212#	(10)Do the compares. If both equal, go to step (13).		#
3213#	(11)Unequal. No update occurs. But, we do write the DST1 op	#
3214#	    back to itself (as w/ the '040) so we can gracefully unlock	#
3215#	    the bus (and assert LOCKE*) using BUSCR and the final move.	#
3216#	(12)Exit.							#
3217#	(13)Write update operand to the DST locations. Use BUSCR to	#
3218#	    assert LOCKE* for the final write operation.		#
3219#	(14)Exit.							#
3220#									#
3221#	The algorithm is actually implemented slightly differently	#
3222# depending on the size of the operation and the misalignment of the	#
3223# operands. A misaligned operand must be written in aligned chunks or	#
3224# else the BUSCR register control gets confused.			#
3225#									#
3226#########################################################################
3227
3228#################################################################
3229# THIS IS THE STATE OF THE INTEGER REGISTER FILE UPON		#
3230# ENTERING _isp_cas2().						#
3231#								#
3232# D0 = xxxxxxxx							#
3233# D1 = xxxxxxxx							#
3234# D2 = cmp operand 1						#
3235# D3 = cmp operand 2						#
3236# D4 = update oper 1						#
3237# D5 = update oper 2						#
3238# D6 = 'xxxxxxff if supervisor mode; 'xxxxxx00 if user mode	#
3239# D7 = 'xxxxxxff if longword operation; 'xxxxxx00 if word	#
3240# A0 = ADDR1							#
3241# A1 = ADDR2							#
3242# A2 = xxxxxxxx							#
3243# A3 = xxxxxxxx							#
3244# A4 = xxxxxxxx							#
3245# A5 = xxxxxxxx							#
3246# A6 = frame pointer						#
3247# A7 = stack pointer						#
3248#################################################################
3249
3250#	align		0x1000
3251# beginning label used by _isp_cas_inrange()
3252	global		_CASLO
3253_CASLO:
3254
3255	global		_isp_cas2
3256_isp_cas2:
3257	tst.b		%d6			# user or supervisor mode?
3258	bne.b		cas2_supervisor		# supervisor
3259cas2_user:
3260	movq.l		&0x1,%d0		# load user data fc
3261	bra.b		cas2_cont
3262cas2_supervisor:
3263	movq.l		&0x5,%d0		# load supervisor data fc
3264cas2_cont:
3265	tst.b		%d7			# word or longword?
3266	beq.w		cas2w			# word
3267
3268####
3269cas2l:
3270	mov.l		%a0,%a2			# copy ADDR1
3271	mov.l		%a1,%a3			# copy ADDR2
3272	mov.l		%a0,%a4			# copy ADDR1
3273	mov.l		%a1,%a5			# copy ADDR2
3274
3275	addq.l		&0x3,%a4		# ADDR1+3
3276	addq.l		&0x3,%a5		# ADDR2+3
3277	mov.l		%a2,%d1			# ADDR1
3278
3279# mask interrupts levels 0-6. save old mask value.
3280	mov.w		%sr,%d7			# save current SR
3281	ori.w		&0x0700,%sr		# inhibit interrupts
3282
3283# load the SFC and DFC with the appropriate mode.
3284	movc		%sfc,%d6		# save old SFC/DFC
3285	movc		%d0,%sfc		# store new SFC
3286	movc		%d0,%dfc		# store new DFC
3287
3288# pre-load the operand ATC. no page faults should occur here because
3289# _real_lock_page() should have taken care of this.
3290	plpaw		(%a2)			# load atc for ADDR1
3291	plpaw		(%a4)			# load atc for ADDR1+3
3292	plpaw		(%a3)			# load atc for ADDR2
3293	plpaw		(%a5)			# load atc for ADDR2+3
3294
3295# push the operand lines from the cache if they exist.
3296	cpushl		%dc,(%a2)		# push line for ADDR1
3297	cpushl		%dc,(%a4)		# push line for ADDR1+3
3298	cpushl		%dc,(%a3)		# push line for ADDR2
3299	cpushl		%dc,(%a5)		# push line for ADDR2+2
3300
3301	mov.l		%d1,%a2			# ADDR1
3302	addq.l		&0x3,%d1
3303	mov.l		%d1,%a4			# ADDR1+3
3304# if ADDR1 was ATC resident before the above "plpaw" and was executed
3305# and it was the next entry scheduled for replacement and ADDR2
3306# shares the same set, then the "plpaw" for ADDR2 can push the ADDR1
3307# entries from the ATC. so, we do a second set of "plpa"s.
3308	plpar		(%a2)			# load atc for ADDR1
3309	plpar		(%a4)			# load atc for ADDR1+3
3310
3311# load the BUSCR values.
3312	mov.l		&0x80000000,%a2		# assert LOCK* buscr value
3313	mov.l		&0xa0000000,%a3		# assert LOCKE* buscr value
3314	mov.l		&0x00000000,%a4		# buscr unlock value
3315
3316# there are three possible mis-aligned cases for longword cas. they
3317# are separated because the final write which asserts LOCKE* must
3318# be aligned.
3319	mov.l		%a0,%d0			# is ADDR1 misaligned?
3320	andi.b		&0x3,%d0
3321	beq.b		CAS2L_ENTER		# no
3322	cmpi.b		%d0,&0x2
3323	beq.w		CAS2L2_ENTER		# yes; word misaligned
3324	bra.w		CAS2L3_ENTER		# yes; byte misaligned
3325
3326#
3327# D0 = dst operand 1 <-
3328# D1 = dst operand 2 <-
3329# D2 = cmp operand 1
3330# D3 = cmp operand 2
3331# D4 = update oper 1
3332# D5 = update oper 2
3333# D6 = old SFC/DFC
3334# D7 = old SR
3335# A0 = ADDR1
3336# A1 = ADDR2
3337# A2 = bus LOCK*  value
3338# A3 = bus LOCKE* value
3339# A4 = bus unlock value
3340# A5 = xxxxxxxx
3341#
3342	align		0x10
3343CAS2L_START:
3344	movc		%a2,%buscr		# assert LOCK*
3345	movs.l		(%a1),%d1		# fetch Dest2[31:0]
3346	movs.l		(%a0),%d0		# fetch Dest1[31:0]
3347	bra.b		CAS2L_CONT
3348CAS2L_ENTER:
3349	bra.b		~+16
3350
3351CAS2L_CONT:
3352	cmp.l		%d0,%d2			# Dest1 - Compare1
3353	bne.b		CAS2L_NOUPDATE
3354	cmp.l		%d1,%d3			# Dest2 - Compare2
3355	bne.b		CAS2L_NOUPDATE
3356	movs.l		%d5,(%a1)		# Update2[31:0] -> DEST2
3357	bra.b		CAS2L_UPDATE
3358	bra.b		~+16
3359
3360CAS2L_UPDATE:
3361	movc		%a3,%buscr		# assert LOCKE*
3362	movs.l		%d4,(%a0)		# Update1[31:0] -> DEST1
3363	movc		%a4,%buscr		# unlock the bus
3364	bra.b		cas2l_update_done
3365	bra.b		~+16
3366
3367CAS2L_NOUPDATE:
3368	movc		%a3,%buscr		# assert LOCKE*
3369	movs.l		%d0,(%a0)		# Dest1[31:0] -> DEST1
3370	movc		%a4,%buscr		# unlock the bus
3371	bra.b		cas2l_noupdate_done
3372	bra.b		~+16
3373
3374CAS2L_FILLER:
3375	nop
3376	nop
3377	nop
3378	nop
3379	nop
3380	nop
3381	nop
3382	bra.b		CAS2L_START
3383
3384####
3385
3386#################################################################
3387# THIS MUST BE THE STATE OF THE INTEGER REGISTER FILE UPON	#
3388# ENTERING _isp_cas2().						#
3389#								#
3390# D0 = destination[31:0] operand 1				#
3391# D1 = destination[31:0] operand 2				#
3392# D2 = cmp[31:0] operand 1					#
3393# D3 = cmp[31:0] operand 2					#
3394# D4 = 'xxxxxx11 -> no reg update; 'xxxxxx00 -> update required	#
3395# D5 = xxxxxxxx							#
3396# D6 = xxxxxxxx							#
3397# D7 = xxxxxxxx							#
3398# A0 = xxxxxxxx							#
3399# A1 = xxxxxxxx							#
3400# A2 = xxxxxxxx							#
3401# A3 = xxxxxxxx							#
3402# A4 = xxxxxxxx							#
3403# A5 = xxxxxxxx							#
3404# A6 = frame pointer						#
3405# A7 = stack pointer						#
3406#################################################################
3407
3408cas2l_noupdate_done:
3409
3410# restore previous SFC/DFC value.
3411	movc		%d6,%sfc		# restore old SFC
3412	movc		%d6,%dfc		# restore old DFC
3413
3414# restore previous interrupt mask level.
3415	mov.w		%d7,%sr			# restore old SR
3416
3417	sf		%d4			# indicate no update was done
3418	bra.l		_isp_cas2_finish
3419
3420cas2l_update_done:
3421
3422# restore previous SFC/DFC value.
3423	movc		%d6,%sfc		# restore old SFC
3424	movc		%d6,%dfc		# restore old DFC
3425
3426# restore previous interrupt mask level.
3427	mov.w		%d7,%sr			# restore old SR
3428
3429	st		%d4			# indicate update was done
3430	bra.l		_isp_cas2_finish
3431####
3432
3433	align		0x10
3434CAS2L2_START:
3435	movc		%a2,%buscr		# assert LOCK*
3436	movs.l		(%a1),%d1		# fetch Dest2[31:0]
3437	movs.l		(%a0),%d0		# fetch Dest1[31:0]
3438	bra.b		CAS2L2_CONT
3439CAS2L2_ENTER:
3440	bra.b		~+16
3441
3442CAS2L2_CONT:
3443	cmp.l		%d0,%d2			# Dest1 - Compare1
3444	bne.b		CAS2L2_NOUPDATE
3445	cmp.l		%d1,%d3			# Dest2 - Compare2
3446	bne.b		CAS2L2_NOUPDATE
3447	movs.l		%d5,(%a1)		# Update2[31:0] -> Dest2
3448	bra.b		CAS2L2_UPDATE
3449	bra.b		~+16
3450
3451CAS2L2_UPDATE:
3452	swap		%d4			# get Update1[31:16]
3453	movs.w		%d4,(%a0)+		# Update1[31:16] -> DEST1
3454	movc		%a3,%buscr		# assert LOCKE*
3455	swap		%d4			# get Update1[15:0]
3456	bra.b		CAS2L2_UPDATE2
3457	bra.b		~+16
3458
3459CAS2L2_UPDATE2:
3460	movs.w		%d4,(%a0)		# Update1[15:0] -> DEST1+0x2
3461	movc		%a4,%buscr		# unlock the bus
3462	bra.w		cas2l_update_done
3463	nop
3464	bra.b		~+16
3465
3466CAS2L2_NOUPDATE:
3467	swap		%d0			# get Dest1[31:16]
3468	movs.w		%d0,(%a0)+		# Dest1[31:16] -> DEST1
3469	movc		%a3,%buscr		# assert LOCKE*
3470	swap		%d0			# get Dest1[15:0]
3471	bra.b		CAS2L2_NOUPDATE2
3472	bra.b		~+16
3473
3474CAS2L2_NOUPDATE2:
3475	movs.w		%d0,(%a0)		# Dest1[15:0] -> DEST1+0x2
3476	movc		%a4,%buscr		# unlock the bus
3477	bra.w		cas2l_noupdate_done
3478	nop
3479	bra.b		~+16
3480
3481CAS2L2_FILLER:
3482	nop
3483	nop
3484	nop
3485	nop
3486	nop
3487	nop
3488	nop
3489	bra.b		CAS2L2_START
3490
3491#################################
3492
3493	align		0x10
3494CAS2L3_START:
3495	movc		%a2,%buscr		# assert LOCK*
3496	movs.l		(%a1),%d1		# fetch Dest2[31:0]
3497	movs.l		(%a0),%d0		# fetch Dest1[31:0]
3498	bra.b		CAS2L3_CONT
3499CAS2L3_ENTER:
3500	bra.b		~+16
3501
3502CAS2L3_CONT:
3503	cmp.l		%d0,%d2			# Dest1 - Compare1
3504	bne.b		CAS2L3_NOUPDATE
3505	cmp.l		%d1,%d3			# Dest2 - Compare2
3506	bne.b		CAS2L3_NOUPDATE
3507	movs.l		%d5,(%a1)		# Update2[31:0] -> DEST2
3508	bra.b		CAS2L3_UPDATE
3509	bra.b		~+16
3510
3511CAS2L3_UPDATE:
3512	rol.l		&0x8,%d4		# get Update1[31:24]
3513	movs.b		%d4,(%a0)+		# Update1[31:24] -> DEST1
3514	swap		%d4			# get Update1[23:8]
3515	movs.w		%d4,(%a0)+		# Update1[23:8] -> DEST1+0x1
3516	bra.b		CAS2L3_UPDATE2
3517	bra.b		~+16
3518
3519CAS2L3_UPDATE2:
3520	rol.l		&0x8,%d4		# get Update1[7:0]
3521	movc		%a3,%buscr		# assert LOCKE*
3522	movs.b		%d4,(%a0)		# Update1[7:0] -> DEST1+0x3
3523	bra.b		CAS2L3_UPDATE3
3524	nop
3525	bra.b		~+16
3526
3527CAS2L3_UPDATE3:
3528	movc		%a4,%buscr		# unlock the bus
3529	bra.w		cas2l_update_done
3530	nop
3531	nop
3532	nop
3533	bra.b		~+16
3534
3535CAS2L3_NOUPDATE:
3536	rol.l		&0x8,%d0		# get Dest1[31:24]
3537	movs.b		%d0,(%a0)+		# Dest1[31:24] -> DEST1
3538	swap		%d0			# get Dest1[23:8]
3539	movs.w		%d0,(%a0)+		# Dest1[23:8] -> DEST1+0x1
3540	bra.b		CAS2L3_NOUPDATE2
3541	bra.b		~+16
3542
3543CAS2L3_NOUPDATE2:
3544	rol.l		&0x8,%d0		# get Dest1[7:0]
3545	movc		%a3,%buscr		# assert LOCKE*
3546	movs.b		%d0,(%a0)		# Update1[7:0] -> DEST1+0x3
3547	bra.b		CAS2L3_NOUPDATE3
3548	nop
3549	bra.b		~+16
3550
3551CAS2L3_NOUPDATE3:
3552	movc		%a4,%buscr		# unlock the bus
3553	bra.w		cas2l_noupdate_done
3554	nop
3555	nop
3556	nop
3557	bra.b		~+14
3558
3559CAS2L3_FILLER:
3560	nop
3561	nop
3562	nop
3563	nop
3564	nop
3565	nop
3566	bra.w		CAS2L3_START
3567
3568#############################################################
3569#############################################################
3570
3571cas2w:
3572	mov.l		%a0,%a2			# copy ADDR1
3573	mov.l		%a1,%a3			# copy ADDR2
3574	mov.l		%a0,%a4			# copy ADDR1
3575	mov.l		%a1,%a5			# copy ADDR2
3576
3577	addq.l		&0x1,%a4		# ADDR1+1
3578	addq.l		&0x1,%a5		# ADDR2+1
3579	mov.l		%a2,%d1			# ADDR1
3580
3581# mask interrupt levels 0-6. save old mask value.
3582	mov.w		%sr,%d7			# save current SR
3583	ori.w		&0x0700,%sr		# inhibit interrupts
3584
3585# load the SFC and DFC with the appropriate mode.
3586	movc		%sfc,%d6		# save old SFC/DFC
3587	movc		%d0,%sfc		# store new SFC
3588	movc		%d0,%dfc		# store new DFC
3589
3590# pre-load the operand ATC. no page faults should occur because
3591# _real_lock_page() should have taken care of this.
3592	plpaw		(%a2)			# load atc for ADDR1
3593	plpaw		(%a4)			# load atc for ADDR1+1
3594	plpaw		(%a3)			# load atc for ADDR2
3595	plpaw		(%a5)			# load atc for ADDR2+1
3596
3597# push the operand cache lines from the cache if they exist.
3598	cpushl		%dc,(%a2)		# push line for ADDR1
3599	cpushl		%dc,(%a4)		# push line for ADDR1+1
3600	cpushl		%dc,(%a3)		# push line for ADDR2
3601	cpushl		%dc,(%a5)		# push line for ADDR2+1
3602
3603	mov.l		%d1,%a2			# ADDR1
3604	addq.l		&0x3,%d1
3605	mov.l		%d1,%a4			# ADDR1+3
3606# if ADDR1 was ATC resident before the above "plpaw" and was executed
3607# and it was the next entry scheduled for replacement and ADDR2
3608# shares the same set, then the "plpaw" for ADDR2 can push the ADDR1
3609# entries from the ATC. so, we do a second set of "plpa"s.
3610	plpar		(%a2)			# load atc for ADDR1
3611	plpar		(%a4)			# load atc for ADDR1+3
3612
3613# load the BUSCR values.
3614	mov.l		&0x80000000,%a2		# assert LOCK* buscr value
3615	mov.l		&0xa0000000,%a3		# assert LOCKE* buscr value
3616	mov.l		&0x00000000,%a4		# buscr unlock value
3617
3618# there are two possible mis-aligned cases for word cas. they
3619# are separated because the final write which asserts LOCKE* must
3620# be aligned.
3621	mov.l		%a0,%d0			# is ADDR1 misaligned?
3622	btst		&0x0,%d0
3623	bne.w		CAS2W2_ENTER		# yes
3624	bra.b		CAS2W_ENTER		# no
3625
3626#
3627# D0 = dst operand 1 <-
3628# D1 = dst operand 2 <-
3629# D2 = cmp operand 1
3630# D3 = cmp operand 2
3631# D4 = update oper 1
3632# D5 = update oper 2
3633# D6 = old SFC/DFC
3634# D7 = old SR
3635# A0 = ADDR1
3636# A1 = ADDR2
3637# A2 = bus LOCK*  value
3638# A3 = bus LOCKE* value
3639# A4 = bus unlock value
3640# A5 = xxxxxxxx
3641#
3642	align		0x10
3643CAS2W_START:
3644	movc		%a2,%buscr		# assert LOCK*
3645	movs.w		(%a1),%d1		# fetch Dest2[15:0]
3646	movs.w		(%a0),%d0		# fetch Dest1[15:0]
3647	bra.b		CAS2W_CONT2
3648CAS2W_ENTER:
3649	bra.b		~+16
3650
3651CAS2W_CONT2:
3652	cmp.w		%d0,%d2			# Dest1 - Compare1
3653	bne.b		CAS2W_NOUPDATE
3654	cmp.w		%d1,%d3			# Dest2 - Compare2
3655	bne.b		CAS2W_NOUPDATE
3656	movs.w		%d5,(%a1)		# Update2[15:0] -> DEST2
3657	bra.b		CAS2W_UPDATE
3658	bra.b		~+16
3659
3660CAS2W_UPDATE:
3661	movc		%a3,%buscr		# assert LOCKE*
3662	movs.w		%d4,(%a0)		# Update1[15:0] -> DEST1
3663	movc		%a4,%buscr		# unlock the bus
3664	bra.b		cas2w_update_done
3665	bra.b		~+16
3666
3667CAS2W_NOUPDATE:
3668	movc		%a3,%buscr		# assert LOCKE*
3669	movs.w		%d0,(%a0)		# Dest1[15:0] -> DEST1
3670	movc		%a4,%buscr		# unlock the bus
3671	bra.b		cas2w_noupdate_done
3672	bra.b		~+16
3673
3674CAS2W_FILLER:
3675	nop
3676	nop
3677	nop
3678	nop
3679	nop
3680	nop
3681	nop
3682	bra.b		CAS2W_START
3683
3684####
3685
3686#################################################################
3687# THIS MUST BE THE STATE OF THE INTEGER REGISTER FILE UPON	#
3688# ENTERING _isp_cas2().						#
3689#								#
3690# D0 = destination[15:0] operand 1				#
3691# D1 = destination[15:0] operand 2				#
3692# D2 = cmp[15:0] operand 1					#
3693# D3 = cmp[15:0] operand 2					#
3694# D4 = 'xxxxxx11 -> no reg update; 'xxxxxx00 -> update required	#
3695# D5 = xxxxxxxx							#
3696# D6 = xxxxxxxx							#
3697# D7 = xxxxxxxx							#
3698# A0 = xxxxxxxx							#
3699# A1 = xxxxxxxx							#
3700# A2 = xxxxxxxx							#
3701# A3 = xxxxxxxx							#
3702# A4 = xxxxxxxx							#
3703# A5 = xxxxxxxx							#
3704# A6 = frame pointer						#
3705# A7 = stack pointer						#
3706#################################################################
3707
3708cas2w_noupdate_done:
3709
3710# restore previous SFC/DFC value.
3711	movc		%d6,%sfc		# restore old SFC
3712	movc		%d6,%dfc		# restore old DFC
3713
3714# restore previous interrupt mask level.
3715	mov.w		%d7,%sr			# restore old SR
3716
3717	sf		%d4			# indicate no update was done
3718	bra.l		_isp_cas2_finish
3719
3720cas2w_update_done:
3721
3722# restore previous SFC/DFC value.
3723	movc		%d6,%sfc		# restore old SFC
3724	movc		%d6,%dfc		# restore old DFC
3725
3726# restore previous interrupt mask level.
3727	mov.w		%d7,%sr			# restore old SR
3728
3729	st		%d4			# indicate update was done
3730	bra.l		_isp_cas2_finish
3731####
3732
3733	align		0x10
3734CAS2W2_START:
3735	movc		%a2,%buscr		# assert LOCK*
3736	movs.w		(%a1),%d1		# fetch Dest2[15:0]
3737	movs.w		(%a0),%d0		# fetch Dest1[15:0]
3738	bra.b		CAS2W2_CONT2
3739CAS2W2_ENTER:
3740	bra.b		~+16
3741
3742CAS2W2_CONT2:
3743	cmp.w		%d0,%d2			# Dest1 - Compare1
3744	bne.b		CAS2W2_NOUPDATE
3745	cmp.w		%d1,%d3			# Dest2 - Compare2
3746	bne.b		CAS2W2_NOUPDATE
3747	movs.w		%d5,(%a1)		# Update2[15:0] -> DEST2
3748	bra.b		CAS2W2_UPDATE
3749	bra.b		~+16
3750
3751CAS2W2_UPDATE:
3752	ror.l		&0x8,%d4		# get Update1[15:8]
3753	movs.b		%d4,(%a0)+		# Update1[15:8] -> DEST1
3754	movc		%a3,%buscr		# assert LOCKE*
3755	rol.l		&0x8,%d4		# get Update1[7:0]
3756	bra.b		CAS2W2_UPDATE2
3757	bra.b		~+16
3758
3759CAS2W2_UPDATE2:
3760	movs.b		%d4,(%a0)		# Update1[7:0] -> DEST1+0x1
3761	movc		%a4,%buscr		# unlock the bus
3762	bra.w		cas2w_update_done
3763	nop
3764	bra.b		~+16
3765
3766CAS2W2_NOUPDATE:
3767	ror.l		&0x8,%d0		# get Dest1[15:8]
3768	movs.b		%d0,(%a0)+		# Dest1[15:8] -> DEST1
3769	movc		%a3,%buscr		# assert LOCKE*
3770	rol.l		&0x8,%d0		# get Dest1[7:0]
3771	bra.b		CAS2W2_NOUPDATE2
3772	bra.b		~+16
3773
3774CAS2W2_NOUPDATE2:
3775	movs.b		%d0,(%a0)		# Dest1[7:0] -> DEST1+0x1
3776	movc		%a4,%buscr		# unlock the bus
3777	bra.w		cas2w_noupdate_done
3778	nop
3779	bra.b		~+16
3780
3781CAS2W2_FILLER:
3782	nop
3783	nop
3784	nop
3785	nop
3786	nop
3787	nop
3788	nop
3789	bra.b		CAS2W2_START
3790
3791#       ######      ##      ######
3792#       #	   #  #     #
3793#	#	  ######    ######
3794#	#	  #    #         #
3795#       ######    #    #    ######
3796
3797#########################################################################
3798# XDEF ****************************************************************	#
3799#	_isp_cas(): "core" emulation code for the cas instruction	#
3800#									#
3801# XREF ****************************************************************	#
3802#	_isp_cas_finish() - only exit point for this emulation code;	#
3803#			    do clean-up					#
3804#									#
3805# INPUT ***************************************************************	#
3806#	*see entry chart below*						#
3807#									#
3808# OUTPUT **************************************************************	#
3809#	*see exit chart below*						#
3810#									#
3811# ALGORITHM ***********************************************************	#
3812#	(1) Make several copies of the effective address.		#
3813#	(2) Save current SR; Then mask off all maskable interrupts.	#
3814#	(3) Save current DFC/SFC (ASSUMED TO BE EQUAL!!!); Then set	#
3815#	    SFC/DFC according to whether exception occurred in user or	#
3816#	    supervisor mode.						#
3817#	(4) Use "plpaw" instruction to pre-load ATC with effective	#
3818#	    address page(s). THIS SHOULD NOT FAULT!!! The relevant	#
3819#	    page(s) should have been made resident prior to entering	#
3820#	    this routine.						#
3821#	(5) Push the operand lines from the cache w/ "cpushl".		#
3822#	    In the 68040, this was done within the locked region. In	#
3823#	    the 68060, it is done outside of the locked region.		#
3824#	(6) Pre-fetch the core emulation instructions by executing one	#
3825#	    branch within each physical line (16 bytes) of the code	#
3826#	    before actually executing the code.				#
3827#	(7) Load the BUSCR with the bus lock value.			#
3828#	(8) Fetch the source operand.					#
3829#	(9) Do the compare. If equal, go to step (12).			#
3830#	(10)Unequal. No update occurs. But, we do write the DST op back	#
3831#	    to itself (as w/ the '040) so we can gracefully unlock	#
3832#	    the bus (and assert LOCKE*) using BUSCR and the final move.	#
3833#	(11)Exit.							#
3834#	(12)Write update operand to the DST location. Use BUSCR to	#
3835#	    assert LOCKE* for the final write operation.		#
3836#	(13)Exit.							#
3837#									#
3838#	The algorithm is actually implemented slightly differently	#
3839# depending on the size of the operation and the misalignment of the	#
3840# operand. A misaligned operand must be written in aligned chunks or	#
3841# else the BUSCR register control gets confused.			#
3842#									#
3843#########################################################################
3844
3845#########################################################
3846# THIS IS THE STATE OF THE INTEGER REGISTER FILE UPON	#
3847# ENTERING _isp_cas().					#
3848#							#
3849# D0 = xxxxxxxx						#
3850# D1 = xxxxxxxx						#
3851# D2 = update operand					#
3852# D3 = xxxxxxxx						#
3853# D4 = compare operand					#
3854# D5 = xxxxxxxx						#
3855# D6 = supervisor ('xxxxxxff) or user mode ('xxxxxx00)	#
3856# D7 = longword ('xxxxxxff) or word size ('xxxxxx00)	#
3857# A0 = ADDR						#
3858# A1 = xxxxxxxx						#
3859# A2 = xxxxxxxx						#
3860# A3 = xxxxxxxx						#
3861# A4 = xxxxxxxx						#
3862# A5 = xxxxxxxx						#
3863# A6 = frame pointer					#
3864# A7 = stack pointer					#
3865#########################################################
3866
3867	global		_isp_cas
3868_isp_cas:
3869	tst.b		%d6			# user or supervisor mode?
3870	bne.b		cas_super		# supervisor
3871cas_user:
3872	movq.l		&0x1,%d0		# load user data fc
3873	bra.b		cas_cont
3874cas_super:
3875	movq.l		&0x5,%d0		# load supervisor data fc
3876
3877cas_cont:
3878	tst.b		%d7			# word or longword?
3879	bne.w		casl			# longword
3880
3881####
3882casw:
3883	mov.l		%a0,%a1			# make copy for plpaw1
3884	mov.l		%a0,%a2			# make copy for plpaw2
3885	addq.l		&0x1,%a2		# plpaw2 points to end of word
3886
3887	mov.l		%d2,%d3			# d3 = update[7:0]
3888	lsr.w		&0x8,%d2		# d2 = update[15:8]
3889
3890# mask interrupt levels 0-6. save old mask value.
3891	mov.w		%sr,%d7			# save current SR
3892	ori.w		&0x0700,%sr		# inhibit interrupts
3893
3894# load the SFC and DFC with the appropriate mode.
3895	movc		%sfc,%d6		# save old SFC/DFC
3896	movc		%d0,%sfc		# load new sfc
3897	movc		%d0,%dfc		# load new dfc
3898
3899# pre-load the operand ATC. no page faults should occur here because
3900# _real_lock_page() should have taken care of this.
3901	plpaw		(%a1)			# load atc for ADDR
3902	plpaw		(%a2)			# load atc for ADDR+1
3903
3904# push the operand lines from the cache if they exist.
3905	cpushl		%dc,(%a1)		# push dirty data
3906	cpushl		%dc,(%a2)		# push dirty data
3907
3908# load the BUSCR values.
3909	mov.l		&0x80000000,%a1		# assert LOCK* buscr value
3910	mov.l		&0xa0000000,%a2		# assert LOCKE* buscr value
3911	mov.l		&0x00000000,%a3		# buscr unlock value
3912
3913# pre-load the instruction cache for the following algorithm.
3914# this will minimize the number of cycles that LOCK* will be asserted.
3915	bra.b		CASW_ENTER		# start pre-loading icache
3916
3917#
3918# D0 = dst operand <-
3919# D1 = update[15:8] operand
3920# D2 = update[7:0]  operand
3921# D3 = xxxxxxxx
3922# D4 = compare[15:0] operand
3923# D5 = xxxxxxxx
3924# D6 = old SFC/DFC
3925# D7 = old SR
3926# A0 = ADDR
3927# A1 = bus LOCK*  value
3928# A2 = bus LOCKE* value
3929# A3 = bus unlock value
3930# A4 = xxxxxxxx
3931# A5 = xxxxxxxx
3932#
3933	align		0x10
3934CASW_START:
3935	movc		%a1,%buscr		# assert LOCK*
3936	movs.w		(%a0),%d0		# fetch Dest[15:0]
3937	cmp.w		%d0,%d4			# Dest - Compare
3938	bne.b		CASW_NOUPDATE
3939	bra.b		CASW_UPDATE
3940CASW_ENTER:
3941	bra.b		~+16
3942
3943CASW_UPDATE:
3944	movs.b		%d2,(%a0)+		# Update[15:8] -> DEST
3945	movc		%a2,%buscr		# assert LOCKE*
3946	movs.b		%d3,(%a0)		# Update[7:0] -> DEST+0x1
3947	bra.b		CASW_UPDATE2
3948	bra.b		~+16
3949
3950CASW_UPDATE2:
3951	movc		%a3,%buscr		# unlock the bus
3952	bra.b		casw_update_done
3953	nop
3954	nop
3955	nop
3956	nop
3957	bra.b		~+16
3958
3959CASW_NOUPDATE:
3960	ror.l		&0x8,%d0		# get Dest[15:8]
3961	movs.b		%d0,(%a0)+		# Dest[15:8] -> DEST
3962	movc		%a2,%buscr		# assert LOCKE*
3963	rol.l		&0x8,%d0		# get Dest[7:0]
3964	bra.b		CASW_NOUPDATE2
3965	bra.b		~+16
3966
3967CASW_NOUPDATE2:
3968	movs.b		%d0,(%a0)		# Dest[7:0] -> DEST+0x1
3969	movc		%a3,%buscr		# unlock the bus
3970	bra.b		casw_noupdate_done
3971	nop
3972	nop
3973	bra.b		~+16
3974
3975CASW_FILLER:
3976	nop
3977	nop
3978	nop
3979	nop
3980	nop
3981	nop
3982	nop
3983	bra.b		CASW_START
3984
3985#################################################################
3986# THIS MUST BE THE STATE OF THE INTEGER REGISTER FILE UPON	#
3987# CALLING _isp_cas_finish().					#
3988#								#
3989# D0 = destination[15:0] operand				#
3990# D1 = 'xxxxxx11 -> no reg update; 'xxxxxx00 -> update required	#
3991# D2 = xxxxxxxx							#
3992# D3 = xxxxxxxx							#
3993# D4 = compare[15:0] operand					#
3994# D5 = xxxxxxxx							#
3995# D6 = xxxxxxxx							#
3996# D7 = xxxxxxxx							#
3997# A0 = xxxxxxxx							#
3998# A1 = xxxxxxxx							#
3999# A2 = xxxxxxxx							#
4000# A3 = xxxxxxxx							#
4001# A4 = xxxxxxxx							#
4002# A5 = xxxxxxxx							#
4003# A6 = frame pointer						#
4004# A7 = stack pointer						#
4005#################################################################
4006
4007casw_noupdate_done:
4008
4009# restore previous SFC/DFC value.
4010	movc		%d6,%sfc		# restore old SFC
4011	movc		%d6,%dfc		# restore old DFC
4012
4013# restore previous interrupt mask level.
4014	mov.w		%d7,%sr			# restore old SR
4015
4016	sf		%d1			# indicate no update was done
4017	bra.l		_isp_cas_finish
4018
4019casw_update_done:
4020
4021# restore previous SFC/DFC value.
4022	movc		%d6,%sfc		# restore old SFC
4023	movc		%d6,%dfc		# restore old DFC
4024
4025# restore previous interrupt mask level.
4026	mov.w		%d7,%sr			# restore old SR
4027
4028	st		%d1			# indicate update was done
4029	bra.l		_isp_cas_finish
4030
4031################
4032
4033# there are two possible mis-aligned cases for longword cas. they
4034# are separated because the final write which asserts LOCKE* must
4035# be an aligned write.
4036casl:
4037	mov.l		%a0,%a1			# make copy for plpaw1
4038	mov.l		%a0,%a2			# make copy for plpaw2
4039	addq.l		&0x3,%a2		# plpaw2 points to end of longword
4040
4041	mov.l		%a0,%d1			# byte or word misaligned?
4042	btst		&0x0,%d1
4043	bne.w		casl2			# byte misaligned
4044
4045	mov.l		%d2,%d3			# d3 = update[15:0]
4046	swap		%d2			# d2 = update[31:16]
4047
4048# mask interrupts levels 0-6. save old mask value.
4049	mov.w		%sr,%d7			# save current SR
4050	ori.w		&0x0700,%sr		# inhibit interrupts
4051
4052# load the SFC and DFC with the appropriate mode.
4053	movc		%sfc,%d6		# save old SFC/DFC
4054	movc		%d0,%sfc		# load new sfc
4055	movc		%d0,%dfc		# load new dfc
4056
4057# pre-load the operand ATC. no page faults should occur here because
4058# _real_lock_page() should have taken care of this.
4059	plpaw		(%a1)			# load atc for ADDR
4060	plpaw		(%a2)			# load atc for ADDR+3
4061
4062# push the operand lines from the cache if they exist.
4063	cpushl		%dc,(%a1)		# push dirty data
4064	cpushl		%dc,(%a2)		# push dirty data
4065
4066# load the BUSCR values.
4067	mov.l		&0x80000000,%a1		# assert LOCK* buscr value
4068	mov.l		&0xa0000000,%a2		# assert LOCKE* buscr value
4069	mov.l		&0x00000000,%a3		# buscr unlock value
4070
4071	bra.b		CASL_ENTER		# start pre-loading icache
4072
4073#
4074# D0 = dst operand <-
4075# D1 = xxxxxxxx
4076# D2 = update[31:16] operand
4077# D3 = update[15:0]  operand
4078# D4 = compare[31:0] operand
4079# D5 = xxxxxxxx
4080# D6 = old SFC/DFC
4081# D7 = old SR
4082# A0 = ADDR
4083# A1 = bus LOCK*  value
4084# A2 = bus LOCKE* value
4085# A3 = bus unlock value
4086# A4 = xxxxxxxx
4087# A5 = xxxxxxxx
4088#
4089	align		0x10
4090CASL_START:
4091	movc		%a1,%buscr		# assert LOCK*
4092	movs.l		(%a0),%d0		# fetch Dest[31:0]
4093	cmp.l		%d0,%d4			# Dest - Compare
4094	bne.b		CASL_NOUPDATE
4095	bra.b		CASL_UPDATE
4096CASL_ENTER:
4097	bra.b		~+16
4098
4099CASL_UPDATE:
4100	movs.w		%d2,(%a0)+		# Update[31:16] -> DEST
4101	movc		%a2,%buscr		# assert LOCKE*
4102	movs.w		%d3,(%a0)		# Update[15:0] -> DEST+0x2
4103	bra.b		CASL_UPDATE2
4104	bra.b		~+16
4105
4106CASL_UPDATE2:
4107	movc		%a3,%buscr		# unlock the bus
4108	bra.b		casl_update_done
4109	nop
4110	nop
4111	nop
4112	nop
4113	bra.b		~+16
4114
4115CASL_NOUPDATE:
4116	swap		%d0			# get Dest[31:16]
4117	movs.w		%d0,(%a0)+		# Dest[31:16] -> DEST
4118	swap		%d0			# get Dest[15:0]
4119	movc		%a2,%buscr		# assert LOCKE*
4120	bra.b		CASL_NOUPDATE2
4121	bra.b		~+16
4122
4123CASL_NOUPDATE2:
4124	movs.w		%d0,(%a0)		# Dest[15:0] -> DEST+0x2
4125	movc		%a3,%buscr		# unlock the bus
4126	bra.b		casl_noupdate_done
4127	nop
4128	nop
4129	bra.b		~+16
4130
4131CASL_FILLER:
4132	nop
4133	nop
4134	nop
4135	nop
4136	nop
4137	nop
4138	nop
4139	bra.b		CASL_START
4140
4141#################################################################
4142# THIS MUST BE THE STATE OF THE INTEGER REGISTER FILE UPON	#
4143# CALLING _isp_cas_finish().					#
4144#								#
4145# D0 = destination[31:0] operand				#
4146# D1 = 'xxxxxx11 -> no reg update; 'xxxxxx00 -> update required	#
4147# D2 = xxxxxxxx							#
4148# D3 = xxxxxxxx							#
4149# D4 = compare[31:0] operand					#
4150# D5 = xxxxxxxx							#
4151# D6 = xxxxxxxx							#
4152# D7 = xxxxxxxx							#
4153# A0 = xxxxxxxx							#
4154# A1 = xxxxxxxx							#
4155# A2 = xxxxxxxx							#
4156# A3 = xxxxxxxx							#
4157# A4 = xxxxxxxx							#
4158# A5 = xxxxxxxx							#
4159# A6 = frame pointer						#
4160# A7 = stack pointer						#
4161#################################################################
4162
4163casl_noupdate_done:
4164
4165# restore previous SFC/DFC value.
4166	movc		%d6,%sfc		# restore old SFC
4167	movc		%d6,%dfc		# restore old DFC
4168
4169# restore previous interrupt mask level.
4170	mov.w		%d7,%sr			# restore old SR
4171
4172	sf		%d1			# indicate no update was done
4173	bra.l		_isp_cas_finish
4174
4175casl_update_done:
4176
4177# restore previous SFC/DFC value.
4178	movc		%d6,%sfc		# restore old SFC
4179	movc		%d6,%dfc		# restore old DFC
4180
4181# restore previous interrupts mask level.
4182	mov.w		%d7,%sr			# restore old SR
4183
4184	st		%d1			# indicate update was done
4185	bra.l		_isp_cas_finish
4186
4187#######################################
4188casl2:
4189	mov.l		%d2,%d5			# d5 = Update[7:0]
4190	lsr.l		&0x8,%d2
4191	mov.l		%d2,%d3			# d3 = Update[23:8]
4192	swap		%d2			# d2 = Update[31:24]
4193
4194# mask interrupts levels 0-6. save old mask value.
4195	mov.w		%sr,%d7			# save current SR
4196	ori.w		&0x0700,%sr		# inhibit interrupts
4197
4198# load the SFC and DFC with the appropriate mode.
4199	movc		%sfc,%d6		# save old SFC/DFC
4200	movc		%d0,%sfc		# load new sfc
4201	movc		%d0,%dfc		# load new dfc
4202
4203# pre-load the operand ATC. no page faults should occur here because
4204# _real_lock_page() should have taken care of this already.
4205	plpaw		(%a1)			# load atc for ADDR
4206	plpaw		(%a2)			# load atc for ADDR+3
4207
4208# puch the operand lines from the cache if they exist.
4209	cpushl		%dc,(%a1)		# push dirty data
4210	cpushl		%dc,(%a2)		# push dirty data
4211
4212# load the BUSCR values.
4213	mov.l		&0x80000000,%a1		# assert LOCK* buscr value
4214	mov.l		&0xa0000000,%a2		# assert LOCKE* buscr value
4215	mov.l		&0x00000000,%a3		# buscr unlock value
4216
4217# pre-load the instruction cache for the following algorithm.
4218# this will minimize the number of cycles that LOCK* will be asserted.
4219	bra.b		CASL2_ENTER		# start pre-loading icache
4220
4221#
4222# D0 = dst operand <-
4223# D1 = xxxxxxxx
4224# D2 = update[31:24] operand
4225# D3 = update[23:8]  operand
4226# D4 = compare[31:0] operand
4227# D5 = update[7:0]  operand
4228# D6 = old SFC/DFC
4229# D7 = old SR
4230# A0 = ADDR
4231# A1 = bus LOCK*  value
4232# A2 = bus LOCKE* value
4233# A3 = bus unlock value
4234# A4 = xxxxxxxx
4235# A5 = xxxxxxxx
4236#
4237	align		0x10
4238CASL2_START:
4239	movc		%a1,%buscr		# assert LOCK*
4240	movs.l		(%a0),%d0		# fetch Dest[31:0]
4241	cmp.l		%d0,%d4			# Dest - Compare
4242	bne.b		CASL2_NOUPDATE
4243	bra.b		CASL2_UPDATE
4244CASL2_ENTER:
4245	bra.b		~+16
4246
4247CASL2_UPDATE:
4248	movs.b		%d2,(%a0)+		# Update[31:24] -> DEST
4249	movs.w		%d3,(%a0)+		# Update[23:8] -> DEST+0x1
4250	movc		%a2,%buscr		# assert LOCKE*
4251	bra.b		CASL2_UPDATE2
4252	bra.b		~+16
4253
4254CASL2_UPDATE2:
4255	movs.b		%d5,(%a0)		# Update[7:0] -> DEST+0x3
4256	movc		%a3,%buscr		# unlock the bus
4257	bra.w		casl_update_done
4258	nop
4259	bra.b		~+16
4260
4261CASL2_NOUPDATE:
4262	rol.l		&0x8,%d0		# get Dest[31:24]
4263	movs.b		%d0,(%a0)+		# Dest[31:24] -> DEST
4264	swap		%d0			# get Dest[23:8]
4265	movs.w		%d0,(%a0)+		# Dest[23:8] -> DEST+0x1
4266	bra.b		CASL2_NOUPDATE2
4267	bra.b		~+16
4268
4269CASL2_NOUPDATE2:
4270	rol.l		&0x8,%d0		# get Dest[7:0]
4271	movc		%a2,%buscr		# assert LOCKE*
4272	movs.b		%d0,(%a0)		# Dest[7:0] -> DEST+0x3
4273	bra.b		CASL2_NOUPDATE3
4274	nop
4275	bra.b		~+16
4276
4277CASL2_NOUPDATE3:
4278	movc		%a3,%buscr		# unlock the bus
4279	bra.w		casl_noupdate_done
4280	nop
4281	nop
4282	nop
4283	bra.b		~+16
4284
4285CASL2_FILLER:
4286	nop
4287	nop
4288	nop
4289	nop
4290	nop
4291	nop
4292	nop
4293	bra.b		CASL2_START
4294
4295####
4296####
4297# end label used by _isp_cas_inrange()
4298	global		_CASHI
4299_CASHI:
4300