[PATCH 5/5] HPET interaction with in-kernel PIT (v6)

[Beth Kon <eak@us.ibm.com>]

Signed-off-by: Beth Kon <eak@us.ibm.com>

---
 arch/x86/include/asm/kvm.h |    1 +
 arch/x86/kvm/i8254.c       |   24 +++++++++++++++++++-----
 arch/x86/kvm/i8254.h       |    3 ++-
 arch/x86/kvm/x86.c         |    5 ++++-
 4 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/arch/x86/include/asm/kvm.h b/arch/x86/include/asm/kvm.h
index 708b9c3..3c44923 100644
--- a/arch/x86/include/asm/kvm.h
+++ b/arch/x86/include/asm/kvm.h
@@ -235,6 +235,7 @@ struct kvm_guest_debug_arch {
 
 struct kvm_pit_state {
 	struct kvm_pit_channel_state channels[3];
+	u8 hpet_legacy_mode;
 };
 
 struct kvm_reinject_control {
diff --git a/arch/x86/kvm/i8254.c b/arch/x86/kvm/i8254.c
index 331705f..bb8382b 100644
--- a/arch/x86/kvm/i8254.c
+++ b/arch/x86/kvm/i8254.c
@@ -340,10 +340,20 @@ static void pit_load_count(struct kvm *kvm, int channel, u32 val)
 	}
 }
 
-void kvm_pit_load_count(struct kvm *kvm, int channel, u32 val)
+void kvm_pit_load_count(struct kvm *kvm, int channel, u32 val, int hpet_legacy_start)
 {
+	u8 saved_mode;
 	mutex_lock(&kvm->arch.vpit->pit_state.lock);
-	pit_load_count(kvm, channel, val);
+	if (hpet_legacy_start) {
+		/* save existing mode for later reenablement */
+		saved_mode = kvm->arch.vpit->pit_state.channels[0].mode;
+		kvm->arch.vpit->pit_state.channels[0].mode = 0xff; /* disable timer */
+		pit_load_count(kvm, channel, val);
+		kvm->arch.vpit->pit_state.channels[0].mode = saved_mode;
+	} else {
+			if (!(channel == 0 && kvm->arch.vpit->pit_state.hpet_legacy_mode))
+			pit_load_count(kvm, channel, val);
+	}
 	mutex_unlock(&kvm->arch.vpit->pit_state.lock);
 }
 
@@ -411,17 +421,20 @@ static void pit_ioport_write(struct kvm_io_device *this,
 		switch (s->write_state) {
 		default:
 		case RW_STATE_LSB:
-			pit_load_count(kvm, addr, val);
+			if (!(addr == 0 && pit_state->hpet_legacy_mode))
+				pit_load_count(kvm, addr, val);
 			break;
 		case RW_STATE_MSB:
-			pit_load_count(kvm, addr, val << 8);
+			if (!(addr == 0 && pit_state->hpet_legacy_mode))
+				pit_load_count(kvm, addr, val << 8);
 			break;
 		case RW_STATE_WORD0:
 			s->write_latch = val;
 			s->write_state = RW_STATE_WORD1;
 			break;
 		case RW_STATE_WORD1:
-			pit_load_count(kvm, addr, s->write_latch | (val << 8));
+			if (!(addr == 0 && pit_state->hpet_legacy_mode))
+				pit_load_count(kvm, addr, s->write_latch | (val << 8));
 			s->write_state = RW_STATE_WORD0;
 			break;
 		}
@@ -548,6 +561,7 @@ void kvm_pit_reset(struct kvm_pit *pit)
 	struct kvm_kpit_channel_state *c;
 
 	mutex_lock(&pit->pit_state.lock);
+	pit->pit_state.hpet_legacy_mode = 0;
 	for (i = 0; i < 3; i++) {
 		c = &pit->pit_state.channels[i];
 		c->mode = 0xff;
diff --git a/arch/x86/kvm/i8254.h b/arch/x86/kvm/i8254.h
index b267018..b5967ca 100644
--- a/arch/x86/kvm/i8254.h
+++ b/arch/x86/kvm/i8254.h
@@ -21,6 +21,7 @@ struct kvm_kpit_channel_state {
 
 struct kvm_kpit_state {
 	struct kvm_kpit_channel_state channels[3];
+	u8 hpet_legacy_mode;
 	struct kvm_timer pit_timer;
 	bool is_periodic;
 	u32    speaker_data_on;
@@ -49,7 +50,7 @@ struct kvm_pit {
 #define KVM_PIT_CHANNEL_MASK	    0x3
 
 void kvm_inject_pit_timer_irqs(struct kvm_vcpu *vcpu);
-void kvm_pit_load_count(struct kvm *kvm, int channel, u32 val);
+void kvm_pit_load_count(struct kvm *kvm, int channel, u32 val, int hpet_legacy_start);
 struct kvm_pit *kvm_create_pit(struct kvm *kvm, u32 flags);
 void kvm_free_pit(struct kvm *kvm);
 void kvm_pit_reset(struct kvm_pit *pit);
diff --git a/arch/x86/kvm/x86.c b/arch/x86/kvm/x86.c
index 1b91ea7..3c70545 100644
--- a/arch/x86/kvm/x86.c
+++ b/arch/x86/kvm/x86.c
@@ -1948,9 +1948,12 @@ static int kvm_vm_ioctl_get_pit(struct kvm *kvm, struct kvm_pit_state *ps)
 static int kvm_vm_ioctl_set_pit(struct kvm *kvm, struct kvm_pit_state *ps)
 {
 	int r = 0;
+	int hpet_legacy_start = 0;
 
+	if (ps->hpet_legacy_mode && !kvm->arch.vpit->pit_state.hpet_legacy_mode)
+		hpet_legacy_start = 1;
 	memcpy(&kvm->arch.vpit->pit_state, ps, sizeof(struct kvm_pit_state));
-	kvm_pit_load_count(kvm, 0, ps->channels[0].count);
+	kvm_pit_load_count(kvm, 0, ps->channels[0].count, hpet_legacy_start);
 	return r;
 }