3 files changed, 87 insertions, 0 deletions

diff --git a/arch/powerpc/include/asm/kvm_book3s.h b/arch/powerpc/include/asm/kvm_book3s.h
index b47b2f516eff..bea76371dbe1 100644
--- a/arch/powerpc/include/asm/kvm_book3s.h
+++ b/arch/powerpc/include/asm/kvm_book3s.h
@@ -131,6 +131,8 @@ extern void kvmppc_rmcall(ulong srr0, ulong srr1);
 extern void kvmppc_load_up_fpu(void);
 extern void kvmppc_load_up_altivec(void);
 extern void kvmppc_load_up_vsx(void);
+extern u32 kvmppc_alignment_dsisr(struct kvm_vcpu *vcpu, unsigned int inst);
+extern ulong kvmppc_alignment_dar(struct kvm_vcpu *vcpu, unsigned int inst);
 
 static inline struct kvmppc_vcpu_book3s *to_book3s(struct kvm_vcpu *vcpu)
 {
diff --git a/arch/powerpc/kvm/book3s.c b/arch/powerpc/kvm/book3s.c
index c058f1a5c095..de12202fe1c6 100644
--- a/arch/powerpc/kvm/book3s.c
+++ b/arch/powerpc/kvm/book3s.c
@@ -906,6 +906,16 @@ program_interrupt:
                 }
                 break;
         }
+        case BOOK3S_INTERRUPT_ALIGNMENT:
+                if (kvmppc_read_inst(vcpu) == EMULATE_DONE) {
+                        to_book3s(vcpu)->dsisr = kvmppc_alignment_dsisr(vcpu,
+                                vcpu->arch.last_inst);
+                        vcpu->arch.dear = kvmppc_alignment_dar(vcpu,
+                                vcpu->arch.last_inst);
+                        kvmppc_book3s_queue_irqprio(vcpu, exit_nr);
+                }
+                r = RESUME_GUEST;
+                break;
         case BOOK3S_INTERRUPT_MACHINE_CHECK:
         case BOOK3S_INTERRUPT_TRACE:
                 kvmppc_book3s_queue_irqprio(vcpu, exit_nr);
diff --git a/arch/powerpc/kvm/book3s_64_emulate.c b/arch/powerpc/kvm/book3s_64_emulate.c
index 39d5003e01f0..1e5cf8d594ea 100644
--- a/arch/powerpc/kvm/book3s_64_emulate.c
+++ b/arch/powerpc/kvm/book3s_64_emulate.c
@@ -44,6 +44,11 @@
 /* DCBZ is actually 1014, but we patch it to 1010 so we get a trap */
 #define OP_31_XOP_DCBZ          1010
 
+#define OP_LFS                  48
+#define OP_LFD                  50
+#define OP_STFS                 52
+#define OP_STFD                 54
+
 #define SPRN_GQR0               912
 #define SPRN_GQR1               913
 #define SPRN_GQR2               914
@@ -474,3 +479,73 @@ int kvmppc_core_emulate_mfspr(struct kvm_vcpu *vcpu, int sprn, int rt)
         return emulated;
 }
 
+u32 kvmppc_alignment_dsisr(struct kvm_vcpu *vcpu, unsigned int inst)
+{
+        u32 dsisr = 0;
+
+        /*
+         * This is what the spec says about DSISR bits (not mentioned = 0):
+         *
+         * 12:13                [DS]    Set to bits 30:31
+         * 15:16                [X]     Set to bits 29:30
+         * 17                   [X]     Set to bit 25
+         *                      [D/DS]  Set to bit 5
+         * 18:21                [X]     Set to bits 21:24
+         *                      [D/DS]  Set to bits 1:4
+         * 22:26                        Set to bits 6:10 (RT/RS/FRT/FRS)
+         * 27:31                        Set to bits 11:15 (RA)
+         */
+
+        switch (get_op(inst)) {
+        /* D-form */
+        case OP_LFS:
+        case OP_LFD:
+        case OP_STFD:
+        case OP_STFS:
+                dsisr |= (inst >> 12) & 0x4000; /* bit 17 */
+                dsisr |= (inst >> 17) & 0x3c00; /* bits 18:21 */
+                break;
+        /* X-form */
+        case 31:
+                dsisr |= (inst << 14) & 0x18000; /* bits 15:16 */
+                dsisr |= (inst << 8)  & 0x04000; /* bit 17 */
+                dsisr |= (inst << 3)  & 0x03c00; /* bits 18:21 */
+                break;
+        default:
+                printk(KERN_INFO "KVM: Unaligned instruction 0x%x\n", inst);
+                break;
+        }
+
+        dsisr |= (inst >> 16) & 0x03ff; /* bits 22:31 */
+
+        return dsisr;
+}
+
+ulong kvmppc_alignment_dar(struct kvm_vcpu *vcpu, unsigned int inst)
+{
+        ulong dar = 0;
+        ulong ra;
+
+        switch (get_op(inst)) {
+        case OP_LFS:
+        case OP_LFD:
+        case OP_STFD:
+        case OP_STFS:
+                ra = get_ra(inst);
+                if (ra)
+                        dar = kvmppc_get_gpr(vcpu, ra);
+                dar += (s32)((s16)inst);
+                break;
+        case 31:
+                ra = get_ra(inst);
+                if (ra)
+                        dar = kvmppc_get_gpr(vcpu, ra);
+                dar += kvmppc_get_gpr(vcpu, get_rb(inst));
+                break;
+        default:
+                printk(KERN_INFO "KVM: Unaligned instruction 0x%x\n", inst);
+                break;
+        }
+
+        return dar;
+}