From: kettenis <kettenis@openbsd.org>
Date: Wed, 13 Jul 2022 09:28:18 +0000 (+0000)
Subject: Implement the fundamentals for suspend/resume on arm64.  This uses PSCI
X-Git-Url: http://artulab.com/gitweb/?a=commitdiff_plain;h=4002e08df2479919957b4d3cf5ed718bd9709133;p=openbsd

Implement the fundamentals for suspend/resume on arm64.  This uses PSCI
to turn off the secondary CPUs and suspend the primary CPU using the
CPU_OFF and SYSTEM_SUSPEND calls.  A new "halt" IPI is added to turn off
the ssecondary CPUs.  This IPI is implemented for the ampintc(4) and
agintc(4) interrupt controllers.  Fulle suspend/resume support is only
implemented for ampintc(4).  This is enough to suspend and resume boards
based on the Allwinner A64 SoC, provided the necessary wakeup interrupts
have been set up (not part of this commit).

ok patrick@
---

diff --git a/sys/arch/arm64/arm64/cpu.c b/sys/arch/arm64/arm64/cpu.c
index dee42ec52ad..59eee39d0d9 100644
--- a/sys/arch/arm64/arm64/cpu.c
+++ b/sys/arch/arm64/arm64/cpu.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: cpu.c,v 1.64 2022/07/12 03:55:34 jsg Exp $	*/
+/*	$OpenBSD: cpu.c,v 1.65 2022/07/13 09:28:18 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2016 Dale Rahn <drahn@dalerahn.com>
@@ -24,6 +24,7 @@
 #include <sys/device.h>
 #include <sys/sysctl.h>
 #include <sys/task.h>
+#include <sys/user.h>
 
 #include <uvm/uvm.h>
 
@@ -609,7 +610,8 @@ cpu_identify(struct cpu_info *ci)
 #endif
 }
 
-int	cpu_hatch_secondary(struct cpu_info *ci, int, uint64_t);
+void	cpu_init(void);
+int	cpu_start_secondary(struct cpu_info *ci, int, uint64_t);
 int	cpu_clockspeed(int *);
 
 int
@@ -634,10 +636,10 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 {
 	struct fdt_attach_args *faa = aux;
 	struct cpu_info *ci;
+	void *kstack;
 #ifdef MULTIPROCESSOR
 	uint64_t mpidr = READ_SPECIALREG(mpidr_el1);
 #endif
-	uint64_t id_aa64mmfr1, sctlr;
 	uint32_t opp;
 
 	KASSERT(faa->fa_nreg > 0);
@@ -666,6 +668,9 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 
 	printf(" mpidr %llx:", ci->ci_mpidr);
 
+	kstack = km_alloc(USPACE, &kv_any, &kp_zero, &kd_waitok);
+	ci->ci_el1_stkend = (vaddr_t)kstack + USPACE - 16;
+
 #ifdef MULTIPROCESSOR
 	if (ci->ci_flags & CPUF_AP) {
 		char buf[32];
@@ -685,7 +690,7 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 		}
 
 		sched_init_cpu(ci);
-		if (cpu_hatch_secondary(ci, spinup_method, spinup_data)) {
+		if (cpu_start_secondary(ci, spinup_method, spinup_data)) {
 			atomic_setbits_int(&ci->ci_flags, CPUF_IDENTIFY);
 			__asm volatile("dsb sy; sev" ::: "memory");
 
@@ -712,17 +717,7 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 			cpu_cpuspeed = cpu_clockspeed;
 		}
 
-		/* Enable PAN. */
-		id_aa64mmfr1 = READ_SPECIALREG(id_aa64mmfr1_el1);
-		if (ID_AA64MMFR1_PAN(id_aa64mmfr1) >= ID_AA64MMFR1_PAN_IMPL) {
-			sctlr = READ_SPECIALREG(sctlr_el1);
-			sctlr &= ~SCTLR_SPAN;
-			WRITE_SPECIALREG(sctlr_el1, sctlr);
-		}
-
-		/* Initialize debug registers. */
-		WRITE_SPECIALREG(mdscr_el1, DBG_MDSCR_TDCC);
-		WRITE_SPECIALREG(oslar_el1, 0);
+		cpu_init();
 #ifdef MULTIPROCESSOR
 	}
 #endif
@@ -734,6 +729,34 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 	printf("\n");
 }
 
+void
+cpu_init(void)
+{
+	uint64_t id_aa64mmfr1, sctlr;
+	uint64_t tcr;
+
+	WRITE_SPECIALREG(ttbr0_el1, pmap_kernel()->pm_pt0pa);
+	__asm volatile("isb");
+	tcr = READ_SPECIALREG(tcr_el1);
+	tcr &= ~TCR_T0SZ(0x3f);
+	tcr |= TCR_T0SZ(64 - USER_SPACE_BITS);
+	tcr |= TCR_A1;
+	WRITE_SPECIALREG(tcr_el1, tcr);
+	cpu_tlb_flush();
+
+	/* Enable PAN. */
+	id_aa64mmfr1 = READ_SPECIALREG(id_aa64mmfr1_el1);
+	if (ID_AA64MMFR1_PAN(id_aa64mmfr1) >= ID_AA64MMFR1_PAN_IMPL) {
+		sctlr = READ_SPECIALREG(sctlr_el1);
+		sctlr &= ~SCTLR_SPAN;
+		WRITE_SPECIALREG(sctlr_el1, sctlr);
+	}
+
+	/* Initialize debug registers. */
+	WRITE_SPECIALREG(mdscr_el1, DBG_MDSCR_TDCC);
+	WRITE_SPECIALREG(oslar_el1, 0);
+}
+
 void
 cpu_flush_bp_noop(void)
 {
@@ -757,7 +780,7 @@ cpu_clockspeed(int *freq)
 #ifdef MULTIPROCESSOR
 
 void cpu_boot_secondary(struct cpu_info *ci);
-void cpu_hatch(void);
+void cpu_hatch_secondary(void);
 
 void
 cpu_boot_secondary_processors(void)
@@ -777,7 +800,7 @@ cpu_boot_secondary_processors(void)
 }
 
 void
-cpu_hatch_spin_table(struct cpu_info *ci, uint64_t start, uint64_t data)
+cpu_start_spin_table(struct cpu_info *ci, uint64_t start, uint64_t data)
 {
 	/* this reuses the zero page for the core */
 	vaddr_t start_pg = zero_page + (PAGE_SIZE * ci->ci_cpuid);
@@ -794,18 +817,14 @@ cpu_hatch_spin_table(struct cpu_info *ci, uint64_t start, uint64_t data)
 }
 
 int
-cpu_hatch_secondary(struct cpu_info *ci, int method, uint64_t data)
+cpu_start_secondary(struct cpu_info *ci, int method, uint64_t data)
 {
 	extern uint64_t pmap_avail_kvo;
 	extern paddr_t cpu_hatch_ci;
 	paddr_t startaddr;
-	void *kstack;
 	uint64_t ttbr1;
 	int rc = 0;
 
-	kstack = km_alloc(USPACE, &kv_any, &kp_zero, &kd_waitok);
-	ci->ci_el1_stkend = (vaddr_t)kstack + USPACE - 16;
-
 	pmap_extract(pmap_kernel(), (vaddr_t)ci, &cpu_hatch_ci);
 
 	__asm("mrs %x0, ttbr1_el1": "=r"(ttbr1));
@@ -814,7 +833,7 @@ cpu_hatch_secondary(struct cpu_info *ci, int method, uint64_t data)
 	cpu_dcache_wb_range((vaddr_t)&cpu_hatch_ci, sizeof(paddr_t));
 	cpu_dcache_wb_range((vaddr_t)ci, sizeof(*ci));
 
-	startaddr = (vaddr_t)cpu_hatch + pmap_avail_kvo;
+	startaddr = (vaddr_t)cpu_hatch_secondary + pmap_avail_kvo;
 
 	switch (method) {
 	case 1:
@@ -825,7 +844,7 @@ cpu_hatch_secondary(struct cpu_info *ci, int method, uint64_t data)
 		break;
 	case 2:
 		/* spin-table */
-		cpu_hatch_spin_table(ci, startaddr, data);
+		cpu_start_spin_table(ci, startaddr, data);
 		rc = 1;
 		break;
 	default:
@@ -847,45 +866,26 @@ cpu_boot_secondary(struct cpu_info *ci)
 }
 
 void
-cpu_start_secondary(struct cpu_info *ci)
+cpu_init_secondary(struct cpu_info *ci)
 {
-	uint64_t id_aa64mmfr1, sctlr;
-	uint64_t tcr;
 	int s;
 
 	ci->ci_flags |= CPUF_PRESENT;
 	__asm volatile("dsb sy" ::: "memory");
 
-	while ((ci->ci_flags & CPUF_IDENTIFY) == 0)
-		__asm volatile("wfe");
+	if ((ci->ci_flags & CPUF_IDENTIFIED) == 0) {
+		while ((ci->ci_flags & CPUF_IDENTIFY) == 0)
+			__asm volatile("wfe");
 
-	cpu_identify(ci);
-	atomic_setbits_int(&ci->ci_flags, CPUF_IDENTIFIED);
-	__asm volatile("dsb sy" ::: "memory");
+		cpu_identify(ci);
+		atomic_setbits_int(&ci->ci_flags, CPUF_IDENTIFIED);
+		__asm volatile("dsb sy" ::: "memory");
+	}
 
 	while ((ci->ci_flags & CPUF_GO) == 0)
 		__asm volatile("wfe");
 
-	WRITE_SPECIALREG(ttbr0_el1, pmap_kernel()->pm_pt0pa);
-	__asm volatile("isb");
-	tcr = READ_SPECIALREG(tcr_el1);
-	tcr &= ~TCR_T0SZ(0x3f);
-	tcr |= TCR_T0SZ(64 - USER_SPACE_BITS);
-	tcr |= TCR_A1;
-	WRITE_SPECIALREG(tcr_el1, tcr);
-	cpu_tlb_flush();
-
-	/* Enable PAN. */
-	id_aa64mmfr1 = READ_SPECIALREG(id_aa64mmfr1_el1);
-	if (ID_AA64MMFR1_PAN(id_aa64mmfr1) >= ID_AA64MMFR1_PAN_IMPL) {
-		sctlr = READ_SPECIALREG(sctlr_el1);
-		sctlr &= ~SCTLR_SPAN;
-		WRITE_SPECIALREG(sctlr_el1, sctlr);
-	}
-
-	/* Initialize debug registers. */
-	WRITE_SPECIALREG(mdscr_el1, DBG_MDSCR_TDCC);
-	WRITE_SPECIALREG(oslar_el1, 0);
+	cpu_init();
 
 	s = splhigh();
 	arm_intr_cpu_enable();
@@ -902,6 +902,24 @@ cpu_start_secondary(struct cpu_info *ci)
 	cpu_switchto(NULL, sched_chooseproc());
 }
 
+void
+cpu_halt(void)
+{
+	struct cpu_info *ci = curcpu();
+
+	KERNEL_ASSERT_UNLOCKED();
+	SCHED_ASSERT_UNLOCKED();
+
+	intr_disable();
+	ci->ci_flags &= ~CPUF_RUNNING;
+#if NPSCI > 0
+	psci_cpu_off();
+#endif
+	for (;;)
+		__asm volatile("wfi");
+	/* NOTREACHED */
+}
+
 void
 cpu_kick(struct cpu_info *ci)
 {
@@ -924,6 +942,110 @@ cpu_unidle(struct cpu_info *ci)
 
 #endif
 
+#ifdef SUSPEND
+
+void cpu_hatch_primary(void);
+
+label_t cpu_suspend_jmpbuf;
+int cpu_suspended;
+
+void
+cpu_init_primary(void)
+{
+	cpu_init();
+
+	cpu_startclock();
+
+	cpu_suspended = 1;
+	longjmp(&cpu_suspend_jmpbuf);
+}
+
+int
+cpu_suspend_primary(void)
+{
+	extern uint64_t pmap_avail_kvo;
+	struct cpu_info *ci = curcpu();
+	paddr_t startaddr, data;
+	uint64_t ttbr1;
+
+	cpu_suspended = 0;
+	setjmp(&cpu_suspend_jmpbuf);
+	if (cpu_suspended) {
+		/* XXX wait for debug output from SCP on Allwinner A64 */
+		delay(200000);
+		return 0;
+	}
+
+	pmap_extract(pmap_kernel(), (vaddr_t)ci, &data);
+
+	__asm("mrs %x0, ttbr1_el1": "=r"(ttbr1));
+	ci->ci_ttbr1 = ttbr1;
+
+	cpu_dcache_wb_range((vaddr_t)&data, sizeof(paddr_t));
+	cpu_dcache_wb_range((vaddr_t)ci, sizeof(*ci));
+
+	startaddr = (vaddr_t)cpu_hatch_primary + pmap_avail_kvo;
+
+#if NPSCI > 0
+	psci_system_suspend(startaddr, data);
+#endif
+
+	return EOPNOTSUPP;
+}
+
+#ifdef MULTIPROCESSOR
+
+void
+cpu_resume_secondary(struct cpu_info *ci)
+{
+	struct proc *p;
+	struct pcb *pcb;
+	struct trapframe *tf;
+	struct switchframe *sf;
+	int timeout = 10000;
+
+	ci->ci_curproc = NULL;
+	ci->ci_curpcb = NULL;
+	ci->ci_curpm = NULL;
+	ci->ci_cpl = IPL_NONE;
+	ci->ci_ipending = 0;
+	ci->ci_idepth = 0;
+	ci->ci_flags &= ~CPUF_PRESENT;
+
+	ci->ci_mutex_level = 0;
+	ci->ci_ttbr1 = 0;
+
+	p = ci->ci_schedstate.spc_idleproc;
+	pcb = &p->p_addr->u_pcb;
+
+	tf = (struct trapframe *)((u_long)p->p_addr
+	    + USPACE
+	    - sizeof(struct trapframe)
+	    - 0x10);
+
+	tf = (struct trapframe *)STACKALIGN(tf);
+	pcb->pcb_tf = tf;
+
+	sf = (struct switchframe *)tf - 1;
+	sf->sf_x19 = (uint64_t)sched_idle;
+	sf->sf_x20 = (uint64_t)ci;
+	sf->sf_lr = (uint64_t)proc_trampoline;
+	pcb->pcb_sp = (uint64_t)sf;
+
+	cpu_start_secondary(ci, 1, 0);
+	while ((ci->ci_flags & CPUF_PRESENT) == 0 && --timeout)
+		delay(1000);
+	if (timeout == 0) {
+		printf("%s: failed to spin up\n",
+		    ci->ci_dev->dv_xname);
+		ci->ci_flags = 0;
+	}
+}
+
+#endif
+
+#endif
+
 /*
  * Dynamic voltage and frequency scaling implementation.
  */
diff --git a/sys/arch/arm64/arm64/genassym.cf b/sys/arch/arm64/arm64/genassym.cf
index e640b0eb037..d82fd0a901f 100644
--- a/sys/arch/arm64/arm64/genassym.cf
+++ b/sys/arch/arm64/arm64/genassym.cf
@@ -1,4 +1,4 @@
-#	$OpenBSD: genassym.cf,v 1.7 2021/05/16 10:40:24 jsg Exp $
+#	$OpenBSD: genassym.cf,v 1.8 2022/07/13 09:28:18 kettenis Exp $
 #	$NetBSD: genassym.cf,v 1.27 2003/11/04 10:33:16 dsl Exp$
 
 # Copyright (c) 1982, 1990 The Regents of the University of California.
@@ -44,11 +44,9 @@ member	sf_sc
 
 struct	cpu_info
 member	ci_curproc
-ifdef MULTIPROCESSOR
 member	ci_el1_stkend
 member	ci_ttbr1
 member	ci_self
-endif
 
 struct	proc
 member	p_addr
diff --git a/sys/arch/arm64/arm64/locore.S b/sys/arch/arm64/arm64/locore.S
index 5219af1bec8..650b880be0b 100644
--- a/sys/arch/arm64/arm64/locore.S
+++ b/sys/arch/arm64/arm64/locore.S
@@ -1,4 +1,4 @@
-/* $OpenBSD: locore.S,v 1.39 2021/09/02 10:48:52 kettenis Exp $ */
+/* $OpenBSD: locore.S,v 1.40 2022/07/13 09:28:18 kettenis Exp $ */
 /*-
  * Copyright (c) 2012-2014 Andrew Turner
  * All rights reserved.
@@ -394,8 +394,8 @@ sigfillsiz:
 	.text
 
 #ifdef MULTIPROCESSOR
-	.globl cpu_hatch
-cpu_hatch:
+	.globl cpu_hatch_secondary
+cpu_hatch_secondary:
 	/* Drop to EL1 */
 	bl	drop_to_el1
 
@@ -421,16 +421,14 @@ cpu_hatch:
 	ldr	x1, [x0, #CI_EL1_STKEND]
 	mov	sp, x1
 
-	adr	x1, .Lcpu_start_secondary
+	adr	x1, .Lcpu_init_secondary
 	ldr	x1, [x1]
 	blr	x1
 	b	.
 
 	.align 3
-.Lcpu_start_secondary:
-	.xword	cpu_start_secondary
-.Lpagetable_l0_ttbr0:
-	.xword	pagetable_l0_ttbr0
+.Lcpu_init_secondary:
+	.xword	cpu_init_secondary
 .Lcpu_hatch_ci:
 	.xword	cpu_hatch_ci
 
@@ -442,3 +440,43 @@ cpu_hatch_ci:
 
 	.text
 #endif
+
+#ifdef SUSPEND
+	.globl cpu_hatch_primary
+cpu_hatch_primary:
+	/* Drop to EL1 */
+	bl	drop_to_el1
+
+	/* Get the virt -> phys offset */
+	bl	get_virt_delta
+
+	/* Set up CPU info */
+	ldr	x1, [x0, #CI_SELF]
+	msr	tpidr_el1, x1
+
+	/* Enable the mmu */
+	adr	x27, .Lpagetable_l0_ttbr0
+	ldr	x27, [x27] 
+	sub	x27, x27, x29
+	ldr	x26, [x0, #CI_TTBR1]
+	bl	start_mmu
+
+	mrs	x0, tpidr_el1
+	ldr	x1, [x0, #CI_EL1_STKEND]
+	mov	sp, x1
+
+	/* Restore registers. */
+	adr	x1, .Lcpu_init_primary
+	ldr	x1, [x1]
+	blr	x1
+	b	.
+
+	.align 3
+.Lcpu_init_primary:
+	.xword	cpu_init_primary
+#endif
+
+	.align 3
+.Lpagetable_l0_ttbr0:
+	.xword	pagetable_l0_ttbr0
+	
diff --git a/sys/arch/arm64/conf/GENERIC b/sys/arch/arm64/conf/GENERIC
index 752643e8ce0..b7848aee9d5 100644
--- a/sys/arch/arm64/conf/GENERIC
+++ b/sys/arch/arm64/conf/GENERIC
@@ -1,4 +1,4 @@
-# $OpenBSD: GENERIC,v 1.231 2022/06/26 20:05:06 sthen Exp $
+# $OpenBSD: GENERIC,v 1.232 2022/07/13 09:28:18 kettenis Exp $
 #
 # GENERIC machine description file
 #
@@ -25,6 +25,7 @@ maxusers	80
 option		PCIVERBOSE
 option		USER_PCICONF	# user-space PCI configuration
 option		USBVERBOSE
+option		SUSPEND
 
 makeoptions	KERNEL_BASE_PHYS="0x00200000"
 makeoptions	KERNEL_BASE_VIRT="0xffffff8000200000"
diff --git a/sys/arch/arm64/dev/agintc.c b/sys/arch/arm64/dev/agintc.c
index ec744b0669b..e2c1d77e58b 100644
--- a/sys/arch/arm64/dev/agintc.c
+++ b/sys/arch/arm64/dev/agintc.c
@@ -1,4 +1,4 @@
-/* $OpenBSD: agintc.c,v 1.37 2022/06/16 20:44:09 kettenis Exp $ */
+/* $OpenBSD: agintc.c,v 1.38 2022/07/13 09:28:18 kettenis Exp $ */
 /*
  * Copyright (c) 2007, 2009, 2011, 2017 Dale Rahn <drahn@dalerahn.com>
  * Copyright (c) 2018 Mark Kettenis <kettenis@openbsd.org>
@@ -161,9 +161,9 @@ struct agintc_softc {
 	struct agintc_dmamem	*sc_prop;
 	struct agintc_dmamem	*sc_pend;
 	struct interrupt_controller sc_ic;
-	int			 sc_ipi_num[2]; /* id for NOP and DDB ipi */
-	int			 sc_ipi_reason[MAXCPUS]; /* NOP or DDB caused */
-	void			*sc_ipi_irq[2]; /* irqhandle for each ipi */
+	int			 sc_ipi_num[3]; /* id for each ipi */
+	int			 sc_ipi_reason[MAXCPUS]; /* cause of ipi */
+	void			*sc_ipi_irq[3]; /* irqhandle for each ipi */
 };
 struct agintc_softc *agintc_sc;
 
@@ -234,6 +234,7 @@ void		agintc_r_wait_rwp(struct agintc_softc *sc);
 uint32_t	agintc_r_ictlr(void);
 
 int		agintc_ipi_ddb(void *v);
+int		agintc_ipi_halt(void *v);
 int		agintc_ipi_nop(void *v);
 int		agintc_ipi_combined(void *);
 void		agintc_send_ipi(struct cpu_info *, int);
@@ -287,7 +288,7 @@ agintc_attach(struct device *parent, struct device *self, void *aux)
 	int			 i, nbits, nintr;
 	int			 offset, nredist;
 #ifdef MULTIPROCESSOR
-	int			 nipi, ipiirq[2];
+	int			 nipi, ipiirq[3];
 #endif
 
 	psw = intr_disable();
@@ -540,9 +541,10 @@ agintc_attach(struct device *parent, struct device *self, void *aux)
 	/* setup IPI interrupts */
 
 	/*
-	 * Ideally we want two IPI interrupts, one for NOP and one for
-	 * DDB, however we can survive if only one is available it is
-	 * possible that most are not available to the non-secure OS.
+	 * Ideally we want three IPI interrupts, one for NOP, one for
+	 * DDB and one for HALT.  However we can survive if only one
+	 * is available; it is possible that most are not available to
+	 * the non-secure OS.
 	 */
 	nipi = 0;
 	for (i = 0; i < 16; i++) {
@@ -569,7 +571,7 @@ agintc_attach(struct device *parent, struct device *self, void *aux)
 		else
 			printf(", %d", i);
 		ipiirq[nipi++] = i;
-		if (nipi == 2)
+		if (nipi == 3)
 			break;
 	}
 
@@ -583,8 +585,20 @@ agintc_attach(struct device *parent, struct device *self, void *aux)
 		    agintc_ipi_combined, sc, "ipi");
 		sc->sc_ipi_num[ARM_IPI_NOP] = ipiirq[0];
 		sc->sc_ipi_num[ARM_IPI_DDB] = ipiirq[0];
+		sc->sc_ipi_num[ARM_IPI_HALT] = ipiirq[0];
 		break;
 	case 2:
+		sc->sc_ipi_irq[0] = agintc_intr_establish(ipiirq[0],
+		    IST_EDGE_RISING, IPL_IPI|IPL_MPSAFE, NULL,
+		    agintc_ipi_nop, sc, "ipinop");
+		sc->sc_ipi_num[ARM_IPI_NOP] = ipiirq[0];
+		sc->sc_ipi_irq[1] = agintc_intr_establish(ipiirq[1],
+		    IST_EDGE_RISING, IPL_IPI|IPL_MPSAFE, NULL,
+		    agintc_ipi_combined, sc, "ipi");
+		sc->sc_ipi_num[ARM_IPI_DDB] = ipiirq[1];
+		sc->sc_ipi_num[ARM_IPI_HALT] = ipiirq[1];
+		break;
+	case 3:
 		sc->sc_ipi_irq[0] = agintc_intr_establish(ipiirq[0],
 		    IST_EDGE_RISING, IPL_IPI|IPL_MPSAFE, NULL,
 		    agintc_ipi_nop, sc, "ipinop");
@@ -593,6 +607,10 @@ agintc_attach(struct device *parent, struct device *self, void *aux)
 		    IST_EDGE_RISING, IPL_IPI|IPL_MPSAFE, NULL,
 		    agintc_ipi_ddb, sc, "ipiddb");
 		sc->sc_ipi_num[ARM_IPI_DDB] = ipiirq[1];
+		sc->sc_ipi_irq[2] = agintc_intr_establish(ipiirq[2],
+		    IST_EDGE_RISING, IPL_IPI|IPL_MPSAFE, NULL,
+		    agintc_ipi_halt, sc, "ipihalt");
+		sc->sc_ipi_num[ARM_IPI_HALT] = ipiirq[2];
 		break;
 	default:
 		panic("nipi unexpected number %d", nipi);
@@ -1167,6 +1185,13 @@ agintc_ipi_ddb(void *v)
 	return 1;
 }
 
+int
+agintc_ipi_halt(void *v)
+{
+	cpu_halt();
+	return 1;
+}
+
 int
 agintc_ipi_nop(void *v)
 {
@@ -1182,6 +1207,9 @@ agintc_ipi_combined(void *v)
 	if (sc->sc_ipi_reason[cpu_number()] == ARM_IPI_DDB) {
 		sc->sc_ipi_reason[cpu_number()] = ARM_IPI_NOP;
 		return agintc_ipi_ddb(v);
+	} else if (sc->sc_ipi_reason[cpu_number()] == ARM_IPI_HALT) {
+		sc->sc_ipi_reason[cpu_number()] = ARM_IPI_NOP;
+		return agintc_ipi_halt(v);
 	} else {
 		return agintc_ipi_nop(v);
 	}
@@ -1196,8 +1224,8 @@ agintc_send_ipi(struct cpu_info *ci, int id)
 	if (ci == curcpu() && id == ARM_IPI_NOP)
 		return;
 
-	/* never overwrite IPI_DDB with IPI_NOP */
-	if (id == ARM_IPI_DDB)
+	/* never overwrite IPI_DDB or IPI_HALT with IPI_NOP */
+	if (id == ARM_IPI_DDB || id == ARM_IPI_HALT)
 		sc->sc_ipi_reason[ci->ci_cpuid] = id;
 
 	/* will only send 1 cpu */
diff --git a/sys/arch/arm64/dev/ampintc.c b/sys/arch/arm64/dev/ampintc.c
index 3983aa5ed08..45a15dd2d57 100644
--- a/sys/arch/arm64/dev/ampintc.c
+++ b/sys/arch/arm64/dev/ampintc.c
@@ -1,4 +1,4 @@
-/* $OpenBSD: ampintc.c,v 1.27 2022/01/02 20:00:21 kettenis Exp $ */
+/* $OpenBSD: ampintc.c,v 1.28 2022/07/13 09:28:18 kettenis Exp $ */
 /*
  * Copyright (c) 2007,2009,2011 Dale Rahn <drahn@openbsd.org>
  *
@@ -142,7 +142,7 @@ struct ampintc_softc {
 	struct evcount		 sc_spur;
 	struct interrupt_controller sc_ic;
 	int			 sc_ipi_reason[ICD_ICTR_CPU_M + 1];
-	int			 sc_ipi_num[2];
+	int			 sc_ipi_num[3];
 };
 struct ampintc_softc *ampintc;
 
@@ -170,6 +170,8 @@ struct intrq {
 
 int		 ampintc_match(struct device *, void *, void *);
 void		 ampintc_attach(struct device *, struct device *, void *);
+int		 ampintc_activate(struct device *, int);
+void		 ampintc_init(struct ampintc_softc *);
 void		 ampintc_cpuinit(void);
 int		 ampintc_spllower(int);
 void		 ampintc_splx(int);
@@ -197,10 +199,12 @@ void		 ampintc_intr_barrier(void *);
 int		 ampintc_ipi_combined(void *);
 int		 ampintc_ipi_nop(void *);
 int		 ampintc_ipi_ddb(void *);
+int		 ampintc_ipi_halt(void *);
 void		 ampintc_send_ipi(struct cpu_info *, int);
 
 const struct cfattach	ampintc_ca = {
-	sizeof (struct ampintc_softc), ampintc_match, ampintc_attach
+	sizeof (struct ampintc_softc), ampintc_match, ampintc_attach,
+	NULL, ampintc_activate
 };
 
 struct cfdriver ampintc_cd = {
@@ -236,7 +240,7 @@ ampintc_attach(struct device *parent, struct device *self, void *aux)
 	int i, nintr, ncpu;
 	uint32_t ictr;
 #ifdef MULTIPROCESSOR
-	int nipi, ipiirq[2];
+	int nipi, ipiirq[3];
 #endif
 
 	ampintc = sc;
@@ -268,23 +272,7 @@ ampintc_attach(struct device *parent, struct device *self, void *aux)
 	sc->sc_cpu_mask[curcpu()->ci_cpuid] =
 	    bus_space_read_1(sc->sc_iot, sc->sc_d_ioh, ICD_IPTRn(0));
 
-	/* Disable all interrupts, clear all pending */
-	for (i = 0; i < nintr/32; i++) {
-		bus_space_write_4(sc->sc_iot, sc->sc_d_ioh,
-		    ICD_ICERn(i*32), ~0);
-		bus_space_write_4(sc->sc_iot, sc->sc_d_ioh,
-		    ICD_ICPRn(i*32), ~0);
-	}
-	for (i = 0; i < nintr; i++) {
-		/* lowest priority ?? */
-		bus_space_write_1(sc->sc_iot, sc->sc_d_ioh, ICD_IPRn(i), 0xff);
-		/* target no cpus */
-		bus_space_write_1(sc->sc_iot, sc->sc_d_ioh, ICD_IPTRn(i), 0);
-	}
-	for (i = 2; i < nintr/16; i++) {
-		/* irq 32 - N */
-		bus_space_write_4(sc->sc_iot, sc->sc_d_ioh, ICD_ICRn(i*16), 0);
-	}
+	ampintc_init(sc);
 
 	/* software reset of the part? */
 	/* set protection bit (kernel only)? */
@@ -308,9 +296,10 @@ ampintc_attach(struct device *parent, struct device *self, void *aux)
 	/* setup IPI interrupts */
 
 	/*
-	 * Ideally we want two IPI interrupts, one for NOP and one for
-	 * DDB, however we can survive if only one is available it is
-	 * possible that most are not available to the non-secure OS.
+	 * Ideally we want three IPI interrupts, one for NOP, one for
+	 * DDB and one for HALT.  However we can survive if only one
+	 * is available; it is possible that most are not available to
+	 * the non-secure OS.
 	 */
 	nipi = 0;
 	for (i = 0; i < 16; i++) {
@@ -335,7 +324,7 @@ ampintc_attach(struct device *parent, struct device *self, void *aux)
 		else
 			printf(", %d", i);
 		ipiirq[nipi++] = i;
-		if (nipi == 2)
+		if (nipi == 3)
 			break;
 	}
 
@@ -348,14 +337,27 @@ ampintc_attach(struct device *parent, struct device *self, void *aux)
 		    IPL_IPI|IPL_MPSAFE, NULL, ampintc_ipi_combined, sc, "ipi");
 		sc->sc_ipi_num[ARM_IPI_NOP] = ipiirq[0];
 		sc->sc_ipi_num[ARM_IPI_DDB] = ipiirq[0];
+		sc->sc_ipi_num[ARM_IPI_HALT] = ipiirq[0];
 		break;
 	case 2:
+		ampintc_intr_establish(ipiirq[0], IST_EDGE_RISING,
+		    IPL_IPI|IPL_MPSAFE, NULL, ampintc_ipi_nop, sc, "ipinop");
+		sc->sc_ipi_num[ARM_IPI_NOP] = ipiirq[0];
+		ampintc_intr_establish(ipiirq[1], IST_EDGE_RISING,
+		    IPL_IPI|IPL_MPSAFE, NULL, ampintc_ipi_combined, sc, "ipi");
+		sc->sc_ipi_num[ARM_IPI_DDB] = ipiirq[1];
+		sc->sc_ipi_num[ARM_IPI_HALT] = ipiirq[1];
+		break;
+	case 3:
 		ampintc_intr_establish(ipiirq[0], IST_EDGE_RISING,
 		    IPL_IPI|IPL_MPSAFE, NULL, ampintc_ipi_nop, sc, "ipinop");
 		sc->sc_ipi_num[ARM_IPI_NOP] = ipiirq[0];
 		ampintc_intr_establish(ipiirq[1], IST_EDGE_RISING,
 		    IPL_IPI|IPL_MPSAFE, NULL, ampintc_ipi_ddb, sc, "ipiddb");
 		sc->sc_ipi_num[ARM_IPI_DDB] = ipiirq[1];
+		ampintc_intr_establish(ipiirq[2], IST_EDGE_RISING,
+		    IPL_IPI|IPL_MPSAFE, NULL, ampintc_ipi_halt, sc, "ipihalt");
+		sc->sc_ipi_num[ARM_IPI_HALT] = ipiirq[2];
 		break;
 	default:
 		panic("nipi unexpected number %d", nipi);
@@ -382,6 +384,61 @@ ampintc_attach(struct device *parent, struct device *self, void *aux)
 	simplebus_attach(parent, &sc->sc_sbus.sc_dev, faa);
 }
 
+int
+ampintc_activate(struct device *self, int act)
+{
+	struct ampintc_softc *sc = (struct ampintc_softc *)self;
+	struct cpu_info *ci;
+	int irq, min;
+
+	switch (act) {
+	case DVACT_RESUME:
+		for (irq = 0; irq < sc->sc_nintr; irq++) {
+			ci = sc->sc_handler[irq].iq_ci;
+			min = sc->sc_handler[irq].iq_irq_min;
+			if (min != IPL_NONE) {
+				ampintc_set_priority(irq, min);
+				ampintc_intr_enable(irq);
+				ampintc_route(irq, IRQ_ENABLE, ci);
+			} else {
+				ampintc_intr_disable(irq);
+			}
+		}
+
+		/* enable interrupts */
+		bus_space_write_4(sc->sc_iot, sc->sc_d_ioh, ICD_DCR, 3);
+		bus_space_write_4(sc->sc_iot, sc->sc_p_ioh, ICPICR, 1);
+		break;
+	}
+
+	return 0;
+}
+
+void
+ampintc_init(struct ampintc_softc *sc)
+{
+	int i;
+
+	/* Disable all interrupts, clear all pending */
+	for (i = 0; i < sc->sc_nintr / 32; i++) {
+		bus_space_write_4(sc->sc_iot, sc->sc_d_ioh,
+		    ICD_ICERn(i * 32), ~0);
+		bus_space_write_4(sc->sc_iot, sc->sc_d_ioh,
+		    ICD_ICPRn(i * 32), ~0);
+	}
+	for (i = 0; i < sc->sc_nintr; i++) {
+		/* lowest priority ?? */
+		bus_space_write_1(sc->sc_iot, sc->sc_d_ioh, ICD_IPRn(i), 0xff);
+		/* target no cpus */
+		bus_space_write_1(sc->sc_iot, sc->sc_d_ioh, ICD_IPTRn(i), 0);
+	}
+	for (i = 2; i < sc->sc_nintr / 16; i++) {
+		/* irq 32 - N */
+		bus_space_write_4(sc->sc_iot, sc->sc_d_ioh,
+		    ICD_ICRn(i * 16), 0);
+	}
+}
+
 void
 ampintc_set_priority(int irq, int pri)
 {
@@ -613,6 +670,15 @@ ampintc_cpuinit(void)
 		else
 			ampintc_route(irq, IRQ_DISABLE, curcpu());
 	}
+
+	/*
+	 * If a secondary CPU is turned off from an IPI handler and
+	 * the GIC did not go through a full reset (for example when
+	 * we fail to suspend) the IPI might still be active.  So
+	 * signal EOI here to make sure new interrupts will be
+	 * serviced.
+	 */
+	ampintc_eoi(sc->sc_ipi_num[ARM_IPI_HALT]);
 }
 
 void
@@ -994,6 +1060,13 @@ ampintc_ipi_ddb(void *v)
 	return 1;
 }
 
+int
+ampintc_ipi_halt(void *v)
+{
+	cpu_halt();
+	return 1;
+}
+
 int
 ampintc_ipi_nop(void *v)
 {
@@ -1009,6 +1082,9 @@ ampintc_ipi_combined(void *v)
 	if (sc->sc_ipi_reason[cpu_number()] == ARM_IPI_DDB) {
 		sc->sc_ipi_reason[cpu_number()] = ARM_IPI_NOP;
 		return ampintc_ipi_ddb(v);
+	} else if (sc->sc_ipi_reason[cpu_number()] == ARM_IPI_HALT) {
+		sc->sc_ipi_reason[cpu_number()] = ARM_IPI_NOP;
+		return ampintc_ipi_halt(v);
 	} else {
 		return ampintc_ipi_nop(v);
 	}
@@ -1023,8 +1099,8 @@ ampintc_send_ipi(struct cpu_info *ci, int id)
 	if (ci == curcpu() && id == ARM_IPI_NOP)
 		return;
 
-	/* never overwrite IPI_DDB with IPI_NOP */
-	if (id == ARM_IPI_DDB)
+	/* never overwrite IPI_DDB or IPI_HALT with IPI_NOP */
+	if (id == ARM_IPI_DDB || id == ARM_IPI_HALT)
 		sc->sc_ipi_reason[ci->ci_cpuid] = id;
 
 	/* currently will only send to one cpu */
diff --git a/sys/arch/arm64/dev/aplintc.c b/sys/arch/arm64/dev/aplintc.c
index 24739fbe3c3..02cca4c52f2 100644
--- a/sys/arch/arm64/dev/aplintc.c
+++ b/sys/arch/arm64/dev/aplintc.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: aplintc.c,v 1.11 2022/04/10 10:43:34 kettenis Exp $	*/
+/*	$OpenBSD: aplintc.c,v 1.12 2022/07/13 09:28:18 kettenis Exp $	*/
 /*
  * Copyright (c) 2021 Mark Kettenis
  *
@@ -601,8 +601,8 @@ aplintc_send_ipi(struct cpu_info *ci, int reason)
 	if (ci == curcpu() && reason == ARM_IPI_NOP)
 		return;
 
-	/* never overwrite IPI_DDB with IPI_NOP */
-	if (reason == ARM_IPI_DDB)
+	/* never overwrite IPI_DDB or IPI_HALT with IPI_NOP */
+	if (reason == ARM_IPI_DDB || reason == ARM_IPI_HALT)
 		sc->sc_ipi_reason[ci->ci_cpuid] = reason;
 	membar_producer();
 
diff --git a/sys/arch/arm64/dev/apm.c b/sys/arch/arm64/dev/apm.c
index 0a8152a570c..bc428838789 100644
--- a/sys/arch/arm64/dev/apm.c
+++ b/sys/arch/arm64/dev/apm.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: apm.c,v 1.16 2022/02/16 06:41:27 deraadt Exp $	*/
+/*	$OpenBSD: apm.c,v 1.17 2022/07/13 09:28:18 kettenis Exp $	*/
 
 /*-
  * Copyright (c) 2001 Alexander Guy.  All rights reserved.
@@ -49,6 +49,11 @@
 #include <machine/acpiapm.h>
 #include <machine/apmvar.h>
 
+#include "psci.h"
+#if NPSCI > 0
+#include <dev/fdt/pscivar.h>
+#endif
+
 #if defined(APMDEBUG)
 #define DPRINTF(x)	printf x
 #else
@@ -352,11 +357,30 @@ apm_record_event(u_int event, const char *src, const char *msg)
 void
 sleep_mp(void)
 {
+	CPU_INFO_ITERATOR cii;
+	struct cpu_info *ci;
+
+	CPU_INFO_FOREACH(cii, ci) {
+		if (CPU_IS_PRIMARY(ci))
+			continue;
+		arm_send_ipi(ci, ARM_IPI_HALT);
+		while (ci->ci_flags & CPUF_RUNNING)
+			CPU_BUSY_CYCLE();
+	}
 }
 
 void
 resume_mp(void)
 {
+	CPU_INFO_ITERATOR cii;
+	struct cpu_info *ci;
+
+	CPU_INFO_FOREACH(cii, ci) {
+		if (CPU_IS_PRIMARY(ci))
+			continue;
+		cpu_resume_secondary(ci);
+	}
+	cpu_boot_secondary_processors();
 }
 
 #endif /* MULTIPROCESSOR */
@@ -364,7 +388,12 @@ resume_mp(void)
 int
 sleep_showstate(void *v, int sleepmode)
 {
-	return 0;
+#if NPSCI > 0
+	if (sleepmode == SLEEP_SUSPEND && psci_can_suspend())
+		return 0;
+#endif
+
+	return EOPNOTSUPP;
 }
 
 int
@@ -376,7 +405,7 @@ sleep_setstate(void *v)
 int
 gosleep(void *v)
 {
-	return EOPNOTSUPP;
+	return cpu_suspend_primary();
 }
 
 void
@@ -393,18 +422,6 @@ sleep_resume(void *v)
 int
 suspend_finish(void *v)
 {
-#if 0
-	extern int lid_action;
-
-	acpi_record_event(sc, APM_NORMAL_RESUME);
-	acpi_indicator(sc, ACPI_SST_WORKING);
-
-	/* XXX won't work, there is no acpi thread on arm64 */
-
-	/* If we woke up but all the lids are closed, go back to sleep */
-	if (acpibtn_numopenlids() == 0 && lid_action != 0)
-		acpi_addtask(sc, acpi_sleep_task, sc, sc->sc_state);
-#endif
 	return 0;
 }
 
diff --git a/sys/arch/arm64/include/cpu.h b/sys/arch/arm64/include/cpu.h
index fe2b56ec1ab..1f3bebe56b4 100644
--- a/sys/arch/arm64/include/cpu.h
+++ b/sys/arch/arm64/include/cpu.h
@@ -1,4 +1,4 @@
-/* $OpenBSD: cpu.h,v 1.26 2022/06/16 20:45:42 kettenis Exp $ */
+/* $OpenBSD: cpu.h,v 1.27 2022/07/13 09:28:19 kettenis Exp $ */
 /*
  * Copyright (c) 2016 Dale Rahn <drahn@dalerahn.com>
  *
@@ -102,12 +102,10 @@ struct cpu_info {
 	u_int32_t		ci_pkg_id;
 
 	struct proc		*ci_curproc;
+	struct pcb		*ci_curpcb;
 	struct pmap		*ci_curpm;
 	u_int32_t		ci_randseed;
 
-	struct pcb		*ci_curpcb;
-	struct pcb		*ci_idle_pcb;
-
 	u_int32_t		ci_ctrl; /* The CPU control register */
 
 	uint32_t		ci_cpl;
@@ -120,6 +118,9 @@ struct cpu_info {
 
 	void			(*ci_flush_bp)(void);
 
+	uint64_t		ci_ttbr1;
+	vaddr_t			ci_el1_stkend;
+	
 	struct opp_table	*ci_opp_table;
 	volatile int		ci_opp_idx;
 	volatile int		ci_opp_max;
@@ -128,8 +129,6 @@ struct cpu_info {
 #ifdef MULTIPROCESSOR
 	struct srp_hazard	ci_srp_hazards[SRP_HAZARD_NUM];
 	volatile int		ci_flags;
-	uint64_t		ci_ttbr1;
-	vaddr_t			ci_el1_stkend;
 
 	volatile int		ci_ddb_paused;
 #define CI_DDB_RUNNING		0
@@ -312,7 +311,10 @@ intr_restore(u_long daif)
 	restore_daif(daif);
 }
 
+void	cpu_halt(void);
 void	cpu_startclock(void);
+int	cpu_suspend_primary(void);
+void	cpu_resume_secondary(struct cpu_info *);
 
 void	delay (unsigned);
 #define	DELAY(x)	delay(x)
diff --git a/sys/arch/arm64/include/intr.h b/sys/arch/arm64/include/intr.h
index d1389c39087..59d1c15906f 100644
--- a/sys/arch/arm64/include/intr.h
+++ b/sys/arch/arm64/include/intr.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: intr.h,v 1.18 2021/05/17 17:25:13 kettenis Exp $ */
+/*	$OpenBSD: intr.h,v 1.19 2022/07/13 09:28:19 kettenis Exp $ */
 
 /*
  * Copyright (c) 2001-2004 Opsycon AB  (www.opsycon.se / www.opsycon.com)
@@ -188,6 +188,7 @@ extern void (*intr_send_ipi_func)(struct cpu_info *, int);
 
 #define ARM_IPI_NOP	0
 #define ARM_IPI_DDB	1
+#define ARM_IPI_HALT	2
 
 #ifdef DIAGNOSTIC
 /*