arch: xtensa: Add support for lazy HiFi ctx switching

When lazy HiFi context switching is enabled, the system starts with
the HiFi coprocessor disabled. Should the thread use that coprocessor,
it will generate an exception which in turn will enable the coprocessor
and save/restore the HiFi registers as appropriate. When switching
to a new thread, the HiFi coprocessor is again disabled.

For simplicity, there are no restrictions as to which thread is allowed
to use the coprocessor.

Signed-off-by: Peter Mitsis <[email protected]>

Author: peter-mitsis

arch/xtensa/core/CMakeLists.txt

@@ -29,7 +29,7 @@ zephyr_library_sources_ifdef(CONFIG_XTENSA_MPU mpu.c)
 zephyr_library_sources_ifdef(CONFIG_USERSPACE userspace.S syscall_helper.c)
 zephyr_library_sources_ifdef(CONFIG_LLEXT elf.c)
 zephyr_library_sources_ifdef(CONFIG_SMP smp.c)
-zephyr_library_sources_ifdef(CONFIG_XTENSA_EAGER_HIFI_SHARING xtensa_hifi.S)
+zephyr_library_sources_ifdef(CONFIG_XTENSA_HIFI_SHARING xtensa_hifi.S)
 
 zephyr_library_sources_ifdef(
   CONFIG_KERNEL_VM_USE_CUSTOM_MEM_RANGE_CHECK

arch/xtensa/core/startup/reset_vector.S

@@ -575,7 +575,17 @@ unpackdone:
 	 * all CPENABLE bits must be set, even though they may not always
 	 * correspond to a coprocessor.
 	 */
+#ifdef CONFIG_XTENSA_LAZY_HIFI_SHARING
+	/*
+	 * Disable HiFi coprocessor by default. Should a thread try using
+         * the HiFi coprocessor, it will trigger an exception to both enable
+	 * it AND save/restore the HiFi state.
+	 */
+
+	movi	a2, 0xFF & ~(1 << XCHAL_CP_ID_AUDIOENGINELX)
+#else
 	movi	a2, 0xFF	 /* enable *all* bits, to allow dynamic TIE */
+#endif
 	wsr	a2, CPENABLE
 # endif
 

arch/xtensa/core/vector_handlers.c

@@ -31,6 +31,16 @@ LOG_MODULE_DECLARE(os, CONFIG_KERNEL_LOG_LEVEL);
 extern char xtensa_arch_except_epc[];
 extern char xtensa_arch_kernel_oops_epc[];
 
+extern void xtensa_lazy_hifi_save(uint8_t *regs);
+extern void xtensa_lazy_hifi_load(uint8_t *regs);
+
+#if defined(CONFIG_XTENSA_LAZY_HIFI_SHARING) && (CONFIG_MP_MAX_NUM_CPUS > 1)
+#define LAZY_COPROCESSOR_LOCK
+
+static struct k_spinlock coprocessor_lock;
+#endif
+
+
 bool xtensa_is_outside_stack_bounds(uintptr_t addr, size_t sz, uint32_t ps)
 {
 	uintptr_t start, end;
@@ -279,6 +289,88 @@ static inline void *return_to(void *interrupted)
 #endif /* CONFIG_MULTITHREADING */
 }
 
+#if defined(LAZY_COPROCESSOR_LOCK)
+/**
+ * Spin until thread is no longer the HiFi owner on specified CPU.
+ * Note: Interrupts are locked on entry. Unlock before spinning to allow
+ * an IPI to be caught and processed; restore them afterwards.
+ */
+static void spin_while_hifi_owner(struct _cpu *cpu, struct k_thread *thread)
+{
+	unsigned int key;
+	unsigned int original;
+	unsigned int unlocked;
+
+	__asm__ volatile("rsr.ps %0" : "=r"(original));
+	unlocked = original & ~PS_INTLEVEL_MASK;
+	__asm__ volatile("wsr.ps %0; rsync" :: "r"(unlocked) : "memory");
+
+	/* Spin until thread is no longer the HiFi owner on the other CPU */
+
+	while ((struct k_thread *)
+	       atomic_ptr_get(&cpu->arch.hifi_owner) == thread) {
+		key = arch_irq_lock();
+		arch_spin_relax();
+		arch_irq_unlock(key);
+	}
+
+	__asm__ volatile("wsr.ps %0; rsync" :: "r"(original) : "memory");
+}
+
+/**
+ * Determine if the thread is the owner of a HiFi on another CPU. This is
+ * called with the coprocessor lock held
+ */
+static struct _cpu *thread_hifi_owner_elsewhere(struct k_thread *thread)
+{
+	struct _cpu *this_cpu = arch_curr_cpu();
+	struct k_thread *owner;
+
+	for (unsigned int i = 0; i < CONFIG_MP_MAX_NUM_CPUS; i++) {
+		owner = (struct k_thread *)
+			atomic_ptr_get(&_kernel.cpus[i].arch.hifi_owner);
+		if ((this_cpu != &_kernel.cpus[i]) && (owner == thread)) {
+			return &_kernel.cpus[i];
+		}
+	}
+	return NULL;
+}
+#endif
+
+/**
+ * This routine only needed for SMP systems with HiFi sharing. It handles the
+ * IPI sent to save the HiFi registers so the owner can load them onto another
+ * CPU.
+ */
+void arch_ipi_lazy_coprocessors_save(void)
+{
+#if defined(LAZY_COPROCESSOR_LOCK)
+	k_spinlock_key_t key = k_spin_lock(&coprocessor_lock);
+	struct _cpu *cpu = arch_curr_cpu();
+	struct k_thread *save_hifi = (struct k_thread *)
+				     atomic_ptr_get(&cpu->arch.save_hifi);
+	struct k_thread *hifi_owner = (struct k_thread *)
+				      atomic_ptr_get(&cpu->arch.hifi_owner);
+
+	if ((save_hifi == hifi_owner) && (save_hifi != NULL)) {
+		unsigned int cp;
+
+		__asm__ volatile("rsr.cpenable %0" : "=r"(cp));
+		cp |= BIT(XCHAL_CP_ID_AUDIOENGINELX);
+		__asm__ volatile("wsr.cpenable %0" :: "r"(cp));
+
+		xtensa_lazy_hifi_save(save_hifi->arch.hifi_regs);
+
+		cp &= ~BIT(XCHAL_CP_ID_AUDIOENGINELX);
+		__asm__ volatile("wsr.cpenable %0" :: "r"(cp));
+
+		atomic_ptr_set(&cpu->arch.hifi_owner, NULL);
+	}
+	atomic_ptr_set(&cpu->arch.save_hifi, NULL);
+	k_spin_unlock(&coprocessor_lock, key);
+#endif
+}
+
 /* The wrapper code lives here instead of in the python script that
  * generates _xtensa_handle_one_int*().  Seems cleaner, still kind of
  * ugly.
@@ -484,6 +576,59 @@ void *xtensa_excint1_c(void *esf)
 		bsa->pc += 3;
 		break;
 #endif /* !CONFIG_USERSPACE */
+#ifdef CONFIG_XTENSA_LAZY_HIFI_SHARING
+	case EXCCAUSE_CP_DISABLED(XCHAL_CP_ID_AUDIOENGINELX):
+		/* Identify the interrupted thread and the old HiFi owner */
+		struct k_thread *thread = _current;
+		struct k_thread *owner;
+		unsigned int cp;
+
+#if defined(LAZY_COPROCESSOR_LOCK)
+		/*
+		 * If the interrupted thread is a HiFi owner on another CPU,
+		 * then send an IPI to that CPU to have it save its HiFi state
+		 * and then return. This CPU will continue to raise the current
+		 * exception (and send IPIs) until the other CPU has both saved
+		 * the HiFi registers and cleared its HiFi owner.
+		 */
+
+		k_spinlock_key_t key  = k_spin_lock(&coprocessor_lock);
+		struct _cpu *cpu = thread_hifi_owner_elsewhere(thread);
+
+		if (cpu != NULL) {
+			cpu->arch.save_hifi = thread;
+			arch_sched_directed_ipi(BIT(cpu->id));
+			k_spin_unlock(&coprocessor_lock, key);
+			spin_while_hifi_owner(cpu, thread);
+			key = k_spin_lock(&coprocessor_lock);
+		}
+#endif
+		owner = (struct k_thread *)
+			atomic_ptr_get(&arch_curr_cpu()->arch.hifi_owner);
+
+		/* Enable the HiFi coprocessor */
+		__asm__ volatile("rsr.cpenable %0" : "=r"(cp));
+		cp |= BIT(XCHAL_CP_ID_AUDIOENGINELX);
+		__asm__ volatile("wsr.cpenable %0" :: "r"(cp));
+
+		if (owner == thread) {
+#if defined(LAZY_COPROCESSOR_LOCK)
+			k_spin_unlock(&coprocessor_lock, key);
+#endif
+			break;
+		}
+
+		if (owner != NULL) {
+			xtensa_lazy_hifi_save(owner->arch.hifi_regs);
+		}
+
+		atomic_ptr_set(&arch_curr_cpu()->arch.hifi_owner, thread);
+#if defined(LAZY_COPROCESSOR_LOCK)
+		k_spin_unlock(&coprocessor_lock, key);
+#endif
+		xtensa_lazy_hifi_load(thread->arch.hifi_regs);
+		break;
+#endif /* CONFIG_XTENSA_LAZY_HIFI_SHARING */
 	default:
 		reason = K_ERR_CPU_EXCEPTION;
 
@@ -549,6 +694,9 @@ void *xtensa_excint1_c(void *esf)
 #ifndef CONFIG_USERSPACE
 	case EXCCAUSE_SYSCALL:
 #endif /* !CONFIG_USERSPACE */
+#ifdef CONFIG_XTENSA_LAZY_HIFI_SHARING
+	case EXCCAUSE_CP_DISABLED(XCHAL_CP_ID_AUDIOENGINELX):
+#endif /* CONFIG_XTENSA_LAZY_HIFI_SHARING */
 		is_fatal_error = false;
 		break;
 	default:

arch/xtensa/core/xtensa_asm2_util.S

@@ -307,6 +307,12 @@ xtensa_switch:
 
 #if defined(CONFIG_XTENSA_EAGER_HIFI_SHARING)
 	call0 _xtensa_hifi_save
+#elif defined(CONFIG_XTENSA_LAZY_HIFI_SHARING)
+	/* Disable HiFi sharing */
+	rsr a6, CPENABLE
+	movi a7, ~(1 << XCHAL_CP_ID_AUDIOENGINELX)
+	and a6, a6, a7
+	wsr a6, CPENABLE
 #endif
 
 	/* Now the high registers */

arch/xtensa/core/xtensa_hifi.S

@@ -9,6 +9,7 @@
 #include <xtensa/config/tie.h>
 #include <xtensa/config/tie-asm.h>
 
+#if defined(CONFIG_XTENSA_EAGER_HIFI_SHARING)
 /*
  * Load the HiFi registers from the hifi buffer in the BSA. Round the address
  * of this buffer up to XCHAL_CP1_SA_ALIGN bytes to guarantee the necessary
@@ -51,3 +52,32 @@ _xtensa_hifi_save:
         xchal_cp1_store a2 a3 a3 a3 a3  /* Only A2 and A3 are used by macro */
 
 	ret
+#elif defined(CONFIG_XTENSA_LAZY_HIFI_SHARING)
+/*
+ * Load the HiFi registers from the HiFi buffer in the k_thread structure.
+ */
+.global xtensa_lazy_hifi_load
+.align 4
+xtensa_lazy_hifi_load:
+	entry a1, 32
+	/* Spill registers onto stack */
+	call8 xthal_window_spill
+
+	/* A2 should be address of hifi storage; A3 is scratch */
+	xchal_cp1_load a2 a3 a3 a3 a3
+	retw
+
+/*
+ * Save the HiFi registers to the HiFi buffer in the k_thread structure.
+ */
+.global xtensa_lazy_hifi_save
+.align 4
+xtensa_lazy_hifi_save:
+	entry a1, 32
+	/* Spill registers onto stack */
+	call8 xthal_window_spill
+
+	/* A2 should be address of hifi storage; A3 is scratch */
+	xchal_cp1_store a2 a3 a3 a3 a3  /* Only A2 and A3 are used by macro */
+	retw
+#endif

kernel/include/kernel_arch_interface.h

@@ -175,6 +175,15 @@ void arch_switch_to_main_thread(struct k_thread *main_thread, char *stack_ptr,
 				k_thread_entry_t _main);
 #endif /* CONFIG_ARCH_HAS_CUSTOM_SWAP_TO_MAIN */
 
+/**
+ * @brief Save coprocessor states on an IPI
+ *
+ * The function, invoked by the IPI handler, is used by cross-CPU lazy context
+ * switches. It saves the relevant coprocessor context(s) before signalling the
+ * waiting CPU that it has finished.
+ */
+void arch_ipi_lazy_coprocessors_save(void);
+
 #if defined(CONFIG_FPU) && defined(CONFIG_FPU_SHARING)
 /**
  * @brief Disable floating point context preservation

kernel/ipi.c

@@ -105,4 +105,8 @@ void z_sched_ipi(void)
 		z_time_slice();
 	}
 #endif /* CONFIG_TIMESLICING */
+
+#ifdef CONFIG_ARCH_IPI_LAZY_COPROCESSORS_SAVE
+	arch_ipi_lazy_coprocessors_save();
+#endif
 }