*       $NetBSD: x_store.sa,v 1.5 2022/04/08 10:17:53 andvar Exp $

*       MOTOROLA MICROPROCESSOR & MEMORY TECHNOLOGY GROUP
*       M68000 Hi-Performance Microprocessor Division
*       M68040 Software Package
*
*       M68040 Software Package Copyright (c) 1993, 1994 Motorola Inc.
*       All rights reserved.
*
*       THE SOFTWARE is provided on an "AS IS" basis and without warranty.
*       To the maximum extent permitted by applicable law,
*       MOTOROLA DISCLAIMS ALL WARRANTIES WHETHER EXPRESS OR IMPLIED,
*       INCLUDING IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A
*       PARTICULAR PURPOSE and any warranty against infringement with
*       regard to the SOFTWARE (INCLUDING ANY MODIFIED VERSIONS THEREOF)
*       and any accompanying written materials.
*
*       To the maximum extent permitted by applicable law,
*       IN NO EVENT SHALL MOTOROLA BE LIABLE FOR ANY DAMAGES WHATSOEVER
*       (INCLUDING WITHOUT LIMITATION, DAMAGES FOR LOSS OF BUSINESS
*       PROFITS, BUSINESS INTERRUPTION, LOSS OF BUSINESS INFORMATION, OR
*       OTHER PECUNIARY LOSS) ARISING OF THE USE OR INABILITY TO USE THE
*       SOFTWARE.  Motorola assumes no responsibility for the maintenance
*       and support of the SOFTWARE.
*
*       You are hereby granted a copyright license to use, modify, and
*       distribute the SOFTWARE so long as this entire notice is retained
*       without alteration in any modified and/or redistributed versions,
*       and that such modified versions are clearly identified as such.
*       No licenses are granted by implication, estoppel or otherwise
*       under any patents or trademarks of Motorola, Inc.

*
*       x_store.sa 3.2 1/24/91
*
*       store --- store operand to memory or register
*
*       Used by underflow and overflow handlers.
*
*       a6 = points to fp value to be stored.
*

X_STORE IDNT    2,1 Motorola 040 Floating Point Software Package

       section 8

fpreg_mask:
       dc.b    $80,$40,$20,$10,$08,$04,$02,$01

       include fpsp.h

       xref    mem_write
       xref    get_fline
       xref    g_opcls
       xref    g_dfmtou
       xref    reg_dest

       xdef    dest_ext
       xdef    dest_dbl
       xdef    dest_sgl

       xdef    store
store:
       btst.b  #E3,E_BYTE(a6)
       beq.b   E1_sto
E3_sto:
       move.l  CMDREG3B(a6),d0
       bfextu  d0{6:3},d0              ;isolate dest. reg from cmdreg3b
sto_fp:
       lea     fpreg_mask,a1
       move.b  (a1,d0.w),d0            ;convert reg# to dynamic register mask
       tst.b   LOCAL_SGN(a0)
       beq.b   is_pos
       bset.b  #sign_bit,LOCAL_EX(a0)
is_pos:
       fmovem.x (a0),d0                ;move to correct register
*
*       if fp0-fp3 is being modified, we must put a copy
*       in the USER_FPn variable on the stack because all exception
*       handlers restore fp0-fp3 from there.
*
       cmp.b   #$80,d0
       bne.b   not_fp0
       fmovem.x fp0,USER_FP0(a6)
       rts
not_fp0:
       cmp.b   #$40,d0
       bne.b   not_fp1
       fmovem.x fp1,USER_FP1(a6)
       rts
not_fp1:
       cmp.b   #$20,d0
       bne.b   not_fp2
       fmovem.x fp2,USER_FP2(a6)
       rts
not_fp2:
       cmp.b   #$10,d0
       bne.b   not_fp3
       fmovem.x fp3,USER_FP3(a6)
       rts
not_fp3:
       rts

E1_sto:
       bsr.l   g_opcls         ;returns opclass in d0
       cmpi.b  #3,d0
       beq     opc011          ;branch if opclass 3
       move.l  CMDREG1B(a6),d0
       bfextu  d0{6:3},d0      ;extract destination register
       bra.b   sto_fp

opc011:
       bsr.l   g_dfmtou        ;returns dest format in d0
*                               ;ext=00, sgl=01, dbl=10
       move.l  a0,a1           ;save source addr in a1
       move.l  EXC_EA(a6),a0   ;get the address
       tst.l   d0              ;if dest format is extended
       beq.w   dest_ext        ;then branch
       cmpi.l  #1,d0           ;if dest format is single
       beq.b   short_dest_sgl  ;then branch
*
*       fall through to dest_dbl
*

*
*       dest_dbl --- write double precision value to user space
*
*Input
*       a0 -> destination address
*       a1 -> source in extended precision
*Output
*       a0 -> destroyed
*       a1 -> destroyed
*       d0 -> 0
*
*Changes extended precision to double precision.
* Note: no attempt is made to round the extended value to double.
*       dbl_sign = ext_sign
*       dbl_exp = ext_exp - $3fff(ext bias) + $7ff(dbl bias)
*       get rid of ext integer bit
*       dbl_mant = ext_mant{62:12}
*
*               ---------------   ---------------    ---------------
*  extended ->  |s|    exp    |   |1| ms mant   |    | ls mant     |
*               ---------------   ---------------    ---------------
*                95         64    63 62       32      31     11   0
*                                    |                       |
*                                    |                       |
*                                    |                       |
*                                    v                       v
*                             ---------------   ---------------
*  double   ->                |s|exp| mant  |   |  mant       |
*                             ---------------   ---------------
*                             63     51   32   31              0
*
dest_dbl:
       clr.l   d0              ;clear d0
       move.w  LOCAL_EX(a1),d0 ;get exponent
       sub.w   #$3fff,d0       ;subtract extended precision bias
       cmp.w   #$4000,d0       ;check if inf
       beq.b   inf             ;if so, special case
       add.w   #$3ff,d0        ;add double precision bias
       swap    d0              ;d0 now in upper word
       lsl.l   #4,d0           ;d0 now in proper place for dbl prec exp
       tst.b   LOCAL_SGN(a1)
       beq.b   get_mant        ;if positive, go process mantissa
       bset.l  #31,d0          ;if negative, put in sign information
*                               ; before continuing
       bra.b   get_mant        ;go process mantissa
inf:
       move.l  #$7ff00000,d0   ;load dbl inf exponent
       clr.l   LOCAL_HI(a1)    ;clear msb
       tst.b   LOCAL_SGN(a1)
       beq.b   dbl_inf         ;if positive, go ahead and write it
       bset.l  #31,d0          ;if negative put in sign information
dbl_inf:
       move.l  d0,LOCAL_EX(a1) ;put the new exp back on the stack
       bra.b   dbl_wrt
get_mant:
       move.l  LOCAL_HI(a1),d1 ;get ms mantissa
       bfextu  d1{1:20},d1     ;get upper 20 bits of ms
       or.l    d1,d0           ;put these bits in ms word of double
       move.l  d0,LOCAL_EX(a1) ;put the new exp back on the stack
       move.l  LOCAL_HI(a1),d1 ;get ms mantissa
       move.l  #21,d0          ;load shift count
       lsl.l   d0,d1           ;put lower 11 bits in upper bits
       move.l  d1,LOCAL_HI(a1) ;build lower lword in memory
       move.l  LOCAL_LO(a1),d1 ;get ls mantissa
       bfextu  d1{0:21},d0     ;get ls 21 bits of double
       or.l    d0,LOCAL_HI(a1) ;put them in double result
dbl_wrt:
       move.l  #$8,d0          ;byte count for double precision number
       exg     a0,a1           ;a0=supervisor source, a1=user dest
       bsr.l   mem_write       ;move the number to the user's memory
       rts
*
*       dest_sgl --- write single precision value to user space
*
*Input
*       a0 -> destination address
*       a1 -> source in extended precision
*
*Output
*       a0 -> destroyed
*       a1 -> destroyed
*       d0 -> 0
*
*Changes extended precision to single precision.
*       sgl_sign = ext_sign
*       sgl_exp = ext_exp - $3fff(ext bias) + $7f(sgl bias)
*       get rid of ext integer bit
*       sgl_mant = ext_mant{62:12}
*
*               ---------------   ---------------    ---------------
*  extended ->  |s|    exp    |   |1| ms mant   |    | ls mant     |
*               ---------------   ---------------    ---------------
*                95         64    63 62    40 32      31     12   0
*                                    |     |
*                                    |     |
*                                    |     |
*                                    v     v
*                             ---------------
*  single   ->                |s|exp| mant  |
*                             ---------------
*                             31     22     0
*
dest_sgl:
short_dest_sgl:
       clr.l   d0
       move.w  LOCAL_EX(a1),d0 ;get exponent
       sub.w   #$3fff,d0       ;subtract extended precision bias
       cmp.w   #$4000,d0       ;check if inf
       beq.b   sinf            ;if so, special case
       add.w   #$7f,d0         ;add single precision bias
       swap    d0              ;put exp in upper word of d0
       lsl.l   #7,d0           ;shift it into single exp bits
       tst.b   LOCAL_SGN(a1)
       beq.b   get_sman        ;if positive, continue
       bset.l  #31,d0          ;if negative, put in sign first
       bra.b   get_sman        ;get mantissa
sinf:
       move.l  #$7f800000,d0   ;load single inf exp to d0
       tst.b   LOCAL_SGN(a1)
       beq.b   sgl_wrt         ;if positive, continue
       bset.l  #31,d0          ;if negative, put in sign info
       bra.b   sgl_wrt

get_sman:
       move.l  LOCAL_HI(a1),d1 ;get ms mantissa
       bfextu  d1{1:23},d1     ;get upper 23 bits of ms
       or.l    d1,d0           ;put these bits in ms word of single

sgl_wrt:
       move.l  d0,L_SCR1(a6)   ;put the new exp back on the stack
       move.l  #$4,d0          ;byte count for single precision number
       tst.l   a0              ;users destination address
       beq.b   sgl_Dn          ;destination is a data register
       exg     a0,a1           ;a0=supervisor source, a1=user dest
       lea.l   L_SCR1(a6),a0   ;point a0 to data
       bsr.l   mem_write       ;move the number to the user's memory
       rts
sgl_Dn:
       bsr.l   get_fline       ;returns fline word in d0
       and.w   #$7,d0          ;isolate register number
       move.l  d0,d1           ;d1 has size:reg formatted for reg_dest
       or.l    #$10,d1         ;reg_dest wants size added to reg#
       bra.l   reg_dest        ;size is X, rts in reg_dest will
*                               ;return to caller of dest_sgl

dest_ext:
       tst.b   LOCAL_SGN(a1)   ;put back sign into exponent word
       beq.b   dstx_cont
       bset.b  #sign_bit,LOCAL_EX(a1)
dstx_cont:
       clr.b   LOCAL_SGN(a1)   ;clear out the sign byte

       move.l  #$0c,d0         ;byte count for extended number
       exg     a0,a1           ;a0=supervisor source, a1=user dest
       bsr.l   mem_write       ;move the number to the user's memory
       rts

       end