Преглед изворни кода

Add Nehalem/Westmere P-state and C-state management

The kernel had no cpufreq or cpuidle driver: the X201 stayed at the
BIOS's maximum non-turbo ratio and only reached C1, so all four threads
sat at 2.4 GHz and the package idled at 63-65 C with the EC fan around
3700 RPM.

power.c probes the Nehalem/Westmere MSRs (model allowlist only, so the
Atom boards never touch them): it enables SpeedStep and C1E, raises the
package C-state limit to C6 when 0xE2 is unlocked, reads the min/max
ratios from PLATFORM_INFO and starts at the maximum non-turbo ratio.  A
per-CPU governor picks a target ratio every 100 ms (min for idle, max for
busy, linear in between, with a short busy streak jumping to max) and
writes IA32_PERF_CTL.  Nehalem takes the target in bits 15:8 with the low
byte zeroed; preserving the BIOS VID byte there made the request
ineffective.  IA32_PERF_STATUS reports the current ratio in bits 7:0.

idle auto is now the default: it asks power.c for the MWAIT hint (C6 on
this family, HLT on the Atom) while the explicit hlt/c1e/c3/c6 modes stay
for testing.  A new power command shows effective MHz from APERF/MPERF,
current/requested ratios and core/package C3/C6 residency, and cpufreq
overrides the governor.

The EHCI async schedule is now enabled only while a transfer or an armed
interrupt endpoint needs it, so an idle controller stops fetching QHs and
qTDs and cannot keep package C-states away.  Disabling clears ASE and
acknowledges STS_IAA; leaving the doorbell status set stalled the next
enable.

Also drop the duplicated 0x prefix in the thermal/tpec/power MSR logs
(serial_puthex already prints one).

Tested on the X201: cores drop from 63-65 C to 49-54 C, the EC sensor
from 72 C to 61 C, package C6 reaches ~76% and the idle cores request
ratio 9 while the busy BSP stays at 18.  QEMU floppy/SMP/USB-image boots,
the power/idle commands on an unsupported CPU, EHCI enumeration with a
high-speed USB-storage device, and the OHCI/UHCI/EHCI cursor and keyboard
tests all pass.
Johannes Zellner пре 2 недеља
родитељ
комит
7fd6be5dc4
12 измењених фајлова са 850 додато и 78 уклоњено
  1. 39 14
      AGENTS.md
  2. 7 4
      Makefile
  3. 89 7
      ehci.c
  4. 60 39
      idle.c
  5. 8 6
      idle.h
  6. 4 0
      kernel.c
  7. 577 0
      power.c
  8. 28 0
      power.h
  9. 3 0
      sched.c
  10. 33 6
      terminal.c
  11. 1 1
      thermal.c
  12. 1 1
      tpec.c

+ 39 - 14
AGENTS.md

@@ -87,8 +87,9 @@ make clean
    Drives below 0x80 use CHS (the same path the boot sector's kernel load
    used); fixed disks use EDD LBA.
 3. `kernel.c` — `kernel_main` zeroes BSS then inits serial → memory/paging →
-   ACPI/MADT → I/O → idle → processes → timer (PIT; 1 kHz busy, 100 Hz idle + IDT) → SMP AP
-   bring-up → mouse → VGA shadow buffer → terminal.
+   ACPI/MADT → I/O → power → idle → processes → timer (PIT; 1 kHz busy,
+   100 Hz idle + IDT) → SMP AP bring-up → mouse → VGA shadow buffer →
+   terminal.
 4. `mbr.S` — MBR (USB/HDD boot); loads the active partition's boot sector
    (partition 1 at LBA 63).
 5. **UEFI (`efi/`)** — an x86-64 PE32+ `BOOTX64.EFI` loads from the ESP. It
@@ -440,15 +441,33 @@ run area), so adding code no longer moves any address.
   can be plugged in at any time (root-port connect-change watcher); `lsusb`
   re-enumerates the bus, after which `usbhid` re-inits itself via a scan
   generation counter.
-- **Power/idle**: `idle.c` — HLT/C1 by default, `idle c2`/`idle c3`
-  (and the `c2ap`/`c3ap` AP-only variants) switch to MONITOR/MWAIT at runtime
-  (the old USB-legacy SMM hang is addressed by the UHCI/OHCI `0xC0`
-  legacy-SMI disable; test the keyboard before making it the default).  The
-  BSP PIT and each AP's LAPIC timer run at 1 kHz while a process runs and drop
-  to 100 Hz/10 ms while the CPU is idle (`timer_set_hz`,
-  `lapic_timer_set_ms`); the PIT stub advances the global millisecond clock by
-  `timer_tick_advance()`, and `smp_resched_wake()` shortens the idle period
-  when new work arrives.
+- **Power/idle**: `idle.c` — `idle auto` (the default) asks `power.c` for
+  the MWAIT hint: C6 (`0x20`) on the Nehalem/Westmere allowlist, HLT
+  everywhere else.  `idle hlt|c1e|c3|c6` (and the `c3ap`/`c6ap` AP-only
+  variants) are explicit overrides for testing; the Atom boards keep HLT
+  because of the old USB-legacy SMM hang (the UHCI/OHCI `0xC0` legacy-SMI
+  disable addresses it).  Arrandale hints are C1 `0x00`, C1E `0x01`, C3
+  `0x10`, C6 `0x20`.  The BSP PIT and each AP's LAPIC timer run at 1 kHz
+  while a process runs and drop to 100 Hz/10 ms while the CPU is idle
+  (`timer_set_hz`, `lapic_timer_set_ms`); the PIT stub advances the global
+  millisecond clock by `timer_tick_advance()`, and `smp_resched_wake()`
+  shortens the idle period when new work arrives.
+- **Frequency/package power**: `power.c/h` — Nehalem/Westmere P-states and
+  C-state setup, gated on the model allowlist (`0x1A/1E/1F/25/2C/2E/2F`)
+  because the MSRs must not be touched elsewhere.  `power_init()` enables
+  SpeedStep (`MISC_ENABLE` bit 16) and C1E (`POWER_CTL` bit 1), raises the
+  package C-state limit in `PKG_CST_CONFIG_CONTROL` to C6 only when it is
+  unlocked (bit 15), reads min/max ratios from `PLATFORM_INFO`, and starts at
+  the maximum non-turbo ratio.  The per-CPU tick runs a race-to-idle
+  governor: 100 ms windows pick min/max/linear target ratios (short busy
+  streaks jump to max immediately) and each CPU writes its own
+  `IA32_PERF_CTL`.  Nehalem/Westmere take the target ratio in bits 15:8 and
+  the low byte is written as zero (a preserved VID byte made the request
+  silently ineffective); `IA32_PERF_STATUS` reports the current ratio in
+  bits 7:0 (the X201 boot dump shows 0x12 at the 18x max non-turbo ratio).
+  The `power` command prints effective MHz (`APERF`/`MPERF`), the current
+  and requested ratios, per-core C3/C6 residency and package C3/C6;
+  `cpufreq [auto|min|max|off|<ratio>]` overrides the governor.
 - **CPU temperature**: `thermal.c/h` — Intel DTS through `IA32_THERM_STATUS`
   (MSR `0x19C`).  Each CPU samples itself once a second from its scheduler
   tick (`struct cpu.temp_c`), and `SYS_CPU_TEMP` (`sys_info.c`) exposes the
@@ -651,7 +670,7 @@ run area), so adding code no longer moves any address.
 `mkdir <path>`, `rmdir <path>`, `hello`, `reboot`, `poweroff`, `run <name>|<file>`, `space`,
 `prog`,
 `display`, `cpuinfo`, `mouseinfo`, `gfxbench`, `lsusb`, `lspci [id]`, `peek <addr> [len]`, `poke <addr> <val>`, `storage`, `meminfo`, `top`, `sched`, `format <device> [size]` (binary units, e.g. `format ide0 100mb` or `1gb`; no size formats the whole disk),
-`ps`, `renice <pid> <weight>`, `kill <pid>`, `idle [hlt|c2|c3|c2ap|c3ap]`, `spin [ms]`, `help`, `clear`, `serial test|recv`, `kexec`, `dbg`, `view <file>` (image viewer; PNG, baseline/progressive JPEG, animated GIF, BMP, TGA, PNM, ICO, PAM), and `browser [url|file]` (disk app launcher).
+`ps`, `renice <pid> <weight>`, `kill <pid>`, `idle [auto|hlt|c1e|c3|c6]`, `power`, `cpufreq [auto|min|max|off|<ratio>]`, `spin [ms]`, `help`, `clear`, `serial test|recv`, `kexec`, `dbg`, `view <file>` (image viewer; PNG, baseline/progressive JPEG, animated GIF, BMP, TGA, PNM, ICO, PAM), and `browser [url|file]` (disk app launcher).
 
 Network: `netinfo`, `netrecv`, `netsend`, `netip [a.b.c.d]`, `dhcp` (re-run the
 DHCP client), `netdev [name]` (kernel command: show the active NIC driver or
@@ -864,8 +883,14 @@ so addresses do not collide.
   with CONFIGFLAG; `port_reset` holds PORT_RESET for 50 ms, then clears it
   (QEMU and hardware complete the reset on that write), waits for PED and
   sets PORT_OWNER for a non-high-speed device.  `ehci.c` supports up to four
-  PCI instances.  Bulk `in`/`out` use one qTD per call (up to five 4 KB
-  buffer pages), which is enough for the ASIX AX88772 RX/TX path.
+  PCI instances.  The async schedule is enabled only while a control/bulk
+  transfer or an armed interrupt endpoint needs it (`ehci_sched_get/put`);
+  with nothing attached the controller stops fetching QHs and qTDs, which
+  keeps package C-states reachable (the port watcher only reads PORTSC).
+  Disabling clears ASE and acknowledges `STS_IAA`; leaving the IAA doorbell
+  status set stalls the next enable.  Bulk `in`/`out` use one qTD per call
+  (up to five 4 KB buffer pages), which is enough for the ASIX AX88772 RX/TX
+  path.
   There is no transaction translator: low/full-speed devices behind a
   high-speed hub stay invisible to EHCI (use the companion controller for
   them).

+ 7 - 4
Makefile

@@ -116,6 +116,9 @@ cpu.o: cpu.c cpu.h
 thermal.o: thermal.c thermal.h cpu.h sched.h serial.h
 	$(CC) $(CFLAGS) -o $@ $<
 
+power.o: power.c power.h cpu.h idle.h io.h sched.h serial.h
+	$(CC) $(CFLAGS) -o $@ $<
+
 tpec.o: tpec.c tpec.h paging.h pci.h serial.h
 	$(CC) $(CFLAGS) -o $@ $<
 
@@ -245,7 +248,7 @@ format.o: format.c blkdev.h disk.h fatformat.h io.h process.h sched.h vga.h vfs.
 mouse.o: mouse.c mouse.h bootinfo.h cirrus.h cursor_shape.h radeon.h timer.h usbhid.h vga.h
 	$(CC) $(CFLAGS) -o $@ $<
 
-idle.o: idle.c idle.h
+idle.o: idle.c idle.h power.h
 	$(CC) $(CFLAGS) -o $@ $<
 
 kexec.o: kexec.c kexec.h io.h serial.h smp.h
@@ -275,7 +278,7 @@ switch.o: switch.S irqret.inc
 entry.o: entry.S
 	$(AS) $(ASFLAGS) -o $@ $<
 
-sched.o: sched.c sched.h process.h paging.h smp.h spinlock.h sys_fs.h thermal.h timer.h tss.h vga.h
+sched.o: sched.c sched.h process.h paging.h smp.h spinlock.h sys_fs.h thermal.h power.h timer.h tss.h vga.h
 	$(CC) $(CFLAGS) -o $@ $<
 
 acpi.o: acpi.c acpi.h bootinfo.h paging.h serial.h
@@ -338,7 +341,7 @@ sys_net.o: sys_net.c sys_net.h syscall.h dhcp.h dns.h net.h netdev.h netlog.h pr
 syscall_stub.o: syscall_stub.S irqret.inc
 	$(AS) $(ASFLAGS) -o $@ $<
 
-kernel.elf: kernel.ld vesa_stage.o entry.o kernel.o timer.o switch.o sched.o process.o mm.o kmalloc.o paging.o elf.o tss.o syscall.o sys_wm.o syscall_stub.o screen.o io.o tty.o rtc.o terminal.o wm.o reboot.o poweroff.o disk.o fat.o run.o serial.o serial_ring_buf.o serial_loop.o serial_recv.o bin_recv.o vga.o blit.o cpu.o thermal.o it87.o tpec.o vga_copy.o gfxbench.o gpu.o ati.o cirrus.o nvidia.o nv50.o radeon.o mtrr.o font_8x8.o fbtext.o gfx.o sys_gfx.o display.o mouse.o uhci.o ohci.o ehci.o xhci.o asix.o usb.o usbhid.o lsusb.o panic.o bootcon.o blkdev.o ata.o ahci.o fatdetect.o fatformat.o format.o vfs.o sys_fs.o sys_info.o sys_mem.o sys_net.o idle.o kexec.o kexec_tramp.o dbg.o lspci.o peek.o ne2000.o r8168.o e1000e.o netdev.o net.o netkexec.o netlog.o dhcp.o dns.o tcp.o acpi.o lapic.o smp.o ap_tramp_embed.o prog_embed.o space_embed.o inflate.o ramfs.o share.o payload.o
+kernel.elf: kernel.ld vesa_stage.o entry.o kernel.o timer.o switch.o sched.o process.o mm.o kmalloc.o paging.o elf.o tss.o syscall.o sys_wm.o syscall_stub.o screen.o io.o tty.o rtc.o terminal.o wm.o reboot.o poweroff.o disk.o fat.o run.o serial.o serial_ring_buf.o serial_loop.o serial_recv.o bin_recv.o vga.o blit.o cpu.o thermal.o it87.o tpec.o power.o vga_copy.o gfxbench.o gpu.o ati.o cirrus.o nvidia.o nv50.o radeon.o mtrr.o font_8x8.o fbtext.o gfx.o sys_gfx.o display.o mouse.o uhci.o ohci.o ehci.o xhci.o asix.o usb.o usbhid.o lsusb.o panic.o bootcon.o blkdev.o ata.o ahci.o fatdetect.o fatformat.o format.o vfs.o sys_fs.o sys_info.o sys_mem.o sys_net.o idle.o kexec.o kexec_tramp.o dbg.o lspci.o peek.o ne2000.o r8168.o e1000e.o netdev.o net.o netkexec.o netlog.o dhcp.o dns.o tcp.o acpi.o lapic.o smp.o ap_tramp_embed.o prog_embed.o space_embed.o inflate.o ramfs.o share.o payload.o
 	$(LD) $(KERNEL_LDFLAGS) -o $@ $(filter-out kernel.ld,$^)
 
 MBR_LDFLAGS = -T mbr.ld -nostdlib -m elf_i386
@@ -1035,4 +1038,4 @@ kernel-net-dev: kernel.bin netkexec_send
 	./netkexec_send $(BOARD_IP) $(NETKEXEC_PORT) 0x$$entry kernel.bin
 
 clean:
-	rm -f boot.o vesa_stage.o entry.o kernel.o timer.o switch.o sched.o process.o mm.o kmalloc.o paging.o elf.o tss.o syscall.o sys_wm.o syscall_stub.o screen.o io.o tty.o rtc.o terminal.o wm.o hello.o reboot.o poweroff.o fat.o run.o disk.o serial.o serial_ring_buf.o serial_loop.o serial_recv.o bin_recv.o vga.o blit.o cpu.o thermal.o it87.o tpec.o vga_copy.o gfxbench.o gpu.o ati.o cirrus.o nvidia.o nv50.o radeon.o mtrr.o font_8x8.o font_8x16.o fbtext.o gfx.o sys_gfx.o display.o mouse.o uhci.o ohci.o ehci.o xhci.o asix.o usb.o usbhid.o lsusb.o panic.o bootcon.o blkdev.o ata.o ahci.o fatdetect.o fatformat.o format.o vfs.o sys_fs.o sys_info.o sys_mem.o inflate.o idle.o kexec.o kexec_tramp.o dbg.o lspci.o peek.o ne2000.o r8168.o e1000e.o netdev.o net.o netkexec.o netlog.o dhcp.o dns.o tcp.o acpi.o lapic.o smp.o ap_tramp.o ap_tramp.elf ap_tramp.bin ap_tramp_embed.o mbr.o boot.elf kernel.elf mbr.elf boot.bin mbr.bin kernel.bin boot.bin.tmp boot.bin.patched image.bin floppy.img usb.img final.bin root.img prog.o prog.elf prog.bin space.o space.elf space.bin hello.elf hello_embed.o prog_embed.o space_embed.o cmds.pack cmds_pack_gz.bin cmds_pack.o rootfs.bin cmds.img mkrootfs ramfs.o share.o payload.o mkpack uwm_lib.o ngui.o img.o img_mem.o img_blit.o img_png.o img_bmp.o img_gif.o img_jpeg.o img_tga.o img_pnm.o img_ico.o img_pam.o img_gif_anim.o img_file.o browser.o browser_html.o uhttp.o browser.elf img_test img_test_asan *.boot.tmp $(addsuffix .o,$(UCMDS)) $(addsuffix .elf,$(UCMDS)) gallery.o gallery.elf $(TLSLIB_OBJS) tls_test bootstrap.o bootstrap.elf bootstrap.bin send_serial kexec_send netkexec_send netmonitor BOOTX64.EFI esp.img usb-efi.img efi/main.o efi/payload.o efi/tramp.o efi/tramp.elf efi/tramp.bin efi/efi_entry.h
+	rm -f boot.o vesa_stage.o entry.o kernel.o timer.o switch.o sched.o process.o mm.o kmalloc.o paging.o elf.o tss.o syscall.o sys_wm.o syscall_stub.o screen.o io.o tty.o rtc.o terminal.o wm.o hello.o reboot.o poweroff.o fat.o run.o disk.o serial.o serial_ring_buf.o serial_loop.o serial_recv.o bin_recv.o vga.o blit.o cpu.o thermal.o it87.o tpec.o power.o vga_copy.o gfxbench.o gpu.o ati.o cirrus.o nvidia.o nv50.o radeon.o mtrr.o font_8x8.o font_8x16.o fbtext.o gfx.o sys_gfx.o display.o mouse.o uhci.o ohci.o ehci.o xhci.o asix.o usb.o usbhid.o lsusb.o panic.o bootcon.o blkdev.o ata.o ahci.o fatdetect.o fatformat.o format.o vfs.o sys_fs.o sys_info.o sys_mem.o inflate.o idle.o kexec.o kexec_tramp.o dbg.o lspci.o peek.o ne2000.o r8168.o e1000e.o netdev.o net.o netkexec.o netlog.o dhcp.o dns.o tcp.o acpi.o lapic.o smp.o ap_tramp.o ap_tramp.elf ap_tramp.bin ap_tramp_embed.o mbr.o boot.elf kernel.elf mbr.elf boot.bin mbr.bin kernel.bin boot.bin.tmp boot.bin.patched image.bin floppy.img usb.img final.bin root.img prog.o prog.elf prog.bin space.o space.elf space.bin hello.elf hello_embed.o prog_embed.o space_embed.o cmds.pack cmds_pack_gz.bin cmds_pack.o rootfs.bin cmds.img mkrootfs ramfs.o share.o payload.o mkpack uwm_lib.o ngui.o img.o img_mem.o img_blit.o img_png.o img_bmp.o img_gif.o img_jpeg.o img_tga.o img_pnm.o img_ico.o img_pam.o img_gif_anim.o img_file.o browser.o browser_html.o uhttp.o browser.elf img_test img_test_asan *.boot.tmp $(addsuffix .o,$(UCMDS)) $(addsuffix .elf,$(UCMDS)) gallery.o gallery.elf $(TLSLIB_OBJS) tls_test bootstrap.o bootstrap.elf bootstrap.bin send_serial kexec_send netkexec_send netmonitor BOOTX64.EFI esp.img usb-efi.img efi/main.o efi/payload.o efi/tramp.o efi/tramp.elf efi/tramp.bin efi/efi_entry.h

+ 89 - 7
ehci.c

@@ -47,6 +47,7 @@
 #define CMD_RESET		0x00000002u
 #define CMD_PSE			0x00000010u
 #define CMD_ASE			0x00000020u
+#define CMD_IAAD		0x00000040u
 #define CMD_ITC_8		0x00080000u
 
 /* USBSTS */
@@ -135,6 +136,8 @@ struct ehci_qtd {
 	volatile unsigned int buf_hi[5];   /* 64-bit addressing extension */
 } __attribute__((aligned(32)));
 
+struct ehci_ctrl;
+
 struct ehci_bulk {
 	struct ehci_qh *qh;
 	struct ehci_qtd *qtd;
@@ -145,6 +148,8 @@ struct ehci_bulk {
 	unsigned int buf_len;
 	unsigned int toggle;
 	int armed;
+	int sched;
+	struct ehci_ctrl *owner;
 };
 
 struct ehci_int {
@@ -155,6 +160,7 @@ struct ehci_int {
 	unsigned int buf_len;
 	unsigned int toggle;
 	int active;
+	struct ehci_ctrl *owner;
 };
 
 struct ehci_ctrl {
@@ -193,6 +199,8 @@ struct ehci_ctrl {
 
 	int ctl_busy;
 	int recovering;
+	int sched_on;
+	int sched_refs;
 	unsigned char addr_port[128];
 };
 
@@ -227,6 +235,54 @@ static unsigned int op_reg(struct ehci_ctrl *c, unsigned int off)
 	return c->base + c->cap_len + off;
 }
 
+/* The asynchronous schedule is only enabled while a transfer or an armed
+ * interrupt endpoint needs it.  With it off the controller stops fetching
+ * QHs and qTDs, which lets the package reach deep C-states while the
+ * machine is idle; the port hotplug watcher only reads PORTSC and does not
+ * need the schedule. */
+static void ehci_sched_set(struct ehci_ctrl *c, int on)
+{
+	unsigned int v, t0;
+
+	if (on == c->sched_on)
+		return;
+	v = rd32(op_reg(c, EHCI_OP_USBCMD));
+	if (on) {
+		wr32(op_reg(c, EHCI_OP_USBCMD), v | CMD_ASE);
+		t0 = get_ticks_ms();
+		while (!(rd32(op_reg(c, EHCI_OP_USBSTS)) & STS_ASS)) {
+			if (get_ticks_ms() - t0 > 10)
+				break;
+		}
+	} else {
+		/* Clear ASE and let the controller retire the current
+		 * microframe; a pending IAA status must be acknowledged or the
+		 * next enable can stall. */
+		wr32(op_reg(c, EHCI_OP_USBCMD), v & ~CMD_ASE);
+		t0 = get_ticks_ms();
+		while (rd32(op_reg(c, EHCI_OP_USBSTS)) & STS_ASS) {
+			if (get_ticks_ms() - t0 > 10)
+				break;
+		}
+		wr32(op_reg(c, EHCI_OP_USBSTS), STS_IAA);
+	}
+	c->sched_on = on;
+}
+
+static void ehci_sched_get(struct ehci_ctrl *c)
+{
+	c->sched_refs++;
+	ehci_sched_set(c, 1);
+}
+
+static void ehci_sched_put(struct ehci_ctrl *c)
+{
+	if (c->sched_refs > 0)
+		c->sched_refs--;
+	if (!c->sched_refs)
+		ehci_sched_set(c, 0);
+}
+
 static unsigned int port_reg(struct ehci_ctrl *c, unsigned int port)
 {
 	return c->base + c->cap_len + EHCI_OP_PORTSC + port * 4u;
@@ -372,6 +428,10 @@ static void bulk_stop(struct ehci_bulk *b)
 	if (b->zqtd)
 		b->zqtd->token = 0;
 	b->armed = 0;
+	if (b->sched && b->owner) {
+		ehci_sched_put(b->owner);
+		b->sched = 0;
+	}
 }
 
 static void bulk_reset(struct ehci_bulk *b)
@@ -404,6 +464,10 @@ static void bulk_configure(struct ehci_bulk *b, unsigned int addr,
 
 static void bulk_arm(struct ehci_bulk *b)
 {
+	if (!b->sched && b->owner) {
+		ehci_sched_get(b->owner);
+		b->sched = 1;
+	}
 	b->qh->token = (b->qh->token & QTD_PING) |
 		       (b->toggle ? QTD_TOGGLE : 0);
 	qh_arm(b->qh, b->qtd);
@@ -549,12 +613,18 @@ static void ehci_int_in_stop(unsigned int slot)
 	in = slot_int[slot];
 	if (!in)
 		return;
-	in->active = 0;
-	if (in->qh)
-		qh_disarm(in->qh);
-	if (in->qtd)
-		in->qtd->token = 0;
-	slot_int[slot] = 0;
+	{
+		int was_active = in->active;
+
+		in->active = 0;
+		if (in->qh)
+			qh_disarm(in->qh);
+		if (in->qtd)
+			in->qtd->token = 0;
+		slot_int[slot] = 0;
+		if (was_active && in->owner)
+			ehci_sched_put(in->owner);
+	}
 }
 
 static int ehci_int_in_start(unsigned int ctrl, unsigned int slot,
@@ -589,6 +659,7 @@ static int ehci_int_in_start(unsigned int ctrl, unsigned int slot,
 	int_arm(in);
 	in->active = 1;
 	slot_int[slot] = in;
+	ehci_sched_get(c);
 	return 0;
 }
 
@@ -652,6 +723,7 @@ static int ehci_control(struct ehci_ctrl *c, unsigned int addr,
 	q0 = c->ctl_qtd[0];
 	q1 = c->ctl_qtd[1];
 	q2 = c->ctl_qtd[2];
+	ehci_sched_get(c);
 	c->ctl_busy = 1;
 
 	for (i = 0; i < 8; i++)
@@ -684,6 +756,7 @@ static int ehci_control(struct ehci_ctrl *c, unsigned int addr,
 		qh_disarm(qh);
 		delay_ms(1);
 		c->ctl_busy = 0;
+		ehci_sched_put(c);
 		return -1;
 	}
 	qh_disarm(qh);
@@ -691,6 +764,7 @@ static int ehci_control(struct ehci_ctrl *c, unsigned int addr,
 	if (qtd_error(q0->token) || qtd_error(q2->token) ||
 	    (len && qtd_error(q1->token))) {
 		c->ctl_busy = 0;
+		ehci_sched_put(c);
 		return -1;
 	}
 
@@ -712,6 +786,7 @@ static int ehci_control(struct ehci_ctrl *c, unsigned int addr,
 	}
 
 	c->ctl_busy = 0;
+	ehci_sched_put(c);
 	return (int)actual;
 }
 
@@ -982,6 +1057,10 @@ static int ehci_sched_init(struct ehci_ctrl *c)
 	c->head->info1 = QH_HEAD;
 	c->head->token = QTD_HALT;
 
+	c->bin.owner = c;
+	c->bin.sched = 0;
+	c->bout.owner = c;
+	c->bout.sched = 0;
 	c->bin.qh = c->bin_qh;
 	c->bin.qtd = c->bin_qtd;
 	c->bin.zqtd = 0;
@@ -995,6 +1074,7 @@ static int ehci_sched_init(struct ehci_ctrl *c)
 		c->ints[i].qtd = c->int_qtd[i];
 		c->ints[i].buf = c->int_buf[i];
 		c->ints[i].active = 0;
+		c->ints[i].owner = c;
 	}
 	for (i = 0; i < 128; i++)
 		c->addr_port[i] = 0xFF;
@@ -1038,7 +1118,9 @@ static int ehci_start(struct ehci_ctrl *c)
 	wr32(op_reg(c, EHCI_OP_USBINTR), 0);
 	wr32(op_reg(c, EHCI_OP_USBSTS), 0x3Fu);
 
-	wr32(op_reg(c, EHCI_OP_USBCMD), CMD_RUN | CMD_ASE | CMD_ITC_8);
+	c->sched_on = 0;
+	c->sched_refs = 0;
+	wr32(op_reg(c, EHCI_OP_USBCMD), CMD_RUN | CMD_ITC_8);
 	__asm__ volatile ("" ::: "memory");
 
 	t0 = get_ticks_ms();

+ 60 - 39
idle.c

@@ -1,19 +1,26 @@
-/* CPU idle: HLT (C1) by default; optional MONITOR/MWAIT C2/C3.
+/* CPU idle: automatic deep C-states on Nehalem/Westmere, HLT elsewhere.
  *
- * MWAIT used to be left off entirely on the ASUS AT3IONT-I (Atom 330 +
- * NVIDIA MCP79): a USB-legacy SMM event (the first key press) hung the
- * machine in C2.  The UHCI/OHCI drivers now disable that legacy SMI
- * emulation (PCI config 0xC0), so C2 is exposed through `idle c2` for a
- * runtime test.  HLT stays the default until the board has been tested;
- * `idle` with no argument shows the current mode.
+ * The default is IDLE_AUTO, which asks power.c for the MWAIT hint of the
+ * CPU: C6 (0x20) on the Arrandale/Nehalem family, and -1 (HLT) on the
+ * Atom, where the old USB-legacy SMM hang made deep states risky.  The
+ * explicit c1e/c3/c6 modes remain for testing; the c3ap/c6ap variants only
+ * apply to the APs.
+ *
+ * Arrandale MWAIT state hints (coreboot model_2065x): C1 0x00, C1E 0x01,
+ * C3 0x10, C6 0x20.  The MWAIT ECX bit 0 (interrupts break the state) is
+ * set so a wake-up never waits for the full hint duration.
  */
 #include "idle.h"
+#include "power.h"
 #include "sched.h"
 
+#define MWAIT_C1E 0x01
+#define MWAIT_C3  0x10
+#define MWAIT_C6  0x20
+
 static int has_mwait;
 static int has_mwait_ext;
-static int current_mode = IDLE_C1;
-static unsigned int mwait_hint = 0x10;
+static int current_mode = IDLE_AUTO;
 
 static void cpuid_idle(unsigned int leaf, unsigned int *eax, unsigned int *ebx,
 		       unsigned int *ecx, unsigned int *edx)
@@ -30,7 +37,7 @@ void idle_init(void)
 
 	has_mwait = 0;
 	has_mwait_ext = 0;
-	current_mode = IDLE_C1;
+	current_mode = IDLE_AUTO;
 
 	cpuid_idle(0, &max_leaf, &ebx, &ecx, &edx);
 	if (max_leaf < 1)
@@ -47,30 +54,18 @@ void idle_init(void)
 
 int idle_set_mode(int mode)
 {
+	if (mode == IDLE_AUTO) {
+		current_mode = IDLE_AUTO;
+		return 0;
+	}
 	if (mode == IDLE_C1) {
 		current_mode = IDLE_C1;
 		return 0;
 	}
 	if (!has_mwait || !has_mwait_ext)
 		return -1;
-	if (mode == IDLE_C2) {
-		mwait_hint = 0x10;
-		current_mode = IDLE_C2;
-		return 0;
-	}
-	if (mode == IDLE_C3) {
-		mwait_hint = 0x20;
-		current_mode = IDLE_C3;
-		return 0;
-	}
-	if (mode == IDLE_C2_AP) {
-		mwait_hint = 0x10;
-		current_mode = IDLE_C2_AP;
-		return 0;
-	}
-	if (mode == IDLE_C3_AP) {
-		mwait_hint = 0x20;
-		current_mode = IDLE_C3_AP;
+	if (mode >= IDLE_C1E && mode <= IDLE_C6_AP) {
+		current_mode = mode;
 		return 0;
 	}
 	return -1;
@@ -89,14 +84,18 @@ int idle_mwait_supported(void)
 const char *idle_mode_name(int mode)
 {
 	switch (mode) {
-	case IDLE_C2:
-		return "mwait c2";
+	case IDLE_C1E:
+		return "mwait c1e";
 	case IDLE_C3:
 		return "mwait c3";
-	case IDLE_C2_AP:
-		return "mwait c2 (APs)";
+	case IDLE_C6:
+		return "mwait c6";
 	case IDLE_C3_AP:
 		return "mwait c3 (APs)";
+	case IDLE_C6_AP:
+		return "mwait c6 (APs)";
+	case IDLE_AUTO:
+		return "auto";
 	default:
 		return "hlt";
 	}
@@ -104,15 +103,37 @@ const char *idle_mode_name(int mode)
 
 void cpu_idle(void)
 {
-	int ap_only = (current_mode == IDLE_C2_AP ||
-		       current_mode == IDLE_C3_AP);
+	static unsigned int wake_addr;
+	int mode = current_mode;
+	int hint = -1;
+
+	if (mode == IDLE_AUTO) {
+		hint = power_idle_hint();
+		if (hint >= 0 && (!has_mwait || !has_mwait_ext))
+			hint = -1;
+	} else if (mode != IDLE_C1) {
+		int ap_only = (mode == IDLE_C3_AP || mode == IDLE_C6_AP);
 
-	if (current_mode != IDLE_C1 && has_mwait && has_mwait_ext &&
-	    (!ap_only || this_cpu()->id != 0)) {
-		volatile unsigned int dummy = 0;
+		if (!has_mwait || !has_mwait_ext ||
+		    (ap_only && this_cpu()->id == 0)) {
+			__asm__ volatile ("hlt");
+			return;
+		}
+		if (mode == IDLE_C1E)
+			hint = MWAIT_C1E;
+		else if (mode == IDLE_C3 || mode == IDLE_C3_AP)
+			hint = MWAIT_C3;
+		else
+			hint = MWAIT_C6;
+	}
 
-		__asm__ volatile ("monitor" : : "a"(&dummy), "c"(0), "d"(0));
-		__asm__ volatile ("mwait" : : "a"(mwait_hint), "c"(1));
+	if (hint >= 0) {
+		/* MONITOR/MWAIT on a cacheable address; ECX=1 in MWAIT lets
+		 * an interrupt break the state immediately. */
+		__asm__ volatile ("monitor"
+				  : : "a"(&wake_addr), "c"(0), "d"(0));
+		__asm__ volatile ("mwait" : : "a"((unsigned int)hint),
+				  "c"(1));
 		return;
 	}
 	__asm__ volatile ("hlt");

+ 8 - 6
idle.h

@@ -1,12 +1,14 @@
-/* CPU idle: HLT (C1) by default; optional MONITOR/MWAIT C2/C3. */
+/* CPU idle: automatic deep C-states on Nehalem/Westmere, HLT elsewhere. */
 #ifndef IDLE_H
 #define IDLE_H
 
-#define IDLE_C1 0
-#define IDLE_C2 1
-#define IDLE_C3 2
-#define IDLE_C2_AP 3
-#define IDLE_C3_AP 4
+#define IDLE_C1     0
+#define IDLE_C1E    1
+#define IDLE_C3     2
+#define IDLE_C6     3
+#define IDLE_AUTO   4
+#define IDLE_C3_AP  5
+#define IDLE_C6_AP  6
 
 void idle_init(void);
 void cpu_idle(void);

+ 4 - 0
kernel.c

@@ -16,6 +16,7 @@
 #include "dhcp.h"
 #include "netlog.h"
 #include "paging.h"
+#include "power.h"
 #include "process.h"
 #include "ramfs.h"
 #include "sched.h"
@@ -377,6 +378,9 @@ void kernel_main(void)
 	boot_val("acpi cpus", acpi_cpu_count());
 	io_init();
 	boot_trace("io");
+	/* P-states/deep C-states first: idle_init() picks its default from
+	 * the power module (deep MWAIT on Nehalem, HLT elsewhere). */
+	power_init();
 	idle_init();
 	thermal_init();
 	/* Board sensors: the ThinkPad EC probe is gated on SMBIOS identifying a

+ 577 - 0
power.c

@@ -0,0 +1,577 @@
+/* Intel Nehalem/Westmere power management: P-states and deep C-states.
+ *
+ * The kernel has no ACPI interpreter and no cpufreq driver, so the CPU used
+ * to stay at whatever P-state the BIOS left (the maximum non-turbo ratio)
+ * and only ever reached C1 (HLT).  An idle X201 therefore ran at 2.4 GHz
+ * with all four threads in C1, which kept the package near 65 C and the EC
+ * fan around 3700 RPM.
+ *
+ * This module adds the two things the hardware needs from the OS:
+ *  - a small race-to-idle P-state governor writing IA32_PERF_CTL (lowest
+ *    ratio for idle CPUs, maximum non-turbo ratio for a CPU that is running
+ *    something, linear in between for mixed windows), and
+ *  - deep C-state hints for idle.c (C1E/C3/C6 through MONITOR/MWAIT).
+ *
+ * Everything is gated on the Nehalem/Westmere model list: those are the
+ * parts known to implement the MSRs used here, and the Atom boards keep
+ * their old HLT behaviour.  The module only touches documented MSRs.
+ */
+#include "power.h"
+#include "cpu.h"
+#include "idle.h"
+#include "io.h"
+#include "sched.h"
+#include "serial.h"
+
+#define MSR_IA32_PERF_STATUS   0x198
+#define MSR_IA32_PERF_CTL      0x199
+#define MSR_PLATFORM_INFO      0x0CE
+#define MSR_MISC_ENABLE        0x1A0
+#define MSR_MISC_PWR_MGMT      0x1AA
+#define MSR_POWER_CTL          0x1FC
+#define MSR_PKG_CST_CONFIG     0x0E2
+#define MSR_PKG_C3_RESIDENCY   0x3F8
+#define MSR_PKG_C6_RESIDENCY   0x3F9
+#define MSR_CORE_C3_RESIDENCY  0x3FC
+#define MSR_CORE_C6_RESIDENCY  0x3FD
+#define MSR_APERF              0x0E8
+#define MSR_MPERF              0x0E7
+#define MSR_TSC                0x010
+
+/* P-state ratios are the bus-ratio multipliers (133.33 MHz on Arrandale). */
+#define BCLK_MHZ 133u
+
+struct power_cpu {
+	/* Governor state (100 ms windows). */
+	unsigned int div;              /* ms in this window */
+	unsigned int busy_ms;          /* ms this CPU ran a non-idle proc */
+	unsigned int streak_ms;        /* consecutive busy ms */
+	unsigned int req;              /* last ratio written */
+
+	/* Once-a-second statistics. */
+	unsigned int stat_div;
+	int have_last;
+	unsigned long long aperf, mperf, c3, c6, tsc;
+	unsigned int mhz;
+	unsigned int ratio;
+	unsigned int status;           /* raw IA32_PERF_STATUS[15:0] */
+	unsigned int pc3, pc6;         /* permille */
+};
+
+static struct power_cpu pcs[MAX_CPUS];
+
+static int supported;
+static int has_arat;
+static int eist_on;
+static int turbo_disabled;
+static int c1e_on;
+static unsigned int cst_limit;
+static int cst_locked;
+static unsigned int min_ratio = 6;
+static unsigned int max_ratio;
+static unsigned int ratio_shift;
+static unsigned int nominal_mhz;
+static int cpufreq_mode = POWER_CPUFREQ_AUTO;
+static unsigned int fixed_ratio;
+static unsigned long long pkg_c3_last, pkg_c6_last;
+static unsigned int pkg_pc3, pkg_pc6;
+static int pkg_have_last;
+
+static void cpuid_leaf(unsigned int leaf, unsigned int *a, unsigned int *b,
+		       unsigned int *c, unsigned int *d)
+{
+	__asm__ volatile ("cpuid"
+			  : "=a"(*a), "=b"(*b), "=c"(*c), "=d"(*d)
+			  : "a"(leaf), "c"(0));
+}
+
+static void rdmsr(unsigned int msr, unsigned int *lo, unsigned int *hi)
+{
+	__asm__ volatile ("rdmsr" : "=a"(*lo), "=d"(*hi) : "c"(msr));
+}
+
+static void wrmsr(unsigned int msr, unsigned int lo, unsigned int hi)
+{
+	__asm__ volatile ("wrmsr" : : "c"(msr), "a"(lo), "d"(hi));
+}
+
+static unsigned long long rdmsr64(unsigned int msr)
+{
+	unsigned int lo, hi;
+
+	rdmsr(msr, &lo, &hi);
+	return ((unsigned long long)hi << 32) | lo;
+}
+
+/* Model allowlist: the Nehalem/Westmere family that implements the MSRs
+ * above.  Bonnell (0x1C/0x1D) is deliberately absent. */
+static int power_model(unsigned int model)
+{
+	switch (model) {
+	case 0x1A:   /* Nehalem */
+	case 0x1E:   /* Lynnfield/Clarksfield */
+	case 0x1F:   /* Nehalem-EX */
+	case 0x25:   /* Arrandale/Clarkdale (X201 i5 M 520) */
+	case 0x2C:   /* Westmere-EP */
+	case 0x2E:   /* Nehalem-EX */
+	case 0x2F:   /* Westmere-EX */
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+/* Write one target ratio.  Nehalem/Westmere use bits 15:8 of
+ * IA32_PERF_CTL for the target ratio; the low byte is reserved and written
+ * as zero (coreboot does the same). */
+static void power_write_ratio(unsigned int ratio)
+{
+	unsigned int lo, hi;
+
+	if (ratio < min_ratio)
+		ratio = min_ratio;
+	if (ratio > max_ratio)
+		ratio = max_ratio;
+	rdmsr(MSR_IA32_PERF_CTL, &lo, &hi);
+	if (ratio_shift) {
+		/* Bits 15:8 carry the ratio and 7:0 are reserved (the old VID
+		 * slot); coreboot writes them as zero.  Keeping the BIOS VID
+		 * byte here made the request silently ineffective. */
+		lo = (lo & 0xFFFF0000u) | (ratio << 8);
+	} else {
+		lo = (lo & ~0xFFu) | ratio;
+	}
+	wrmsr(MSR_IA32_PERF_CTL, lo, hi);
+}
+
+static void power_apply(struct power_cpu *p, unsigned int ratio)
+{
+	if (ratio < min_ratio)
+		ratio = min_ratio;
+	if (ratio > max_ratio)
+		ratio = max_ratio;
+	if (ratio == p->req)
+		return;
+	power_write_ratio(ratio);
+	p->req = ratio;
+}
+
+void power_init(void)
+{
+	unsigned int max_leaf, a, b, c, d;
+	unsigned int family, model;
+	unsigned int lo, hi;
+
+	supported = 0;
+	has_arat = 0;
+	cpufreq_mode = POWER_CPUFREQ_AUTO;
+	if (!cpu_has_cpuid()) {
+		serial_puts(SERIAL_COM1, "power: no CPUID, disabled\n");
+		return;
+	}
+
+	cpuid_leaf(0, &max_leaf, &b, &c, &d);
+	if (max_leaf < 1) {
+		serial_puts(SERIAL_COM1, "power: no CPUID leaf 1\n");
+		return;
+	}
+	cpuid_leaf(1, &a, &b, &c, &d);
+	family = (a >> 8) & 0xF;
+	model = (a >> 4) & 0xF;
+	if (family == 0xF)
+		family += (a >> 20) & 0xFF;
+	if (family == 0x6 || family == 0xF)
+		model += ((a >> 16) & 0xF) << 4;
+	if (family != 0x6 || !power_model(model)) {
+		serial_puts(SERIAL_COM1, "power: CPU family ");
+		serial_putdec(SERIAL_COM1, family);
+		serial_puts(SERIAL_COM1, " model ");
+		serial_putdec(SERIAL_COM1, model);
+		serial_puts(SERIAL_COM1, ", no Nehalem/Westmere PM\n");
+		return;
+	}
+
+	rdmsr(MSR_PLATFORM_INFO, &lo, &hi);
+	max_ratio = (lo >> 8) & 0xFF;
+	min_ratio = (hi >> 8) & 0xFF;
+	if (max_ratio < 5 || max_ratio > 60 || min_ratio < 3 ||
+	    min_ratio > max_ratio) {
+		serial_puts(SERIAL_COM1, "power: implausible platform info, disabled\n");
+		return;
+	}
+	nominal_mhz = max_ratio * BCLK_MHZ + max_ratio / 3;
+
+	/* Nehalem/Westmere keep the target ratio in bits 15:8 of
+	 * IA32_PERF_CTL/STATUS; the low byte is the (now unused) VID field.
+	 * coreboot's set_max_ratio writes (ratio << 8) for this family, and
+	 * reading the low byte on the X201 gives VID-looking values (0x11,
+	 * 0x18) that would otherwise masquerade as ratios. */
+	rdmsr(MSR_IA32_PERF_STATUS, &lo, &hi);
+	ratio_shift = 8;
+
+	if (max_leaf >= 6) {
+		cpuid_leaf(6, &a, &b, &c, &d);
+		has_arat = (a & 4u) ? 1 : 0;
+	}
+
+	rdmsr(MSR_MISC_ENABLE, &lo, &hi);
+	eist_on = (lo >> 16) & 1;
+	turbo_disabled = (hi >> 6) & 1;
+	if (!eist_on) {
+		wrmsr(MSR_MISC_ENABLE, lo | (1u << 16), hi);
+		eist_on = 1;
+	}
+
+	rdmsr(MSR_POWER_CTL, &lo, &hi);
+	c1e_on = (lo >> 1) & 1;
+	if (!c1e_on) {
+		wrmsr(MSR_POWER_CTL, lo | (1u << 1), hi);
+		c1e_on = 1;
+	}
+
+	rdmsr(MSR_PKG_CST_CONFIG, &lo, &hi);
+	cst_limit = lo & 7u;
+	cst_locked = (lo >> 15) & 1;
+	if (!cst_locked && cst_limit < 3) {
+		wrmsr(MSR_PKG_CST_CONFIG, (lo & ~7u) | 3u, hi);
+		cst_limit = 3;
+	}
+
+	rdmsr(MSR_MISC_PWR_MGMT, &lo, &hi);
+	supported = 1;
+
+	/* Start at the maximum non-turbo ratio; the governor drops to the
+	 * minimum after the first idle window. */
+	rdmsr(MSR_IA32_PERF_STATUS, &lo, &hi);
+	serial_puts(SERIAL_COM1, "power: perf status ");
+	serial_puthex(SERIAL_COM1, lo);
+	pcs[0].req = max_ratio;
+	power_write_ratio(max_ratio);
+	rdmsr(MSR_IA32_PERF_CTL, &lo, &hi);
+	serial_puts(SERIAL_COM1, ", perf ctl ");
+	serial_puthex(SERIAL_COM1, lo);
+	rdmsr(MSR_IA32_PERF_STATUS, &lo, &hi);
+	serial_puts(SERIAL_COM1, ", status now ");
+	serial_puthex(SERIAL_COM1, lo);
+	serial_puts(SERIAL_COM1, "\n");
+	pcs[0].aperf = rdmsr64(MSR_APERF);
+	pcs[0].mperf = rdmsr64(MSR_MPERF);
+	pcs[0].c3 = rdmsr64(MSR_CORE_C3_RESIDENCY);
+	pcs[0].c6 = rdmsr64(MSR_CORE_C6_RESIDENCY);
+	pcs[0].tsc = rdmsr64(MSR_TSC);
+	pcs[0].have_last = 1;
+	pkg_c3_last = rdmsr64(MSR_PKG_C3_RESIDENCY);
+	pkg_c6_last = rdmsr64(MSR_PKG_C6_RESIDENCY);
+	pkg_have_last = 1;
+
+	serial_puts(SERIAL_COM1, "power: Nehalem/Westmere, ratios ");
+	serial_putdec(SERIAL_COM1, min_ratio);
+	serial_puts(SERIAL_COM1, "..");
+	serial_putdec(SERIAL_COM1, max_ratio);
+	serial_puts(SERIAL_COM1, " (");
+	serial_putdec(SERIAL_COM1, nominal_mhz);
+	serial_puts(SERIAL_COM1, " MHz), perfctl shift ");
+	serial_putdec(SERIAL_COM1, ratio_shift);
+	serial_puts(SERIAL_COM1, "\npower: eist ");
+	serial_puts(SERIAL_COM1, eist_on ? "on" : "off");
+	serial_puts(SERIAL_COM1, ", turbo ");
+	serial_puts(SERIAL_COM1, turbo_disabled ? "disabled" : "enabled");
+	serial_puts(SERIAL_COM1, ", c1e ");
+	serial_puts(SERIAL_COM1, c1e_on ? "on" : "off");
+	serial_puts(SERIAL_COM1, ", cst ");
+	serial_putdec(SERIAL_COM1, cst_limit);
+	serial_puts(SERIAL_COM1, cst_locked ? " locked" : " unlocked");
+	serial_puts(SERIAL_COM1, ", arat ");
+	serial_puts(SERIAL_COM1, has_arat ? "yes" : "no");
+	serial_puts(SERIAL_COM1, ", pwr mgmt ");
+	serial_puthex(SERIAL_COM1, lo);
+	serial_puts(SERIAL_COM1, "\n");
+}
+
+int power_idle_hint(void)
+{
+	if (!supported)
+		return -1;
+	/* An AP can only use C3/C6 when its local APIC timer keeps running
+	 * there (ARAT); otherwise C1E is the deepest safe state and the BSP
+	 * PIT still wakes it up via IPIs. */
+	if (this_cpu()->id != 0 && !has_arat)
+		return 0x01;
+	return 0x20;   /* C6 hint: the CPU uses the deepest state it has */
+}
+
+void power_tick(int idle, unsigned int ms)
+{
+	struct cpu *c = this_cpu();
+	struct power_cpu *p;
+	unsigned int load, r;
+
+	if (!supported || c->id >= MAX_CPUS || ms == 0)
+		return;
+	p = &pcs[c->id];
+
+	/* ---- P-state governor (100 ms windows) ---- */
+	if (cpufreq_mode == POWER_CPUFREQ_AUTO) {
+		if (idle) {
+			p->streak_ms = 0;
+		} else {
+			p->busy_ms += ms;
+			if (p->streak_ms < 1000)
+				p->streak_ms += ms;
+			/* A short busy streak means interactive work or a
+			 * command: jump to max immediately, then let the next
+			 * window decide. */
+			if (p->streak_ms >= 4 && p->req != max_ratio)
+				power_apply(p, max_ratio);
+		}
+		p->div += ms;
+		if (p->div >= 100) {
+			load = p->busy_ms * 100u / p->div;
+			p->div = 0;
+			p->busy_ms = 0;
+			if (load < 15)
+				r = min_ratio;
+			else if (load > 80)
+				r = max_ratio;
+			else
+				r = min_ratio +
+				    (max_ratio - min_ratio) * load / 100u;
+			power_apply(p, r);
+		}
+	} else if (cpufreq_mode != POWER_CPUFREQ_OFF) {
+		if (cpufreq_mode == POWER_CPUFREQ_MIN)
+			r = min_ratio;
+		else if (cpufreq_mode == POWER_CPUFREQ_MAX)
+			r = max_ratio;
+		else
+			r = fixed_ratio;
+		power_apply(p, r);
+	}
+
+	/* ---- once-a-second residency/clock statistics ---- */
+	p->stat_div += ms;
+	if (p->stat_div < 1000)
+		return;
+	p->stat_div = 0;
+	{
+		unsigned long long aperf = rdmsr64(MSR_APERF);
+		unsigned long long mperf = rdmsr64(MSR_MPERF);
+		unsigned long long tsc = rdmsr64(MSR_TSC);
+		unsigned long long c3 = rdmsr64(MSR_CORE_C3_RESIDENCY);
+		unsigned long long c6 = rdmsr64(MSR_CORE_C6_RESIDENCY);
+		unsigned int dtsc, d3, d6, da, dm;
+
+		if (!p->have_last) {
+			p->aperf = aperf;
+			p->mperf = mperf;
+			p->c3 = c3;
+			p->c6 = c6;
+			p->tsc = tsc;
+			p->have_last = 1;
+			return;
+		}
+		dtsc = (unsigned int)((tsc - p->tsc) >> 12);
+		d3 = (unsigned int)((c3 - p->c3) >> 12);
+		d6 = (unsigned int)((c6 - p->c6) >> 12);
+		da = (unsigned int)((aperf - p->aperf) >> 12);
+		dm = (unsigned int)((mperf - p->mperf) >> 12);
+		p->aperf = aperf;
+		p->mperf = mperf;
+		p->c3 = c3;
+		p->c6 = c6;
+		p->tsc = tsc;
+
+		p->pc3 = dtsc ? d3 * 1000u / dtsc : 0;
+		p->pc6 = dtsc ? d6 * 1000u / dtsc : 0;
+		if (p->pc3 > 1000)
+			p->pc3 = 1000;
+		if (p->pc6 > 1000)
+			p->pc6 = 1000;
+		p->mhz = dm ? da * nominal_mhz / dm : 0;
+		/* IA32_PERF_STATUS reports the current ratio in bits 7:0
+		 * (the boot dump shows 0x12 = 18 at the max non-turbo ratio),
+		 * unlike PERF_CTL, which takes the target in bits 15:8. */
+		rdmsr(MSR_IA32_PERF_STATUS, &da, &dm);
+		p->status = da & 0xFFFFu;
+		p->ratio = p->status & 0xFFu;
+
+		if (c->id == 0) {
+			unsigned long long pc3 = rdmsr64(MSR_PKG_C3_RESIDENCY);
+			unsigned long long pc6 = rdmsr64(MSR_PKG_C6_RESIDENCY);
+
+			if (pkg_have_last) {
+				pkg_pc3 = dtsc ?
+					(unsigned int)((pc3 - pkg_c3_last) >> 12) *
+					1000u / dtsc : 0;
+				pkg_pc6 = dtsc ?
+					(unsigned int)((pc6 - pkg_c6_last) >> 12) *
+					1000u / dtsc : 0;
+				if (pkg_pc3 > 1000)
+					pkg_pc3 = 1000;
+				if (pkg_pc6 > 1000)
+					pkg_pc6 = 1000;
+			}
+			pkg_c3_last = pc3;
+			pkg_c6_last = pc6;
+			pkg_have_last = 1;
+		}
+	}
+}
+
+const char *power_cpufreq_name(void)
+{
+	switch (cpufreq_mode) {
+	case POWER_CPUFREQ_MIN:
+		return "min";
+	case POWER_CPUFREQ_MAX:
+		return "max";
+	case POWER_CPUFREQ_OFF:
+		return "off";
+	case POWER_CPUFREQ_FIXED:
+		return "fixed";
+	default:
+		return "auto";
+	}
+}
+
+static void put_dec(unsigned int v)
+{
+	char buf[12];
+	unsigned int n = 0;
+
+	if (!v) {
+		io_putchar('0');
+		return;
+	}
+	while (v && n < sizeof(buf)) {
+		buf[n++] = (char)('0' + v % 10);
+		v /= 10;
+	}
+	while (n)
+		io_putchar(buf[--n]);
+}
+
+static void put_pct(unsigned int permille)
+{
+	put_dec(permille / 10);
+	io_putchar('.');
+	io_putchar((char)('0' + permille % 10));
+	io_putchar('%');
+}
+
+void power_print(void)
+{
+	unsigned int i;
+
+	if (!supported) {
+		io_putstr("power: not available on this CPU\n");
+		return;
+	}
+	io_putstr("power: nehalem/westmere, idle ");
+	io_putstr(idle_mode_name(idle_mode()));
+	io_putstr(", cpufreq ");
+	io_putstr(power_cpufreq_name());
+	if (cpufreq_mode == POWER_CPUFREQ_FIXED) {
+		io_putchar(' ');
+		put_dec(fixed_ratio);
+	}
+	io_putstr(" (ratios ");
+	put_dec(min_ratio);
+	io_putstr("..");
+	put_dec(max_ratio);
+	io_putstr(")\n");
+	io_putstr("cst ");
+	put_dec(cst_limit);
+	io_putstr(cst_locked ? " locked" : " unlocked");
+	io_putstr(", c1e ");
+	io_putstr(c1e_on ? "on" : "off");
+	io_putstr(", arat ");
+	io_putstr(has_arat ? "yes\n" : "no\n");
+	io_putstr("pkg c3 ");
+	put_pct(pkg_pc3);
+	io_putstr("  c6 ");
+	put_pct(pkg_pc6);
+	io_putchar('\n');
+	for (i = 0; i < cpu_count && i < MAX_CPUS; i++) {
+		struct power_cpu *p = &pcs[i];
+
+		io_putstr("cpu");
+		put_dec(i);
+		io_putchar(' ');
+		put_dec(p->mhz);
+		io_putstr(" MHz  cur r");
+		if (p->ratio >= min_ratio && p->ratio <= max_ratio + 8) {
+			if (p->ratio < 10)
+				io_putchar('0');
+			put_dec(p->ratio);
+		} else {
+			io_putstr("--");
+		}
+		io_putstr("  req r");
+		if (p->req < 10)
+			io_putchar('0');
+		put_dec(p->req);
+		io_putstr("  c3 ");
+		put_pct(p->pc3);
+		io_putstr("  c6 ");
+		put_pct(p->pc6);
+		io_putchar('\n');
+	}
+}
+
+int power_cpufreq_command(const char *arg)
+{
+	unsigned int ratio = 0;
+	int have_ratio = 0;
+
+	while (*arg == ' ')
+		arg++;
+	if (!supported) {
+		io_putstr("cpufreq: not available on this CPU\n");
+		return -1;
+	}
+	if (!*arg) {
+		io_putstr("cpufreq ");
+		io_putstr(power_cpufreq_name());
+		if (cpufreq_mode == POWER_CPUFREQ_FIXED) {
+			io_putchar(' ');
+			put_dec(fixed_ratio);
+		}
+		io_putchar('\n');
+		return 0;
+	}
+	if (arg[0] == 'a' && arg[1] == 'u' && arg[2] == 't' &&
+	    arg[3] == 'o' && !arg[4]) {
+		cpufreq_mode = POWER_CPUFREQ_AUTO;
+	} else if (arg[0] == 'm' && arg[1] == 'i' && arg[2] == 'n' &&
+		   !arg[3]) {
+		cpufreq_mode = POWER_CPUFREQ_MIN;
+	} else if (arg[0] == 'm' && arg[1] == 'a' && arg[2] == 'x' &&
+		   !arg[3]) {
+		cpufreq_mode = POWER_CPUFREQ_MAX;
+	} else if (arg[0] == 'o' && arg[1] == 'f' && arg[2] == 'f' &&
+		   !arg[3]) {
+		cpufreq_mode = POWER_CPUFREQ_OFF;
+	} else {
+		while (*arg >= '0' && *arg <= '9') {
+			ratio = ratio * 10u + (unsigned int)(*arg - '0');
+			arg++;
+			have_ratio = 1;
+		}
+		if (!have_ratio || *arg || ratio < min_ratio ||
+		    ratio > max_ratio) {
+			io_putstr("cpufreq: usage cpufreq [auto|min|max|off|<ratio>]\n");
+			return -1;
+		}
+		fixed_ratio = ratio;
+		cpufreq_mode = POWER_CPUFREQ_FIXED;
+	}
+	io_putstr("cpufreq ");
+	io_putstr(power_cpufreq_name());
+	if (cpufreq_mode == POWER_CPUFREQ_FIXED) {
+		io_putchar(' ');
+		put_dec(fixed_ratio);
+	}
+	io_putchar('\n');
+	return 0;
+}

+ 28 - 0
power.h

@@ -0,0 +1,28 @@
+/* Intel Nehalem/Westmere P-state and deep C-state management. */
+#ifndef POWER_H
+#define POWER_H
+
+#define POWER_CPUFREQ_AUTO  0
+#define POWER_CPUFREQ_MIN   1
+#define POWER_CPUFREQ_MAX   2
+#define POWER_CPUFREQ_FIXED 3
+#define POWER_CPUFREQ_OFF   4
+
+/* Probe the CPU and MSRs, enable SpeedStep/C1E/C6 and start at the maximum
+ * non-turbo ratio.  Called once on the BSP before idle_init(); logs to COM1
+ * and is a no-op on CPUs outside the Nehalem/Westmere allowlist. */
+void power_init(void);
+
+/* Once per scheduler tick with the CPU's idle flag and the tick length. */
+void power_tick(int idle, unsigned int ms);
+
+/* Deep C-state hint for idle.c's automatic mode: -1 = use HLT, otherwise
+ * the MWAIT hint (0x20 C6; 0x01 C1E on an AP whose local timer stops). */
+int power_idle_hint(void);
+
+/* `power` / `cpufreq` command support (terminal.c). */
+void power_print(void);
+int power_cpufreq_command(const char *arg);
+const char *power_cpufreq_name(void);
+
+#endif

+ 3 - 0
sched.c

@@ -21,6 +21,7 @@
 #include "smp.h"
 #include "spinlock.h"
 #include "syscall.h"
+#include "power.h"
 #include "thermal.h"
 #include "timer.h"
 #include "tss.h"
@@ -764,6 +765,7 @@ int sched_tick_bsp(void)
 
 	thermal_tick();
 	p = c->current;
+	power_tick(p == c->idle, timer_tick_ms());
 	wake_sleepers_locked();
 	r = tick_decide_locked(c, p, 0, timer_tick_ms());
 	sched_lock_release(flags);
@@ -781,6 +783,7 @@ int sched_tick_ap(void)
 
 	thermal_tick();
 	p = c->current;
+	power_tick(p == c->idle, c->tick_ms);
 	r = tick_decide_locked(c, p, 1, c->tick_ms);
 	if (!r && p == c->idle && c->tick_ms != 10) {
 		/* Entered idle without a schedule(): stretch this CPU's LAPIC

+ 33 - 6
terminal.c

@@ -2,6 +2,7 @@
    Commands are spawned as separate processes for preemptive multitasking. */
 
 #include "io.h"
+#include "power.h"
 #include "bootcon.h"
 #include "dbg.h"
 #include "idle.h"
@@ -93,7 +94,9 @@ static int cmd_dbg(struct term *t, const char *args);
 static int cmd_format(struct term *t, const char *args);
 static int cmd_help(struct term *t, const char *args);
 static int cmd_idle(struct term *t, const char *args);
+static int cmd_cpufreq(struct term *t, const char *args);
 static int cmd_path(struct term *t, const char *args);
+static int cmd_power(struct term *t, const char *args);
 static int cmd_kexec(struct term *t, const char *args);
 static int cmd_kill(struct term *t, const char *args);
 static int cmd_poweroff(struct term *t, const char *args);
@@ -127,7 +130,9 @@ static const struct kcmd cmds[] = {
 	{ "poke",       "poke <addr> <val>",             CK_SPAWN_ARG, CMD_REQARG, 0, program_poke_main, 0, 0 },
 	{ "ps",         "ps",                            CK_CUSTOM, 0, 0, 0, 0, cmd_ps },
 	{ "sched",      "sched",                         CK_CUSTOM, 0, 0, 0, 0, cmd_sched },
-	{ "idle",       "idle [hlt|c2|c3|c2ap|c3ap]",    CK_CUSTOM, 0, 0, 0, 0, cmd_idle },
+	{ "power",      "power",                         CK_CUSTOM, 0, 0, 0, 0, cmd_power },
+	{ "cpufreq",    "cpufreq [auto|min|max|off|n]",  CK_CUSTOM, 0, 0, 0, 0, cmd_cpufreq },
+	{ "idle",       "idle [auto|hlt|c1e|c3|c6]",     CK_CUSTOM, 0, 0, 0, 0, cmd_idle },
 	{ "renice",     "renice <pid> <weight>",         CK_CUSTOM, CMD_REQARG, 0, 0, 0, cmd_renice },
 	{ "kill",       "kill <pid>",                    CK_CUSTOM, 0, 0, 0, 0, cmd_kill },
 	{ "kexec",      "kexec",                         CK_CUSTOM, 0, 0, 0, 0, cmd_kexec },
@@ -547,6 +552,11 @@ static int cmd_idle(struct term *t, const char *args)
 		io_putchar('\n');
 		return 0;
 	}
+	if (arg_is(args, "auto")) {
+		idle_set_mode(IDLE_AUTO);
+		io_putstr("idle auto\n");
+		return 0;
+	}
 	if (arg_is(args, "hlt")) {
 		idle_set_mode(IDLE_C1);
 		io_putstr("idle hlt\n");
@@ -555,14 +565,16 @@ static int cmd_idle(struct term *t, const char *args)
 	{
 		int mode = -1;
 
-		if (arg_is(args, "c2"))
-			mode = IDLE_C2;
+		if (arg_is(args, "c1e"))
+			mode = IDLE_C1E;
 		else if (arg_is(args, "c3"))
 			mode = IDLE_C3;
-		else if (arg_is(args, "c2ap"))
-			mode = IDLE_C2_AP;
+		else if (arg_is(args, "c6"))
+			mode = IDLE_C6;
 		else if (arg_is(args, "c3ap"))
 			mode = IDLE_C3_AP;
+		else if (arg_is(args, "c6ap"))
+			mode = IDLE_C6_AP;
 		if (mode >= 0) {
 			if (idle_set_mode(mode) < 0) {
 				io_putstr("idle: mwait not available\n");
@@ -574,7 +586,22 @@ static int cmd_idle(struct term *t, const char *args)
 			return 0;
 		}
 	}
-	io_putstr("idle: usage idle [hlt|c2|c3|c2ap|c3ap]\n");
+	io_putstr("idle: usage idle [auto|hlt|c1e|c3|c6|c3ap|c6ap]\n");
+	return 0;
+}
+
+static int cmd_power(struct term *t, const char *args)
+{
+	(void)t;
+	(void)args;
+	power_print();
+	return 0;
+}
+
+static int cmd_cpufreq(struct term *t, const char *args)
+{
+	(void)t;
+	power_cpufreq_command(args);
 	return 0;
 }
 

+ 1 - 1
thermal.c

@@ -214,7 +214,7 @@ out:
 		serial_putdec(SERIAL_COM1, (unsigned int)thermal_tjmax_c);
 		serial_puts(SERIAL_COM1, " C");
 		if (msr_probed) {
-			serial_puts(SERIAL_COM1, " (msr 0x1A2=0x");
+			serial_puts(SERIAL_COM1, " (msr 0x1A2=");
 			serial_puthex(SERIAL_COM1, thermal_tjmax_raw);
 			serial_puts(SERIAL_COM1,
 				    thermal_tjmax_msr ? ")" : ", out of range)");

+ 1 - 1
tpec.c

@@ -299,7 +299,7 @@ void tpec_init(void)
 	log_info();
 	serial_puts(SERIAL_COM1, ": EC ok, fw rev ");
 	serial_putdec(SERIAL_COM1, tpec_ec_rev);
-	serial_puts(SERIAL_COM1, ", fan level 0x");
+	serial_puts(SERIAL_COM1, ", fan level ");
 	serial_puthex(SERIAL_COM1, probe);
 	serial_puts(SERIAL_COM1, "\n");
 }