cpu: enabling and disabling slave cores

src/arch/xtensa/Makefile.am

@@ -52,13 +52,15 @@ if BUILD_XTENSA_SMP
 sof_SOURCES += \
 	smp/xtos/crt1-boards.S \
 	smp/xtos/_vectors.S \
+	smp/cpu.c \
 	smp/init.c \
 	smp/schedule.c \
 	smp/task.c
 else
 sof_SOURCES += \
 	up/xtos/crt1-boards.S \
 	up/xtos/_vectors.S \
+	up/cpu.c \
 	up/init.c \
 	up/schedule.c \
 	up/task.c

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

@@ -35,7 +35,11 @@
 #include <xtensa/config/core.h>
 #include <platform/platcfg.h>
 
-static inline int cpu_get_id(void)
+void arch_cpu_enable_core(int id);
+
+void arch_cpu_disable_core(int id);
+
+static inline int arch_cpu_get_id(void)
 {
 	int prid;
 #if XCHAL_HAVE_PRID

src/arch/xtensa/smp/cpu.c

@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2018, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the name of the Intel Corporation nor the
+ *     names of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Tomasz Lauda <tomasz.lauda@linux.intel.com>
+ *
+ */
+
+/**
+ * \file arch/xtensa/smp/cpu.c
+ * \brief Xtensa SMP CPU implementation file
+ * \authors Tomasz Lauda <tomasz.lauda@linux.intel.com>
+ */
+
+#include <arch/alloc.h>
+#include <arch/atomic.h>
+#include <arch/cpu.h>
+#include <arch/idc.h>
+#include <platform/platform.h>
+#include <sof/lock.h>
+#include <sof/schedule.h>
+
+static uint32_t active_cores_mask = 0x1;
+static spinlock_t lock = { 0 };
+
+void arch_cpu_enable_core(int id)
+{
+	struct idc_msg power_up = {
+		IDC_POWER_UP_MESSAGE, IDC_POWER_UP_EXTENSION, id };
+	uint32_t flags;
+
+	spin_lock_irq(&lock, flags);
+
+	if (!(active_cores_mask & (1 << id))) {
+		/* allocate resources for core */
+		alloc_core_context(id);
+
+		/* enable IDC interrupt for the the slave core */
+		idc_enable_interrupts(id, arch_cpu_get_id());
+
+		/* send IDC power up message */
+		arch_idc_send_msg(&power_up);
+
+		active_cores_mask |= (1 << id);
+	}
+
+	spin_unlock_irq(&lock, flags);
+}
+
+void arch_cpu_disable_core(int id)
+{
+	struct idc_msg power_down = { IDC_POWER_DOWN_MESSAGE, 0, id };
+	uint32_t flags;
+
+	spin_lock_irq(&lock, flags);
+
+	if (active_cores_mask & (1 << id)) {
+		arch_idc_send_msg(&power_down);
+
+		active_cores_mask ^= (1 << id);
+	}
+
+	spin_unlock_irq(&lock, flags);
+}
+
+void cpu_power_down_core(void)
+{
+	arch_interrupt_global_disable();
+
+	idc_free();
+
+	scheduler_free();
+
+	free_core_context(arch_cpu_get_id());
+
+	dcache_writeback_invalidate_all();
+
+	while (1)
+		arch_wait_for_interrupt(0);
+}

src/arch/xtensa/smp/include/arch/Makefile.am

@@ -1,2 +1,3 @@
 noinst_HEADERS = \
+	alloc.h \
 	idc.h

src/arch/xtensa/smp/include/arch/alloc.h

@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2018, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the name of the Intel Corporation nor the
+ *     names of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Tomasz Lauda <tomasz.lauda@linux.intel.com>
+ */
+
+/**
+ * \file arch/xtensa/smp/include/arch/alloc.h
+ * \brief Xtensa SMP memory allocation header file
+ * \authors Tomasz Lauda <tomasz.lauda@linux.intel.com>
+ */
+
+#ifndef __ARCH_ALLOC_H__
+#define __ARCH_ALLOC_H__
+
+#include <xtos-structs.h>
+#include <sof/alloc.h>
+
+extern struct core_context *core_ctx_ptr[PLATFORM_CORE_COUNT];
+extern struct xtos_core_data *core_data_ptr[PLATFORM_CORE_COUNT];
+
+/**
+ * \brief Allocates memory for core specific data.
+ * \param[in] core Slave core for which data needs to be allocated.
+ */
+static inline void alloc_core_context(int core)
+{
+	struct core_context *core_ctx;
+
+	core_ctx = rzalloc(RZONE_RUNTIME, SOF_MEM_CAPS_RAM, sizeof(*core_ctx));
+	dcache_writeback_invalidate_region(core_ctx, sizeof(*core_ctx));
+
+	/* xtos_core_data is a big struct, so allocate it from system heap
+	 * and never free again. Allocating from runtime heap would be
+	 * a waste of a very big memory block.
+	 */
+	if (!core_data_ptr[core])
+		core_data_ptr[core] = rzalloc(RZONE_SYS, SOF_MEM_CAPS_RAM,
+					      sizeof(*core_data_ptr[core]));
+
+	core_data_ptr[core]->thread_data_ptr = &core_ctx->td;
+	dcache_writeback_invalidate_region(core_data_ptr[core],
+					   sizeof(*core_data_ptr[core]));
+
+	dcache_writeback_invalidate_region(core_data_ptr,
+					   sizeof(core_data_ptr));
+
+	core_ctx_ptr[core] = core_ctx;
+	dcache_writeback_invalidate_region(core_ctx_ptr,
+					   sizeof(core_ctx_ptr));
+}
+
+/**
+ * \brief Frees memory allocated for core specific data.
+ * \param[in] core Slave core for which data needs to be freed.
+ */
+static inline void free_core_context(int core)
+{
+	rfree(core_ctx_ptr[core]);
+	dcache_writeback_invalidate_region(core_ctx_ptr[core],
+					   sizeof(*core_ctx_ptr[core]));
+}
+
+#endif

src/arch/xtensa/smp/include/arch/idc.h

@@ -54,11 +54,25 @@
 /** \brief IDC trace error function. */
 #define trace_idc_error(__e)	trace_error(TRACE_CLASS_IDC, __e)
 
-/** \brief IDC header mask. */
-#define IDC_HEADER(x)		((x) & 0x7fffffff)
 
-/** \brief IDC extension mask. */
-#define IDC_EXTENSION(x)	((x) & 0x0fffffff)
+/** \brief ROM wake version parsed by ROM during core wake up. */
+#define IDC_ROM_WAKE_VERSION	0x2
+
+/** \brief ROM control version parsed by ROM during core wake up. */
+#define IDC_ROM_CONTROL_VERSION	0x1
+
+// TODO: refactor below defines after universal IDC message template
+//       will be defined and ready
+
+/** \brief Power up message header. */
+#define IDC_POWER_UP_MESSAGE \
+		(IDC_ROM_WAKE_VERSION | (IDC_ROM_CONTROL_VERSION << 24))
+
+/** \brief Power up message extension. */
+#define IDC_POWER_UP_EXTENSION	(SOF_TEXT_START >> 2)
+
+/** \brief Power down message header. */
+#define IDC_POWER_DOWN_MESSAGE	0x7FFFFFFF
 
 /** \brief IDC message. */
 struct idc_msg {
@@ -76,6 +90,8 @@ struct idc {
 	struct idc_msg received_msg;	/**< received message */
 };
 
+extern void cpu_power_down_core(void);
+
 /**
  * \brief Returns IDC data.
  * \return Pointer to pointer of IDC data.
@@ -87,14 +103,21 @@ static inline struct idc **idc_get(void)
 	return &ctx->idc;
 }
 
+static inline void idc_enable_interrupts(int target_core, int source_core)
+{
+	idc_write(IPC_IDCCTL, target_core,
+		  IPC_IDCCTL_IDCTBIE(source_core));
+	platform_interrupt_unmask(PLATFORM_IDC_INTERRUPT(target_core), 0);
+}
+
 /**
  * \brief IDC interrupt handler.
  * \param[in,out] arg Pointer to IDC data.
  */
 static void idc_irq_handler(void *arg)
 {
 	struct idc *idc = arg;
-	int core = cpu_get_id();
+	int core = arch_cpu_get_id();
 	uint32_t idctfc;
 	uint32_t idctefc;
 	uint32_t idcietc;
@@ -146,7 +169,7 @@ static void idc_irq_handler(void *arg)
 static inline void arch_idc_send_msg(struct idc_msg *msg)
 {
 	struct idc *idc = *idc_get();
-	int core = cpu_get_id();
+	int core = arch_cpu_get_id();
 	uint32_t flags;
 
 	tracev_idc("Msg");
@@ -166,7 +189,11 @@ static inline void arch_idc_send_msg(struct idc_msg *msg)
  */
 static inline int32_t idc_cmd(struct idc_msg *msg)
 {
-	/* execute message based on type */
+	/* right now we only handle power down */
+	/* TODO: universal implementation */
+	if (msg->header == IDC_POWER_DOWN_MESSAGE)
+		cpu_power_down_core();
+
 	return 0;
 }
 
@@ -176,7 +203,7 @@ static inline int32_t idc_cmd(struct idc_msg *msg)
  */
 static inline void idc_do_cmd(struct idc *idc)
 {
-	int core = cpu_get_id();
+	int core = arch_cpu_get_id();
 	int initiator = idc->received_msg.core;
 
 	trace_idc("Cmd");
@@ -253,7 +280,7 @@ static inline uint32_t idc_get_done_bit_mask(int core)
  */
 static inline void arch_idc_init(void)
 {
-	int core = cpu_get_id();
+	int core = arch_cpu_get_id();
 
 	trace_idc("IDI");
 

src/arch/xtensa/smp/init.c

@@ -71,7 +71,7 @@ struct xtos_core_data *core_data_ptr[PLATFORM_CORE_COUNT];
  */
 static void initialize_pointers_per_core(void)
 {
-	int core = cpu_get_id();
+	int core = arch_cpu_get_id();
 	struct xtos_core_data *core_data = core_data_ptr[core];
 
 	if (core == PLATFORM_MASTER_CORE_ID) {

src/arch/xtensa/smp/xtos/int-sethandler.c

@@ -42,7 +42,7 @@ _xtos_handler _xtos_set_interrupt_handler_arg( int n, _xtos_handler f, void *arg
         ret = 0;     /* priority level too high to safely handle in C */
     }
     else {
-        entry = &(core_data_ptr[cpu_get_id()]->xtos_int_data.xtos_interrupt_table.array[MAPINT(n)]);
+        entry = &(core_data_ptr[arch_cpu_get_id()]->xtos_int_data.xtos_interrupt_table.array[MAPINT(n)]);
         old = entry->handler;
         if (f) {
             entry->handler = f;

src/arch/xtensa/up/cpu.c

@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2018, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the name of the Intel Corporation nor the
+ *     names of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Tomasz Lauda <tomasz.lauda@linux.intel.com>
+ *
+ */
+
+/**
+ * \file arch/xtensa/up/cpu.c
+ * \brief Xtensa UP CPU implementation file
+ * \authors Tomasz Lauda <tomasz.lauda@linux.intel.com>
+ */
+
+#include <arch/cpu.h>
+
+void arch_cpu_enable_core(int id) { }
+
+void arch_cpu_disable_core(int id) { }

src/include/sof/Makefile.am

@@ -9,6 +9,7 @@ include_HEADERS = \
 	alloc.h \
 	atomic.h \
 	clock.h \
+	cpu.h \
 	dai.h \
 	debug.h \
 	dma.h \