Add Tercel PHY reset synchronization
[microwatt.git] / fpu.vhdl
index 0cbd43fabd91194229c13df1f54444521180145f..93fa9d68880c996be4aefcdb961bd9430284222f 100644 (file)
--- a/fpu.vhdl
+++ b/fpu.vhdl
@@ -37,20 +37,26 @@ architecture behaviour of fpu is
 
     type state_t is (IDLE,
                      DO_MCRFS, DO_MTFSB, DO_MTFSFI, DO_MFFS, DO_MTFSF,
-                     DO_FMR, DO_FMRG, DO_FCMP,
+                     DO_FMR, DO_FMRG, DO_FCMP, DO_FTDIV, DO_FTSQRT,
                      DO_FCFID, DO_FCTI,
                      DO_FRSP, DO_FRI,
-                     DO_FADD, DO_FMUL, DO_FDIV,
+                     DO_FADD, DO_FMUL, DO_FDIV, DO_FSQRT, DO_FMADD,
                      DO_FRE, DO_FRSQRTE,
                      DO_FSEL,
                      FRI_1,
-                     ADD_SHIFT, ADD_2, ADD_3,
+                     ADD_1, ADD_SHIFT, ADD_2, ADD_3,
                      CMP_1, CMP_2,
                      MULT_1,
+                     FMADD_1, FMADD_2, FMADD_3,
+                     FMADD_4, FMADD_5, FMADD_6,
                      LOOKUP,
                      DIV_2, DIV_3, DIV_4, DIV_5, DIV_6,
                      FRE_1,
                      RSQRT_1,
+                     FTDIV_1,
+                     SQRT_1, SQRT_2, SQRT_3, SQRT_4,
+                     SQRT_5, SQRT_6, SQRT_7, SQRT_8,
+                     SQRT_9, SQRT_10, SQRT_11, SQRT_12,
                      INT_SHIFT, INT_ROUND, INT_ISHIFT,
                      INT_FINAL, INT_CHECK, INT_OFLOW,
                      FINISH, NORMALIZE,
@@ -59,15 +65,19 @@ architecture behaviour of fpu is
                      DENORM,
                      RENORM_A, RENORM_A2,
                      RENORM_B, RENORM_B2,
-                     RENORM_C, RENORM_C2);
+                     RENORM_C, RENORM_C2,
+                     NAN_RESULT, EXC_RESULT);
 
     type reg_type is record
         state        : state_t;
         busy         : std_ulogic;
         instr_done   : std_ulogic;
         do_intr      : std_ulogic;
+        illegal      : std_ulogic;
         op           : insn_type_t;
         insn         : std_ulogic_vector(31 downto 0);
+        nia          : std_ulogic_vector(63 downto 0);
+        instr_tag    : instr_tag_t;
         dest_fpr     : gspr_index_t;
         fe_mode      : std_ulogic;
         rc           : std_ulogic;
@@ -78,6 +88,7 @@ architecture behaviour of fpu is
         b            : fpu_reg_type;
         c            : fpu_reg_type;
         r            : std_ulogic_vector(63 downto 0);  -- 10.54 format
+        s            : std_ulogic_vector(55 downto 0);  -- extended fraction
         x            : std_ulogic;
         p            : std_ulogic_vector(63 downto 0);  -- 8.56 format
         y            : std_ulogic_vector(63 downto 0);  -- 8.56 format
@@ -97,11 +108,20 @@ architecture behaviour of fpu is
         round_mode   : std_ulogic_vector(2 downto 0);
         is_subtract  : std_ulogic;
         exp_cmp      : std_ulogic;
+        madd_cmp     : std_ulogic;
         add_bsmall   : std_ulogic;
         is_multiply  : std_ulogic;
         is_sqrt      : std_ulogic;
         first        : std_ulogic;
         count        : unsigned(1 downto 0);
+        doing_ftdiv  : std_ulogic_vector(1 downto 0);
+        opsel_a      : std_ulogic_vector(1 downto 0);
+        use_a        : std_ulogic;
+        use_b        : std_ulogic;
+        use_c        : std_ulogic;
+        invalid      : std_ulogic;
+        negate       : std_ulogic;
+        longmask     : std_ulogic;
     end record;
 
     type lookup_table is array(0 to 1023) of std_ulogic_vector(17 downto 0);
@@ -109,11 +129,11 @@ architecture behaviour of fpu is
     signal r, rin : reg_type;
 
     signal fp_result     : std_ulogic_vector(63 downto 0);
-    signal opsel_a       : std_ulogic_vector(1 downto 0);
     signal opsel_b       : std_ulogic_vector(1 downto 0);
     signal opsel_r       : std_ulogic_vector(1 downto 0);
+    signal opsel_s       : std_ulogic_vector(1 downto 0);
     signal opsel_ainv    : std_ulogic;
-    signal opsel_amask   : std_ulogic;
+    signal opsel_mask    : std_ulogic;
     signal opsel_binv    : std_ulogic;
     signal in_a          : std_ulogic_vector(63 downto 0);
     signal in_b          : std_ulogic_vector(63 downto 0);
@@ -122,6 +142,7 @@ architecture behaviour of fpu is
     signal lost_bits     : std_ulogic;
     signal r_hi_nz       : std_ulogic;
     signal r_lo_nz       : std_ulogic;
+    signal s_nz          : std_ulogic;
     signal misc_sel      : std_ulogic_vector(3 downto 0);
     signal f_to_multiply : MultiplyInputType;
     signal multiply_to_f : MultiplyOutputType;
@@ -139,13 +160,19 @@ architecture behaviour of fpu is
 
     constant BIN_ZERO : std_ulogic_vector(1 downto 0) := "00";
     constant BIN_R    : std_ulogic_vector(1 downto 0) := "01";
-    constant BIN_MASK : std_ulogic_vector(1 downto 0) := "10";
+    constant BIN_RND  : std_ulogic_vector(1 downto 0) := "10";
+    constant BIN_PS6  : std_ulogic_vector(1 downto 0) := "11";
 
     constant RES_SUM   : std_ulogic_vector(1 downto 0) := "00";
     constant RES_SHIFT : std_ulogic_vector(1 downto 0) := "01";
     constant RES_MULT  : std_ulogic_vector(1 downto 0) := "10";
     constant RES_MISC  : std_ulogic_vector(1 downto 0) := "11";
 
+    constant S_ZERO  : std_ulogic_vector(1 downto 0) := "00";
+    constant S_NEG   : std_ulogic_vector(1 downto 0) := "01";
+    constant S_SHIFT : std_ulogic_vector(1 downto 0) := "10";
+    constant S_MULT  : std_ulogic_vector(1 downto 0) := "11";
+
     -- msel values
     constant MUL1_A : std_ulogic_vector(1 downto 0) := "00";
     constant MUL1_B : std_ulogic_vector(1 downto 0) := "01";
@@ -157,9 +184,10 @@ architecture behaviour of fpu is
     constant MUL2_P   : std_ulogic_vector(1 downto 0) := "10";
     constant MUL2_R   : std_ulogic_vector(1 downto 0) := "11";
 
-    constant MULADD_ZERO : std_ulogic_vector(1 downto 0) := "00";
+    constant MULADD_ZERO  : std_ulogic_vector(1 downto 0) := "00";
     constant MULADD_CONST : std_ulogic_vector(1 downto 0) := "01";
     constant MULADD_A     : std_ulogic_vector(1 downto 0) := "10";
+    constant MULADD_RS    : std_ulogic_vector(1 downto 0) := "11";
 
     -- Inverse lookup table, indexed by the top 8 fraction bits
     -- The first 256 entries are the reciprocal (1/x) lookup table,
@@ -546,9 +574,9 @@ begin
 
     e_out.busy <= r.busy;
     e_out.exception <= r.fpscr(FPSCR_FEX);
-    e_out.interrupt <= r.do_intr;
 
     w_out.valid <= r.instr_done and not r.do_intr;
+    w_out.instr_tag <= r.instr_tag;
     w_out.write_enable <= r.writing_back;
     w_out.write_reg <= r.dest_fpr;
     w_out.write_data <= fp_result;
@@ -556,6 +584,10 @@ begin
     w_out.write_cr_mask <= r.cr_mask;
     w_out.write_cr_data <= r.cr_result & r.cr_result & r.cr_result & r.cr_result &
                            r.cr_result & r.cr_result & r.cr_result & r.cr_result;
+    w_out.interrupt <= r.do_intr;
+    w_out.intr_vec <= 16#700#;
+    w_out.srr0 <= r.nia;
+    w_out.srr1 <= (47-44 => r.illegal, 47-43 => not r.illegal, others => '0');
 
     fpu_1: process(all)
         variable v           : reg_type;
@@ -591,19 +623,23 @@ begin
         variable need_check  : std_ulogic;
         variable msb         : std_ulogic;
         variable is_add      : std_ulogic;
-        variable qnan_result : std_ulogic;
-        variable longmask    : std_ulogic;
         variable set_a       : std_ulogic;
         variable set_b       : std_ulogic;
         variable set_c       : std_ulogic;
-        variable px_nz       : std_ulogic;
-        variable maddend     : std_ulogic_vector(127 downto 0);
         variable set_y       : std_ulogic;
+        variable set_s       : std_ulogic;
+        variable qnan_result : std_ulogic;
+        variable px_nz       : std_ulogic;
         variable pcmpb_eq    : std_ulogic;
         variable pcmpb_lt    : std_ulogic;
         variable pshift      : std_ulogic;
         variable renorm_sqrt : std_ulogic;
         variable sqrt_exp    : signed(EXP_BITS-1 downto 0);
+        variable shiftin     : std_ulogic;
+        variable mulexp      : signed(EXP_BITS-1 downto 0);
+        variable maddend     : std_ulogic_vector(127 downto 0);
+        variable sum         : std_ulogic_vector(63 downto 0);
+        variable round_inc   : std_ulogic_vector(63 downto 0);
     begin
         v := r;
         illegal := '0';
@@ -613,10 +649,13 @@ begin
         -- capture incoming instruction
         if e_in.valid = '1' then
             v.insn := e_in.insn;
+            v.nia := e_in.nia;
             v.op := e_in.op;
+            v.instr_tag := e_in.itag;
             v.fe_mode := or (e_in.fe_mode);
             v.dest_fpr := e_in.frt;
             v.single_prec := e_in.single;
+            v.longmask := e_in.single;
             v.int_result := '0';
             v.rc := e_in.rc;
             v.is_cmp := e_in.out_cr;
@@ -637,6 +676,8 @@ begin
             v.is_multiply := '0';
             v.is_sqrt := '0';
             v.add_bsmall := '0';
+            v.doing_ftdiv := "00";
+
             adec := decode_dp(e_in.fra, int_input);
             bdec := decode_dp(e_in.frb, int_input);
             cdec := decode_dp(e_in.frc, int_input);
@@ -648,14 +689,27 @@ begin
             if adec.exponent > bdec.exponent then
                 v.exp_cmp := '1';
             end if;
+            v.madd_cmp := '0';
+            if (adec.exponent + cdec.exponent + 1) >= bdec.exponent then
+                v.madd_cmp := '1';
+            end if;
         end if;
 
         r_hi_nz <= or (r.r(55 downto 31));
         r_lo_nz <= or (r.r(30 downto 2));
+        s_nz <= or (r.s);
 
         if r.single_prec = '0' then
-            max_exp := to_signed(1023, EXP_BITS);
-            min_exp := to_signed(-1022, EXP_BITS);
+            if r.doing_ftdiv(1) = '0' then
+                max_exp := to_signed(1023, EXP_BITS);
+            else
+                max_exp := to_signed(1020, EXP_BITS);
+            end if;
+            if r.doing_ftdiv(0) = '0' then
+                min_exp := to_signed(-1022, EXP_BITS);
+            else
+                min_exp := to_signed(-1021, EXP_BITS);
+            end if;
             bias_exp := to_signed(1536, EXP_BITS);
         else
             max_exp := to_signed(127, EXP_BITS);
@@ -688,12 +742,13 @@ begin
         v.update_fprf := '0';
         v.shift := to_signed(0, EXP_BITS);
         v.first := '0';
-        opsel_a <= AIN_R;
+        v.opsel_a := AIN_R;
         opsel_ainv <= '0';
-        opsel_amask <= '0';
+        opsel_mask <= '0';
         opsel_b <= BIN_ZERO;
         opsel_binv <= '0';
         opsel_r <= RES_SUM;
+        opsel_s <= S_ZERO;
         carry_in <= '0';
         misc_sel <= "0000";
         fpscr_mask := (others => '1');
@@ -704,10 +759,10 @@ begin
         renormalize := '0';
         set_x := '0';
         qnan_result := '0';
-        longmask := r.single_prec;
         set_a := '0';
         set_b := '0';
         set_c := '0';
+        set_s := '0';
         f_to_multiply.is_32bit <= '0';
         f_to_multiply.valid <= '0';
         msel_1 <= MUL1_A;
@@ -717,14 +772,27 @@ begin
         set_y := '0';
         pshift := '0';
         renorm_sqrt := '0';
+        shiftin := '0';
         case r.state is
             when IDLE =>
+                v.use_a := '0';
+                v.use_b := '0';
+                v.use_c := '0';
+                v.invalid := '0';
+                v.negate := '0';
                 if e_in.valid = '1' then
                     case e_in.insn(5 downto 1) is
                         when "00000" =>
-                            if e_in.insn(7) = '1' then
+                            if e_in.insn(8) = '1' then
+                                if e_in.insn(6) = '0' then
+                                    v.state := DO_FTDIV;
+                                else
+                                    v.state := DO_FTSQRT;
+                                end if;
+                            elsif e_in.insn(7) = '1' then
                                 v.state := DO_MCRFS;
                             else
+                                v.opsel_a := AIN_B;
                                 v.state := DO_FCMP;
                             end if;
                         when "00110" =>
@@ -744,14 +812,17 @@ begin
                                 v.state := DO_MTFSF;
                             end if;
                         when "01000" =>
+                            v.opsel_a := AIN_B;
                             if e_in.insn(9 downto 8) /= "11" then
                                 v.state := DO_FMR;
                             else
                                 v.state := DO_FRI;
                             end if;
                         when "01100" =>
+                            v.opsel_a := AIN_B;
                             v.state := DO_FRSP;
                         when "01110" =>
+                            v.opsel_a := AIN_B;
                             if int_input = '1' then
                                 -- fcfid[u][s]
                                 v.state := DO_FCFID;
@@ -760,27 +831,53 @@ begin
                             end if;
                         when "01111" =>
                             v.round_mode := "001";
+                            v.opsel_a := AIN_B;
                             v.state := DO_FCTI;
                         when "10010" =>
+                            v.opsel_a := AIN_A;
+                            if v.b.mantissa(54) = '0' and v.a.mantissa(54) = '1' then
+                                v.opsel_a := AIN_B;
+                            end if;
                             v.state := DO_FDIV;
                         when "10100" | "10101" =>
+                            v.opsel_a := AIN_A;
                             v.state := DO_FADD;
+                        when "10110" =>
+                            v.is_sqrt := '1';
+                            v.opsel_a := AIN_B;
+                            v.state := DO_FSQRT;
                         when "10111" =>
                             v.state := DO_FSEL;
                         when "11000" =>
+                            v.opsel_a := AIN_B;
                             v.state := DO_FRE;
                         when "11001" =>
                             v.is_multiply := '1';
+                            v.opsel_a := AIN_A;
+                            if v.c.mantissa(54) = '0' and v.a.mantissa(54) = '1' then
+                                v.opsel_a := AIN_C;
+                            end if;
                             v.state := DO_FMUL;
                         when "11010" =>
                             v.is_sqrt := '1';
+                            v.opsel_a := AIN_B;
                             v.state := DO_FRSQRTE;
+                        when "11100" | "11101" | "11110" | "11111" =>
+                            if v.a.mantissa(54) = '0' then
+                                v.opsel_a := AIN_A;
+                            elsif v.c.mantissa(54) = '0' then
+                                v.opsel_a := AIN_C;
+                            else
+                                v.opsel_a := AIN_B;
+                            end if;
+                            v.state := DO_FMADD;
                         when others =>
                             illegal := '1';
                     end case;
                 end if;
                 v.x := '0';
                 v.old_exc := r.fpscr(FPSCR_VX downto FPSCR_XX);
+                set_s := '1';
 
             when DO_MCRFS =>
                 j := to_integer(unsigned(insn_bfa(r.insn)));
@@ -795,13 +892,44 @@ begin
                 v.instr_done := '1';
                 v.state := IDLE;
 
+            when DO_FTDIV =>
+                v.instr_done := '1';
+                v.state := IDLE;
+                v.cr_result := "0000";
+                if r.a.class = INFINITY or r.b.class = ZERO or r.b.class = INFINITY or
+                    (r.b.class = FINITE and r.b.mantissa(53) = '0') then
+                    v.cr_result(2) := '1';
+                end if;
+                if r.a.class = NAN or r.a.class = INFINITY or
+                    r.b.class = NAN or r.b.class = ZERO or r.b.class = INFINITY or
+                    (r.a.class = FINITE and r.a.exponent <= to_signed(-970, EXP_BITS)) then
+                    v.cr_result(1) := '1';
+                else
+                    v.doing_ftdiv := "11";
+                    v.first := '1';
+                    v.state := FTDIV_1;
+                    v.instr_done := '0';
+                end if;
+
+            when DO_FTSQRT =>
+                v.instr_done := '1';
+                v.state := IDLE;
+                v.cr_result := "0000";
+                if r.b.class = ZERO or r.b.class = INFINITY or
+                    (r.b.class = FINITE and r.b.mantissa(53) = '0') then
+                    v.cr_result(2) := '1';
+                end if;
+                if r.b.class = NAN or r.b.class = INFINITY or r.b.class = ZERO
+                    or r.b.negative = '1' or r.b.exponent <= to_signed(-970, EXP_BITS) then
+                    v.cr_result(1) := '0';
+                end if;
+
             when DO_FCMP =>
                 -- fcmp[uo]
+                -- r.opsel_a = AIN_B
                 v.instr_done := '1';
                 v.state := IDLE;
                 update_fx := '1';
-                opsel_a <= AIN_B;
-                opsel_r <= RES_SUM;
                 v.result_exp := r.b.exponent;
                 if (r.a.class = NAN and r.a.mantissa(53) = '0') or
                     (r.b.class = NAN and r.b.mantissa(53) = '0') then
@@ -847,6 +975,7 @@ begin
                     -- Prepare to subtract mantissas, put B in R
                     v.cr_result := "0000";
                     v.instr_done := '0';
+                    v.opsel_a := AIN_A;
                     v.state := CMP_1;
                 end if;
                 v.fpscr(FPSCR_FL downto FPSCR_FU) := v.cr_result;
@@ -934,7 +1063,7 @@ begin
                 v.state := IDLE;
 
             when DO_FMR =>
-                opsel_a <= AIN_B;
+                -- r.opsel_a = AIN_B
                 v.result_class := r.b.class;
                 v.result_exp := r.b.exponent;
                 v.quieten_nan := '0';
@@ -954,7 +1083,7 @@ begin
                 v.state := IDLE;
 
             when DO_FRI =>    -- fri[nzpm]
-                opsel_a <= AIN_B;
+                -- r.opsel_a = AIN_B
                 v.result_class := r.b.class;
                 v.result_sign := r.b.negative;
                 v.result_exp := r.b.exponent;
@@ -979,7 +1108,7 @@ begin
                 end if;
 
             when DO_FRSP =>
-                opsel_a <= AIN_B;
+                -- r.opsel_a = AIN_B, r.shift = 0
                 v.result_class := r.b.class;
                 v.result_sign := r.b.negative;
                 v.result_exp := r.b.exponent;
@@ -998,7 +1127,6 @@ begin
                     elsif r.b.exponent > to_signed(127, EXP_BITS) then
                         v.state := ROUND_OFLOW;
                     else
-                        v.shift := to_signed(-2, EXP_BITS);
                         v.state := ROUNDING;
                     end if;
                 else
@@ -1009,7 +1137,7 @@ begin
                 -- instr bit 9: 1=dword 0=word
                 -- instr bit 8: 1=unsigned 0=signed
                 -- instr bit 1: 1=round to zero 0=use fpscr[RN]
-                opsel_a <= AIN_B;
+                -- r.opsel_a = AIN_B
                 v.result_class := r.b.class;
                 v.result_sign := r.b.negative;
                 v.result_exp := r.b.exponent;
@@ -1047,8 +1175,8 @@ begin
                 end case;
 
             when DO_FCFID =>
+                -- r.opsel_a = AIN_B
                 v.result_sign := '0';
-                opsel_a <= AIN_B;
                 if r.insn(8) = '0' and r.b.negative = '1' then
                     -- fcfid[s] with negative operand, set R = -B
                     opsel_ainv <= '1';
@@ -1067,96 +1195,78 @@ begin
 
             when DO_FADD =>
                 -- fadd[s] and fsub[s]
-                opsel_a <= AIN_A;
+                -- r.opsel_a = AIN_A
                 v.result_sign := r.a.negative;
                 v.result_class := r.a.class;
                 v.result_exp := r.a.exponent;
                 v.fpscr(FPSCR_FR) := '0';
                 v.fpscr(FPSCR_FI) := '0';
+                v.use_a := '1';
+                v.use_b := '1';
                 is_add := r.a.negative xor r.b.negative xor r.insn(1);
                 if r.a.class = FINITE and r.b.class = FINITE then
                     v.is_subtract := not is_add;
                     v.add_bsmall := r.exp_cmp;
+                    v.opsel_a := AIN_B;
                     if r.exp_cmp = '0' then
                         v.shift := r.a.exponent - r.b.exponent;
                         v.result_sign := r.b.negative xnor r.insn(1);
                         if r.a.exponent = r.b.exponent then
                             v.state := ADD_2;
                         else
+                            v.longmask := '0';
                             v.state := ADD_SHIFT;
                         end if;
                     else
-                        opsel_a <= AIN_B;
-                        v.shift := r.b.exponent - r.a.exponent;
-                        v.result_exp := r.b.exponent;
-                        v.state := ADD_SHIFT;
+                        v.state := ADD_1;
                     end if;
                 else
-                    if (r.a.class = NAN and r.a.mantissa(53) = '0') or
-                        (r.b.class = NAN and r.b.mantissa(53) = '0') then
-                        -- Signalling NAN
-                        v.fpscr(FPSCR_VXSNAN) := '1';
-                        invalid := '1';
-                    end if;
-                    if r.a.class = NAN then
-                        -- nothing to do, result is A
-                    elsif r.b.class = NAN then
-                        v.result_class := NAN;
-                        v.result_sign := r.b.negative;
-                        opsel_a <= AIN_B;
+                    if r.a.class = NAN or r.b.class = NAN then
+                        v.state := NAN_RESULT;
                     elsif r.a.class = INFINITY and r.b.class = INFINITY and is_add = '0' then
                         -- invalid operation, construct QNaN
                         v.fpscr(FPSCR_VXISI) := '1';
                         qnan_result := '1';
+                        arith_done := '1';
                     elsif r.a.class = ZERO and r.b.class = ZERO and is_add = '0' then
                         -- return -0 for rounding to -infinity
                         v.result_sign := r.round_mode(1) and r.round_mode(0);
+                        arith_done := '1';
                     elsif r.a.class = INFINITY or r.b.class = ZERO then
-                        -- nothing to do, result is A
+                        -- result is A
+                        v.opsel_a := AIN_A;
+                        v.state := EXC_RESULT;
                     else
                         -- result is +/- B
-                        v.result_sign := r.b.negative xnor r.insn(1);
-                        v.result_class := r.b.class;
-                        v.result_exp := r.b.exponent;
-                        opsel_a <= AIN_B;
+                        v.opsel_a := AIN_B;
+                        v.negate := not r.insn(1);
+                        v.state := EXC_RESULT;
                     end if;
-                    arith_done := '1';
                 end if;
 
             when DO_FMUL =>
                 -- fmul[s]
-                opsel_a <= AIN_A;
-                v.result_sign := r.a.negative;
+                -- r.opsel_a = AIN_A unless C is denorm and A isn't
+                v.result_sign := r.a.negative xor r.c.negative;
                 v.result_class := r.a.class;
-                v.result_exp := r.a.exponent;
                 v.fpscr(FPSCR_FR) := '0';
                 v.fpscr(FPSCR_FI) := '0';
+                v.use_a := '1';
+                v.use_c := '1';
                 if r.a.class = FINITE and r.c.class = FINITE then
-                    v.result_sign := r.a.negative xor r.c.negative;
                     v.result_exp := r.a.exponent + r.c.exponent;
                     -- Renormalize denorm operands
                     if r.a.mantissa(54) = '0' then
                         v.state := RENORM_A;
                     elsif r.c.mantissa(54) = '0' then
-                        opsel_a <= AIN_C;
                         v.state := RENORM_C;
                     else
                         f_to_multiply.valid <= '1';
                         v.state := MULT_1;
                     end if;
                 else
-                    if (r.a.class = NAN and r.a.mantissa(53) = '0') or
-                        (r.c.class = NAN and r.c.mantissa(53) = '0') then
-                        -- Signalling NAN
-                        v.fpscr(FPSCR_VXSNAN) := '1';
-                        invalid := '1';
-                    end if;
-                    if r.a.class = NAN then
-                    -- result is A
-                    elsif r.c.class = NAN then
-                        v.result_class := NAN;
-                        v.result_sign := r.c.negative;
-                        opsel_a <= AIN_C;
+                    if r.a.class = NAN or r.c.class = NAN then
+                        v.state := NAN_RESULT;
                     elsif (r.a.class = INFINITY and r.c.class = ZERO) or
                         (r.a.class = ZERO and r.c.class = INFINITY) then
                         -- invalid operation, construct QNaN
@@ -1164,22 +1274,22 @@ begin
                         qnan_result := '1';
                     elsif r.a.class = ZERO or r.a.class = INFINITY then
                         -- result is +/- A
-                        v.result_sign := r.a.negative xor r.c.negative;
+                        arith_done := '1';
                     else
                         -- r.c.class is ZERO or INFINITY
-                        v.result_class := r.c.class;
-                        v.result_sign := r.a.negative xor r.c.negative;
+                        v.opsel_a := AIN_C;
+                        v.negate := r.a.negative;
+                        v.state := EXC_RESULT;
                     end if;
-                    arith_done := '1';
                 end if;
 
             when DO_FDIV =>
-                opsel_a <= AIN_A;
-                v.result_sign := r.a.negative;
+                -- r.opsel_a = AIN_A unless B is denorm and A isn't
                 v.result_class := r.a.class;
-                v.result_exp := r.a.exponent;
                 v.fpscr(FPSCR_FR) := '0';
                 v.fpscr(FPSCR_FI) := '0';
+                v.use_a := '1';
+                v.use_b := '1';
                 v.result_sign := r.a.negative xor r.b.negative;
                 v.result_exp := r.a.exponent - r.b.exponent;
                 v.count := "00";
@@ -1188,26 +1298,14 @@ begin
                     if r.a.mantissa(54) = '0' then
                         v.state := RENORM_A;
                     elsif r.b.mantissa(54) = '0' then
-                        opsel_a <= AIN_B;
                         v.state := RENORM_B;
                     else
                         v.first := '1';
                         v.state := DIV_2;
                     end if;
                 else
-                    if (r.a.class = NAN and r.a.mantissa(53) = '0') or
-                        (r.b.class = NAN and r.b.mantissa(53) = '0') then
-                        -- Signalling NAN
-                        v.fpscr(FPSCR_VXSNAN) := '1';
-                        invalid := '1';
-                    end if;
-                    if r.a.class = NAN then
-                        -- result is A
-                        v.result_sign := r.a.negative;
-                    elsif r.b.class = NAN then
-                        v.result_class := NAN;
-                        v.result_sign := r.b.negative;
-                        opsel_a <= AIN_B;
+                    if r.a.class = NAN or r.b.class = NAN then
+                        v.state := NAN_RESULT;
                     elsif r.b.class = INFINITY then
                         if r.a.class = INFINITY then
                             v.fpscr(FPSCR_VXIDI) := '1';
@@ -1215,6 +1313,7 @@ begin
                         else
                             v.result_class := ZERO;
                         end if;
+                        arith_done := '1';
                     elsif r.b.class = ZERO then
                         if r.a.class = ZERO then
                             v.fpscr(FPSCR_VXZDZ) := '1';
@@ -1225,39 +1324,65 @@ begin
                             end if;
                             v.result_class := INFINITY;
                         end if;
-                    -- else r.b.class = FINITE, result_class = r.a.class
+                        arith_done := '1';
+                    else -- r.b.class = FINITE, result_class = r.a.class
+                        arith_done := '1';
                     end if;
-                    arith_done := '1';
                 end if;
 
             when DO_FSEL =>
-                opsel_a <= AIN_A;
                 v.fpscr(FPSCR_FR) := '0';
                 v.fpscr(FPSCR_FI) := '0';
                 if r.a.class = ZERO or (r.a.negative = '0' and r.a.class /= NAN) then
-                    v.result_sign := r.c.negative;
-                    v.result_exp := r.c.exponent;
-                    v.result_class := r.c.class;
-                    opsel_a <= AIN_C;
+                    v.opsel_a := AIN_C;
                 else
-                    v.result_sign := r.b.negative;
-                    v.result_exp := r.b.exponent;
-                    v.result_class := r.b.class;
-                    opsel_a <= AIN_B;
+                    v.opsel_a := AIN_B;
                 end if;
                 v.quieten_nan := '0';
-                arith_done := '1';
+                v.state := EXC_RESULT;
+
+            when DO_FSQRT =>
+                -- r.opsel_a = AIN_B
+                v.result_class := r.b.class;
+                v.result_sign := r.b.negative;
+                v.fpscr(FPSCR_FR) := '0';
+                v.fpscr(FPSCR_FI) := '0';
+                v.use_b := '1';
+                case r.b.class is
+                    when FINITE =>
+                        v.result_exp := r.b.exponent;
+                        if r.b.negative = '1' then
+                            v.fpscr(FPSCR_VXSQRT) := '1';
+                            qnan_result := '1';
+                        elsif r.b.mantissa(54) = '0' then
+                            v.state := RENORM_B;
+                        elsif r.b.exponent(0) = '0' then
+                            v.state := SQRT_1;
+                        else
+                            v.shift := to_signed(1, EXP_BITS);
+                            v.state := RENORM_B2;
+                        end if;
+                    when NAN =>
+                        v.state := NAN_RESULT;
+                    when ZERO =>
+                        -- result is B
+                        arith_done := '1';
+                    when INFINITY =>
+                        if r.b.negative = '1' then
+                            v.fpscr(FPSCR_VXSQRT) := '1';
+                            qnan_result := '1';
+                        -- else result is B
+                        end if;
+                        arith_done := '1';
+                end case;
 
             when DO_FRE =>
-                opsel_a <= AIN_B;
+                -- r.opsel_a = AIN_B
                 v.result_class := r.b.class;
                 v.result_sign := r.b.negative;
                 v.fpscr(FPSCR_FR) := '0';
                 v.fpscr(FPSCR_FI) := '0';
-                if r.b.class = NAN and r.b.mantissa(53) = '0' then
-                    v.fpscr(FPSCR_VXSNAN) := '1';
-                    invalid := '1';
-                end if;
+                v.use_b := '1';
                 case r.b.class is
                     when FINITE =>
                         v.result_exp := - r.b.exponent;
@@ -1267,8 +1392,7 @@ begin
                             v.state := FRE_1;
                         end if;
                     when NAN =>
-                        -- result is B
-                        arith_done := '1';
+                        v.state := NAN_RESULT;
                     when INFINITY =>
                         v.result_class := ZERO;
                         arith_done := '1';
@@ -1279,15 +1403,12 @@ begin
                 end case;
 
             when DO_FRSQRTE =>
-                opsel_a <= AIN_B;
+                -- r.opsel_a = AIN_B
                 v.result_class := r.b.class;
                 v.result_sign := r.b.negative;
                 v.fpscr(FPSCR_FR) := '0';
                 v.fpscr(FPSCR_FI) := '0';
-                if r.b.class = NAN and r.b.mantissa(53) = '0' then
-                    v.fpscr(FPSCR_VXSNAN) := '1';
-                    invalid := '1';
-                end if;
+                v.use_b := '1';
                 v.shift := to_signed(1, EXP_BITS);
                 case r.b.class is
                     when FINITE =>
@@ -1295,7 +1416,6 @@ begin
                         if r.b.negative = '1' then
                             v.fpscr(FPSCR_VXSQRT) := '1';
                             qnan_result := '1';
-                            arith_done := '1';
                         elsif r.b.mantissa(54) = '0' then
                             v.state := RENORM_B;
                         elsif r.b.exponent(0) = '0' then
@@ -1304,8 +1424,7 @@ begin
                             v.state := RENORM_B2;
                         end if;
                     when NAN =>
-                        -- result is B
-                        arith_done := '1';
+                        v.state := NAN_RESULT;
                     when INFINITY =>
                         if r.b.negative = '1' then
                             v.fpscr(FPSCR_VXSQRT) := '1';
@@ -1320,28 +1439,117 @@ begin
                         arith_done := '1';
                 end case;
 
+            when DO_FMADD =>
+                -- fmadd, fmsub, fnmadd, fnmsub
+                -- r.opsel_a = AIN_A if A is denorm, else AIN_C if C is denorm,
+                -- else AIN_B
+                v.result_sign := r.a.negative;
+                v.result_class := r.a.class;
+                v.result_exp := r.a.exponent;
+                v.fpscr(FPSCR_FR) := '0';
+                v.fpscr(FPSCR_FI) := '0';
+                v.use_a := '1';
+                v.use_b := '1';
+                v.use_c := '1';
+                is_add := r.a.negative xor r.c.negative xor r.b.negative xor r.insn(1);
+                if r.a.class = FINITE and r.c.class = FINITE and
+                    (r.b.class = FINITE or r.b.class = ZERO) then
+                    v.is_subtract := not is_add;
+                    mulexp := r.a.exponent + r.c.exponent;
+                    v.result_exp := mulexp;
+                    -- Make sure A and C are normalized
+                    if r.a.mantissa(54) = '0' then
+                        v.state := RENORM_A;
+                    elsif r.c.mantissa(54) = '0' then
+                        v.state := RENORM_C;
+                    elsif r.b.class = ZERO then
+                        -- no addend, degenerates to multiply
+                        v.result_sign := r.a.negative xor r.c.negative xor r.insn(2);
+                        f_to_multiply.valid <= '1';
+                        v.is_multiply := '1';
+                        v.state := MULT_1;
+                    elsif r.madd_cmp = '0' then
+                        -- addend is bigger, do multiply first
+                        v.result_sign := not (r.b.negative xor r.insn(1) xor r.insn(2));
+                        f_to_multiply.valid <= '1';
+                        v.state := FMADD_1;
+                    else
+                        -- product is bigger, shift B right and use it as the
+                        -- addend to the multiplier
+                        v.shift := r.b.exponent - mulexp + to_signed(64, EXP_BITS);
+                        -- for subtract, multiplier does B - A * C
+                        v.result_sign := not (r.a.negative xor r.c.negative xor r.insn(2) xor is_add);
+                        v.result_exp := r.b.exponent;
+                        v.state := FMADD_2;
+                    end if;
+                else
+                    if r.a.class = NAN or r.b.class = NAN or r.c.class = NAN then
+                        v.state := NAN_RESULT;
+                    elsif (r.a.class = ZERO and r.c.class = INFINITY) or
+                        (r.a.class = INFINITY and r.c.class = ZERO) then
+                        -- invalid operation, construct QNaN
+                        v.fpscr(FPSCR_VXIMZ) := '1';
+                        qnan_result := '1';
+                    elsif r.a.class = INFINITY or r.c.class = INFINITY then
+                        if r.b.class = INFINITY and is_add = '0' then
+                            -- invalid operation, construct QNaN
+                            v.fpscr(FPSCR_VXISI) := '1';
+                            qnan_result := '1';
+                        else
+                            -- result is infinity
+                            v.result_class := INFINITY;
+                            v.result_sign := r.a.negative xor r.c.negative xor r.insn(2);
+                            arith_done := '1';
+                        end if;
+                    else
+                        -- Here A is zero, C is zero, or B is infinity
+                        -- Result is +/-B in all of those cases
+                        v.opsel_a := AIN_B;
+                        if r.b.class /= ZERO or is_add = '1' then
+                            v.negate := not (r.insn(1) xor r.insn(2));
+                        else
+                            -- have to be careful about rule for 0 - 0 result sign
+                            v.negate := r.b.negative xor (r.round_mode(1) and r.round_mode(0)) xor r.insn(2);
+                        end if;
+                        v.state := EXC_RESULT;
+                    end if;
+                end if;
+
             when RENORM_A =>
                 renormalize := '1';
                 v.state := RENORM_A2;
+                if r.insn(4) = '1' then
+                    v.opsel_a := AIN_C;
+                else
+                    v.opsel_a := AIN_B;
+                end if;
 
             when RENORM_A2 =>
+                -- r.opsel_a = AIN_C for fmul/fmadd, AIN_B for fdiv
                 set_a := '1';
                 v.result_exp := new_exp;
                 if r.insn(4) = '1' then
-                    opsel_a <= AIN_C;
                     if r.c.mantissa(54) = '1' then
-                        v.first := '1';
-                        v.state := MULT_1;
+                        if r.insn(3) = '0' or r.b.class = ZERO then
+                            v.first := '1';
+                            v.state := MULT_1;
+                        else
+                            v.madd_cmp := '0';
+                            if new_exp + 1 >= r.b.exponent then
+                                v.madd_cmp := '1';
+                            end if;
+                            v.opsel_a := AIN_B;
+                            v.state := DO_FMADD;
+                        end if;
                     else
                         v.state := RENORM_C;
                     end if;
                 else
-                        opsel_a <= AIN_B;
-                        if r.b.mantissa(54) = '1' then
-                            v.first := '1';
-                            v.state := DIV_2;
-                        else
-                            v.state := RENORM_B;
+                    if r.b.mantissa(54) = '1' then
+                        v.first := '1';
+                        v.state := DIV_2;
+                    else
+                        v.state := RENORM_B;
                     end if;
                 end if;
 
@@ -1357,6 +1565,7 @@ begin
                 else
                     v.result_exp := new_exp;
                 end if;
+                v.opsel_a := AIN_B;
                 v.state := LOOKUP;
 
             when RENORM_C =>
@@ -1366,21 +1575,40 @@ begin
             when RENORM_C2 =>
                 set_c := '1';
                 v.result_exp := new_exp;
-                v.first := '1';
-                v.state := MULT_1;
+                if r.insn(3) = '0' or r.b.class = ZERO then
+                    v.first := '1';
+                    v.state := MULT_1;
+                else
+                    v.madd_cmp := '0';
+                    if new_exp + 1 >= r.b.exponent then
+                        v.madd_cmp := '1';
+                    end if;
+                    v.opsel_a := AIN_B;
+                    v.state := DO_FMADD;
+                end if;
+
+            when ADD_1 =>
+                -- transferring B to R
+                v.shift := r.b.exponent - r.a.exponent;
+                v.result_exp := r.b.exponent;
+                v.longmask := '0';
+                v.state := ADD_SHIFT;
 
             when ADD_SHIFT =>
+                -- r.shift = - exponent difference, r.longmask = 0
                 opsel_r <= RES_SHIFT;
+                v.x := s_nz;
                 set_x := '1';
-                longmask := '0';
-                v.state := ADD_2;
-
-            when ADD_2 =>
+                v.longmask := r.single_prec;
                 if r.add_bsmall = '1' then
-                    opsel_a <= AIN_A;
+                    v.opsel_a := AIN_A;
                 else
-                    opsel_a <= AIN_B;
+                    v.opsel_a := AIN_B;
                 end if;
+                v.state := ADD_2;
+
+            when ADD_2 =>
+                -- r.opsel_a = AIN_A if r.add_bsmall = 1 else AIN_B
                 opsel_b <= BIN_R;
                 opsel_binv <= r.is_subtract;
                 carry_in <= r.is_subtract and not r.x;
@@ -1389,6 +1617,7 @@ begin
 
             when ADD_3 =>
                 -- check for overflow or negative result (can't get both)
+                -- r.shift = -1
                 if r.r(63) = '1' then
                     -- result is opposite sign to expected
                     v.result_sign := not r.result_sign;
@@ -1399,7 +1628,6 @@ begin
                     -- sum overflowed, shift right
                     opsel_r <= RES_SHIFT;
                     set_x := '1';
-                    v.shift := to_signed(-2, EXP_BITS);
                     if exp_huge = '1' then
                         v.state := ROUND_OFLOW;
                     else
@@ -1407,7 +1635,6 @@ begin
                     end if;
                 elsif r.r(54) = '1' then
                     set_x := '1';
-                    v.shift := to_signed(-2, EXP_BITS);
                     v.state := ROUNDING;
                 elsif (r_hi_nz or r_lo_nz or r.r(1) or r.r(0)) = '0' then
                     -- r.x must be zero at this point
@@ -1423,7 +1650,7 @@ begin
                 end if;
 
             when CMP_1 =>
-                opsel_a <= AIN_A;
+                -- r.opsel_a = AIN_A
                 opsel_b <= BIN_R;
                 opsel_binv <= '1';
                 carry_in <= '1';
@@ -1449,12 +1676,89 @@ begin
                     v.state := FINISH;
                 end if;
 
+            when FMADD_1 =>
+                -- Addend is bigger here
+                v.result_sign := not (r.b.negative xor r.insn(1) xor r.insn(2));
+                -- note v.shift is at most -2 here
+                v.shift := r.result_exp - r.b.exponent;
+                opsel_r <= RES_MULT;
+                opsel_s <= S_MULT;
+                set_s := '1';
+                f_to_multiply.valid <= r.first;
+                if multiply_to_f.valid = '1' then
+                    v.longmask := '0';
+                    v.state := ADD_SHIFT;
+                end if;
+
+            when FMADD_2 =>
+                -- Product is potentially bigger here
+                -- r.shift = addend exp - product exp + 64, r.r = r.b.mantissa
+                set_s := '1';
+                opsel_s <= S_SHIFT;
+                v.shift := r.shift - to_signed(64, EXP_BITS);
+                v.state := FMADD_3;
+
+            when FMADD_3 =>
+                -- r.shift = addend exp - product exp
+                opsel_r <= RES_SHIFT;
+                v.first := '1';
+                v.state := FMADD_4;
+
+            when FMADD_4 =>
+                msel_add <= MULADD_RS;
+                f_to_multiply.valid <= r.first;
+                msel_inv <= r.is_subtract;
+                opsel_r <= RES_MULT;
+                opsel_s <= S_MULT;
+                set_s := '1';
+                if multiply_to_f.valid = '1' then
+                    v.state := FMADD_5;
+                end if;
+
+            when FMADD_5 =>
+                -- negate R:S:X if negative
+                if r.r(63) = '1' then
+                    v.result_sign := not r.result_sign;
+                    opsel_ainv <= '1';
+                    carry_in <= not (s_nz or r.x);
+                    opsel_s <= S_NEG;
+                    set_s := '1';
+                end if;
+                v.shift := to_signed(56, EXP_BITS);
+                v.state := FMADD_6;
+
+            when FMADD_6 =>
+                -- r.shift = 56 (or 0, but only if r is now nonzero)
+                if (r.r(56) or r_hi_nz or r_lo_nz or r.r(1) or r.r(0)) = '0' then
+                    if s_nz = '0' then
+                        -- must be a subtraction, and r.x must be zero
+                        v.result_class := ZERO;
+                        v.result_sign := r.round_mode(1) and r.round_mode(0);
+                        arith_done := '1';
+                    else
+                        -- R is all zeroes but there are non-zero bits in S
+                        -- so shift them into R and set S to 0
+                        opsel_r <= RES_SHIFT;
+                        set_s := '1';
+                        -- stay in state FMADD_6
+                    end if;
+                elsif r.r(56 downto 54) = "001" then
+                    v.state := FINISH;
+                else
+                    renormalize := '1';
+                    v.state := NORMALIZE;
+                end if;
+
             when LOOKUP =>
-                opsel_a <= AIN_B;
+                -- r.opsel_a = AIN_B
                 -- wait one cycle for inverse_table[B] lookup
                 v.first := '1';
                 if r.insn(4) = '0' then
-                    v.state := DIV_2;
+                    if r.insn(3) = '0' then
+                        v.state := DIV_2;
+                    else
+                        v.state := SQRT_1;
+                    end if;
                 elsif r.insn(2) = '0' then
                     v.state := FRE_1;
                 else
@@ -1537,6 +1841,16 @@ begin
                 v.shift := to_signed(1, EXP_BITS);
                 v.state := NORMALIZE;
 
+            when FTDIV_1 =>
+                v.cr_result(1) := exp_tiny or exp_huge;
+                if exp_tiny = '1' or exp_huge = '1' or r.a.class = ZERO or r.first = '0' then
+                    v.instr_done := '1';
+                    v.state := IDLE;
+                else
+                    v.shift := r.a.exponent;
+                    v.doing_ftdiv := "10";
+                end if;
+
             when RSQRT_1 =>
                 opsel_r <= RES_MISC;
                 misc_sel <= "0111";
@@ -1545,13 +1859,166 @@ begin
                 v.shift := to_signed(1, EXP_BITS);
                 v.state := NORMALIZE;
 
+            when SQRT_1 =>
+                -- put invsqr[B] in R and compute P = invsqr[B] * B
+                -- also transfer B (in R) to A
+                set_a := '1';
+                opsel_r <= RES_MISC;
+                misc_sel <= "0111";
+                msel_1 <= MUL1_B;
+                msel_2 <= MUL2_LUT;
+                f_to_multiply.valid <= '1';
+                v.shift := to_signed(-1, EXP_BITS);
+                v.count := "00";
+                v.state := SQRT_2;
+
+            when SQRT_2 =>
+                -- shift R right one place
+                -- not expecting multiplier result yet
+                -- r.shift = -1
+                opsel_r <= RES_SHIFT;
+                v.first := '1';
+                v.state := SQRT_3;
+
+            when SQRT_3 =>
+                -- put R into Y, wait for product from multiplier
+                msel_2 <= MUL2_R;
+                set_y := r.first;
+                pshift := '1';
+                if multiply_to_f.valid = '1' then
+                    -- put result into R
+                    opsel_r <= RES_MULT;
+                    v.first := '1';
+                    v.state := SQRT_4;
+                end if;
+
+            when SQRT_4 =>
+                -- compute 1.5 - Y * P
+                msel_1 <= MUL1_Y;
+                msel_2 <= MUL2_P;
+                msel_add <= MULADD_CONST;
+                msel_inv <= '1';
+                f_to_multiply.valid <= r.first;
+                pshift := '1';
+                if multiply_to_f.valid = '1' then
+                    v.state := SQRT_5;
+                end if;
+
+            when SQRT_5 =>
+                -- compute Y = Y * P
+                msel_1 <= MUL1_Y;
+                msel_2 <= MUL2_P;
+                f_to_multiply.valid <= '1';
+                v.first := '1';
+                v.state := SQRT_6;
+
+            when SQRT_6 =>
+                -- pipeline in R = R * P
+                msel_1 <= MUL1_R;
+                msel_2 <= MUL2_P;
+                f_to_multiply.valid <= r.first;
+                pshift := '1';
+                if multiply_to_f.valid = '1' then
+                    v.first := '1';
+                    v.state := SQRT_7;
+                end if;
+
+            when SQRT_7 =>
+                -- first multiply is done, put result in Y
+                msel_2 <= MUL2_P;
+                set_y := r.first;
+                -- wait for second multiply (should be here already)
+                pshift := '1';
+                if multiply_to_f.valid = '1' then
+                    -- put result into R
+                    opsel_r <= RES_MULT;
+                    v.first := '1';
+                    v.count := r.count + 1;
+                    if r.count < 2 then
+                        v.state := SQRT_4;
+                    else
+                        v.first := '1';
+                        v.state := SQRT_8;
+                    end if;
+                end if;
+
+            when SQRT_8 =>
+                -- compute P = A - R * R, which can be +ve or -ve
+                -- we arranged for B to be put into A earlier
+                msel_1 <= MUL1_R;
+                msel_2 <= MUL2_R;
+                msel_add <= MULADD_A;
+                msel_inv <= '1';
+                pshift := '1';
+                f_to_multiply.valid <= r.first;
+                if multiply_to_f.valid = '1' then
+                    v.first := '1';
+                    v.state := SQRT_9;
+                end if;
+
+            when SQRT_9 =>
+                -- compute P = P * Y
+                -- since Y is an estimate of 1/sqrt(B), this makes P an
+                -- estimate of the adjustment needed to R.  Since the error
+                -- could be negative and we have an unsigned multiplier, the
+                -- upper bits can be wrong, but it turns out the lowest 8 bits
+                -- are correct and are all we need (given 3 iterations through
+                -- SQRT_4 to SQRT_7).
+                msel_1 <= MUL1_Y;
+                msel_2 <= MUL2_P;
+                pshift := '1';
+                f_to_multiply.valid <= r.first;
+                if multiply_to_f.valid = '1' then
+                    v.state := SQRT_10;
+                end if;
+
+            when SQRT_10 =>
+                -- Add the bottom 8 bits of P, sign-extended,
+                -- divided by 4, onto R.
+                -- The division by 4 is because R is 10.54 format
+                -- whereas P is 8.56 format.
+                opsel_b <= BIN_PS6;
+                sqrt_exp := r.b.exponent(EXP_BITS-1) & r.b.exponent(EXP_BITS-1 downto 1);
+                v.result_exp := sqrt_exp;
+                v.shift := to_signed(1, EXP_BITS);
+                v.first := '1';
+                v.state := SQRT_11;
+
+            when SQRT_11 =>
+                -- compute P = A - R * R (remainder)
+                -- also put 2 * R + 1 into B for comparison with P
+                msel_1 <= MUL1_R;
+                msel_2 <= MUL2_R;
+                msel_add <= MULADD_A;
+                msel_inv <= '1';
+                f_to_multiply.valid <= r.first;
+                shiftin := '1';
+                set_b := r.first;
+                if multiply_to_f.valid = '1' then
+                    v.state := SQRT_12;
+                end if;
+
+            when SQRT_12 =>
+                -- test if remainder is 0 or >= B = 2*R + 1
+                if pcmpb_lt = '1' then
+                    -- square root is correct, set X if remainder non-zero
+                    v.x := r.p(58) or px_nz;
+                else
+                    -- square root needs to be incremented by 1
+                    carry_in <= '1';
+                    v.x := not pcmpb_eq;
+                end if;
+                v.state := FINISH;
+
             when INT_SHIFT =>
+                -- r.shift = b.exponent - 52
                 opsel_r <= RES_SHIFT;
                 set_x := '1';
                 v.state := INT_ROUND;
                 v.shift := to_signed(-2, EXP_BITS);
 
             when INT_ROUND =>
+                -- r.shift = -2
                 opsel_r <= RES_SHIFT;
                 round := fp_rounding(r.r, r.x, '0', r.round_mode, r.result_sign);
                 v.fpscr(FPSCR_FR downto FPSCR_FI) := round;
@@ -1564,6 +2031,7 @@ begin
                 end if;
 
             when INT_ISHIFT =>
+                -- r.shift = b.exponent - 54;
                 opsel_r <= RES_SHIFT;
                 v.state := INT_FINAL;
 
@@ -1621,9 +2089,9 @@ begin
                 arith_done := '1';
 
             when FRI_1 =>
+                -- r.shift = b.exponent - 52
                 opsel_r <= RES_SHIFT;
                 set_x := '1';
-                v.shift := to_signed(-2, EXP_BITS);
                 v.state := ROUNDING;
 
             when FINISH =>
@@ -1641,13 +2109,13 @@ begin
                     elsif exp_huge = '1' then
                         v.state := ROUND_OFLOW;
                     else
-                        v.shift := to_signed(-2, EXP_BITS);
                         v.state := ROUNDING;
                     end if;
                 end if;
 
             when NORMALIZE =>
                 -- Shift so we have 9 leading zeroes (we know R is non-zero)
+                -- r.shift = clz(r.r) - 9
                 opsel_r <= RES_SHIFT;
                 set_x := '1';
                 if exp_tiny = '1' then
@@ -1656,18 +2124,17 @@ begin
                 elsif exp_huge = '1' then
                     v.state := ROUND_OFLOW;
                 else
-                    v.shift := to_signed(-2, EXP_BITS);
                     v.state := ROUNDING;
                 end if;
 
             when ROUND_UFLOW =>
+                -- r.shift = - amount by which exponent underflows
                 v.tiny := '1';
                 if r.fpscr(FPSCR_UE) = '0' then
                     -- disabled underflow exception case
                     -- have to denormalize before rounding
                     opsel_r <= RES_SHIFT;
                     set_x := '1';
-                    v.shift := to_signed(-2, EXP_BITS);
                     v.state := ROUNDING;
                 else
                     -- enabled underflow exception case
@@ -1678,7 +2145,6 @@ begin
                         renormalize := '1';
                         v.state := NORMALIZE;
                     else
-                        v.shift := to_signed(-2, EXP_BITS);
                         v.state := ROUNDING;
                     end if;
                 end if;
@@ -1705,18 +2171,16 @@ begin
                 else
                     -- enabled overflow exception
                     v.result_exp := r.result_exp - bias_exp;
-                    v.shift := to_signed(-2, EXP_BITS);
                     v.state := ROUNDING;
                 end if;
 
             when ROUNDING =>
-                opsel_amask <= '1';
+                opsel_mask <= '1';
                 round := fp_rounding(r.r, r.x, r.single_prec, r.round_mode, r.result_sign);
                 v.fpscr(FPSCR_FR downto FPSCR_FI) := round;
                 if round(1) = '1' then
-                    -- set mask to increment the LSB for the precision
-                    opsel_b <= BIN_MASK;
-                    carry_in <= '1';
+                    -- increment the LSB for the precision
+                    opsel_b <= BIN_RND;
                     v.shift := to_signed(-1, EXP_BITS);
                     v.state := ROUNDING_2;
                 else
@@ -1738,6 +2202,7 @@ begin
 
             when ROUNDING_2 =>
                 -- Check for overflow during rounding
+                -- r.shift = -1
                 v.x := '0';
                 if r.r(55) = '1' then
                     opsel_r <= RES_SHIFT;
@@ -1755,6 +2220,7 @@ begin
                 end if;
 
             when ROUNDING_3 =>
+                -- r.shift = clz(r.r) - 9
                 mant_nz := r_hi_nz or (r_lo_nz and not r.single_prec);
                 if mant_nz = '0' then
                     v.result_class := ZERO;
@@ -1776,9 +2242,45 @@ begin
                 end if;
 
             when DENORM =>
+                -- r.shift = result_exp - -1022
                 opsel_r <= RES_SHIFT;
                 arith_done := '1';
 
+            when NAN_RESULT =>
+                if (r.use_a = '1' and r.a.class = NAN and r.a.mantissa(53) = '0') or
+                    (r.use_b = '1' and r.b.class = NAN and r.b.mantissa(53) = '0') or
+                    (r.use_c = '1' and r.c.class = NAN and r.c.mantissa(53) = '0') then
+                    -- Signalling NAN
+                    v.fpscr(FPSCR_VXSNAN) := '1';
+                    invalid := '1';
+                end if;
+                if r.use_a = '1' and r.a.class = NAN then
+                    v.opsel_a := AIN_A;
+                elsif r.use_b = '1' and r.b.class = NAN then
+                    v.opsel_a := AIN_B;
+                elsif r.use_c = '1' and r.c.class = NAN then
+                    v.opsel_a := AIN_C;
+                end if;
+                v.state := EXC_RESULT;
+
+            when EXC_RESULT =>
+                -- r.opsel_a = AIN_A, AIN_B or AIN_C according to which input is the result
+                case r.opsel_a is
+                    when AIN_B =>
+                        v.result_sign := r.b.negative xor r.negate;
+                        v.result_exp := r.b.exponent;
+                        v.result_class := r.b.class;
+                    when AIN_C =>
+                        v.result_sign := r.c.negative xor r.negate;
+                        v.result_exp := r.c.exponent;
+                        v.result_class := r.c.class;
+                    when others =>
+                        v.result_sign := r.a.negative xor r.negate;
+                        v.result_exp := r.a.exponent;
+                        v.result_class := r.a.class;
+                end case;
+                arith_done := '1';
+
         end case;
 
         if zero_divide = '1' then
@@ -1790,11 +2292,15 @@ begin
             v.result_sign := '0';
             misc_sel <= "0001";
             opsel_r <= RES_MISC;
+            arith_done := '1';
+        end if;
+        if invalid = '1' then
+            v.invalid := '1';
         end if;
         if arith_done = '1' then
             -- Enabled invalid exception doesn't write result or FPRF
             -- Neither does enabled zero-divide exception
-            if (invalid and r.fpscr(FPSCR_VE)) = '0' and
+            if (v.invalid and r.fpscr(FPSCR_VE)) = '0' and
                 (zero_divide and r.fpscr(FPSCR_ZE)) = '0' then
                 v.writing_back := '1';
                 v.update_fprf := '1';
@@ -1828,11 +2334,18 @@ begin
         maddend := (others => '0');
         case msel_add is
             when MULADD_CONST =>
-                -- addend is 2.0 in 16.112 format
-                maddend(113) := '1';                -- 2.0
+                -- addend is 2.0 or 1.5 in 16.112 format
+                if r.is_sqrt = '0' then
+                    maddend(113) := '1';                -- 2.0
+                else
+                    maddend(112 downto 111) := "11";    -- 1.5
+                end if;
             when MULADD_A =>
                 -- addend is A in 16.112 format
                 maddend(121 downto 58) := r.a.mantissa;
+            when MULADD_RS =>
+                -- addend is concatenation of R and S in 16.112 format
+                maddend := "000000" & r.r & r.s & "00";
             when others =>
         end case;
         if msel_inv = '1' then
@@ -1855,7 +2368,7 @@ begin
         -- Data path.
         -- This has A and B input multiplexers, an adder, a shifter,
         -- count-leading-zeroes logic, and a result mux.
-        if longmask = '1' then
+        if r.longmask = '1' then
             mshift := r.shift + to_signed(-29, EXP_BITS);
         else
             mshift := r.shift;
@@ -1867,7 +2380,7 @@ begin
         else
             mask := right_mask(unsigned(mshift(5 downto 0)));
         end if;
-        case opsel_a is
+        case r.opsel_a is
             when AIN_R =>
                 in_a0 := r.r;
             when AIN_A =>
@@ -1883,33 +2396,39 @@ begin
         if opsel_ainv = '1' then
             in_a0 := not in_a0;
         end if;
-        if opsel_amask = '1' then
-            in_a0 := in_a0 and not mask;
-        end if;
         in_a <= in_a0;
         case opsel_b is
             when BIN_ZERO =>
                 in_b0 := (others => '0');
             when BIN_R =>
                 in_b0 := r.r;
-            when BIN_MASK =>
-                in_b0 := mask;
+            when BIN_RND =>
+                round_inc := (31 => r.single_prec, 2 => not r.single_prec, others => '0');
+                in_b0 := round_inc;
             when others =>
-                in_b0 := (others => '0');
+                -- BIN_PS6, 6 LSBs of P/4 sign-extended to 64
+                in_b0 := std_ulogic_vector(resize(signed(r.p(7 downto 2)), 64));
         end case;
         if opsel_binv = '1' then
             in_b0 := not in_b0;
         end if;
         in_b <= in_b0;
         if r.shift >= to_signed(-64, EXP_BITS) and r.shift <= to_signed(63, EXP_BITS) then
-            shift_res := shifter_64(r.r & x"00000000000000",
+            shift_res := shifter_64(r.r & (shiftin or r.s(55)) & r.s(54 downto 0),
                                     std_ulogic_vector(r.shift(6 downto 0)));
         else
             shift_res := (others => '0');
         end if;
+        sum := std_ulogic_vector(unsigned(in_a) + unsigned(in_b) + carry_in);
+        if opsel_mask = '1' then
+            sum(1 downto 0) := "00";
+            if r.single_prec = '1' then
+                sum(30 downto 2) := (others => '0');
+            end if;
+        end if;
         case opsel_r is
             when RES_SUM =>
-                result <= std_ulogic_vector(unsigned(in_a) + unsigned(in_b) + carry_in);
+                result <= sum;
             when RES_SHIFT =>
                 result <= shift_res;
             when RES_MULT =>
@@ -1965,6 +2484,21 @@ begin
                 result <= misc;
         end case;
         v.r := result;
+        if set_s = '1' then
+            case opsel_s is
+                when S_NEG =>
+                    v.s := std_ulogic_vector(unsigned(not r.s) + (not r.x));
+                when S_MULT =>
+                    v.s := multiply_to_f.result(57 downto 2);
+                when S_SHIFT =>
+                    v.s := shift_res(63 downto 8);
+                    if shift_res(7 downto 0) /= x"00" then
+                        v.x := '1';
+                    end if;
+                when others =>
+                    v.s := (others => '0');
+            end case;
+        end if;
 
         if set_a = '1' then
             v.a.exponent := new_exp;
@@ -2015,9 +2549,10 @@ begin
             v.cr_result := v.fpscr(FPSCR_FX downto FPSCR_OX);
         end if;
 
+        v.illegal := illegal;
         if illegal = '1' then
             v.instr_done := '0';
-            v.do_intr := '0';
+            v.do_intr := '1';
             v.writing_back := '0';
             v.busy := '0';
             v.state := IDLE;
@@ -2029,7 +2564,6 @@ begin
         end if;
 
         rin <= v;
-        e_out.illegal <= illegal;
     end process;
 
 end architecture behaviour;