From 08d213fca2b306d550c49e7ce503cda75a038c08 Mon Sep 17 00:00:00 2001 From: Pierre-Louis Bossart Date: Wed, 7 Apr 2021 14:50:28 -0500 Subject: [PATCH 01/10] ASoC: codecs: add SoundWire mockup device support To test the host controller and bus management code, it is currently required to have a physical SoundWire peripheral attached to the bus. To help with pre-silicon or early hardware enablement, it would be very useful to have a SoundWire 'mockup' device that is exposed in platform firmware but does not drive any signal on the bus. This is different to the existing ASoC 'dummy' codec uses for I2S/TDM, the SoundWire spec makes it clear that a device that is not attached to the bus is not permitted to interact with the bus, be it for command/control or data. This patch exposes a 'mockup' device, with a minimalist driver, with 4 partID values reserved by Intel for such test configurations. The mockup device exposes one full-duplex DAI based on 2 ports (DP1 for playback and DP8 for capture). The capture data port is just virtual, such a mockup device is prevented by the SoundWire specification from presenting any data generated by a Source port without being Attached. All the callbacks exposed by the SoundWire Slave interface are populated, even if they just return immediately. This is intentional to describe what a minimal codec driver should do and implement and help new codec vendors provide support for their devices. Signed-off-by: Pierre-Louis Bossart --- sound/soc/codecs/Kconfig | 18 ++ sound/soc/codecs/Makefile | 2 + sound/soc/codecs/sdw-mockup.c | 310 ++++++++++++++++++++++++++++++++++ 3 files changed, 330 insertions(+) create mode 100644 sound/soc/codecs/sdw-mockup.c diff --git a/sound/soc/codecs/Kconfig b/sound/soc/codecs/Kconfig index 2a7b3e36306953..f7cb28317bb000 100644 --- a/sound/soc/codecs/Kconfig +++ b/sound/soc/codecs/Kconfig @@ -186,6 +186,7 @@ config SND_SOC_ALL_CODECS imply SND_SOC_RT715_SDCA_SDW imply SND_SOC_RT1308_SDW imply SND_SOC_RT1316_SDW + imply SND_SOC_SDW_MOCKUP imply SND_SOC_SGTL5000 imply SND_SOC_SI476X imply SND_SOC_SIMPLE_AMPLIFIER @@ -1278,6 +1279,23 @@ config SND_SOC_RT715_SDCA_SDW select REGMAP_SOUNDWIRE select REGMAP_SOUNDWIRE_MBQ +config SND_SOC_SDW_MOCKUP + tristate "SoundWire mockup codec" + depends on EXPERT + depends on SOUNDWIRE + help + This option enables a SoundWire mockup codec that does not drive the + bus, take part in the command/command protocol or generate data on a + Source port. + This option is only intended to be used for tests on a device + with a connector, in combination with a bus analyzer, or to test new + topologies that differ from the actual hardware layout. + This mockup device could be totally virtual but could also be a + real physical one with one key restriction: it is not allowed by the + SoundWire specification to be configured via a sideband mechanism and + generate audio data for capture. However, nothing prevents such a + peripheral device from snooping the bus. + #Freescale sgtl5000 codec config SND_SOC_SGTL5000 tristate "Freescale SGTL5000 CODEC" diff --git a/sound/soc/codecs/Makefile b/sound/soc/codecs/Makefile index 0efdba609048b1..65450fb1f30a29 100644 --- a/sound/soc/codecs/Makefile +++ b/sound/soc/codecs/Makefile @@ -202,6 +202,7 @@ snd-soc-rt711-objs := rt711.o rt711-sdw.o snd-soc-rt711-sdca-objs := rt711-sdca.o rt711-sdca-sdw.o snd-soc-rt715-objs := rt715.o rt715-sdw.o snd-soc-rt715-sdca-objs := rt715-sdca.o rt715-sdca-sdw.o +snd-soc-sdw-mockup-objs := sdw-mockup.o snd-soc-sgtl5000-objs := sgtl5000.o snd-soc-alc5623-objs := alc5623.o snd-soc-alc5632-objs := alc5632.o @@ -524,6 +525,7 @@ obj-$(CONFIG_SND_SOC_RT711) += snd-soc-rt711.o obj-$(CONFIG_SND_SOC_RT711_SDCA_SDW) += snd-soc-rt711-sdca.o obj-$(CONFIG_SND_SOC_RT715) += snd-soc-rt715.o obj-$(CONFIG_SND_SOC_RT715_SDCA_SDW) += snd-soc-rt715-sdca.o +obj-$(CONFIG_SND_SOC_SDW_MOCKUP) += snd-soc-sdw-mockup.o obj-$(CONFIG_SND_SOC_SGTL5000) += snd-soc-sgtl5000.o obj-$(CONFIG_SND_SOC_SIGMADSP) += snd-soc-sigmadsp.o obj-$(CONFIG_SND_SOC_SIGMADSP_I2C) += snd-soc-sigmadsp-i2c.o diff --git a/sound/soc/codecs/sdw-mockup.c b/sound/soc/codecs/sdw-mockup.c new file mode 100644 index 00000000000000..a4f79eb2c69d27 --- /dev/null +++ b/sound/soc/codecs/sdw-mockup.c @@ -0,0 +1,310 @@ +// SPDX-License-Identifier: GPL-2.0-only +// +// sdw-mockup.c -- a mockup SoundWire codec for tests where only the host +// drives the bus. +// +// Copyright(c) 2021 Intel Corporation +// +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct sdw_mockup_priv { + struct sdw_slave *slave; +}; + +struct sdw_stream_data { + struct sdw_stream_runtime *sdw_stream; +}; + +static int sdw_mockup_component_probe(struct snd_soc_component *component) +{ + return 0; +} + +static void sdw_mockup_component_remove(struct snd_soc_component *component) +{ +} + +static const struct snd_soc_component_driver snd_soc_sdw_mockup_component = { + .probe = sdw_mockup_component_probe, + .remove = sdw_mockup_component_remove, +}; + +static int sdw_mockup_set_sdw_stream(struct snd_soc_dai *dai, void *sdw_stream, + int direction) +{ + struct sdw_stream_data *stream; + + if (!sdw_stream) + return 0; + + stream = kzalloc(sizeof(*stream), GFP_KERNEL); + if (!stream) + return -ENOMEM; + + stream->sdw_stream = sdw_stream; + + /* Use tx_mask or rx_mask to configure stream tag and set dma_data */ + if (direction == SNDRV_PCM_STREAM_PLAYBACK) + dai->playback_dma_data = stream; + else + dai->capture_dma_data = stream; + + return 0; +} + +static void sdw_mockup_shutdown(struct snd_pcm_substream *substream, + struct snd_soc_dai *dai) +{ + struct sdw_stream_data *stream; + + stream = snd_soc_dai_get_dma_data(dai, substream); + snd_soc_dai_set_dma_data(dai, substream, NULL); + kfree(stream); +} + +static int sdw_mockup_pcm_hw_params(struct snd_pcm_substream *substream, + struct snd_pcm_hw_params *params, + struct snd_soc_dai *dai) +{ + struct snd_soc_component *component = dai->component; + struct sdw_mockup_priv *sdw_mockup = snd_soc_component_get_drvdata(component); + struct sdw_stream_config stream_config; + struct sdw_port_config port_config; + enum sdw_data_direction direction; + struct sdw_stream_data *stream; + int num_channels; + int port; + int ret; + + stream = snd_soc_dai_get_dma_data(dai, substream); + if (!stream) + return -EINVAL; + + if (!sdw_mockup->slave) + return -EINVAL; + + /* SoundWire specific configuration */ + if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) { + direction = SDW_DATA_DIR_RX; + port = 1; + } else { + direction = SDW_DATA_DIR_TX; + port = 8; + } + + stream_config.frame_rate = params_rate(params); + stream_config.ch_count = params_channels(params); + stream_config.bps = snd_pcm_format_width(params_format(params)); + stream_config.direction = direction; + + num_channels = params_channels(params); + port_config.ch_mask = (1 << num_channels) - 1; + port_config.num = port; + + ret = sdw_stream_add_slave(sdw_mockup->slave, &stream_config, + &port_config, 1, stream->sdw_stream); + if (ret) + dev_err(dai->dev, "Unable to configure port\n"); + + return ret; +} + +static int sdw_mockup_pcm_hw_free(struct snd_pcm_substream *substream, + struct snd_soc_dai *dai) +{ + struct snd_soc_component *component = dai->component; + struct sdw_mockup_priv *sdw_mockup = snd_soc_component_get_drvdata(component); + struct sdw_stream_data *stream = + snd_soc_dai_get_dma_data(dai, substream); + + if (!sdw_mockup->slave) + return -EINVAL; + + sdw_stream_remove_slave(sdw_mockup->slave, stream->sdw_stream); + return 0; +} + +static const struct snd_soc_dai_ops sdw_mockup_ops = { + .hw_params = sdw_mockup_pcm_hw_params, + .hw_free = sdw_mockup_pcm_hw_free, + .set_sdw_stream = sdw_mockup_set_sdw_stream, + .shutdown = sdw_mockup_shutdown, +}; + +static struct snd_soc_dai_driver sdw_mockup_dai[] = { + { + .name = "sdw-mockup-aif1", + .id = 1, + .playback = { + .stream_name = "DP1 Playback", + .channels_min = 1, + .channels_max = 2, + }, + .capture = { + .stream_name = "DP8 Capture", + .channels_min = 1, + .channels_max = 2, + }, + .ops = &sdw_mockup_ops, + }, +}; + +static int sdw_mockup_update_status(struct sdw_slave *slave, + enum sdw_slave_status status) +{ + return 0; +} + +static int sdw_mockup_read_prop(struct sdw_slave *slave) +{ + struct sdw_slave_prop *prop = &slave->prop; + int nval; + int i, j; + u32 bit; + unsigned long addr; + struct sdw_dpn_prop *dpn; + + prop->paging_support = false; + + /* + * first we need to allocate memory for set bits in port lists + * the port allocation is completely arbitrary: + * DP0 is not supported + * DP1 is sink + * DP8 is source + */ + prop->source_ports = BIT(8); + prop->sink_ports = BIT(1); + + nval = hweight32(prop->source_ports); + prop->src_dpn_prop = devm_kcalloc(&slave->dev, nval, + sizeof(*prop->src_dpn_prop), + GFP_KERNEL); + if (!prop->src_dpn_prop) + return -ENOMEM; + + i = 0; + dpn = prop->src_dpn_prop; + addr = prop->source_ports; + for_each_set_bit(bit, &addr, 32) { + dpn[i].num = bit; + dpn[i].type = SDW_DPN_FULL; + dpn[i].simple_ch_prep_sm = true; + i++; + } + + /* do this again for sink now */ + nval = hweight32(prop->sink_ports); + prop->sink_dpn_prop = devm_kcalloc(&slave->dev, nval, + sizeof(*prop->sink_dpn_prop), + GFP_KERNEL); + if (!prop->sink_dpn_prop) + return -ENOMEM; + + j = 0; + dpn = prop->sink_dpn_prop; + addr = prop->sink_ports; + for_each_set_bit(bit, &addr, 32) { + dpn[j].num = bit; + dpn[j].type = SDW_DPN_FULL; + dpn[j].simple_ch_prep_sm = true; + j++; + } + + prop->simple_clk_stop_capable = true; + + /* wake-up event */ + prop->wake_capable = 0; + + return 0; +} + +static int sdw_mockup_bus_config(struct sdw_slave *slave, + struct sdw_bus_params *params) +{ + return 0; +} + +static int sdw_mockup_interrupt_callback(struct sdw_slave *slave, + struct sdw_slave_intr_status *status) +{ + return 0; +} + +static const struct sdw_slave_ops sdw_mockup_slave_ops = { + .read_prop = sdw_mockup_read_prop, + .interrupt_callback = sdw_mockup_interrupt_callback, + .update_status = sdw_mockup_update_status, + .bus_config = sdw_mockup_bus_config, +}; + +static int sdw_mockup_sdw_probe(struct sdw_slave *slave, + const struct sdw_device_id *id) +{ + struct device *dev = &slave->dev; + struct sdw_mockup_priv *sdw_mockup; + int ret; + + sdw_mockup = devm_kzalloc(dev, sizeof(*sdw_mockup), GFP_KERNEL); + if (!sdw_mockup) + return -ENOMEM; + + dev_set_drvdata(dev, sdw_mockup); + sdw_mockup->slave = slave; + + ret = devm_snd_soc_register_component(dev, + &snd_soc_sdw_mockup_component, + sdw_mockup_dai, + ARRAY_SIZE(sdw_mockup_dai)); + + return ret; +} + +static int sdw_mockup_sdw_remove(struct sdw_slave *slave) +{ + return 0; +} + +/* + * Intel reserved parts ID with the following mapping expected: + * 0xAAAA: generic full-duplex codec + * 0xAA55: headset codec (mock-up of RT711/RT5682) - full-duplex + * 0x55AA: amplifier (mock-up of RT1308/Maxim 98373) - playback only with + * IV feedback + * 0x5555: mic codec (mock-up of RT715) - capture-only + */ +static const struct sdw_device_id sdw_mockup_id[] = { + SDW_SLAVE_ENTRY_EXT(0x0105, 0xAAAA, 0x0, 0, 0), + SDW_SLAVE_ENTRY_EXT(0x0105, 0xAA55, 0x0, 0, 0), + SDW_SLAVE_ENTRY_EXT(0x0105, 0x55AA, 0x0, 0, 0), + SDW_SLAVE_ENTRY_EXT(0x0105, 0x5555, 0x0, 0, 0), + {}, +}; +MODULE_DEVICE_TABLE(sdw, sdw_mockup_id); + +static struct sdw_driver sdw_mockup_sdw_driver = { + .driver = { + .name = "sdw-mockup", + .owner = THIS_MODULE, + }, + .probe = sdw_mockup_sdw_probe, + .remove = sdw_mockup_sdw_remove, + .ops = &sdw_mockup_slave_ops, + .id_table = sdw_mockup_id, +}; +module_sdw_driver(sdw_mockup_sdw_driver); + +MODULE_DESCRIPTION("ASoC SDW mockup codec driver"); +MODULE_AUTHOR("Pierre-Louis Bossart "); +MODULE_LICENSE("GPL"); From fca3dd512799908821d130ff865c0ab26e082751 Mon Sep 17 00:00:00 2001 From: Pierre-Louis Bossart Date: Wed, 7 Apr 2021 16:05:24 -0500 Subject: [PATCH 02/10] ASoC: soc-acpi: cnl: add table for SoundWire mockup devices Add support for two configurations with SoundWire mockup devices that emulate the two CometLake configurations with one and two amps. This patch helps test the SOF firmware on an UpExtreme board without any hardware connected, e.g. by doing a loopback of the playback stream on capture streams. The mapping of the partIDs is as follows: 0xAAAA: generic full-duplex codec (not currently used) 0xAA55: headset codec (mock-up of RT711/RT5682) - full-duplex 0x55AA: amplifier (mock-up of RT1308/RT1316/Maxim 98373) - playback with IV sense feedback 0x5555: mic codec (mock-up of RT715) - capture-only The tables are added in a separate file to allow for mockup codecs to be added on other platforms, but the mapping to specific topologies remains platform-specific. Signed-off-by: Pierre-Louis Bossart --- sound/soc/intel/common/Makefile | 3 +- .../intel/common/soc-acpi-intel-cnl-match.c | 15 ++ .../common/soc-acpi-intel-sdw-mockup-match.c | 166 ++++++++++++++++++ .../common/soc-acpi-intel-sdw-mockup-match.h | 17 ++ 4 files changed, 200 insertions(+), 1 deletion(-) create mode 100644 sound/soc/intel/common/soc-acpi-intel-sdw-mockup-match.c create mode 100644 sound/soc/intel/common/soc-acpi-intel-sdw-mockup-match.h diff --git a/sound/soc/intel/common/Makefile b/sound/soc/intel/common/Makefile index 12a205ccdeeb6f..fef0b2d1de68b1 100644 --- a/sound/soc/intel/common/Makefile +++ b/sound/soc/intel/common/Makefile @@ -9,7 +9,8 @@ snd-soc-acpi-intel-match-objs := soc-acpi-intel-byt-match.o soc-acpi-intel-cht-m soc-acpi-intel-cml-match.o soc-acpi-intel-icl-match.o \ soc-acpi-intel-tgl-match.o soc-acpi-intel-ehl-match.o \ soc-acpi-intel-jsl-match.o soc-acpi-intel-adl-match.o \ - soc-acpi-intel-hda-match.o + soc-acpi-intel-hda-match.o \ + soc-acpi-intel-sdw-mockup-match.o obj-$(CONFIG_SND_SOC_INTEL_SST) += snd-soc-sst-dsp.o snd-soc-sst-ipc.o obj-$(CONFIG_SND_SOC_ACPI_INTEL_MATCH) += snd-soc-acpi-intel-match.o diff --git a/sound/soc/intel/common/soc-acpi-intel-cnl-match.c b/sound/soc/intel/common/soc-acpi-intel-cnl-match.c index ec77a57a07ba02..6e8a01791f1521 100644 --- a/sound/soc/intel/common/soc-acpi-intel-cnl-match.c +++ b/sound/soc/intel/common/soc-acpi-intel-cnl-match.c @@ -9,6 +9,7 @@ #include #include #include "../skylake/skl.h" +#include "soc-acpi-intel-sdw-mockup-match.h" static struct skl_machine_pdata cnl_pdata = { .use_tplg_pcm = true, @@ -60,6 +61,20 @@ struct snd_soc_acpi_mach snd_soc_acpi_intel_cnl_sdw_machines[] = { .sof_fw_filename = "sof-cnl.ri", .sof_tplg_filename = "sof-cnl-rt5682-sdw2.tplg" }, + { + .link_mask = GENMASK(3, 0), + .links = sdw_mockup_headset_2amps_mic, + .drv_name = "sof_sdw", + .sof_fw_filename = "sof-cnl.ri", + .sof_tplg_filename = "sof-cml-rt711-rt1308-rt715.tplg", + }, + { + .link_mask = BIT(0) | BIT(1) | BIT(3), + .links = sdw_mockup_headset_1amp_mic, + .drv_name = "sof_sdw", + .sof_fw_filename = "sof-cnl.ri", + .sof_tplg_filename = "sof-cml-rt711-rt1308-mono-rt715.tplg", + }, {} }; EXPORT_SYMBOL_GPL(snd_soc_acpi_intel_cnl_sdw_machines); diff --git a/sound/soc/intel/common/soc-acpi-intel-sdw-mockup-match.c b/sound/soc/intel/common/soc-acpi-intel-sdw-mockup-match.c new file mode 100644 index 00000000000000..a3d33997736ab4 --- /dev/null +++ b/sound/soc/intel/common/soc-acpi-intel-sdw-mockup-match.c @@ -0,0 +1,166 @@ +// SPDX-License-Identifier: GPL-2.0-only +// +// soc-acpi-intel-sdw-mockup-match.c - tables and support for SoundWire +// mockup device ACPI enumeration. +// +// Copyright (c) 2021, Intel Corporation. +// + +#include +#include +#include "soc-acpi-intel-sdw-mockup-match.h" + +static const struct snd_soc_acpi_endpoint sdw_mockup_single_endpoint = { + .num = 0, + .aggregated = 0, + .group_position = 0, + .group_id = 0, +}; + +static const struct snd_soc_acpi_endpoint sdw_mockup_l_endpoint = { + .num = 0, + .aggregated = 1, + .group_position = 0, + .group_id = 1, +}; + +static const struct snd_soc_acpi_endpoint sdw_mockup_r_endpoint = { + .num = 0, + .aggregated = 1, + .group_position = 1, + .group_id = 1, +}; + +static const struct snd_soc_acpi_adr_device sdw_mockup_headset_0_adr[] = { + { + .adr = 0x0000000105AA5500ull, + .num_endpoints = 1, + .endpoints = &sdw_mockup_single_endpoint, + .name_prefix = "sdw_mockup_headset0" + } +}; + +static const struct snd_soc_acpi_adr_device sdw_mockup_headset_1_adr[] = { + { + .adr = 0x0001000105AA5500ull, + .num_endpoints = 1, + .endpoints = &sdw_mockup_single_endpoint, + .name_prefix = "sdw_mockup_headset1" + } +}; + +static const struct snd_soc_acpi_adr_device sdw_mockup_amp_1_adr[] = { + { + .adr = 0x000100010555AA00ull, + .num_endpoints = 1, + .endpoints = &sdw_mockup_single_endpoint, + .name_prefix = "sdw_mockup_amp1" + } +}; + +static const struct snd_soc_acpi_adr_device sdw_mockup_amp_2_adr[] = { + { + .adr = 0x000200010555AA00ull, + .num_endpoints = 1, + .endpoints = &sdw_mockup_single_endpoint, + .name_prefix = "sdw_mockup_amp2" + } +}; + +static const struct snd_soc_acpi_adr_device sdw_mockup_mic_0_adr[] = { + { + .adr = 0x0000000105555500ull, + .num_endpoints = 1, + .endpoints = &sdw_mockup_single_endpoint, + .name_prefix = "sdw_mockup_mic0" + } +}; + +static const struct snd_soc_acpi_adr_device sdw_mockup_mic_3_adr[] = { + { + .adr = 0x0003000105555500ull, + .num_endpoints = 1, + .endpoints = &sdw_mockup_single_endpoint, + .name_prefix = "sdw_mockup_mic3" + } +}; + +static const struct snd_soc_acpi_adr_device sdw_mockup_amp_1_group1_adr[] = { + { + .adr = 0x000100010555AA00ull, + .num_endpoints = 1, + .endpoints = &sdw_mockup_l_endpoint, + .name_prefix = "sdw_mockup_amp1_l" + } +}; + +static const struct snd_soc_acpi_adr_device sdw_mockup_amp_2_group1_adr[] = { + { + .adr = 0x000200010555AA00ull, + .num_endpoints = 1, + .endpoints = &sdw_mockup_r_endpoint, + .name_prefix = "sdw_mockup_amp2_r" + } +}; + +const struct snd_soc_acpi_link_adr sdw_mockup_headset_1amp_mic[] = { + { + .mask = BIT(0), + .num_adr = ARRAY_SIZE(sdw_mockup_headset_0_adr), + .adr_d = sdw_mockup_headset_0_adr, + }, + { + .mask = BIT(1), + .num_adr = ARRAY_SIZE(sdw_mockup_amp_1_adr), + .adr_d = sdw_mockup_amp_1_adr, + }, + { + .mask = BIT(3), + .num_adr = ARRAY_SIZE(sdw_mockup_mic_3_adr), + .adr_d = sdw_mockup_mic_3_adr, + }, + {} +}; + +const struct snd_soc_acpi_link_adr sdw_mockup_headset_2amps_mic[] = { + { + .mask = BIT(0), + .num_adr = ARRAY_SIZE(sdw_mockup_headset_0_adr), + .adr_d = sdw_mockup_headset_0_adr, + }, + { + .mask = BIT(1), + .num_adr = ARRAY_SIZE(sdw_mockup_amp_1_group1_adr), + .adr_d = sdw_mockup_amp_1_group1_adr, + }, + { + .mask = BIT(2), + .num_adr = ARRAY_SIZE(sdw_mockup_amp_2_group1_adr), + .adr_d = sdw_mockup_amp_2_group1_adr, + }, + { + .mask = BIT(3), + .num_adr = ARRAY_SIZE(sdw_mockup_mic_3_adr), + .adr_d = sdw_mockup_mic_3_adr, + }, + {} +}; + +const struct snd_soc_acpi_link_adr sdw_mockup_mic_headset_1amp[] = { + { + .mask = BIT(1), + .num_adr = ARRAY_SIZE(sdw_mockup_headset_1_adr), + .adr_d = sdw_mockup_headset_1_adr, + }, + { + .mask = BIT(2), + .num_adr = ARRAY_SIZE(sdw_mockup_amp_2_adr), + .adr_d = sdw_mockup_amp_2_adr, + }, + { + .mask = BIT(0), + .num_adr = ARRAY_SIZE(sdw_mockup_mic_0_adr), + .adr_d = sdw_mockup_mic_0_adr, + }, + {} +}; diff --git a/sound/soc/intel/common/soc-acpi-intel-sdw-mockup-match.h b/sound/soc/intel/common/soc-acpi-intel-sdw-mockup-match.h new file mode 100644 index 00000000000000..c99eecd19e0349 --- /dev/null +++ b/sound/soc/intel/common/soc-acpi-intel-sdw-mockup-match.h @@ -0,0 +1,17 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * soc-acpi-intel-sdw-mockup-match.h - tables and support for SoundWire + * mockup device ACPI enumeration. + * + * Copyright (c) 2021, Intel Corporation. + * + */ + +#ifndef _SND_SOC_ACPI_INTEL_SDW_MOCKUP_MATCH +#define _SND_SOC_ACPI_INTEL_SDW_MOCKUP_MATCH + +extern const struct snd_soc_acpi_link_adr sdw_mockup_headset_1amp_mic[]; +extern const struct snd_soc_acpi_link_adr sdw_mockup_headset_2amps_mic[]; +extern const struct snd_soc_acpi_link_adr sdw_mockup_mic_headset_1amp[]; + +#endif From 00e1fe4e6289f78a2ab8724e0999bcfe8d1f3034 Mon Sep 17 00:00:00 2001 From: Pierre-Louis Bossart Date: Thu, 6 May 2021 11:30:49 -0500 Subject: [PATCH 03/10] ASoC: soc-acpi: tgl: add table for SoundWire mockup devices Follow the same idea as for CNL/UpExtreme and add mockup test first. They will only be selected if the SSDT is modified to add such mockup devices. Signed-off-by: Pierre-Louis Bossart --- .../intel/common/soc-acpi-intel-tgl-match.c | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/sound/soc/intel/common/soc-acpi-intel-tgl-match.c b/sound/soc/intel/common/soc-acpi-intel-tgl-match.c index b5f05b81a584d5..cd3a197b47ee82 100644 --- a/sound/soc/intel/common/soc-acpi-intel-tgl-match.c +++ b/sound/soc/intel/common/soc-acpi-intel-tgl-match.c @@ -8,6 +8,7 @@ #include #include +#include "soc-acpi-intel-sdw-mockup-match.h" static const struct snd_soc_acpi_codecs tgl_codecs = { .num_codecs = 1, @@ -351,6 +352,28 @@ EXPORT_SYMBOL_GPL(snd_soc_acpi_intel_tgl_machines); /* this table is used when there is no I2S codec present */ struct snd_soc_acpi_mach snd_soc_acpi_intel_tgl_sdw_machines[] = { + /* mockup tests need to be first */ + { + .link_mask = GENMASK(3, 0), + .links = sdw_mockup_headset_2amps_mic, + .drv_name = "sof_sdw", + .sof_fw_filename = "sof-tgl.ri", + .sof_tplg_filename = "sof-tgl-rt711-rt1308-rt715.tplg", + }, + { + .link_mask = BIT(0) | BIT(1) | BIT(3), + .links = sdw_mockup_headset_1amp_mic, + .drv_name = "sof_sdw", + .sof_fw_filename = "sof-tgl.ri", + .sof_tplg_filename = "sof-tgl-rt711-rt1308-mono-rt715.tplg", + }, + { + .link_mask = BIT(0) | BIT(1) | BIT(2), + .links = sdw_mockup_mic_headset_1amp, + .drv_name = "sof_sdw", + .sof_fw_filename = "sof-tgl.ri", + .sof_tplg_filename = "sof-tgl-rt715-rt711-rt1308-mono.tplg", + }, { .link_mask = 0x7, .links = tgl_sdw_rt711_link1_rt1308_link2_rt715_link0, From 1c2be90921b7a40d3dc662e25504fa7f9e1ee862 Mon Sep 17 00:00:00 2001 From: Pierre-Louis Bossart Date: Wed, 7 Apr 2021 16:13:35 -0500 Subject: [PATCH 04/10] ASoC: Intel: boards: sof_sdw: add SoundWire mockup codecs for tests Add support for SoundWire mockup devices. The configurations assume the same topology as the CML SoundWire devices and can be used to test the SOF firmware on a development board (RVP, UpExtreme) without any hardware connected. Signed-off-by: Pierre-Louis Bossart --- sound/soc/intel/boards/sof_sdw.c | 41 ++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/sound/soc/intel/boards/sof_sdw.c b/sound/soc/intel/boards/sof_sdw.c index 1da5ad3d5a7de3..9cce6b2b3934e6 100644 --- a/sound/soc/intel/boards/sof_sdw.c +++ b/sound/soc/intel/boards/sof_sdw.c @@ -315,6 +315,19 @@ static const struct snd_soc_ops sdw_ops = { .shutdown = sdw_shutdown, }; +static int sof_sdw_mic_codec_mockup_init(const struct snd_soc_acpi_link_adr *link, + struct snd_soc_dai_link *dai_links, + struct sof_sdw_codec_info *info, + bool playback) +{ + /* + * force DAI link to use same ID as RT715 and DMIC + * to reuse topologies + */ + dai_links->id = SDW_DMIC_DAI_ID; + return 0; +} + static struct sof_sdw_codec_info codec_info_list[] = { { .part_id = 0x700, @@ -397,6 +410,34 @@ static struct sof_sdw_codec_info codec_info_list[] = { .dai_name = "rt5682-sdw", .init = sof_sdw_rt5682_init, }, + { + .part_id = 0xaaaa, /* generic codec mockup */ + .version_id = 0, + .direction = {true, true}, + .dai_name = "sdw-mockup-aif1", + .init = NULL, + }, + { + .part_id = 0xaa55, /* headset codec mockup */ + .version_id = 0, + .direction = {true, true}, + .dai_name = "sdw-mockup-aif1", + .init = NULL, + }, + { + .part_id = 0x55aa, /* amplifier mockup */ + .version_id = 0, + .direction = {true, false}, + .dai_name = "sdw-mockup-aif1", + .init = NULL, + }, + { + .part_id = 0x5555, + .version_id = 0, + .direction = {false, true}, + .dai_name = "sdw-mockup-aif1", + .init = sof_sdw_mic_codec_mockup_init, + }, }; static inline int find_codec_info_part(u64 adr) From 38251472635e5223acdf262ff6bc235ddec4d6f7 Mon Sep 17 00:00:00 2001 From: Bard Liao Date: Tue, 20 Apr 2021 14:23:35 -0500 Subject: [PATCH 05/10] soundwire: stream: don't abort bank switch on Command_Ignored/-ENODATA This change is needed for support of mockup devices, which by construction will not provide any answer to a bank switch, but it's also legit for regular cases. If for some reason a device loses sync and cannot handle a bank switch, we should go ahead anyways. The devices can always resync later. The only case where the error flow should be used is when there is a Command_Aborted composite answer from SoundWire devices. Signed-off-by: Bard Liao --- drivers/soundwire/stream.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/soundwire/stream.c b/drivers/soundwire/stream.c index 1eaedaaba09441..1652aedebf54df 100644 --- a/drivers/soundwire/stream.c +++ b/drivers/soundwire/stream.c @@ -698,7 +698,7 @@ static int sdw_bank_switch(struct sdw_bus *bus, int m_rt_count) else ret = sdw_transfer(bus, wr_msg); - if (ret < 0) { + if (ret < 0 && ret != -ENODATA) { dev_err(bus->dev, "Slave frame_ctrl reg write failed\n"); goto error; } From a3fa3a2fe4d44118f9a99a3a0a5d4d842f80134c Mon Sep 17 00:00:00 2001 From: Pierre-Louis Bossart Date: Wed, 7 Apr 2021 17:58:27 -0500 Subject: [PATCH 06/10] soundwire: add flag to ignore all command/control for mockup devices SoundWire mockup devices don't take part in the command/control protocol, so all commands will complete with -ENODATA or Command_Ignored results. With a flag, we can suppress such errors in the bus management and make it appear as if all read/writes succeed. Signed-off-by: Pierre-Louis Bossart --- include/linux/soundwire/sdw.h | 3 +++ sound/soc/codecs/sdw-mockup.c | 2 ++ 2 files changed, 5 insertions(+) diff --git a/include/linux/soundwire/sdw.h b/include/linux/soundwire/sdw.h index 8ca736e92d5af5..34a49429eb26fa 100644 --- a/include/linux/soundwire/sdw.h +++ b/include/linux/soundwire/sdw.h @@ -661,6 +661,8 @@ struct sdw_slave_ops { * initialized * @first_interrupt_done: status flag tracking if the interrupt handling * for a Slave happens for the first time after enumeration + * @is_mockup_device: status flag used to squelch errors in the command/control + * protocol for SoundWire mockup devices */ struct sdw_slave { struct sdw_slave_id id; @@ -683,6 +685,7 @@ struct sdw_slave { struct completion initialization_complete; u32 unattach_request; bool first_interrupt_done; + bool is_mockup_device; }; #define dev_to_sdw_dev(_dev) container_of(_dev, struct sdw_slave, dev) diff --git a/sound/soc/codecs/sdw-mockup.c b/sound/soc/codecs/sdw-mockup.c index a4f79eb2c69d27..8ea13cfa9f8ede 100644 --- a/sound/soc/codecs/sdw-mockup.c +++ b/sound/soc/codecs/sdw-mockup.c @@ -263,6 +263,8 @@ static int sdw_mockup_sdw_probe(struct sdw_slave *slave, dev_set_drvdata(dev, sdw_mockup); sdw_mockup->slave = slave; + slave->is_mockup_device = true; + ret = devm_snd_soc_register_component(dev, &snd_soc_sdw_mockup_component, sdw_mockup_dai, From ce469da621f217df872bb20990893d9a534e9558 Mon Sep 17 00:00:00 2001 From: Pierre-Louis Bossart Date: Wed, 7 Apr 2021 17:53:00 -0500 Subject: [PATCH 07/10] soundwire: bus: squelch error returned by mockup devices All read and writes from/to SoundWire mockup devices will return -ENODATA/Command_Ignored, this patch forces a Command_OK result to let the bus perform the required configurations, e.g. for the Data Ports, which will only have an effect on the Master side. Signed-off-by: Pierre-Louis Bossart --- drivers/soundwire/bus.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/drivers/soundwire/bus.c b/drivers/soundwire/bus.c index 85bcf60f969706..a6c7d9a2817a4e 100644 --- a/drivers/soundwire/bus.c +++ b/drivers/soundwire/bus.c @@ -390,7 +390,10 @@ sdw_nread_no_pm(struct sdw_slave *slave, u32 addr, size_t count, u8 *val) if (ret < 0) return ret; - return sdw_transfer(slave->bus, &msg); + ret = sdw_transfer(slave->bus, &msg); + if (slave->is_mockup_device) + ret = 0; + return ret; } static int @@ -404,7 +407,10 @@ sdw_nwrite_no_pm(struct sdw_slave *slave, u32 addr, size_t count, u8 *val) if (ret < 0) return ret; - return sdw_transfer(slave->bus, &msg); + ret = sdw_transfer(slave->bus, &msg); + if (slave->is_mockup_device) + ret = 0; + return ret; } int sdw_write_no_pm(struct sdw_slave *slave, u32 addr, u8 value) From ac32416fa6c296952a182477a58bf0f4ee5d7643 Mon Sep 17 00:00:00 2001 From: Bard Liao Date: Tue, 20 Apr 2021 14:30:59 -0500 Subject: [PATCH 08/10] soundwire: stream: don't program mockup device ports Mockup devices don't take part in command/control operations and their virtual ports shall not be programmed. Signed-off-by: Bard Liao --- drivers/soundwire/stream.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/drivers/soundwire/stream.c b/drivers/soundwire/stream.c index 1652aedebf54df..2d1b548a44b9e7 100644 --- a/drivers/soundwire/stream.c +++ b/drivers/soundwire/stream.c @@ -133,6 +133,9 @@ static int sdw_program_slave_port_params(struct sdw_bus *bus, int ret; u8 wbuf; + if (s_rt->slave->is_mockup_device) + return 0; + dpn_prop = sdw_get_slave_dpn_prop(s_rt->slave, s_rt->direction, t_params->port_num); From 3dbf15293920c161ebb511df133a4e4b5b714713 Mon Sep 17 00:00:00 2001 From: Pierre-Louis Bossart Date: Wed, 14 Apr 2021 16:05:47 -0500 Subject: [PATCH 09/10] soundwire: cadence: add debugfs interface for PDI loopbacks For debug, it's interesting to create a loopback stream for each link and use debugfs to set a source and target PDI. The target PDI would need to be an RX port and use the same register configurations as the source PDI. This capability allows e.g. for the headphone playback stream to be snooped on the headset capture stream, or alternatively for the addition of a dedicated loopback stream, in addition of regular capture for that link. This patch only adds the debugfs part, the port/PDI handling will be handled in the next patches. Signed-off-by: Pierre-Louis Bossart --- drivers/soundwire/cadence_master.c | 44 ++++++++++++++++++++++++++++++ drivers/soundwire/cadence_master.h | 3 ++ 2 files changed, 47 insertions(+) diff --git a/drivers/soundwire/cadence_master.c b/drivers/soundwire/cadence_master.c index 25950422b085ee..834aa70a1cdd3d 100644 --- a/drivers/soundwire/cadence_master.c +++ b/drivers/soundwire/cadence_master.c @@ -450,6 +450,40 @@ static int cdns_parity_error_injection(void *data, u64 value) DEFINE_DEBUGFS_ATTRIBUTE(cdns_parity_error_fops, NULL, cdns_parity_error_injection, "%llu\n"); +static int cdns_set_pdi_loopback_source(void *data, u64 value) +{ + struct sdw_cdns *cdns = data; + unsigned int pdi_out_num = cdns->pcm.num_bd + cdns->pcm.num_out; + + if (value > pdi_out_num) + return -EINVAL; + + /* Userspace changed the hardware state behind the kernel's back */ + add_taint(TAINT_USER, LOCKDEP_STILL_OK); + + cdns->pdi_loopback_source = value; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(cdns_pdi_loopback_source_fops, NULL, cdns_set_pdi_loopback_source, "%llu\n"); + +static int cdns_set_pdi_loopback_target(void *data, u64 value) +{ + struct sdw_cdns *cdns = data; + unsigned int pdi_in_num = cdns->pcm.num_bd + cdns->pcm.num_in; + + if (value > pdi_in_num) + return -EINVAL; + + /* Userspace changed the hardware state behind the kernel's back */ + add_taint(TAINT_USER, LOCKDEP_STILL_OK); + + cdns->pdi_loopback_target = value; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(cdns_pdi_loopback_target_fops, NULL, cdns_set_pdi_loopback_target, "%llu\n"); + /** * sdw_cdns_debugfs_init() - Cadence debugfs init * @cdns: Cadence instance @@ -464,6 +498,16 @@ void sdw_cdns_debugfs_init(struct sdw_cdns *cdns, struct dentry *root) debugfs_create_file("cdns-parity-error-injection", 0200, root, cdns, &cdns_parity_error_fops); + + cdns->pdi_loopback_source = -1; + cdns->pdi_loopback_target = -1; + + debugfs_create_file("cdns-pdi-loopback-source", 0200, root, cdns, + &cdns_pdi_loopback_source_fops); + + debugfs_create_file("cdns-pdi-loopback-target", 0200, root, cdns, + &cdns_pdi_loopback_target_fops); + } EXPORT_SYMBOL_GPL(sdw_cdns_debugfs_init); diff --git a/drivers/soundwire/cadence_master.h b/drivers/soundwire/cadence_master.h index 4d1aab5b5ec2d6..86d582c54de789 100644 --- a/drivers/soundwire/cadence_master.h +++ b/drivers/soundwire/cadence_master.h @@ -129,6 +129,9 @@ struct sdw_cdns { struct sdw_cdns_streams pcm; struct sdw_cdns_streams pdm; + int pdi_loopback_source; + int pdi_loopback_target; + void __iomem *registers; bool link_up; From 79160ee2b0c2c9707c8fc97485384139bd955522 Mon Sep 17 00:00:00 2001 From: Pierre-Louis Bossart Date: Wed, 14 Apr 2021 16:44:31 -0500 Subject: [PATCH 10/10] soundwire: cadence: override PDI configurations to create loopback When we set a source PDI, the target PDI parameters will be overridden by the source register values. The loopback streams can be independently enabled on each link. While the loopback source and target can be configured before any stream is active on each link, the loopback stream should only be prepared/triggered when the playback stream is prepared. Otherwise all registers might be programmed to their reset values and the loopback will not succeed. The SoundWire bus driver currently does not allow two streams to be triggered at the same time, so the playback will have to be started first, and later the loopback. Signed-off-by: Pierre-Louis Bossart --- drivers/soundwire/cadence_master.c | 130 +++++++++++++++++++++-------- 1 file changed, 95 insertions(+), 35 deletions(-) diff --git a/drivers/soundwire/cadence_master.c b/drivers/soundwire/cadence_master.c index 834aa70a1cdd3d..cc40880bff88a2 100644 --- a/drivers/soundwire/cadence_master.c +++ b/drivers/soundwire/cadence_master.c @@ -1330,20 +1330,37 @@ static int cdns_port_params(struct sdw_bus *bus, struct sdw_port_params *p_params, unsigned int bank) { struct sdw_cdns *cdns = bus_to_cdns(bus); - int dpn_config = 0, dpn_config_off; + int dpn_config_off_source; + int dpn_config_off_target; + int target_num = p_params->num; + int source_num = p_params->num; + bool override = false; + int dpn_config; + + if (target_num == cdns->pdi_loopback_target && + cdns->pdi_loopback_source != -1) { + source_num = cdns->pdi_loopback_source; + override = true; + } - if (bank) - dpn_config_off = CDNS_DPN_B1_CONFIG(p_params->num); - else - dpn_config_off = CDNS_DPN_B0_CONFIG(p_params->num); + if (bank) { + dpn_config_off_source = CDNS_DPN_B1_CONFIG(source_num); + dpn_config_off_target = CDNS_DPN_B1_CONFIG(target_num); + } else { + dpn_config_off_source = CDNS_DPN_B0_CONFIG(source_num); + dpn_config_off_target = CDNS_DPN_B0_CONFIG(target_num); + } - dpn_config = cdns_readl(cdns, dpn_config_off); + dpn_config = cdns_readl(cdns, dpn_config_off_source); - u32p_replace_bits(&dpn_config, (p_params->bps - 1), CDNS_DPN_CONFIG_WL); - u32p_replace_bits(&dpn_config, p_params->flow_mode, CDNS_DPN_CONFIG_PORT_FLOW); - u32p_replace_bits(&dpn_config, p_params->data_mode, CDNS_DPN_CONFIG_PORT_DAT); + /* use port params if there is no loopback, otherwise use source as is */ + if (!override) { + u32p_replace_bits(&dpn_config, p_params->bps - 1, CDNS_DPN_CONFIG_WL); + u32p_replace_bits(&dpn_config, p_params->flow_mode, CDNS_DPN_CONFIG_PORT_FLOW); + u32p_replace_bits(&dpn_config, p_params->data_mode, CDNS_DPN_CONFIG_PORT_DAT); + } - cdns_writel(cdns, dpn_config_off, dpn_config); + cdns_writel(cdns, dpn_config_off_target, dpn_config); return 0; } @@ -1353,11 +1370,27 @@ static int cdns_transport_params(struct sdw_bus *bus, enum sdw_reg_bank bank) { struct sdw_cdns *cdns = bus_to_cdns(bus); - int dpn_offsetctrl = 0, dpn_offsetctrl_off; - int dpn_config = 0, dpn_config_off; - int dpn_hctrl = 0, dpn_hctrl_off; - int num = t_params->port_num; - int dpn_samplectrl_off; + int dpn_config; + int dpn_config_off_source; + int dpn_config_off_target; + int dpn_hctrl; + int dpn_hctrl_off_source; + int dpn_hctrl_off_target; + int dpn_offsetctrl; + int dpn_offsetctrl_off_source; + int dpn_offsetctrl_off_target; + int dpn_samplectrl; + int dpn_samplectrl_off_source; + int dpn_samplectrl_off_target; + int source_num = t_params->port_num; + int target_num = t_params->port_num; + bool override = false; + + if (target_num == cdns->pdi_loopback_target && + cdns->pdi_loopback_source != -1) { + source_num = cdns->pdi_loopback_source; + override = true; + } /* * Note: Only full data port is supported on the Master side for @@ -1365,32 +1398,59 @@ static int cdns_transport_params(struct sdw_bus *bus, */ if (bank) { - dpn_config_off = CDNS_DPN_B1_CONFIG(num); - dpn_samplectrl_off = CDNS_DPN_B1_SAMPLE_CTRL(num); - dpn_hctrl_off = CDNS_DPN_B1_HCTRL(num); - dpn_offsetctrl_off = CDNS_DPN_B1_OFFSET_CTRL(num); + dpn_config_off_source = CDNS_DPN_B1_CONFIG(source_num); + dpn_hctrl_off_source = CDNS_DPN_B1_HCTRL(source_num); + dpn_offsetctrl_off_source = CDNS_DPN_B1_OFFSET_CTRL(source_num); + dpn_samplectrl_off_source = CDNS_DPN_B1_SAMPLE_CTRL(source_num); + + dpn_config_off_target = CDNS_DPN_B1_CONFIG(target_num); + dpn_hctrl_off_target = CDNS_DPN_B1_HCTRL(target_num); + dpn_offsetctrl_off_target = CDNS_DPN_B1_OFFSET_CTRL(target_num); + dpn_samplectrl_off_target = CDNS_DPN_B1_SAMPLE_CTRL(target_num); + } else { - dpn_config_off = CDNS_DPN_B0_CONFIG(num); - dpn_samplectrl_off = CDNS_DPN_B0_SAMPLE_CTRL(num); - dpn_hctrl_off = CDNS_DPN_B0_HCTRL(num); - dpn_offsetctrl_off = CDNS_DPN_B0_OFFSET_CTRL(num); + dpn_config_off_source = CDNS_DPN_B0_CONFIG(source_num); + dpn_hctrl_off_source = CDNS_DPN_B0_HCTRL(source_num); + dpn_offsetctrl_off_source = CDNS_DPN_B0_OFFSET_CTRL(source_num); + dpn_samplectrl_off_source = CDNS_DPN_B0_SAMPLE_CTRL(source_num); + + dpn_config_off_target = CDNS_DPN_B0_CONFIG(target_num); + dpn_hctrl_off_target = CDNS_DPN_B0_HCTRL(target_num); + dpn_offsetctrl_off_target = CDNS_DPN_B0_OFFSET_CTRL(target_num); + dpn_samplectrl_off_target = CDNS_DPN_B0_SAMPLE_CTRL(target_num); } - dpn_config = cdns_readl(cdns, dpn_config_off); - u32p_replace_bits(&dpn_config, t_params->blk_grp_ctrl, CDNS_DPN_CONFIG_BGC); - u32p_replace_bits(&dpn_config, t_params->blk_pkg_mode, CDNS_DPN_CONFIG_BPM); - cdns_writel(cdns, dpn_config_off, dpn_config); + dpn_config = cdns_readl(cdns, dpn_config_off_source); + if (!override) { + u32p_replace_bits(&dpn_config, t_params->blk_grp_ctrl, CDNS_DPN_CONFIG_BGC); + u32p_replace_bits(&dpn_config, t_params->blk_pkg_mode, CDNS_DPN_CONFIG_BPM); + } + cdns_writel(cdns, dpn_config_off_target, dpn_config); - u32p_replace_bits(&dpn_offsetctrl, t_params->offset1, CDNS_DPN_OFFSET_CTRL_1); - u32p_replace_bits(&dpn_offsetctrl, t_params->offset2, CDNS_DPN_OFFSET_CTRL_2); - cdns_writel(cdns, dpn_offsetctrl_off, dpn_offsetctrl); + if (!override) { + dpn_offsetctrl = 0; + u32p_replace_bits(&dpn_offsetctrl, t_params->offset1, CDNS_DPN_OFFSET_CTRL_1); + u32p_replace_bits(&dpn_offsetctrl, t_params->offset2, CDNS_DPN_OFFSET_CTRL_2); + } else { + dpn_offsetctrl = cdns_readl(cdns, dpn_offsetctrl_off_source); + } + cdns_writel(cdns, dpn_offsetctrl_off_target, dpn_offsetctrl); - u32p_replace_bits(&dpn_hctrl, t_params->hstart, CDNS_DPN_HCTRL_HSTART); - u32p_replace_bits(&dpn_hctrl, t_params->hstop, CDNS_DPN_HCTRL_HSTOP); - u32p_replace_bits(&dpn_hctrl, t_params->lane_ctrl, CDNS_DPN_HCTRL_LCTRL); + if (!override) { + dpn_hctrl = 0; + u32p_replace_bits(&dpn_hctrl, t_params->hstart, CDNS_DPN_HCTRL_HSTART); + u32p_replace_bits(&dpn_hctrl, t_params->hstop, CDNS_DPN_HCTRL_HSTOP); + u32p_replace_bits(&dpn_hctrl, t_params->lane_ctrl, CDNS_DPN_HCTRL_LCTRL); + } else { + dpn_hctrl = cdns_readl(cdns, dpn_hctrl_off_source); + } + cdns_writel(cdns, dpn_hctrl_off_target, dpn_hctrl); - cdns_writel(cdns, dpn_hctrl_off, dpn_hctrl); - cdns_writel(cdns, dpn_samplectrl_off, (t_params->sample_interval - 1)); + if (!override) + dpn_samplectrl = t_params->sample_interval - 1; + else + dpn_samplectrl = cdns_readl(cdns, dpn_samplectrl_off_source); + cdns_writel(cdns, dpn_samplectrl_off_target, dpn_samplectrl); return 0; }