home *** CD-ROM | disk | FTP | other *** search
/ Geek Gadgets 1 / ADE-1.bin / ade-dist / ixemul-45.0-src.tgz / tar.out / contrib / ixemul / library / trap.s < prev    next >
Text File  |  1996-09-28  |  16KB  |  608 lines
  1. /*
  2.  *  This file is part of ixemul.library for the Amiga.
  3.  *  Copyright (C) 1991, 1992  Markus M. Wild
  4.  *  Portions Copyright (C) 1994 Rafael W. Luebbert
  5.  *
  6.  *  This library is free software; you can redistribute it and/or
  7.  *  modify it under the terms of the GNU Library General Public
  8.  *  License as published by the Free Software Foundation; either
  9.  *  version 2 of the License, or (at your option) any later version.
  10.  *
  11.  *  This library is distributed in the hope that it will be useful,
  12.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14.  *  Library General Public License for more details.
  15.  *
  16.  *  You should have received a copy of the GNU Library General Public
  17.  *  License along with this library; if not, write to the Free
  18.  *  Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  19.  *
  20.  *  $Id: trap.s,v 1.1 1994/06/19 15:17:35 rluebbert Exp $
  21.  *
  22.  *  $Log: trap.s,v $
  23.  *  Revision 1.1  1994/06/19  15:17:35  rluebbert
  24.  *  Initial revision
  25.  *
  26.  */
  27.  
  28.     .globl    _trap_00
  29.     .globl    _restore_00
  30.     .globl    _sup00_do_sigresume
  31.     .globl    _sup00_do_sigreturn
  32.     .globl    _sup00_do_sigreturn_ssp
  33.     .globl    _trap_20
  34.     .globl    _restore_20
  35.     .globl    _sup20_do_sigresume
  36.     .globl    _sup20_do_sigreturn
  37.     .globl    _sup20_do_sigreturn_ssp
  38.     .globl    _supervisor
  39.     .globl    _do_sigreturn
  40.     .globl    _launch_glue
  41.     .globl    _addupc
  42.     .globl    _resetfpu
  43.     .globl    _install_vector
  44.     .globl    _restore_vector
  45.     .globl  _vector_install_count
  46.     .globl    _vector_old_pc
  47.     .globl    _vector_nop
  48.  
  49. #undef DEBUG
  50.  
  51.     | This is the trap processing function for the mc68000.
  52.     | Things are quite easy here, just save the general purpose registers
  53.     | and the pc/sr combo, call trap(), then restore the previous
  54.     | context and return
  55. _trap_00:
  56.     movel    a5,sp@-        | need a scratch register
  57.     movel    usp,a5        | get usp
  58.     movel    a5,a5@(-10)    | store usp
  59.     lea    a5@(-10),a5    | make room for sr, pc and usp.
  60.     moveml    d0-d7/a0-a6,a5@-| store registers on usp
  61.     movel    sp@+,a5@(0x34)    | insert the saved a5 into the saveset
  62.     movel    sp@+,d1        | remember trapnumber
  63.     movew    sp@+,a5@(0x44)    | copy SR
  64.     movel    sp@+,d2        | remember and
  65.     movel    d2,a5@(0x40)    | copy (offending?) PC
  66.     movel    a5,a3        | save pointer to registers
  67.  
  68.     movel    #0,a5@-        | pass NULL pointer to stored fpu registers
  69.     movel    a3,a5@-        | pass pointer to stored registers
  70.     movel    d2,a5@-        | pass offending PC
  71.     addl    d1,d1        | convert the passed trap number into a fake
  72.     addl    d1,d1        | 68020 frame format word
  73.     movel    d1,a5@-        | pass frame format word
  74.     |
  75.     | pass return address and ssp-value on userstack
  76.     | This happens for the same reason as we have a glue_launch entry.
  77.     | trap cleans up these 8 bytes on the user stack itself
  78.     movel    sp,a5@-
  79.     movel    #_restore_00,a5@-
  80.     movel    a5,usp        | and remember current value of usp (a5)
  81.  
  82.     | fine, now process this trap. This might (doesn't have to) push
  83.     | additional frames. If not, we just return where the exception 
  84.     | took place (and probably will again...)
  85.  
  86.     jsr    _trap        | to the higher level C trap processor 
  87.  
  88. _restore_00:
  89.     movel    usp,a5        | get usp
  90.     lea    a5@(16),a5    | skip trap argument
  91.     movel    a5@(0x3c),a1    | get stored usp
  92.     movel    a1,usp
  93.     
  94.     | set up the original supervisor stack frame
  95.     movel    a5@(0x40),sp@-    | copy PC
  96.     movew    a5@(0x44),sp@-    | and SR
  97.     moveml    a5@,d0-d7/a0-a6    | and the other registers
  98.     rte
  99.  
  100.  
  101.  
  102.     | This is the trap processor for the mc68020 and above, paired with
  103.     | an fpu (don't *need* an fpu though).
  104.     | What is done: start is same as with 68000, but then the complete
  105.     | additional exception frame is saved on the usp, together with the
  106.     | fpu state. Then trap() is called, and then the previous context
  107.     | is restored (involves copying back the frame from the usp over to the ssp)
  108. _trap_20:
  109.     movel    a5,sp@        | nuke the trap number, we use the frame format word
  110.     movel    usp,a5        | get usp
  111.     movel    a5,a5@(-10)    | store usp
  112.     lea    a5@(-10),a5    | make room for sr, pc and usp.
  113.     moveml    d0-d7/a0-a6,a5@-| store registers on usp
  114.     movel    sp@+,a5@(0x34)    | insert the saved a5 into the saveset
  115.     movew    sp@+,a5@(0x44)    | copy SR
  116.     movel    sp@+,d2        | remember and
  117.     movel    d2,a5@(0x40)    | copy (offending?) PC
  118.     movel    a5,a3        | save pointer to registers
  119.  
  120.     | find out more about the frame (according to the MC68030 user manual)
  121.     clrl    d1
  122.     movew    sp@+,d1        | remember frame format word
  123.     movew    d1,d0
  124.     andw    #0xf000,d0
  125.     beq    Lfmt_S0        | S0
  126.     cmpw    #0x1000,d0
  127.     beq    Lfmt_S1        | S1 this (interrupt) frame shouldn't be here...
  128.     cmpw    #0x2000,d0
  129.     beq    Lfmt_S2        | CHK{2},cpTRAPcc,TRAPV,Trace,Div0,MMUcfg,cp post instr
  130.     cmpw    #0x9000,d0
  131.     beq    Lfmt_S9        | cp mid instr,main det prot viol,int during cp instr
  132.     cmpw    #0xa000,d0
  133.     beq    Lfmt_SA_SB    | address or bus error, short and long frame
  134.     cmpw    #0xb000,d0
  135.     bne    Lfmt_S0        | ??? frame, this will probably not fully cleanup sp..
  136.  
  137. Lfmt_SA_SB:
  138.     | this part (upto Lbe10) inspired by locore.s in sys/hp300/ of BSD4.3-reno
  139.     movew    sp@(2),d0    | grab SSW for fault processing
  140.     btst    #12,d0        | RB set?
  141.     beq    LbeX0        | no, test RC
  142.     bset    #14,d0        | yes, must set FB
  143.     movew    d0,sp@(2)    | for hardware too
  144. LbeX0:
  145.     btst    #13,d0        | RC set?
  146.     beq    LbeX1        | no, skip
  147.     bset    #15,d0        | yes, must set FC
  148.     movew    d0,sp@(2)    | for hardware too
  149. LbeX1:
  150.     btst    #8,d0        | data fault?
  151.     beq    Lbe0        | no, check for hard cases
  152.     movel    sp@(8),d2    | fault address is as given in frame
  153.     bra    Lbe10        | thats it
  154. Lbe0:
  155.     btst    #12,d1        | long (type B) stack frame?
  156.     bne    Lbe4        | yes, go handle
  157.     btst    #14,d0        | no, can use saved PC. FB set?
  158.     beq    Lbe3        | no, try FC
  159.     addql    #4,d2        | yes, adjust address
  160.     bra    Lbe10        | done
  161. Lbe3:
  162.     btst    #15,d0        | FC set?
  163.     beq    Lbe10        | no, done
  164.     addql    #2,d2        | yes, adjust address
  165.     bra    Lbe10        | done
  166. Lbe4:
  167.     movel    sp@(28),d2    | long format, use stage B address
  168.     btst    #15,d0        | FC set?
  169.     beq    Lbe10        | no, all done
  170.     subql    #2,d2        | yes, adjust address
  171. Lbe10:
  172.  
  173.     | now move the frame over to the usp (6/21 longwords remain)
  174.     
  175.     moveml    sp@+,d3-d7/a0    | may trash as many registers as I like, I saved
  176.     moveml    d3-d7/a0,a5@-    | them already ;-) First copy 6 longs
  177.  
  178.     btst    #12,d1        | long (type B) stack frame?
  179.     beq    Lfmt_S0        | nope, done
  180.  
  181.     moveml    sp@+,d3-d7/a0-a2 | first copy 8 longs
  182.     moveml    d3-d7/a0-a2,a5@-
  183.     moveml    sp@+,d3-d7/a0-a1 | plus 7 gives 15, plus already stored 6 is 21
  184.     moveml    d3-d7/a0-a1,a5@-
  185.     bra    Lfmt_S0        | finito
  186.  
  187. Lfmt_S9:
  188.     movel    sp@+,a5@-    | S9 is an S2 plus 4 internal (word length) registers
  189.     movel    sp@+,a5@-    | so store those registers, and fall into S2
  190.  
  191. Lfmt_S2:
  192.     movel    sp@+,d2        | S2 contains the offending instruction address
  193.                 | and the frame format word
  194.     movel    d2,a5@-        | we have the offending instruction address here
  195.  
  196.     | fall into
  197.  
  198. Lfmt_S0:
  199. Lfmt_S1:
  200.     movew    d1,a5@-        | and as the last thing store the frame format word
  201.  
  202.     |
  203.     | now lets look at the fpu, if there is an fpu in the system
  204.     |
  205.  
  206.     movel    #0,a4        | clear pointer to fpu registers
  207.     movel   4:w,a0
  208.     btst    #4,a0@(0x129)    | is AFB_68881 set in SysBase->AttnFlags ??
  209.     beq    Lno_fpu
  210.     fsave    a5@-        | dump the fpu state onto the usp
  211.     moveb    a5@,d0        | and get the fpu state identifier
  212.     beq    Lno_fpu        | null frame?
  213.  
  214.     fmoveml    fpcr/fpsr/fpi,a5@-    | push the fpu control registers and
  215.     fmovemx    fp0-fp7,a5@-        | the fpu data registers
  216.     movel    a5,a4        | store pointer to fpu registers
  217.  
  218.     movew    #-1,a5@-    | mark that there is fpu stuff on the stack
  219. Lno_fpu:
  220.  
  221.     |
  222.     | pass return address and ssp-value on userstack
  223.     | This happens for the same reason as we have a glue_launch entry.
  224.     | trap cleans up these 8 bytes on the user stack itself
  225.     movel    a4,a5@-        | pass pointer to stored fpu registers
  226.     movel    a3,a5@-        | pass pointer to stored registers
  227.     movel    d2,a5@-        | pass offending PC
  228.     movel    d1,a5@-        | pass frame format word
  229.  
  230.     movel    sp,a5@-
  231.     movel    #_restore_20,a5@-
  232.  
  233.     movel    a5,usp        | set the new value of the usp
  234.  
  235.     | that's it, phew.. now process this frame, and perhaps throw some
  236.     | frames on it as well to deal with the signal
  237.     
  238.     jsr    _trap        | do distribution in C ;-)
  239.  
  240. _restore_20:
  241.     
  242.     |
  243.     | restore the saved stack frame from the usp, and copy the necessary
  244.     | parts over to the ssp
  245.     |
  246.  
  247.     movel    usp,a5
  248.     lea    a5@(16),a5    | skip trap() arguments
  249.     
  250.     | first deal with fpu stuff, if there's an fpu
  251.  
  252.         movel   4:w,a0
  253.     btst    #4,a0@(0x129)    | is AFB_68881 set in SysBase->AttnFlags ??
  254.     beq    Lno_fpu2
  255.     tstb    a5@
  256.     beq    Lrst_fpu_frame    | there's only the null frame, go and restore it
  257.  
  258.     lea    a5@(2),a5    | skip fpu indicator
  259.     fmovemx    a5@+,fp0-fp7        | restore fpu data registers and
  260.     fmoveml    a5@+,fpcr/fpsr/fpi    | fpu control registers
  261.     
  262. Lrst_fpu_frame:
  263.     frestore a5@+        | and restore the internal fpu state
  264. Lno_fpu2:
  265.     movew    a5@+,d1        | get frame format word
  266.     movew    d1,d0
  267.     andw    #0xf000,d0
  268.     beq    Lrfmt_S0    | S0
  269.     cmpw    #0x1000,d0
  270.     beq    Lrfmt_S1    | S1
  271.     cmpw    #0x2000,d0
  272.     beq    Lrfmt_S2    | S2
  273.     cmpw    #0x9000,d0
  274.     beq    Lrfmt_S9    | S9
  275.     cmpw    #0xa000,d0
  276.     beq    Lrfmt_SA    | SA
  277.     cmpw    #0xb000,d0
  278.     bne    Lrfmt_S0    | ??? frame
  279.  
  280. Lrfmt_SB:
  281.     moveml    a5@+,d3-d7/a0-a2
  282.     moveml    d3-d7/a0-a2,sp@-
  283.     moveml    a5@+,d3-d7/a0-a1
  284.     moveml    d3-d7/a0-a1,sp@- | copy 15 longs
  285.  
  286. Lrfmt_SA:
  287.     movel    a5@+,sp@-    | copy  3 longs
  288.     movel    a5@+,sp@-
  289.     movel    a5@+,sp@-
  290.     
  291. Lrfmt_S9:
  292.     movel    a5@+,sp@-    | copy  2 longs
  293.     movel    a5@+,sp@-
  294.  
  295. Lrfmt_S2:
  296.     movel    a5@+,sp@-    | copy  1 long
  297.  
  298. Lrfmt_S1:
  299. Lrfmt_S0:
  300.     movew    d1,sp@-        | insert frame format word
  301.     movel    a5@(0x40),sp@-    | copy PC
  302.     movew    a5@(0x44),sp@-    | and SR
  303.     
  304.     movel    a5@(0x3c),a1    | get the stored usp
  305.     movel    a1,usp
  306.     
  307.     moveml    a5@,d0-d7/a0-a6    | restore the cpu registers
  308.     rte            | that's it (finally) .. 
  309.  
  310.     |
  311.     | jump to the given argument in supervisor mode
  312.     | does NOT return
  313.     |
  314. _supervisor:
  315.     movel    sp@(4),a5    | where to go in supervisor
  316.     movel    sp@(8),sp@-    | with this usp
  317.     movel    a6,sp@-
  318.     movel    4:w,a6
  319.     jmp    a6@(-0x1e)    | do it (Supervisor() system call)
  320.  
  321.     |
  322.     | restore signal context
  323.     | argument comes in usp
  324.     |
  325.     | mc68020 entry
  326. _sup20_do_sigresume:
  327.     jsr    _resume_signal_check
  328.     bra    _sup20_do_sigreturn
  329.  
  330. _sup20_do_sigreturn_ssp:    | entry via jsr from supervisor mode
  331.     movel    sp@(4),sp    | set ssp
  332. _sup20_do_sigreturn:
  333.     lea    sp@(-8),sp    | make room for an exception frame
  334.     movew    #0x20,sp@(6)    | fake format word
  335.     bra    fromsup_sigreturn
  336.  
  337.     | mc68000 entry
  338. _sup00_do_sigresume:
  339.     jsr    _resume_signal_check
  340.     bra    _sup00_do_sigreturn
  341.  
  342. _sup00_do_sigreturn_ssp:
  343.     movel    sp@(4),sp    | set ssp
  344. _sup00_do_sigreturn:
  345.     lea    sp@(-6),sp    | make room for an exception frame
  346.  
  347. fromsup_sigreturn:
  348.     moveml    d0/d1/a0/a1,sp@-
  349.  
  350.     movel    usp,a0        | get signal context
  351.     bra    resume_sigreturn
  352.  
  353.     | Supervisor() entry (already comes on exception frame)
  354. _do_sigreturn:
  355.     | make the sigreturn() function preserve all registers
  356.     moveml    d0/d1/a0/a1,sp@-
  357.  
  358.     movel    usp,a0        | get signal context
  359.     movel    a0@+,a6        | restore the trashed a6 register
  360.     movel    a0@,a0
  361.  
  362. resume_sigreturn:
  363.  
  364.  
  365.     movel    4:w,a1        | SysBase
  366.     movel    a1,d1        | remember for later setting of sc_ap
  367.     movel    a1@(0x114),a1    | ThisTask
  368.     movel    a1@(0x2e),a1    | TrapData -> (struct user *)
  369.     
  370.     | struct user offsets are determined by create_defines
  371. #include "ix_internals.h"
  372.     | ok, now a0:sc, a1:u
  373.  
  374.     movel    a0@+,a1@(U_ONSTACK_OFFSET)    | u.u_onstack = sc->sc_onstack
  375.     movel    a0@+,a1@(P_SIGMASK_OFFSET)    | u.p_sigmask = sc->sc_mask
  376.     movel    a0@+,a1        | usp = sc->sc_sp
  377.     movel    a1,usp
  378.     movel    a0@+,a5        | fp  = sc->sc_fp
  379.  
  380.     movel    d1,a1        | get back SysBase
  381.     movel    a1@(0x114),a1    | ThisTask
  382.     lea    a0@(2),a0    | skip unused part of sc_ap
  383.     movew    a0@,a1@(0x10)    | store IDNestCnt,TDNestCnt
  384.     movel    d1,a1
  385.     movew    a0@+,d1        | get IDNestCnt and TDNestCnt
  386.     movew    d1,a1@(0x126)    | store them in SysBase
  387.     tstb    a1@(0x126)
  388.     bmi    Lenable
  389. Ldisabled:
  390.     movew    #0x4000,0xdff09a    | disable interrupts
  391.     bra    Lint_twiddle
  392. Lenable:
  393.     movew    #0xc000,0xdff09a    | enable interrupts
  394. Lint_twiddle:
  395.     | set pc and sr in current exception frame
  396.     movel    a0@+,sp@(2+4*4)    | set PC
  397.     movew    a0@(2),sp@(4*4)    | and SR
  398.     moveml    sp@+,d0/d1/a0/a1
  399.     rte
  400.  
  401.  
  402.  
  403.     |
  404.     | launch_glue is used to invoke the sig_launch handler. We have to care to
  405.     | clean the supervisor stack, if we should call a signal handler from
  406.     | the launch handler.
  407.     | The bad thing is, that doing this right needs information on how
  408.     | the tc_Launch entry is called from the OS. Thus I pass two parameters
  409.     | on the user stack, the value of the `virgin' ssp and the value of
  410.     | a4, which happens to contain the address of the context restore
  411.     | function.
  412.     | It is assumed, that tc_Launch is called via 
  413.     | ... jsr sub
  414.     | ...
  415.     | sub:jsr tc_Launch
  416.     | Thus we have to backup the sp by two jsr's, which is 8.
  417.  
  418. _launch_glue:
  419.     movel    4:w,a0
  420.     movel    a0@(0x114),a0    | tid = SysBase->ThisTask
  421.     movel    a0@(0x36),a0    | usp isn't setup correctly, do it now from tc_SPReg
  422.     movel    sp,a0@-
  423.     addl    #8,a0@
  424.     movel    a4,a0@-
  425.     movel    a0,usp
  426.     
  427.     jsr    _sig_launch    | sig_launch `(void *pc_ret, *ssp_ret)' (`' on usp)
  428.  
  429.     | sig_launch() already corrects the usp to pop those two arguments
  430.     rts
  431.  
  432. /*
  433.  * update profiling information for the user
  434.  * addupc(pc, &u.u_prof, ticks)
  435.  */
  436. _addupc:
  437.     movl    a2,sp@-            | scratch register
  438.     movl    sp@(12),a2        | get &u.u_prof
  439.     movl    sp@(8),d0        | get user pc
  440.     subl    a2@(8),d0        | pc -= pr->pr_off
  441.     jlt    Lauexit            | less than 0, skip it
  442.     movl    a2@(12),d1        | get pr->pr_scale
  443.     lsrl    #1,d0            | pc /= 2
  444.     lsrl    #1,d1            | scale /= 2
  445.  
  446.     movel    d1,sp@-
  447.     movel    d0,sp@-
  448.     jsr    ___mulsi3
  449.     addqw    #8,sp
  450.  
  451.     moveq    #14,d1
  452.     lsrl    d1,d0            | pc >>= 14
  453.     bclr    #0,d0            | pc &= ~1
  454.     cmpl    a2@(4),d0        | too big for buffer?
  455.     jge    Lauexit            | yes, screw it
  456.     addl    a2@,d0            | no, add base
  457.  
  458.     movel    d0,a0
  459.     movew    a0@,d0
  460.  
  461.     addw    sp@(18),d0        | add tick to current value
  462.  
  463.     movew    d0,a0@
  464.  
  465. Lauexit:
  466.     movl    sp@+,a2            | restore scratch reg
  467.     rts
  468.  
  469. Lreset_fpu:
  470.     clrl    sp@-            | prepare 060 fpu null state frame
  471.     clrl    sp@-            | needs 2 additional longs on stack
  472.     clrl    sp@-
  473.     frestore sp@+
  474.  
  475.     | Note that after using frestore with a null state frame we have
  476.     | to set up the control register to restore rounding to truncation
  477.     | rather than round-to-nearest, as required by the ANSI C standard.
  478.  
  479.     moveq    #72,d0
  480.     addl    d0,d0
  481.     fmovel    d0,fpcr
  482.     rte
  483.  
  484. _resetfpu:
  485.     movel    a6,sp@-
  486.     movel    a5,sp@-
  487.     movel    4:w,a6
  488.  
  489.     btst    #7,a6@(0x129)        | is AFB_68060 set in SysBase->AttnFlags ?
  490.     lea    Lreset_fpu,a5
  491.     jne    L_skip
  492.     addqw    #4,a5
  493. L_skip:
  494.     jsr    a6@(-30)        | Supervisor()
  495.  
  496.     movel    sp@+,a5
  497.     movel    sp@+,a6
  498.     rts
  499.  
  500. | See the Enforcer manual for the original assembler code of the
  501. | following code.
  502.  
  503. | This is the new bus error handler for use with Enforcer & GDB.
  504. | There are two entry points, one for the 68020 and 68030, and one
  505. | for the 68040.
  506.  
  507. _vector_nop:
  508.     nop
  509.  
  510. | make sure next data is on a 4 byte boundary!
  511.  
  512.     .align    2
  513.  
  514. MyExecBase:
  515.     .long    0            | The local copy
  516. OldVector:
  517.     .long    0            | One long word
  518. _vector_install_count:
  519.     .long    0            | use count
  520. _vector_old_pc:
  521.     .long    0            | store old pc
  522.  
  523. _new_vector_040:
  524.     cmpl    #4,sp@(20)        | 68040
  525.     jeq    TraceSkip        | If AbsExecBase, OK
  526.     jra    TraceProcess
  527.  
  528. _new_vector:
  529.     cmpl    #4,sp@(16)
  530.     jeq    TraceSkip        | If AbsExecBase, OK
  531.  
  532. | Check if the current Task is running under ixemul control and
  533. | is being traced by ixemul.
  534.  
  535. TraceProcess:
  536.     movel    a0,sp@-            | Save this...
  537.     movel    d0,sp@-
  538.  
  539.     movel    MyExecBase,a0        | Get ExecBase...
  540.     movel    a0@(276:W),a0        | Get ThisTask
  541.     movel    a0@(46:W),d0        | TrapData -> (struct user *)
  542.     movel    d0,a0
  543.     jeq    NoTrace            | Not ixemul
  544.     movel    a0@,d0            | get u_ixbase
  545.     cmpl    _ixemulbase,d0        | == ixemulbase?
  546.     jne    NoTrace            | Nope, not an ixemul program
  547.     btst    #4,a0@(P_FLAG_OFFSET+3)    | Test STRC flag
  548.     beq    NoTrace
  549.     btst    #7,sp@(8:W)        | Are we already single stepping?
  550.     jne    NoTrace
  551.     bset    #7,sp@(8:W)        | Set trace bit...
  552.     lea    _vector_old_pc,a0
  553.     tstl    a0@            | is this variable already in use?
  554.     jne    NoTrace
  555.     movel    sp@(10:W),a0@        | store old pc
  556.     lea    _vector_nop,a0
  557.     movel    a0,sp@(10:W)        | store new pc
  558.  
  559. NoTrace:
  560.     movel    sp@+,d0            | Restore...
  561.     movel    sp@+,a0
  562.  
  563. TraceSkip:
  564.     movel    OldVector,sp@-        | Ready to return
  565.     rts
  566.  
  567. | Call this function in Supervisor mode
  568.  
  569. _restore_vector:
  570.     btst    #3,@(4)@(0x129)        | is AFB_68040 set in SysBase->AttnFlags ??
  571.     lea    _new_vector,a0
  572.     jeq    got_vector1
  573.     lea    _new_vector_040,a0
  574.  
  575. got_vector1:
  576.     lea    _vector_install_count,a1
  577.     tstl    a1@
  578.     beq    restore_exit
  579.     subql    #1,a1@            | decrease use count
  580.     bne    restore_exit
  581.     movec    vbr,a1            | get Vector Base Register
  582.     cmpl    a1@(8:W),a0        | sanity check
  583.     jne    restore_exit
  584.     movel    OldVector,a1@(8:W)
  585.  
  586. restore_exit:
  587.     rte
  588.  
  589. | Call this function in Supervisor mode
  590.  
  591. _install_vector:
  592.     btst    #3,@(4)@(0x129)        | is AFB_68040 set in SysBase->AttnFlags ??
  593.     lea    _new_vector,a0
  594.     jeq    got_vector2
  595.     lea    _new_vector_040,a0
  596.  
  597. got_vector2:
  598.     addql    #1,_vector_install_count | increase use count
  599.     movec    vbr,a1            | get Vector Base Register
  600.     cmpl    a1@(8:W),a0
  601.     jeq    install_done        | sanity check
  602.     movel    4:W,MyExecBase
  603.     movel    a1@(8:W),OldVector
  604.     movel    a0,a1@(8:W)
  605.  
  606. install_done:
  607.     rte
  608.