PowerPC fixes (Jocelyn Mayer)

git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@483 c046a42c-6fe2-441c-8c8c-71466251a162

PowerPC fixes (Jocelyn Mayer)
git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@483 c046a42c-6fe2-441c-8c8c-71466251a162
bellard
1 parent 07ad1b93
Showing 6 changed files with 382 additions and 180 deletions
target-ppc/cpu.h
target-ppc/exec.h
target-ppc/helper.c
target-ppc/op.c
target-ppc/op_template.h
target-ppc/translate.c
@@ -152,7 +152,7 @@ typedef struct CPUPPCState {
     /* general purpose registers */
     uint32_t gpr[32];
     /* floating point registers */
-    uint64_t fpr[32];
+    double fpr[32];
     /* segment registers */
     ppc_sr_t sr[16];
     /* special purpose registers */
@@ -172,7 +172,10 @@ typedef struct CPUPPCState {
     uint32_t exception;
     /* qemu dedicated */
-    uint64_t ft0; /* temporary float register */
+     /* temporary float registers */
+    double ft0;
+    double ft1;
+    double ft2;
     int interrupt_request;
     jmp_buf jmp_env;
     int exception_index;
@@ -374,35 +377,4 @@ enum {
     EXCP_BRANCH        = 0x104,   /* branch instruction               */
 };
-/*
- * We need to put in some extra aux table entries to tell glibc what
- * the cache block size is, so it can use the dcbz instruction safely.
- */
-#define AT_DCACHEBSIZE          19
-#define AT_ICACHEBSIZE          20
-#define AT_UCACHEBSIZE          21
-/* A special ignored type value for PPC, for glibc compatibility.  */
-#define AT_IGNOREPPC            22
-/*
- * The requirements here are:
- * - keep the final alignment of sp (sp & 0xf)
- * - make sure the 32-bit value at the first 16 byte aligned position of
- *   AUXV is greater than 16 for glibc compatibility.
- *   AT_IGNOREPPC is used for that.
- * - for compatibility with glibc ARCH_DLINFO must always be defined on PPC,
- *   even if DLINFO_ARCH_ITEMS goes to zero or is undefined.
- */
-#define DLINFO_ARCH_ITEMS       3
-#define ARCH_DLINFO                                                     \
-do {                                                                    \
-        /*                                                              \
-         * Now handle glibc compatibility.                              \
-         */                                                             \
-        NEW_AUX_ENT(AT_IGNOREPPC, AT_IGNOREPPC);                        \
-        NEW_AUX_ENT(AT_IGNOREPPC, AT_IGNOREPPC);                        \
-                                                                        \
-        NEW_AUX_ENT(AT_DCACHEBSIZE, 0x20);                              \
-        NEW_AUX_ENT(AT_ICACHEBSIZE, 0x20);                              \
-        NEW_AUX_ENT(AT_UCACHEBSIZE, 0);                                 \
- } while (0)
 #endif /* !defined (__CPU_PPC_H__) */
@@ -29,6 +29,12 @@ register uint32_t T2 asm(AREG3);
 #define PARAM(n) ((uint32_t)PARAM##n)
 #define SPARAM(n) ((int32_t)PARAM##n)
+#define FT0 (env->ft0)
+#define FT1 (env->ft1)
+#define FT2 (env->ft2)
+#define FTS0 ((float)env->ft0)
+#define FTS1 ((float)env->ft1)
+#define FTS2 ((float)env->ft2)
 #define RETURN() __asm__ __volatile__("");
@@ -145,8 +151,8 @@ uint32_t do_load_xer (void);
 void do_store_xer (uint32_t value);
 uint32_t do_load_msr (void);
 void do_store_msr (uint32_t msr_value);
-uint32_t do_load_fpscr (void);
-void do_store_fpscr (uint8_t mask, uint32_t fp);
+void do_load_fpscr (void);
+void do_store_fpscr (uint32_t mask);
 int32_t do_sraw(int32_t Ta, uint32_t Tb);
 void do_lmw (int reg, uint32_t src);
@@ -154,4 +160,7 @@ void do_stmw (int reg, uint32_t dest);
 void do_lsw (uint32_t reg, int count, uint32_t src);
 void do_stsw (uint32_t reg, int count, uint32_t dest);
+void do_dcbz (void);
+void do_icbi (void);
+
 #endif /* !defined (__PPC_H__) */
@@ -121,67 +121,67 @@ void do_store_msr (uint32_t msr_value)
 }
 /* The 32 MSB of the target fpr are undefined. They'll be zero... */
-uint32_t do_load_fpscr (void)
+/* Floating point operations helpers */
+void do_load_fpscr (void)
 {
-    return (fpscr_fx  << FPSCR_FX) |
-        (fpscr_fex    << FPSCR_FEX) |
-        (fpscr_vx     << FPSCR_VX) |
-        (fpscr_ox     << FPSCR_OX) |
-        (fpscr_ux     << FPSCR_UX) |
-        (fpscr_zx     << FPSCR_ZX) |
-        (fpscr_xx     << FPSCR_XX) |
-        (fpscr_vsxnan << FPSCR_VXSNAN) |
-        (fpscr_vxisi  << FPSCR_VXISI) |
-        (fpscr_vxidi  << FPSCR_VXIDI) |
-        (fpscr_vxzdz  << FPSCR_VXZDZ) |
-        (fpscr_vximz  << FPSCR_VXIMZ) |
-        (fpscr_fr     << FPSCR_FR) |
-        (fpscr_fi     << FPSCR_FI) |
-        (fpscr_fprf   << FPSCR_FPRF) |
-        (fpscr_vxsoft << FPSCR_VXSOFT) |
-        (fpscr_vxsqrt << FPSCR_VXSQRT) |
-        (fpscr_oe     << FPSCR_OE) |
-        (fpscr_ue     << FPSCR_UE) |
-        (fpscr_ze     << FPSCR_ZE) |
-        (fpscr_xe     << FPSCR_XE) |
-        (fpscr_ni     << FPSCR_NI) |
-        (fpscr_rn     << FPSCR_RN);
+    /* The 32 MSB of the target fpr are undefined.
+     * They'll be zero...
+     */
+    union {
+        double d;
+        struct {
+            uint32_t u[2];
+        } s;
+    } u;
+    int i;
+
+    u.s.u[0] = 0;
+    u.s.u[1] = 0;
+    for (i = 0; i < 8; i++)
+        u.s.u[1] |= env->fpscr[i] << (4 * i);
+    FT0 = u.d;
 }
-/* We keep only 32 bits of input... */
-/* For now, this is COMPLETELY BUGGY ! */
-void do_store_fpscr (uint8_t mask, uint32_t fp)
+void do_store_fpscr (uint32_t mask)
 {
+    /*
+     * We use only the 32 LSB of the incoming fpr
+     */
+    union {
+        double d;
+        struct {
+            uint32_t u[2];
+        } s;
+    } u;
     int i;
-    for (i = 0; i < 7; i++) {
-        if ((mask & (1 << i)) == 0)
-            fp &= ~(0xf << (4 * i));
+    u.d = FT0;
+    if (mask & 0x80)
+        env->fpscr[0] = (env->fpscr[0] & 0x9) | ((u.s.u[1] >> 28) & ~0x9);
+    for (i = 1; i < 7; i++) {
+        if (mask & (1 << (7 - i)))
+            env->fpscr[i] = (u.s.u[1] >> (4 * (7 - i))) & 0xF;
+    }
+    /* TODO: update FEX & VX */
+    /* Set rounding mode */
+    switch (env->fpscr[0] & 0x3) {
+    case 0:
+        /* Best approximation (round to nearest) */
+        fesetround(FE_TONEAREST);
+        break;
+    case 1:
+        /* Smaller magnitude (round toward zero) */
+        fesetround(FE_TOWARDZERO);
+        break;
+    case 2:
+        /* Round toward +infinite */
+        fesetround(FE_UPWARD);
+        break;
+    case 3:
+        /* Round toward -infinite */
+        fesetround(FE_DOWNWARD);
+        break;
     }
-    if ((mask & 80) != 0)
-        fpscr_fx = (fp >> FPSCR_FX) & 0x01;
-    fpscr_fex = (fp >> FPSCR_FEX) & 0x01;
-    fpscr_vx = (fp >> FPSCR_VX) & 0x01;
-    fpscr_ox = (fp >> FPSCR_OX) & 0x01;
-    fpscr_ux = (fp >> FPSCR_UX) & 0x01;
-    fpscr_zx = (fp >> FPSCR_ZX) & 0x01;
-    fpscr_xx = (fp >> FPSCR_XX) & 0x01;
-    fpscr_vsxnan = (fp >> FPSCR_VXSNAN) & 0x01;
-    fpscr_vxisi = (fp >> FPSCR_VXISI) & 0x01;
-    fpscr_vxidi = (fp >> FPSCR_VXIDI) & 0x01;
-    fpscr_vxzdz = (fp >> FPSCR_VXZDZ) & 0x01;
-    fpscr_vximz = (fp >> FPSCR_VXIMZ) & 0x01;
-    fpscr_fr = (fp >> FPSCR_FR) & 0x01;
-    fpscr_fi = (fp >> FPSCR_FI) & 0x01;
-    fpscr_fprf = (fp >> FPSCR_FPRF) & 0x1F;
-    fpscr_vxsoft = (fp >> FPSCR_VXSOFT) & 0x01;
-    fpscr_vxsqrt = (fp >> FPSCR_VXSQRT) & 0x01;
-    fpscr_oe = (fp >> FPSCR_OE) & 0x01;
-    fpscr_ue = (fp >> FPSCR_UE) & 0x01;
-    fpscr_ze = (fp >> FPSCR_ZE) & 0x01;
-    fpscr_xe = (fp >> FPSCR_XE) & 0x01;
-    fpscr_ni = (fp >> FPSCR_NI) & 0x01;
-    fpscr_rn = (fp >> FPSCR_RN) & 0x03;
 }
 int32_t do_sraw(int32_t value, uint32_t shift)
@@ -220,20 +220,14 @@ void do_lsw (uint32_t reg, int count, uint32_t src)
     int sh;
     for (; count > 3; count -= 4, src += 4) {
-        if (reg == 32)
-            reg = 0;
         ugpr(reg++) = ld32(src);
+        if (T2 == 32)
+            T2 = 0;
     }
     if (count > 0) {
-        for (sh = 24, tmp = 0; count > 0; count--, src++, sh -= 8) {
-            if (reg == 32)
-                reg = 0;
-            tmp |= ld8(src) << sh;
-            if (sh == 0) {
-                sh = 32;
-                ugpr(reg++) = tmp;
                 tmp = 0;
-            }
+        for (sh = 24; count > 0; count--, src++, sh -= 8) {
+            tmp |= ld8(src) << sh;
         }
         ugpr(reg) = tmp;
     }
@@ -244,19 +238,30 @@ void do_stsw (uint32_t reg, int count, uint32_t dest)
     int sh;
     for (; count > 3; count -= 4, dest += 4) {
+        st32(dest, ugpr(reg++));
         if (reg == 32)
             reg = 0;
-        st32(dest, ugpr(reg++));
     }
     if (count > 0) {
         for (sh = 24; count > 0; count--, dest++, sh -= 8) {
-            if (reg == 32)
-                reg = 0;
             st8(dest, (ugpr(reg) >> sh) & 0xFF);
-            if (sh == 0) {
-                sh = 32;
-                reg++;
             }
         }
+}
+
+void do_dcbz (void)
+{
+    int i;
+
+    /* Assume cache line size is 32 */
+    for (i = 0; i < 8; i++) {
+        st32(T0, 0);
+        T0 += 4;
     }
 }
+    
+/* Instruction cache invalidation helper */
+void do_icbi (void)
+{
+    tb_invalidate_page(T0);
+}
@@ -27,6 +27,12 @@
 #define Ts2 (int32_t)T2
 #define FT0 (env->ft0)
+#define FT1 (env->ft1)
+#define FT2 (env->ft2)
+
+#define FTS0 ((float)env->ft0)
+#define FTS1 ((float)env->ft1)
+#define FTS2 ((float)env->ft2)
 #define PPC_OP(name) void op_##name(void)
@@ -173,6 +179,13 @@ PPC_OP(set_Rc0_1)
     RETURN();
 }
+/* Set Rc1 (for floating point arithmetic) */
+PPC_OP(set_Rc1)
+{
+    env->crf[1] = regs->fpscr[7];
+    RETURN();
+}
+
 PPC_OP(set_T0)
 {
     T0 = PARAM(1);
@@ -278,6 +291,25 @@ PPC_OP(load_lr)
     RETURN();
 }
+/* FPSCR */
+PPC_OP(load_fpscr)
+{
+    do_load_fpscr();
+    RETURN();
+}
+
+PPC_OP(store_fpscr)
+{
+    do_store_fpscr(PARAM(1));
+    RETURN();
+}
+
+PPC_OP(reset_scrfx)
+{
+    regs->fpscr[7] &= ~0x8;
+    RETURN();
+}
+
 /* Set reservation */
 PPC_OP(set_reservation)
 {
@@ -988,7 +1020,7 @@ PPC_OP(xori)
 /* rotate left word immediate then mask insert */
 PPC_OP(rlwimi)
 {
-    T0 = rotl(T0, PARAM(1) & PARAM(2)) | (T0 & PARAM(3));
+    T0 = (rotl(T0, PARAM(1)) & PARAM(2)) | (T1 & PARAM(3));
     RETURN();
 }
@@ -1216,123 +1248,171 @@ PPC_OP(store_spr)
     regs->spr[PARAM(1)] = T0;
 }
-/* FPSCR */
-PPC_OP(load_fpscr)
-{
-    T0 = do_load_fpscr();
-}
-
-PPC_OP(store_fpscr)
-{
-    do_store_fpscr(PARAM(1), T0);
-}
-
 /***                         Floating-point store                          ***/
-static inline uint32_t dtos(uint64_t f)
-{
-    unsigned int e, m, s;
-    e = (((f >> 52) & 0x7ff) - 1022) + 126;
-    s = (f >> 63);
-    m = (f >> 29);
-    return (s << 31) | (e << 23) | m;
-}
-
-static inline uint64_t stod(uint32_t f)
-{
-    unsigned int e, m, s;
-    e = ((f >> 23) & 0xff) - 126 + 1022;
-    s = f >> 31;
-    m = f & ((1 << 23) - 1);
-    return ((uint64_t)s << 63) | ((uint64_t)e << 52) | ((uint64_t)m << 29);
-}
-
 PPC_OP(stfd_z_FT0)
 {
-    st64(SPARAM(1), FT0);
+    stfq((void *)SPARAM(1), FT0);
 }
 PPC_OP(stfd_FT0)
 {
     T0 += SPARAM(1);
-    st64(T0, FT0);
+    stfq((void *)T0, FT0);
 }
 PPC_OP(stfdx_z_FT0)
 {
-    st64(T0, FT0);
+    stfq((void *)T0, FT0);
 }
 PPC_OP(stfdx_FT0)
 {
     T0 += T1;
-    st64(T0, FT0);
+    stfq((void *)T0, FT0);
 }
-
 PPC_OP(stfs_z_FT0)
 {
-    st32(SPARAM(1), dtos(FT0));
+    float tmp = FT0;
+    stfl((void *)SPARAM(1), tmp);
 }
 PPC_OP(stfs_FT0)
 {
+    float tmp = FT0;
     T0 += SPARAM(1);
-    st32(T0, dtos(FT0));
+    stfl((void *)T0, tmp);
 }
 PPC_OP(stfsx_z_FT0)
 {
-    st32(T0, dtos(FT0));
+    float tmp = FT0;
+    stfl((void *)T0, tmp);
 }
 PPC_OP(stfsx_FT0)
 {
+    float tmp = FT0;
     T0 += T1;
-    st32(T0, dtos(FT0));
+    stfl((void *)T0, tmp);
 }
 /***                         Floating-point load                          ***/
 PPC_OP(lfd_z_FT0)
 {
-    FT0 = ld64(SPARAM(1));
+    FT0 = ldfq((void *)SPARAM(1));
 }
 PPC_OP(lfd_FT0)
 {
     T0 += SPARAM(1);
-    FT0 = ld64(T0);
+    FT0 = ldfq((void *)T0);
 }
 PPC_OP(lfdx_z_FT0)
 {
-    FT0 = ld64(T0);
+    FT0 = ldfq((void *)T0);
 }
 PPC_OP(lfdx_FT0)
 {
     T0 += T1;
-    FT0 = ld64(T0);
+    FT0 = ldfq((void *)T0);
 }
 PPC_OP(lfs_z_FT0)
 {
-    FT0 = stod(ld32(SPARAM(1)));
+    float tmp = ldfl((void *)SPARAM(1));
+    FT0 = tmp;
 }
 PPC_OP(lfs_FT0)
 {
+    float tmp;
     T0 += SPARAM(1);
-    FT0 = stod(ld32(T0));
+    tmp = ldfl((void *)T0);
+    FT0 = tmp;
 }
 PPC_OP(lfsx_z_FT0)
 {
-    FT0 = stod(ld32(T0));
+    float tmp;
+    tmp = ldfl((void *)T0);
+    FT0 = tmp;
 }
 PPC_OP(lfsx_FT0)
 {
+    float tmp;
+    T0 += T1;
+    tmp = ldfl((void *)T0);
+    FT0 = tmp;
+}
+
+PPC_OP(lwarx_z)
+{
+    T1 = ld32(T0);
+    regs->reserve = T0;
+    RETURN();
+}
+
+PPC_OP(lwarx)
+{
+    T0 += T1;
+    T1 = ld32(T0);
+    regs->reserve = T0;
+    RETURN();
+}
+
+PPC_OP(stwcx_z)
+{
+    if (regs->reserve != T0) {
+        env->crf[0] = xer_ov;
+    } else {
+        st32(T0, T1);
+        env->crf[0] = xer_ov | 0x02;
+    }
+    regs->reserve = 0;
+    RETURN();
+}
+
+PPC_OP(stwcx)
+{
     T0 += T1;
-    FT0 = stod(ld32(T0));
+    if (regs->reserve != (T0 & ~0x03)) {
+        env->crf[0] = xer_ov;
+    } else {
+        st32(T0, T2);
+        env->crf[0] = xer_ov | 0x02;
+    }
+    regs->reserve = 0;
+    RETURN();
+}
+
+PPC_OP(dcbz_z)
+{
+    do_dcbz();
+    RETURN();
+}
+
+PPC_OP(dcbz)
+{
+    T0 += T1;
+    do_dcbz();
+    RETURN();
+}
+
+/* Instruction cache block invalidate */
+PPC_OP(icbi_z)
+{
+    do_icbi();
+    RETURN();
+}
+
+PPC_OP(icbi)
+{
+    T0 += T1;
+    do_icbi();
+    RETURN();
 }
@@ -70,18 +70,90 @@ void OPPROTO glue(op_store_T1_crf_crf, REG)(void)
     regs->crf[REG] = T1;
 }
+/* Floating point condition and status register moves */
+void OPPROTO glue(op_load_fpscr_T0_fpscr, REG)(void)
+{
+    T0 = regs->fpscr[REG];
+    RETURN();
+}
+
+#if REG == 0
+void OPPROTO glue(op_store_T0_fpscr_fpscr, REG)(void)
+{
+    regs->fpscr[REG] = (regs->fpscr[REG] & 0x9) | (T0 & ~0x9);
+    RETURN();
+}
+
+void OPPROTO glue(op_store_T0_fpscri_fpscr, REG)(void)
+{
+    regs->fpscr[REG] = (regs->fpscr[REG] & ~0x9) | (PARAM(1) & 0x9);
+    RETURN();
+}
+
+void OPPROTO glue(op_clear_fpscr_fpscr, REG)(void)
+{
+    regs->fpscr[REG] = (regs->fpscr[REG] & 0x9);
+    RETURN();
+}
+#else
+void OPPROTO glue(op_store_T0_fpscr_fpscr, REG)(void)
+{
+    regs->fpscr[REG] = T0;
+    RETURN();
+}
+
+void OPPROTO glue(op_store_T0_fpscri_fpscr, REG)(void)
+{
+    regs->fpscr[REG] = PARAM(1);
+    RETURN();
+}
+
+void OPPROTO glue(op_clear_fpscr_fpscr, REG)(void)
+{
+    regs->fpscr[REG] = 0x0;
+    RETURN();
+}
+#endif
+
 #endif /* REG <= 7 */
 /* float moves */
-void OPPROTO glue(op_load_FT0_fpr, REG)(void)
+/* floating point registers moves */
+void OPPROTO glue(op_load_fpr_FT0_fpr, REG)(void)
 {
     FT0 = env->fpr[REG];
+    RETURN();
 }
-void OPPROTO glue(op_store_FT0_fpr, REG)(void)
+void OPPROTO glue(op_store_FT0_fpr_fpr, REG)(void)
 {
     env->fpr[REG] = FT0;
+    RETURN();
+}
+
+void OPPROTO glue(op_load_fpr_FT1_fpr, REG)(void)
+{
+    FT1 = env->fpr[REG];
+    RETURN();
+}
+
+void OPPROTO glue(op_store_FT1_fpr_fpr, REG)(void)
+{
+    env->fpr[REG] = FT1;
+    RETURN();
+}
+
+void OPPROTO glue(op_load_fpr_FT2_fpr, REG)(void)
+{
+    FT2 = env->fpr[REG];
+    RETURN();
+}
+
+void OPPROTO glue(op_store_FT2_fpr_fpr, REG)(void)
+{
+    env->fpr[REG] = FT2;
+    RETURN();
 }
 #undef REG
@@ -38,6 +38,9 @@ static uint32_t *gen_opparam_ptr;
 #include "gen-op.h"
 typedef void (GenOpFunc)(void);
+typedef void (GenOpFunc1)(long);
+typedef void (GenOpFunc2)(long, long);
+typedef void (GenOpFunc3)(long, long, long);
 #define GEN8(func, NAME) \
 static GenOpFunc *NAME ## _table [8] = {\
@@ -70,6 +73,25 @@ GEN8(gen_op_load_crf_T1, gen_op_load_crf_T1_crf)
 GEN8(gen_op_store_T0_crf, gen_op_store_T0_crf_crf)
 GEN8(gen_op_store_T1_crf, gen_op_store_T1_crf_crf)
+/* Floating point condition and status register moves */
+GEN8(gen_op_load_fpscr_T0, gen_op_load_fpscr_T0_fpscr);
+GEN8(gen_op_store_T0_fpscr, gen_op_store_T0_fpscr_fpscr);
+GEN8(gen_op_clear_fpscr, gen_op_clear_fpscr_fpscr);
+static GenOpFunc1 *gen_op_store_T0_fpscri_fpscr_table[8] = {
+    &gen_op_store_T0_fpscri_fpscr0,
+    &gen_op_store_T0_fpscri_fpscr1,
+    &gen_op_store_T0_fpscri_fpscr2,
+    &gen_op_store_T0_fpscri_fpscr3,
+    &gen_op_store_T0_fpscri_fpscr4,
+    &gen_op_store_T0_fpscri_fpscr5,
+    &gen_op_store_T0_fpscri_fpscr6,
+    &gen_op_store_T0_fpscri_fpscr7,
+};
+static inline void gen_op_store_T0_fpscri(int n, uint8_t param)
+{
+    (*gen_op_store_T0_fpscri_fpscr_table[n])(param);
+}
+
 GEN32(gen_op_load_gpr_T0, gen_op_load_gpr_T0_gpr)
 GEN32(gen_op_load_gpr_T1, gen_op_load_gpr_T1_gpr)
 GEN32(gen_op_load_gpr_T2, gen_op_load_gpr_T2_gpr)
@@ -78,8 +100,13 @@ GEN32(gen_op_store_T0_gpr, gen_op_store_T0_gpr_gpr)
 GEN32(gen_op_store_T1_gpr, gen_op_store_T1_gpr_gpr)
 GEN32(gen_op_store_T2_gpr, gen_op_store_T2_gpr_gpr)
-GEN32(gen_op_load_FT0_fpr, gen_op_load_FT0_fpr)
-GEN32(gen_op_store_FT0_fpr, gen_op_store_FT0_fpr)
+/* floating point registers moves */
+GEN32(gen_op_load_fpr_FT0, gen_op_load_fpr_FT0_fpr);
+GEN32(gen_op_load_fpr_FT1, gen_op_load_fpr_FT1_fpr);
+GEN32(gen_op_load_fpr_FT2, gen_op_load_fpr_FT2_fpr);
+GEN32(gen_op_store_FT0_fpr, gen_op_store_FT0_fpr_fpr);
+GEN32(gen_op_store_FT1_fpr, gen_op_store_FT1_fpr_fpr);
+GEN32(gen_op_store_FT2_fpr, gen_op_store_FT2_fpr_fpr);
 static uint8_t  spr_access[1024 / 2];
@@ -198,10 +225,14 @@ EXTRACT_HELPER(SH, 11, 5);
 EXTRACT_HELPER(MB, 6, 5);
 /* Mask end */
 EXTRACT_HELPER(ME, 1, 5);
+/* Trap operand */
+EXTRACT_HELPER(TO, 21, 5);
 EXTRACT_HELPER(CRM, 12, 8);
 EXTRACT_HELPER(FM, 17, 8);
 EXTRACT_HELPER(SR, 16, 4);
+EXTRACT_HELPER(FPIMM, 20, 4);
+
 /***                            Jump target decoding                       ***/
 /* Displacement */
 EXTRACT_SHELPER(d, 0, 16);
@@ -597,6 +628,7 @@ GEN_HANDLER(rlwimi, 0x14, 0xFF, 0xFF, 0x00000000, PPC_INTEGER)
     mb = MB(ctx->opcode);
     me = ME(ctx->opcode);
     gen_op_load_gpr_T0(rS(ctx->opcode));
+    gen_op_load_gpr_T1(rA(ctx->opcode));
     gen_op_rlwimi(SH(ctx->opcode), MASK(mb, me), ~MASK(mb, me));
     if (Rc(ctx->opcode) != 0)
         gen_op_set_Rc0();
@@ -847,47 +879,67 @@ GEN_HANDLER(fcmpu, 0x3F, 0x00, 0x01, 0x00600001, PPC_FLOAT)
 /* mcrfs */
 GEN_HANDLER(mcrfs, 0x3F, 0x00, 0x02, 0x0063F801, PPC_FLOAT)
 {
-    SET_RETVAL(EXCP_INVAL);
+    gen_op_load_fpscr_T0(crfS(ctx->opcode));
+    gen_op_store_T0_crf(crfD(ctx->opcode));
+    gen_op_clear_fpscr(crfS(ctx->opcode));
+    SET_RETVAL(0);
 }
 /* mffs */
 GEN_HANDLER(mffs, 0x3F, 0x07, 0x12, 0x001FF800, PPC_FLOAT)
 {
     gen_op_load_fpscr();
-    gen_op_store_T0_gpr(rD(ctx->opcode));
-    if (Rc(ctx->opcode)) {
-        /* Update CR1 */
-    }
+    gen_op_store_FT0_fpr(rD(ctx->opcode));
+    if (Rc(ctx->opcode))
+        gen_op_set_Rc1();
     SET_RETVAL(0);
 }
 /* mtfsb0 */
 GEN_HANDLER(mtfsb0, 0x3F, 0x06, 0x02, 0x001FF800, PPC_FLOAT)
 {
-    SET_RETVAL(EXCP_INVAL);
+    uint8_t crb;
+    
+    crb = crbD(ctx->opcode) >> 2;
+    gen_op_load_fpscr_T0(crb);
+    gen_op_andi_(~(1 << (crbD(ctx->opcode) & 0x03)));
+    gen_op_store_T0_fpscr(crb);
+    if (Rc(ctx->opcode))
+        gen_op_set_Rc1();
+    SET_RETVAL(0);
 }
 /* mtfsb1 */
 GEN_HANDLER(mtfsb1, 0x3F, 0x06, 0x01, 0x001FF800, PPC_FLOAT)
 {
-    SET_RETVAL(EXCP_INVAL);
+    uint8_t crb;
+    
+    crb = crbD(ctx->opcode) >> 2;
+    gen_op_load_fpscr_T0(crb);
+    gen_op_ori(1 << (crbD(ctx->opcode) & 0x03));
+    gen_op_store_T0_fpscr(crb);
+    if (Rc(ctx->opcode))
+        gen_op_set_Rc1();
+    SET_RETVAL(0);
 }
 /* mtfsf */
 GEN_HANDLER(mtfsf, 0x3F, 0x07, 0x16, 0x02010000, PPC_FLOAT)
 {
-    gen_op_load_gpr_T0(rB(ctx->opcode));
+    gen_op_load_fpr_FT0(rB(ctx->opcode));
     gen_op_store_fpscr(FM(ctx->opcode));
-    if (Rc(ctx->opcode)) {
-        /* Update CR1 */
-    }
+    if (Rc(ctx->opcode))
+        gen_op_set_Rc1();
     SET_RETVAL(0);
 }
 /* mtfsfi */
 GEN_HANDLER(mtfsfi, 0x3F, 0x06, 0x04, 0x006f0800, PPC_FLOAT)
 {
-    SET_RETVAL(EXCP_INVAL);
+    gen_op_store_T0_fpscri(crbD(ctx->opcode) >> 2, FPIMM(ctx->opcode));
+    if (Rc(ctx->opcode))
+        gen_op_set_Rc1();
+    SET_RETVAL(0);
 }
 /***                             Integer load                              ***/
@@ -1179,13 +1231,11 @@ GEN_HANDLER(lwarx, 0x1F, 0x14, 0xFF, 0x00000001, PPC_MEM)
     reserve = 1;
     if (rA(ctx->opcode) == 0) {
         gen_op_load_gpr_T0(rB(ctx->opcode));
-        gen_op_lwzx_z();
-        gen_op_set_reservation();
+        gen_op_lwarx_z();
     } else {
         gen_op_load_gpr_T0(rA(ctx->opcode));
         gen_op_load_gpr_T1(rB(ctx->opcode));
-        gen_op_lwzx();
-        gen_op_set_reservation();
+        gen_op_lwarx();
     }
     gen_op_store_T1_gpr(rD(ctx->opcode));
     SET_RETVAL(0);
@@ -1207,8 +1257,6 @@ GEN_HANDLER(stwcx_, 0x1F, 0x16, 0x04, 0x00000000, PPC_MEM)
             gen_op_load_gpr_T2(rS(ctx->opcode));
             gen_op_stwx();
         }
-        gen_op_set_Rc0_1();
-        gen_op_reset_reservation();
     }
     SET_RETVAL(0);
 }
@@ -1294,7 +1342,7 @@ GEN_LDF(s, 0x10);
 GEN_HANDLER(stf##width, opc, 0xFF, 0xFF, 0x00000000, PPC_FLOAT)               \
 {                                                                             \
     uint32_t simm = SIMM(ctx->opcode);                                        \
-    gen_op_load_FT0_fpr(rS(ctx->opcode));\
+    gen_op_load_fpr_FT0(rS(ctx->opcode));\
     if (rA(ctx->opcode) == 0) {                                               \
         gen_op_stf##width##_z_FT0(simm);                         \
     } else {                                                                  \
@@ -1310,7 +1358,7 @@ GEN_HANDLER(stf##width##u, opc, 0xFF, 0xFF, 0x00000000, PPC_FLOAT)            \
     if (rA(ctx->opcode) == 0)                                                 \
         SET_RETVAL(EXCP_INVAL);                                               \
     gen_op_load_gpr_T0(rA(ctx->opcode));                                      \
-    gen_op_load_FT0_fpr(rS(ctx->opcode));\
+    gen_op_load_fpr_FT0(rS(ctx->opcode));\
     gen_op_stf##width##_FT0(SIMM(ctx->opcode));                    \
     gen_op_store_T0_gpr(rA(ctx->opcode));                                     \
     SET_RETVAL(0);                                                            \
@@ -1323,7 +1371,7 @@ GEN_HANDLER(stf##width##ux, 0x1F, 0x17, opc, 0x00000001, PPC_FLOAT)           \
         SET_RETVAL(EXCP_INVAL);                                               \
     gen_op_load_gpr_T0(rA(ctx->opcode));                                      \
     gen_op_load_gpr_T1(rB(ctx->opcode));                                      \
-    gen_op_load_FT0_fpr(rS(ctx->opcode));\
+    gen_op_load_fpr_FT0(rS(ctx->opcode));\
     gen_op_stf##width##x_FT0();                                    \
     gen_op_store_T0_gpr(rA(ctx->opcode));                                     \
     SET_RETVAL(0);                                                            \
@@ -1332,7 +1380,7 @@ GEN_HANDLER(stf##width##ux, 0x1F, 0x17, opc, 0x00000001, PPC_FLOAT)           \
 #define GEN_STFX(width, opc)                                                  \
 GEN_HANDLER(stf##width##x, 0x1F, 0x17, opc, 0x00000001, PPC_FLOAT)            \
 {                                                                             \
-    gen_op_load_FT0_fpr(rS(ctx->opcode));\
+    gen_op_load_fpr_FT0(rS(ctx->opcode));\
     if (rA(ctx->opcode) == 0) {                                               \
         gen_op_load_gpr_T0(rB(ctx->opcode));                                  \
         gen_op_stf##width##x_z_FT0();                              \
@@ -1811,12 +1859,28 @@ GEN_HANDLER(dcbtst, 0x1F, 0x16, 0x02, 0x03E00001, PPC_MEM)
 /* dcbz */
 GEN_HANDLER(dcbz, 0x1F, 0x16, 0x08, 0x03E00001, PPC_MEM)
 {
+    if (rA(ctx->opcode) == 0) {
+        gen_op_load_gpr_T0(rB(ctx->opcode));
+        gen_op_dcbz_z();
+    } else {
+        gen_op_load_gpr_T0(rA(ctx->opcode));
+        gen_op_load_gpr_T1(rB(ctx->opcode));
+        gen_op_dcbz();
+    }
     SET_RETVAL(0);
 }
 /* icbi */
 GEN_HANDLER(icbi, 0x1F, 0x16, 0x1E, 0x03E00001, PPC_MEM)
 {
+    if (rA(ctx->opcode) == 0) {
+        gen_op_load_gpr_T0(rB(ctx->opcode));
+        gen_op_icbi_z();
+    } else {
+        gen_op_load_gpr_T0(rA(ctx->opcode));
+        gen_op_load_gpr_T1(rB(ctx->opcode));
+        gen_op_icbi();
+    }
     SET_RETVAL(0);
 }
@@ -2252,7 +2316,7 @@ void cpu_ppc_dump_state(CPUPPCState *env, FILE *f, int flags)
         for (i = 0; i < 16; i++) {
             if ((i & 3) == 0)
                 fprintf(logfile, "FPR%02d:", i);
-            fprintf(logfile, " %016llx", env->fpr[i]);
+            fprintf(logfile, " %016llx", *((uint64_t *)(&env->fpr[i])));
             if ((i & 3) == 3)
                 fprintf(logfile, "\n");
         }
@@ -2361,7 +2425,7 @@ int gen_intermediate_code_internal (CPUState *env, TranslationBlock *tb,
 #endif
     }
 #if defined (DO_STEP_FLUSH)
-        tb_flush();
+    tb_flush(env);
 #endif
     /* We need to update the time base */
     if (!search_pc)