lps: restore secondary cores on wake up

src/arch/host/include/arch/lib/cpu.h

@@ -34,6 +34,16 @@ static inline int arch_cpu_get_id(void)
 	return 0;
 }
 
+static inline int arch_cpu_restore_secondary_cores(void)
+{
+	return 0;
+}
+
+static inline int arch_cpu_secondary_cores_prepare_d0ix(void)
+{
+	return 0;
+}
+
 static inline void cpu_write_threadptr(int threadptr)
 {
 }

src/arch/xtensa/include/arch/lib/cpu.h

@@ -11,10 +11,17 @@
 #define __ARCH_LIB_CPU_H__
 
 #include <xtensa/config/core-isa.h>
+#include <stdint.h>
 
 #if CONFIG_MULTICORE
 
-void cpu_power_down_core(void);
+/** \brief CPU power down available flags */
+#define CPU_POWER_DOWN_MEMORY_ON	BIT(0) /**< Power down core with memory
+ * enabled (required in d0ix
+ * flow)
+ */
+
+void cpu_power_down_core(uint32_t flags);
 
 void cpu_alloc_core_context(int id);
 
@@ -26,6 +33,10 @@ int arch_cpu_is_core_enabled(int id);
 
 int arch_cpu_enabled_cores(void);
 
+int arch_cpu_restore_secondary_cores(void);
+
+int arch_cpu_secondary_cores_prepare_d0ix(void);
+
 #else
 
 static inline int arch_cpu_enable_core(int id) { return 0; }
@@ -36,6 +47,10 @@ static inline int arch_cpu_is_core_enabled(int id) { return 1; }
 
 static inline int arch_cpu_enabled_cores(void) { return 1; }
 
+static inline int arch_cpu_restore_secondary_cores(void) {return 0; }
+
+static inline int arch_cpu_secondary_cores_prepare_d0ix(void) {return 0; }
+
 #endif
 
 static inline int arch_cpu_get_id(void)

src/arch/xtensa/lib/cpu.c

@@ -160,25 +160,44 @@ void cpu_alloc_core_context(int core)
 	dcache_writeback_region(sof_get(), sizeof(*sof_get()));
 }
 
-void cpu_power_down_core(void)
+void cpu_power_down_core(uint32_t flags)
 {
 	arch_interrupt_global_disable();
 
-	idc_free();
+	/* Power down with memory on is performed by secondary cores during
+ * d0 -> d0ix before they are disabled by primary core.
+ */
+	if (flags & CPU_POWER_DOWN_MEMORY_ON) {
+ /* disable idc interrupts */
+ idc_free(IDC_FREE_IRQ_ONLY);
+
+ /* disable scheduler interrupts */
+ schedule_free(SOF_SCHEDULER_FREE_IRQ_ONLY);
+
+ /* data writeback/invalidate */
+ dcache_writeback_invalidate_all();
 
-	schedule_free();
+ /* after writeback/invalidate secondary core is prepared for
+ * powered off - prepare_d0ix_core_mask flag can be disabled
+ */
+ platform_pm_runtime_prepare_d0ix_dis(cpu_get_id());
+	} else {
+ idc_free(0);
 
-free_system_notify();
+	schedule_free(0);
 
-	/* free entire sys heap, an instance dedicated for this core */
-	free_heap(SOF_MEM_ZONE_SYS);
+ free_system_notify();
 
-	dcache_writeback_invalidate_all();
+ /* free entire sys heap, an instance dedicated for this core */
+ free_heap(SOF_MEM_ZONE_SYS);
 
-	/* Turn off stack memory for core */
-	pm_runtime_put(CORE_MEMORY_POW, cpu_get_id());
+ dcache_writeback_invalidate_all();
 
-	pm_runtime_put(PM_RUNTIME_DSP, PWRD_BY_TPLG | cpu_get_id());
+ /* Turn off stack memory for core */
+ pm_runtime_put(CORE_MEMORY_POW, cpu_get_id());
+
+ pm_runtime_put(PM_RUNTIME_DSP, PWRD_BY_TPLG | cpu_get_id());
+	}
 
 	trace_point(0);
 
@@ -189,3 +208,50 @@ void cpu_power_down_core(void)
 	while (1)
  arch_wait_for_interrupt(0);
 }
+
+int arch_cpu_restore_secondary_cores(void)
+{
+	struct idc_msg power_up = { IDC_MSG_POWER_UP, IDC_MSG_POWER_UP_EXT };
+	int ret, id;
+
+	for (id = 0; id < CONFIG_CORE_COUNT; id++) {
+ if (arch_cpu_is_core_enabled(id) && id != PLATFORM_PRIMARY_CORE_ID) {
+ power_up.core = id;
+
+ /* Power up secondary core */
+ pm_runtime_get(PM_RUNTIME_DSP, id);
+
+ /* enable IDC interrupt for the secondary core */
+ idc_enable_interrupts(id, cpu_get_id());
+
+ /* send IDC power up message */
+ ret = idc_send_msg(&power_up, IDC_POWER_UP);
+ if (ret < 0)
+ return ret;
+ }
+	}
+
+	return 0;
+}
+
+int arch_cpu_secondary_cores_prepare_d0ix(void)
+{
+	struct idc_msg prepare_msg = { IDC_MSG_PREPARE_D0ix,
+ IDC_MSG_PREPARE_D0ix_EXT };
+	int ret, id;
+
+	for (id = 0; id < CONFIG_CORE_COUNT; id++) {
+ if (arch_cpu_is_core_enabled(id) && id != PLATFORM_PRIMARY_CORE_ID) {
+ prepare_msg.core = id;
+
+ /* send IDC prepare message to all enabled secondary
+ * cores.
+ */
+ ret = idc_send_msg(&prepare_msg, IDC_BLOCKING);
+ if (ret < 0)
+ return ret;
+ }
+	}
+
+	return 0;
+}

src/drivers/intel/cavs/idc.c

@@ -243,10 +243,43 @@ int platform_idc_init(void)
 	return 0;
 }
 
+/**
+ * \brief Restores IDC interrupt. During D0->D0ix/D0ix->D0 flow primary core
+ * disables all secondary cores - this is not cold boot process, because
+ * memory has not been powered off. In that case, we should only enable
+ * idc interrupts, because all required structures alreade exist.
+ */
+int platform_idc_restore(void)
+{
+	struct idc *idc = *idc_get();
+	int core = cpu_get_id();
+	int ret;
+
+	idc->irq = interrupt_get_irq(PLATFORM_IDC_INTERRUPT,
+ PLATFORM_IDC_INTERRUPT_NAME);
+	if (idc->irq < 0) {
+ tr_err(&idc_tr, "platform_idc_restore(): getting irq failed.");
+ return idc->irq;
+	}
+
+	ret = interrupt_register(idc->irq, idc_irq_handler, idc);
+	if (ret < 0) {
+ tr_err(&idc_tr, "platform_idc_restore(): registering irq failed.");
+ return ret;
+	}
+
+	interrupt_enable(idc->irq, idc);
+
+	/* enable BUSY interrupt */
+	idc_write(IPC_IDCCTL, core, idc->busy_bit_mask);
+
+	return 0;
+}
+
 /**
  * \brief Frees IDC data and unregisters interrupt.
  */
-void idc_free(void)
+void idc_free(uint32_t flags)
 {
 	struct idc *idc = *idc_get();
 	int core = cpu_get_id();
@@ -266,5 +299,8 @@ void idc_free(void)
  idc_write(IPC_IDCTFC(i), core, idctfc);
 	}
 
+	if (flags & IDC_FREE_IRQ_ONLY)
+ return;
+
 	schedule_task_free(&idc->idc_task);
 }

src/idc/idc.c

@@ -17,6 +17,7 @@
 #include <sof/lib/cpu.h>
 #include <sof/lib/memory.h>
 #include <sof/lib/notifier.h>
+#include <sof/lib/pm_runtime.h>
 #include <sof/lib/uuid.h>
 #include <sof/platform.h>
 #include <arch/lib/wait.h>
@@ -268,6 +269,16 @@ static int idc_reset(uint32_t comp_id)
 	return ret;
 }
 
+static void idc_prepare_d0ix(void)
+{
+	/* set prepare_d0ix flag, which indicates that in the next
+ * platform_wait_for_interrupt invocation(), core should get ready for
+ * d0ix power down - it is required by D0->D0ix flow, when primary
+ * core disables all secondary cores.
+ */
+	platform_pm_runtime_prepare_d0ix_en(cpu_get_id());
+}
+
 /**
  * \brief Executes IDC message based on type.
  * \param[in,out] msg Pointer to IDC message.
@@ -279,7 +290,7 @@ void idc_cmd(struct idc_msg *msg)
 
 	switch (type) {
 	case iTS(IDC_MSG_POWER_DOWN):
- cpu_power_down_core();
+ cpu_power_down_core(0);
  break;
 	case iTS(IDC_MSG_NOTIFY):
  notifier_notify_remote();
@@ -299,6 +310,9 @@ void idc_cmd(struct idc_msg *msg)
 	case iTS(IDC_MSG_RESET):
  ret = idc_reset(msg->extension);
  break;
+	case iTS(IDC_MSG_PREPARE_D0ix):
+ idc_prepare_d0ix();
+ break;
 	default:
  tr_err(&idc_tr, "idc_cmd(): invalid msg->header = %u",
  msg->header);
@@ -336,3 +350,23 @@ int idc_init(void)
 	return 0;
 #endif
 }
+
+int idc_restore(void)
+{
+	struct idc **idc = idc_get();
+
+	tr_info(&idc_tr, "idc_restore()");
+
+	/* idc_restore() is invoked during D0->D0ix/D0ix->D0 flow. In that
+ * case basic core structures e.g. idc struct should be already
+ * allocated (in D0->D0ix primary core disables all secondary cores, but
+ * memory has not been powered off).
+ */
+	assert(*idc);
+
+#ifndef __ZEPHYR__
+	return platform_idc_restore();
+#endif
+
+	return 0;
+}

src/include/sof/drivers/idc.h

@@ -88,12 +88,19 @@
 #define IDC_MSG_RESET IDC_TYPE(0x8)
 #define IDC_MSG_RESET_EXT(x)	IDC_EXTENSION(x)
 
+/** \brief IDC prepare D0ix message. */
+#define IDC_MSG_PREPARE_D0ix IDC_TYPE(0x9)
+#define IDC_MSG_PREPARE_D0ix_EXT	IDC_EXTENSION(0x0)
+
 /** \brief Decodes IDC message type. */
 #define iTS(x)	(((x) >> IDC_TYPE_SHIFT) & IDC_TYPE_MASK)
 
 /** \brief Max IDC message payload size in bytes. */
 #define IDC_MAX_PAYLOAD_SIZE	96
 
+/** \brief IDC free function flags */
+#define IDC_FREE_IRQ_ONLY	BIT(0)	/**< disable only irqs */
+
 /** \brief IDC message payload. */
 struct idc_payload {
 	uint8_t data[IDC_MAX_PAYLOAD_SIZE];
@@ -128,10 +135,12 @@ static inline struct idc_payload *idc_payload_get(struct idc *idc,
 
 void idc_enable_interrupts(int target_core, int source_core);
 
-void idc_free(void);
+void idc_free(uint32_t flags);
 
 int platform_idc_init(void);
 
+int platform_idc_restore(void);
+
 enum task_state idc_do_cmd(void *data);
 
 void idc_cmd(struct idc_msg *msg);

src/include/sof/lib/cpu.h

@@ -56,6 +56,16 @@ static inline int cpu_enabled_cores(void)
 	return arch_cpu_enabled_cores();
 }
 
+static inline int cpu_restore_secondary_cores(void)
+{
+	return arch_cpu_restore_secondary_cores();
+}
+
+static inline int cpu_secondary_cores_prepare_d0ix(void)
+{
+	return arch_cpu_secondary_cores_prepare_d0ix();
+}
+
 #endif
 
 #endif /* __SOF_LIB_CPU_H__ */