linux/arch/m68k/ifpsp060/src/isp.S
<<
>>
Prefs
   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 regsiter, 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 efective       #
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