From 186152df4d431650154c3e3aefc7d3e1004987c8 Mon Sep 17 00:00:00 2001
From: Simon Trimmer <simont@opensource.cirrus.com>
Date: Mon, 13 Sep 2021 17:00:49 +0100
Subject: [PATCH] ASoC: wm_adsp: Split DSP power operations into helper functions
Git-commit: 186152df4d431650154c3e3aefc7d3e1004987c8
Patch-mainline: v5.16-rc1
References: bsc#1203699

This is preparation for moving the generic DSP support out of
ASoC. This change separates the generic handling of power and state
transitions from the DAPM API wrapper.

Signed-off-by: Simon Trimmer <simont@opensource.cirrus.com>
Signed-off-by: Charles Keepax <ckeepax@opensource.cirrus.com>
Link: https://lore.kernel.org/r/20210913160057.103842-9-simont@opensource.cirrus.com
Signed-off-by: Mark Brown <broonie@kernel.org>
Acked-by: Takashi Iwai <tiwai@suse.de>

---
 sound/soc/codecs/wm_adsp.c | 495 ++++++++++++++++++++-----------------
 1 file changed, 273 insertions(+), 222 deletions(-)

diff --git a/sound/soc/codecs/wm_adsp.c b/sound/soc/codecs/wm_adsp.c
index c0ca46e04418..1bca3922a6b8 100644
--- a/sound/soc/codecs/wm_adsp.c
+++ b/sound/soc/codecs/wm_adsp.c
@@ -2808,114 +2808,129 @@ int wm_adsp1_init(struct wm_adsp *dsp)
 }
 EXPORT_SYMBOL_GPL(wm_adsp1_init);
 
-int wm_adsp1_event(struct snd_soc_dapm_widget *w,
-		   struct snd_kcontrol *kcontrol,
-		   int event)
+static int cs_dsp_adsp1_power_up(struct wm_adsp *dsp)
 {
-	struct snd_soc_component *component = snd_soc_dapm_to_component(w->dapm);
-	struct wm_adsp *dsps = snd_soc_component_get_drvdata(component);
-	struct wm_adsp *dsp = &dsps[w->shift];
-	struct wm_coeff_ctl *ctl;
-	int ret;
 	unsigned int val;
-
-	dsp->component = component;
+	int ret;
 
 	mutex_lock(&dsp->pwr_lock);
 
-	switch (event) {
-	case SND_SOC_DAPM_POST_PMU:
-		regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
-				   ADSP1_SYS_ENA, ADSP1_SYS_ENA);
-
-		/*
-		 * For simplicity set the DSP clock rate to be the
-		 * SYSCLK rate rather than making it configurable.
-		 */
-		if (dsp->sysclk_reg) {
-			ret = regmap_read(dsp->regmap, dsp->sysclk_reg, &val);
-			if (ret != 0) {
-				cs_dsp_err(dsp, "Failed to read SYSCLK state: %d\n", ret);
-				goto err_mutex;
-			}
-
-			val = (val & dsp->sysclk_mask) >> dsp->sysclk_shift;
+	regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
+			   ADSP1_SYS_ENA, ADSP1_SYS_ENA);
 
-			ret = regmap_update_bits(dsp->regmap,
-						 dsp->base + ADSP1_CONTROL_31,
-						 ADSP1_CLK_SEL_MASK, val);
-			if (ret != 0) {
-				cs_dsp_err(dsp, "Failed to set clock rate: %d\n", ret);
-				goto err_mutex;
-			}
+	/*
+	 * For simplicity set the DSP clock rate to be the
+	 * SYSCLK rate rather than making it configurable.
+	 */
+	if (dsp->sysclk_reg) {
+		ret = regmap_read(dsp->regmap, dsp->sysclk_reg, &val);
+		if (ret != 0) {
+			cs_dsp_err(dsp, "Failed to read SYSCLK state: %d\n", ret);
+			goto err_mutex;
 		}
 
-		ret = cs_dsp_load(dsp);
-		if (ret != 0)
-			goto err_ena;
+		val = (val & dsp->sysclk_mask) >> dsp->sysclk_shift;
 
-		ret = cs_dsp_adsp1_setup_algs(dsp);
-		if (ret != 0)
-			goto err_ena;
+		ret = regmap_update_bits(dsp->regmap,
+					 dsp->base + ADSP1_CONTROL_31,
+					 ADSP1_CLK_SEL_MASK, val);
+		if (ret != 0) {
+			cs_dsp_err(dsp, "Failed to set clock rate: %d\n", ret);
+			goto err_mutex;
+		}
+	}
 
-		ret = cs_dsp_load_coeff(dsp);
-		if (ret != 0)
-			goto err_ena;
+	ret = cs_dsp_load(dsp);
+	if (ret != 0)
+		goto err_ena;
 
-		/* Initialize caches for enabled and unset controls */
-		ret = cs_dsp_coeff_init_control_caches(dsp);
-		if (ret != 0)
-			goto err_ena;
+	ret = cs_dsp_adsp1_setup_algs(dsp);
+	if (ret != 0)
+		goto err_ena;
 
-		/* Sync set controls */
-		ret = cs_dsp_coeff_sync_controls(dsp);
-		if (ret != 0)
-			goto err_ena;
+	ret = cs_dsp_load_coeff(dsp);
+	if (ret != 0)
+		goto err_ena;
 
-		dsp->booted = true;
+	/* Initialize caches for enabled and unset controls */
+	ret = cs_dsp_coeff_init_control_caches(dsp);
+	if (ret != 0)
+		goto err_ena;
 
-		/* Start the core running */
-		regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
-				   ADSP1_CORE_ENA | ADSP1_START,
-				   ADSP1_CORE_ENA | ADSP1_START);
+	/* Sync set controls */
+	ret = cs_dsp_coeff_sync_controls(dsp);
+	if (ret != 0)
+		goto err_ena;
 
-		dsp->running = true;
-		break;
+	dsp->booted = true;
 
-	case SND_SOC_DAPM_PRE_PMD:
-		dsp->running = false;
-		dsp->booted = false;
+	/* Start the core running */
+	regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
+			   ADSP1_CORE_ENA | ADSP1_START,
+			   ADSP1_CORE_ENA | ADSP1_START);
 
-		/* Halt the core */
-		regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
-				   ADSP1_CORE_ENA | ADSP1_START, 0);
+	dsp->running = true;
 
-		regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_19,
-				   ADSP1_WDMA_BUFFER_LENGTH_MASK, 0);
+	mutex_unlock(&dsp->pwr_lock);
 
-		regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
-				   ADSP1_SYS_ENA, 0);
+	return 0;
 
-		list_for_each_entry(ctl, &dsp->ctl_list, list)
-			ctl->enabled = 0;
+err_ena:
+	regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
+			   ADSP1_SYS_ENA, 0);
+err_mutex:
+	mutex_unlock(&dsp->pwr_lock);
+	return ret;
+}
 
+static void cs_dsp_adsp1_power_down(struct wm_adsp *dsp)
+{
+	struct wm_coeff_ctl *ctl;
 
-		cs_dsp_free_alg_regions(dsp);
-		break;
+	mutex_lock(&dsp->pwr_lock);
 
-	default:
-		break;
-	}
+	dsp->running = false;
+	dsp->booted = false;
 
-	mutex_unlock(&dsp->pwr_lock);
+	/* Halt the core */
+	regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
+			   ADSP1_CORE_ENA | ADSP1_START, 0);
 
-	return 0;
+	regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_19,
+			   ADSP1_WDMA_BUFFER_LENGTH_MASK, 0);
 
-err_ena:
 	regmap_update_bits(dsp->regmap, dsp->base + ADSP1_CONTROL_30,
 			   ADSP1_SYS_ENA, 0);
-err_mutex:
+
+	list_for_each_entry(ctl, &dsp->ctl_list, list)
+		ctl->enabled = 0;
+
+	cs_dsp_free_alg_regions(dsp);
+
 	mutex_unlock(&dsp->pwr_lock);
+}
+
+int wm_adsp1_event(struct snd_soc_dapm_widget *w,
+		   struct snd_kcontrol *kcontrol,
+		   int event)
+{
+	struct snd_soc_component *component = snd_soc_dapm_to_component(w->dapm);
+	struct wm_adsp *dsps = snd_soc_component_get_drvdata(component);
+	struct wm_adsp *dsp = &dsps[w->shift];
+	int ret = 0;
+
+	dsp->component = component;
+
+	switch (event) {
+	case SND_SOC_DAPM_POST_PMU:
+		ret = cs_dsp_adsp1_power_up(dsp);
+		break;
+	case SND_SOC_DAPM_PRE_PMD:
+		cs_dsp_adsp1_power_down(dsp);
+		break;
+	default:
+		break;
+	}
 
 	return ret;
 }
@@ -3019,63 +3034,6 @@ static void cs_dsp_adsp2v2_disable_core(struct wm_adsp *dsp)
 	regmap_write(dsp->regmap, dsp->base + ADSP2V2_WDMA_CONFIG_2, 0);
 }
 
-static void wm_adsp_boot_work(struct work_struct *work)
-{
-	struct wm_adsp *dsp = container_of(work,
-					   struct wm_adsp,
-					   boot_work);
-	int ret;
-
-	mutex_lock(&dsp->pwr_lock);
-
-	if (dsp->ops->enable_memory) {
-		ret = dsp->ops->enable_memory(dsp);
-		if (ret != 0)
-			goto err_mutex;
-	}
-
-	if (dsp->ops->enable_core) {
-		ret = dsp->ops->enable_core(dsp);
-		if (ret != 0)
-			goto err_mem;
-	}
-
-	ret = cs_dsp_load(dsp);
-	if (ret != 0)
-		goto err_ena;
-
-	ret = dsp->ops->setup_algs(dsp);
-	if (ret != 0)
-		goto err_ena;
-
-	ret = cs_dsp_load_coeff(dsp);
-	if (ret != 0)
-		goto err_ena;
-
-	/* Initialize caches for enabled and unset controls */
-	ret = cs_dsp_coeff_init_control_caches(dsp);
-	if (ret != 0)
-		goto err_ena;
-
-	if (dsp->ops->disable_core)
-		dsp->ops->disable_core(dsp);
-
-	dsp->booted = true;
-
-	mutex_unlock(&dsp->pwr_lock);
-
-	return;
-
-err_ena:
-	if (dsp->ops->disable_core)
-		dsp->ops->disable_core(dsp);
-err_mem:
-	if (dsp->ops->disable_memory)
-		dsp->ops->disable_memory(dsp);
-err_mutex:
-	mutex_unlock(&dsp->pwr_lock);
-}
-
 static int cs_dsp_halo_configure_mpu(struct wm_adsp *dsp, unsigned int lock_regions)
 {
 	struct reg_sequence config[] = {
@@ -3185,39 +3143,108 @@ static void cs_dsp_halo_stop_watchdog(struct wm_adsp *dsp)
 			   HALO_WDT_EN_MASK, 0);
 }
 
+static void cs_dsp_power_up(struct wm_adsp *dsp)
+{
+	int ret;
+
+	mutex_lock(&dsp->pwr_lock);
+
+	if (dsp->ops->enable_memory) {
+		ret = dsp->ops->enable_memory(dsp);
+		if (ret != 0)
+			goto err_mutex;
+	}
+
+	if (dsp->ops->enable_core) {
+		ret = dsp->ops->enable_core(dsp);
+		if (ret != 0)
+			goto err_mem;
+	}
+
+	ret = cs_dsp_load(dsp);
+	if (ret != 0)
+		goto err_ena;
+
+	ret = dsp->ops->setup_algs(dsp);
+	if (ret != 0)
+		goto err_ena;
+
+	ret = cs_dsp_load_coeff(dsp);
+	if (ret != 0)
+		goto err_ena;
+
+	/* Initialize caches for enabled and unset controls */
+	ret = cs_dsp_coeff_init_control_caches(dsp);
+	if (ret != 0)
+		goto err_ena;
+
+	if (dsp->ops->disable_core)
+		dsp->ops->disable_core(dsp);
+
+	dsp->booted = true;
+
+	mutex_unlock(&dsp->pwr_lock);
+
+	return;
+
+err_ena:
+	if (dsp->ops->disable_core)
+		dsp->ops->disable_core(dsp);
+err_mem:
+	if (dsp->ops->disable_memory)
+		dsp->ops->disable_memory(dsp);
+err_mutex:
+	mutex_unlock(&dsp->pwr_lock);
+}
+
+static void cs_dsp_power_down(struct wm_adsp *dsp)
+{
+	struct wm_coeff_ctl *ctl;
+
+	mutex_lock(&dsp->pwr_lock);
+
+	cs_dsp_debugfs_clear(dsp);
+
+	dsp->fw_id = 0;
+	dsp->fw_id_version = 0;
+
+	dsp->booted = false;
+
+	if (dsp->ops->disable_memory)
+		dsp->ops->disable_memory(dsp);
+
+	list_for_each_entry(ctl, &dsp->ctl_list, list)
+		ctl->enabled = 0;
+
+	cs_dsp_free_alg_regions(dsp);
+
+	mutex_unlock(&dsp->pwr_lock);
+
+	cs_dsp_dbg(dsp, "Shutdown complete\n");
+}
+
+static void wm_adsp_boot_work(struct work_struct *work)
+{
+	struct wm_adsp *dsp = container_of(work,
+					   struct wm_adsp,
+					   boot_work);
+
+	cs_dsp_power_up(dsp);
+}
+
 int wm_adsp_early_event(struct snd_soc_dapm_widget *w,
 			struct snd_kcontrol *kcontrol, int event)
 {
 	struct snd_soc_component *component = snd_soc_dapm_to_component(w->dapm);
 	struct wm_adsp *dsps = snd_soc_component_get_drvdata(component);
 	struct wm_adsp *dsp = &dsps[w->shift];
-	struct wm_coeff_ctl *ctl;
 
 	switch (event) {
 	case SND_SOC_DAPM_PRE_PMU:
 		queue_work(system_unbound_wq, &dsp->boot_work);
 		break;
 	case SND_SOC_DAPM_PRE_PMD:
-		mutex_lock(&dsp->pwr_lock);
-
-		cs_dsp_debugfs_clear(dsp);
-
-		dsp->fw_id = 0;
-		dsp->fw_id_version = 0;
-
-		dsp->booted = false;
-
-		if (dsp->ops->disable_memory)
-			dsp->ops->disable_memory(dsp);
-
-		list_for_each_entry(ctl, &dsp->ctl_list, list)
-			ctl->enabled = 0;
-
-		cs_dsp_free_alg_regions(dsp);
-
-		mutex_unlock(&dsp->pwr_lock);
-
-		cs_dsp_dbg(dsp, "Shutdown complete\n");
+		cs_dsp_power_down(dsp);
 		break;
 	default:
 		break;
@@ -3240,102 +3267,126 @@ static void cs_dsp_adsp2_stop_core(struct wm_adsp *dsp)
 			   ADSP2_CORE_ENA | ADSP2_START, 0);
 }
 
-int wm_adsp_event(struct snd_soc_dapm_widget *w,
-		  struct snd_kcontrol *kcontrol, int event)
+static int wm_adsp_event_post_run(struct wm_adsp *dsp)
+{
+	if (wm_adsp_fw[dsp->fw].num_caps != 0)
+		return wm_adsp_buffer_init(dsp);
+
+	return 0;
+}
+
+static void wm_adsp_event_post_stop(struct wm_adsp *dsp)
+{
+	if (wm_adsp_fw[dsp->fw].num_caps != 0)
+		wm_adsp_buffer_free(dsp);
+
+	dsp->fatal_error = false;
+}
+
+static int cs_dsp_run(struct wm_adsp *dsp)
 {
-	struct snd_soc_component *component = snd_soc_dapm_to_component(w->dapm);
-	struct wm_adsp *dsps = snd_soc_component_get_drvdata(component);
-	struct wm_adsp *dsp = &dsps[w->shift];
 	int ret;
 
-	switch (event) {
-	case SND_SOC_DAPM_POST_PMU:
-		flush_work(&dsp->boot_work);
+	mutex_lock(&dsp->pwr_lock);
 
-		mutex_lock(&dsp->pwr_lock);
+	if (!dsp->booted) {
+		ret = -EIO;
+		goto err;
+	}
 
-		if (!dsp->booted) {
-			ret = -EIO;
+	if (dsp->ops->enable_core) {
+		ret = dsp->ops->enable_core(dsp);
+		if (ret != 0)
 			goto err;
-		}
+	}
 
-		if (dsp->ops->enable_core) {
-			ret = dsp->ops->enable_core(dsp);
-			if (ret != 0)
-				goto err;
+	/* Sync set controls */
+	ret = cs_dsp_coeff_sync_controls(dsp);
+	if (ret != 0)
+		goto err;
+
+	if (dsp->ops->lock_memory) {
+		ret = dsp->ops->lock_memory(dsp, dsp->lock_regions);
+		if (ret != 0) {
+			cs_dsp_err(dsp, "Error configuring MPU: %d\n", ret);
+			goto err;
 		}
+	}
 
-		/* Sync set controls */
-		ret = cs_dsp_coeff_sync_controls(dsp);
+	if (dsp->ops->start_core) {
+		ret = dsp->ops->start_core(dsp);
 		if (ret != 0)
 			goto err;
+	}
 
-		if (dsp->ops->lock_memory) {
-			ret = dsp->ops->lock_memory(dsp, dsp->lock_regions);
-			if (ret != 0) {
-				cs_dsp_err(dsp, "Error configuring MPU: %d\n", ret);
-				goto err;
-			}
-		}
+	dsp->running = true;
 
-		if (dsp->ops->start_core) {
-			ret = dsp->ops->start_core(dsp);
-			if (ret != 0)
-				goto err;
-		}
+	ret = wm_adsp_event_post_run(dsp);
+	if (ret < 0)
+		goto err;
 
-		dsp->running = true;
+	mutex_unlock(&dsp->pwr_lock);
 
-		if (wm_adsp_fw[dsp->fw].num_caps != 0) {
-			ret = wm_adsp_buffer_init(dsp);
-			if (ret < 0)
-				goto err;
-		}
+	return 0;
 
-		mutex_unlock(&dsp->pwr_lock);
-		break;
+err:
+	if (dsp->ops->stop_core)
+		dsp->ops->stop_core(dsp);
+	if (dsp->ops->disable_core)
+		dsp->ops->disable_core(dsp);
+	mutex_unlock(&dsp->pwr_lock);
 
-	case SND_SOC_DAPM_PRE_PMD:
-		/* Tell the firmware to cleanup */
-		cs_dsp_signal_event_controls(dsp, CS_DSP_FW_EVENT_SHUTDOWN);
+	return ret;
+}
+
+static void cs_dsp_stop(struct wm_adsp *dsp)
+{
+	/* Tell the firmware to cleanup */
+	cs_dsp_signal_event_controls(dsp, CS_DSP_FW_EVENT_SHUTDOWN);
 
-		if (dsp->ops->stop_watchdog)
-			dsp->ops->stop_watchdog(dsp);
+	if (dsp->ops->stop_watchdog)
+		dsp->ops->stop_watchdog(dsp);
 
-		/* Log firmware state, it can be useful for analysis */
-		if (dsp->ops->show_fw_status)
-			dsp->ops->show_fw_status(dsp);
+	/* Log firmware state, it can be useful for analysis */
+	if (dsp->ops->show_fw_status)
+		dsp->ops->show_fw_status(dsp);
 
-		mutex_lock(&dsp->pwr_lock);
+	mutex_lock(&dsp->pwr_lock);
 
-		dsp->running = false;
+	dsp->running = false;
 
-		if (dsp->ops->stop_core)
-			dsp->ops->stop_core(dsp);
-		if (dsp->ops->disable_core)
-			dsp->ops->disable_core(dsp);
+	if (dsp->ops->stop_core)
+		dsp->ops->stop_core(dsp);
+	if (dsp->ops->disable_core)
+		dsp->ops->disable_core(dsp);
 
-		if (wm_adsp_fw[dsp->fw].num_caps != 0)
-			wm_adsp_buffer_free(dsp);
+	wm_adsp_event_post_stop(dsp);
 
-		dsp->fatal_error = false;
+	mutex_unlock(&dsp->pwr_lock);
 
-		mutex_unlock(&dsp->pwr_lock);
+	cs_dsp_dbg(dsp, "Execution stopped\n");
+}
 
-		cs_dsp_dbg(dsp, "Execution stopped\n");
-		break;
+int wm_adsp_event(struct snd_soc_dapm_widget *w,
+		  struct snd_kcontrol *kcontrol, int event)
+{
+	struct snd_soc_component *component = snd_soc_dapm_to_component(w->dapm);
+	struct wm_adsp *dsps = snd_soc_component_get_drvdata(component);
+	struct wm_adsp *dsp = &dsps[w->shift];
+	int ret = 0;
 
+	switch (event) {
+	case SND_SOC_DAPM_POST_PMU:
+		flush_work(&dsp->boot_work);
+		ret = cs_dsp_run(dsp);
+		break;
+	case SND_SOC_DAPM_PRE_PMD:
+		cs_dsp_stop(dsp);
+		break;
 	default:
 		break;
 	}
 
-	return 0;
-err:
-	if (dsp->ops->stop_core)
-		dsp->ops->stop_core(dsp);
-	if (dsp->ops->disable_core)
-		dsp->ops->disable_core(dsp);
-	mutex_unlock(&dsp->pwr_lock);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(wm_adsp_event);
-- 
2.35.3