KVM: s390: emulate stctl and stctg

Introduce the methods to emulate the stctl and stctg instruction. Added tracing
code.

Signed-off-by: David Hildenbrand <dahi@linux.vnet.ibm.com>
Signed-off-by: Christian Borntraeger <borntraeger@de.ibm.com>

6 files changed, 104 insertions, 0 deletions

diff --git a/arch/s390/include/asm/kvm_host.h b/arch/s390/include/asm/kvm_host.h
index 5e5a14db8c21..5d9648925a8e 100644
--- a/arch/s390/include/asm/kvm_host.h
+++ b/arch/s390/include/asm/kvm_host.h
@@ -175,6 +175,8 @@ struct kvm_vcpu_stat {
         u32 exit_instruction;
         u32 instruction_lctl;
         u32 instruction_lctlg;
+        u32 instruction_stctl;
+        u32 instruction_stctg;
         u32 exit_program_interruption;
         u32 exit_instr_and_program;
         u32 deliver_external_call;
diff --git a/arch/s390/kvm/intercept.c b/arch/s390/kvm/intercept.c
index 4c3311e41727..c0e6b49191ba 100644
--- a/arch/s390/kvm/intercept.c
+++ b/arch/s390/kvm/intercept.c
@@ -29,6 +29,7 @@ static const intercept_handler_t instruction_handlers[256] = {
         [0x83] = kvm_s390_handle_diag,
         [0xae] = kvm_s390_handle_sigp,
         [0xb2] = kvm_s390_handle_b2,
+        [0xb6] = kvm_s390_handle_stctl,
         [0xb7] = kvm_s390_handle_lctl,
         [0xb9] = kvm_s390_handle_b9,
         [0xe5] = kvm_s390_handle_e5,
diff --git a/arch/s390/kvm/kvm-s390.c b/arch/s390/kvm/kvm-s390.c
index ae7c1265fcc1..7ae8c26065fb 100644
--- a/arch/s390/kvm/kvm-s390.c
+++ b/arch/s390/kvm/kvm-s390.c
@@ -52,6 +52,8 @@ struct kvm_stats_debugfs_item debugfs_entries[] = {
         { "exit_instr_and_program_int", VCPU_STAT(exit_instr_and_program) },
         { "instruction_lctlg", VCPU_STAT(instruction_lctlg) },
         { "instruction_lctl", VCPU_STAT(instruction_lctl) },
+        { "instruction_stctl", VCPU_STAT(instruction_stctl) },
+        { "instruction_stctg", VCPU_STAT(instruction_stctg) },
         { "deliver_emergency_signal", VCPU_STAT(deliver_emergency_signal) },
         { "deliver_external_call", VCPU_STAT(deliver_external_call) },
         { "deliver_service_signal", VCPU_STAT(deliver_service_signal) },
diff --git a/arch/s390/kvm/kvm-s390.h b/arch/s390/kvm/kvm-s390.h
index dc506f3782ea..5f00fc1e9785 100644
--- a/arch/s390/kvm/kvm-s390.h
+++ b/arch/s390/kvm/kvm-s390.h
@@ -147,6 +147,7 @@ int kvm_s390_handle_e5(struct kvm_vcpu *vcpu);
 int kvm_s390_handle_01(struct kvm_vcpu *vcpu);
 int kvm_s390_handle_b9(struct kvm_vcpu *vcpu);
 int kvm_s390_handle_lpsw(struct kvm_vcpu *vcpu);
+int kvm_s390_handle_stctl(struct kvm_vcpu *vcpu);
 int kvm_s390_handle_lctl(struct kvm_vcpu *vcpu);
 int kvm_s390_handle_eb(struct kvm_vcpu *vcpu);
 
diff --git a/arch/s390/kvm/priv.c b/arch/s390/kvm/priv.c
index 5fb503a6c443..27f9051a78f8 100644
--- a/arch/s390/kvm/priv.c
+++ b/arch/s390/kvm/priv.c
@@ -789,6 +789,42 @@ int kvm_s390_handle_lctl(struct kvm_vcpu *vcpu)
         return 0;
 }
 
+int kvm_s390_handle_stctl(struct kvm_vcpu *vcpu)
+{
+        int reg1 = (vcpu->arch.sie_block->ipa & 0x00f0) >> 4;
+        int reg3 = vcpu->arch.sie_block->ipa & 0x000f;
+        u64 ga;
+        u32 val;
+        int reg, rc;
+
+        vcpu->stat.instruction_stctl++;
+
+        if (vcpu->arch.sie_block->gpsw.mask & PSW_MASK_PSTATE)
+                return kvm_s390_inject_program_int(vcpu, PGM_PRIVILEGED_OP);
+
+        ga = kvm_s390_get_base_disp_rs(vcpu);
+
+        if (ga & 3)
+                return kvm_s390_inject_program_int(vcpu, PGM_SPECIFICATION);
+
+        VCPU_EVENT(vcpu, 5, "stctl r1:%x, r3:%x, addr:%llx", reg1, reg3, ga);
+        trace_kvm_s390_handle_stctl(vcpu, 0, reg1, reg3, ga);
+
+        reg = reg1;
+        do {
+                val = vcpu->arch.sie_block->gcr[reg] &  0x00000000fffffffful;
+                rc = write_guest(vcpu, ga, &val, sizeof(val));
+                if (rc)
+                        return kvm_s390_inject_prog_cond(vcpu, rc);
+                ga += 4;
+                if (reg == reg3)
+                        break;
+                reg = (reg + 1) % 16;
+        } while (1);
+
+        return 0;
+}
+
 static int handle_lctlg(struct kvm_vcpu *vcpu)
 {
         int reg1 = (vcpu->arch.sie_block->ipa & 0x00f0) >> 4;
@@ -825,8 +861,45 @@ static int handle_lctlg(struct kvm_vcpu *vcpu)
         return 0;
 }
 
+static int handle_stctg(struct kvm_vcpu *vcpu)
+{
+        int reg1 = (vcpu->arch.sie_block->ipa & 0x00f0) >> 4;
+        int reg3 = vcpu->arch.sie_block->ipa & 0x000f;
+        u64 ga, val;
+        int reg, rc;
+
+        vcpu->stat.instruction_stctg++;
+
+        if (vcpu->arch.sie_block->gpsw.mask & PSW_MASK_PSTATE)
+                return kvm_s390_inject_program_int(vcpu, PGM_PRIVILEGED_OP);
+
+        ga = kvm_s390_get_base_disp_rsy(vcpu);
+
+        if (ga & 7)
+                return kvm_s390_inject_program_int(vcpu, PGM_SPECIFICATION);
+
+        reg = reg1;
+
+        VCPU_EVENT(vcpu, 5, "stctg r1:%x, r3:%x, addr:%llx", reg1, reg3, ga);
+        trace_kvm_s390_handle_stctl(vcpu, 1, reg1, reg3, ga);
+
+        do {
+                val = vcpu->arch.sie_block->gcr[reg];
+                rc = write_guest(vcpu, ga, &val, sizeof(val));
+                if (rc)
+                        return kvm_s390_inject_prog_cond(vcpu, rc);
+                ga += 8;
+                if (reg == reg3)
+                        break;
+                reg = (reg + 1) % 16;
+        } while (1);
+
+        return 0;
+}
+
 static const intercept_handler_t eb_handlers[256] = {
         [0x2f] = handle_lctlg,
+        [0x25] = handle_stctg,
 };
 
 int kvm_s390_handle_eb(struct kvm_vcpu *vcpu)
diff --git a/arch/s390/kvm/trace.h b/arch/s390/kvm/trace.h
index a4bf7d78a0db..abf6ba52769e 100644
--- a/arch/s390/kvm/trace.h
+++ b/arch/s390/kvm/trace.h
@@ -315,6 +315,31 @@ TRACE_EVENT(kvm_s390_handle_lctl,
                            __entry->reg1, __entry->reg3, __entry->addr)
         );
 
+TRACE_EVENT(kvm_s390_handle_stctl,
+            TP_PROTO(VCPU_PROTO_COMMON, int g, int reg1, int reg3, u64 addr),
+            TP_ARGS(VCPU_ARGS_COMMON, g, reg1, reg3, addr),
+
+            TP_STRUCT__entry(
+                    VCPU_FIELD_COMMON
+                    __field(int, g)
+                    __field(int, reg1)
+                    __field(int, reg3)
+                    __field(u64, addr)
+                    ),
+
+            TP_fast_assign(
+                    VCPU_ASSIGN_COMMON
+                    __entry->g = g;
+                    __entry->reg1 = reg1;
+                    __entry->reg3 = reg3;
+                    __entry->addr = addr;
+                    ),
+
+            VCPU_TP_PRINTK("%s: storing cr %x-%x to %016llx",
+                           __entry->g ? "stctg" : "stctl",
+                           __entry->reg1, __entry->reg3, __entry->addr)
+        );
+
 TRACE_EVENT(kvm_s390_handle_prefix,
             TP_PROTO(VCPU_PROTO_COMMON, int set, u32 address),
             TP_ARGS(VCPU_ARGS_COMMON, set, address),