From c58360df3e3bf779ec47325c15000c62a70c3188 Mon Sep 17 00:00:00 2001
From: OBattler <oubattler@gmail.com>
Date: Sat, 16 Jul 2022 03:21:09 +0200
Subject: [PATCH] Chipsets.

---
 src/chipset/intel_420ex.c | 21 +++++++--------------
 src/chipset/intel_piix.c  | 24 +++++++-----------------
 src/chipset/intel_sio.c   | 23 +++++++----------------
 3 files changed, 21 insertions(+), 47 deletions(-)

diff --git a/src/chipset/intel_420ex.c b/src/chipset/intel_420ex.c
index 8c8603efc..1590bc34c 100644
--- a/src/chipset/intel_420ex.c
+++ b/src/chipset/intel_420ex.c
@@ -316,10 +316,8 @@ i420ex_write(int func, int addr, uint8_t val, void *priv)
 				dev->fast_off_period = PCICLK * 32768.0;
 				break;
 		}
-		cpu_fast_off_count = dev->regs[0xa8] + 1;
-		timer_disable(&dev->fast_off_timer);
-		if (dev->fast_off_period != 0.0)
-			timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+		cpu_fast_off_count = cpu_fast_off_val + 1;
+		cpu_fast_off_period_set(cpu_fast_off_val, dev->fast_off_period);
 		break;
 	case 0xa2:
 		dev->regs[addr] = val & 0xff;
@@ -347,9 +345,7 @@ i420ex_write(int func, int addr, uint8_t val, void *priv)
 		dev->regs[addr] = val & 0xff;
 		cpu_fast_off_val = val;
 		cpu_fast_off_count = val + 1;
-		timer_disable(&dev->fast_off_timer);
-		if (dev->fast_off_period != 0.0)
-			timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+		cpu_fast_off_period_set(cpu_fast_off_val, dev->fast_off_period);
 		break;
     }
 }
@@ -422,13 +418,8 @@ i420ex_fast_off_count(void *priv)
 
     cpu_fast_off_count--;
 
-    if (cpu_fast_off_count == 0) {
-	smi_raise();
-	dev->regs[0xaa] |= 0x20;
-	cpu_fast_off_count = dev->regs[0xa8] + 1;
-    }
-
-    timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+    smi_raise();
+    dev->regs[0xaa] |= 0x20;
 }
 
 
@@ -513,6 +504,8 @@ i420ex_init(const device_t *info)
     cpu_fast_off_val = dev->regs[0xa8];
     cpu_fast_off_count = cpu_fast_off_val + 1;
 
+    cpu_register_fast_off_handler(&dev->fast_off_timer);
+
     dev->apm = device_add(&apm_pci_device);
     /* APM intercept handler to update 82420EX SMI status on APM SMI. */
     io_sethandler(0x00b2, 0x0001, NULL, NULL, NULL, i420ex_apm_out, NULL, NULL, dev);
diff --git a/src/chipset/intel_piix.c b/src/chipset/intel_piix.c
index 55002405f..c0f28914b 100644
--- a/src/chipset/intel_piix.c
+++ b/src/chipset/intel_piix.c
@@ -628,10 +628,8 @@ piix_write(int func, int addr, uint8_t val, void *priv)
 					dev->fast_off_period = PCICLK * 32768.0;
 					break;
 			}
-			cpu_fast_off_count = fregs[0xa8] + 1;
-			timer_disable(&dev->fast_off_timer);
-			if (dev->fast_off_period != 0.0)
-				timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+			cpu_fast_off_count = cpu_fast_off_val + 1;
+			cpu_fast_off_period_set(cpu_fast_off_val, dev->fast_off_period);
 		}
 		break;
 	case 0xa2:
@@ -679,9 +677,7 @@ piix_write(int func, int addr, uint8_t val, void *priv)
 			fregs[addr] = val & 0xff;
 			cpu_fast_off_val = val;
 			cpu_fast_off_count = val + 1;
-			timer_disable(&dev->fast_off_timer);
-			if (dev->fast_off_period != 0.0)
-				timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+			cpu_fast_off_period_set(cpu_fast_off_val, dev->fast_off_period);
 		}
 		break;
 	case 0xaa:
@@ -1331,15 +1327,8 @@ piix_fast_off_count(void *priv)
 {
     piix_t *dev = (piix_t *) priv;
 
-    cpu_fast_off_count--;
-
-    if (cpu_fast_off_count == 0) {
-	smi_raise();
-	dev->regs[0][0xaa] |= 0x20;
-	cpu_fast_off_count = dev->regs[0][0xa8] + 1;
-    }
-
-    timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+    smi_raise();
+    dev->regs[0][0xaa] |= 0x20;
 }
 
 
@@ -1446,7 +1435,7 @@ piix_speed_changed(void *priv)
 
     timer_stop(&dev->fast_off_timer);
     if (te)
-	timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+	timer_on_auto(&dev->fast_off_timer, ((double) cpu_fast_off_val + 1) * dev->fast_off_period);
 }
 
 
@@ -1510,6 +1499,7 @@ static void
     if (dev->type < 4) {
 	cpu_fast_off_val = dev->regs[0][0xa8];
 	cpu_fast_off_count = cpu_fast_off_val + 1;
+	cpu_fast_off_register(&dev->fast_off_timer);
     } else
 	cpu_fast_off_val = cpu_fast_off_count = 0;
 
diff --git a/src/chipset/intel_sio.c b/src/chipset/intel_sio.c
index 75aef516c..8c75e88c2 100644
--- a/src/chipset/intel_sio.c
+++ b/src/chipset/intel_sio.c
@@ -264,10 +264,8 @@ sio_write(int func, int addr, uint8_t val, void *priv)
 					dev->fast_off_period = PCICLK * 32768.0;
 					break;
 			}
-			cpu_fast_off_count = dev->regs[0xa8] + 1;
-			timer_disable(&dev->fast_off_timer);
-			if (dev->fast_off_period != 0.0)
-				timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+			cpu_fast_off_count = cpu_fast_off_val + 1;
+			cpu_fast_off_period_set(cpu_fast_off_val, dev->fast_off_period);
 		}
 		break;
 	case 0xa2:
@@ -306,9 +304,7 @@ sio_write(int func, int addr, uint8_t val, void *priv)
 		dev->regs[addr] = val & 0xff;
 		cpu_fast_off_val = val;
 		cpu_fast_off_count = val + 1;
-		timer_disable(&dev->fast_off_timer);
-		if (dev->fast_off_period != 0.0)
-			timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+		cpu_fast_off_period_set(cpu_fast_off_val, dev->fast_off_period);
 		break;
     }
 }
@@ -429,15 +425,8 @@ sio_fast_off_count(void *priv)
 {
     sio_t *dev = (sio_t *) priv;
 
-    cpu_fast_off_count--;
-
-    if (cpu_fast_off_count == 0) {
-	smi_raise();
-	dev->regs[0xaa] |= 0x20;
-	cpu_fast_off_count = dev->regs[0xa8] + 1;
-    }
-
-    timer_on_auto(&dev->fast_off_timer, dev->fast_off_period);
+    smi_raise();
+    dev->regs[0xaa] |= 0x20;
 }
 
 
@@ -513,6 +502,8 @@ sio_init(const device_t *info)
     if (dev->id == 0x03) {
 	cpu_fast_off_val = dev->regs[0xa8];
 	cpu_fast_off_count = cpu_fast_off_val + 1;
+
+	cpu_fast_off_register(&dev->fast_off_timer);
     } else
 	cpu_fast_off_val = cpu_fast_off_count = 0;