~hc/RK356X_SDK_RELEASE.git

..	..	@@ -8,15 +8,23 @@
8	8
9	9	#include <linux/delay.h>
10	10	#include <linux/device.h>
	11	+#include <linux/debugfs.h>
11	12	#include <linux/interrupt.h>
	13	+#include <linux/io.h>
12	14	#include <linux/module.h>
13	15	#include <linux/mod_devicetable.h>
	16	+#include <linux/pm_runtime.h>
14	17	#include <linux/soundwire/sdw_registers.h>
15	18	#include <linux/soundwire/sdw.h>
16	19	#include <sound/pcm_params.h>
17	20	#include <sound/soc.h>
	21	+#include <linux/workqueue.h>
18	22	#include "bus.h"
19	23	#include "cadence_master.h"
	24	+
	25	+static int interrupt_mask;
	26	+module_param_named(cnds_mcp_int_mask, interrupt_mask, int, 0444);
	27	+MODULE_PARM_DESC(cdns_mcp_int_mask, "Cadence MCP IntMask");
20	28
21	29	#define CDNS_MCP_CONFIG 0x0
22	30
..	..	@@ -42,11 +50,15 @@
42	50	#define CDNS_MCP_CONTROL_CMD_ACCEPT BIT(1)
43	51	#define CDNS_MCP_CONTROL_BLOCK_WAKEUP BIT(0)
44	52
45		-
46	53	#define CDNS_MCP_CMDCTRL 0x8
	54	+
	55	+#define CDNS_MCP_CMDCTRL_INSERT_PARITY_ERR BIT(2)
	56	+
47	57	#define CDNS_MCP_SSPSTAT 0xC
48	58	#define CDNS_MCP_FRAME_SHAPE 0x10
49	59	#define CDNS_MCP_FRAME_SHAPE_INIT 0x14
	60	+#define CDNS_MCP_FRAME_SHAPE_COL_MASK GENMASK(2, 0)
	61	+#define CDNS_MCP_FRAME_SHAPE_ROW_MASK GENMASK(7, 3)
50	62
51	63	#define CDNS_MCP_CONFIG_UPDATE 0x18
52	64	#define CDNS_MCP_CONFIG_UPDATE_BIT BIT(0)
..	..	@@ -56,6 +68,7 @@
56	68	#define CDNS_MCP_SSP_CTRL1 0x28
57	69	#define CDNS_MCP_CLK_CTRL0 0x30
58	70	#define CDNS_MCP_CLK_CTRL1 0x38
	71	+#define CDNS_MCP_CLK_MCLKD_MASK GENMASK(7, 0)
59	72
60	73	#define CDNS_MCP_STAT 0x40
61	74
..	..	@@ -66,6 +79,7 @@
66	79	#define CDNS_MCP_INTMASK 0x48
67	80
68	81	#define CDNS_MCP_INT_IRQ BIT(31)
	82	+#define CDNS_MCP_INT_RESERVED1 GENMASK(30, 17)
69	83	#define CDNS_MCP_INT_WAKEUP BIT(16)
70	84	#define CDNS_MCP_INT_SLAVE_RSVD BIT(15)
71	85	#define CDNS_MCP_INT_SLAVE_ALERT BIT(14)
..	..	@@ -75,9 +89,14 @@
75	89	#define CDNS_MCP_INT_DPINT BIT(11)
76	90	#define CDNS_MCP_INT_CTRL_CLASH BIT(10)
77	91	#define CDNS_MCP_INT_DATA_CLASH BIT(9)
	92	+#define CDNS_MCP_INT_PARITY BIT(8)
78	93	#define CDNS_MCP_INT_CMD_ERR BIT(7)
	94	+#define CDNS_MCP_INT_RESERVED2 GENMASK(6, 4)
	95	+#define CDNS_MCP_INT_RX_NE BIT(3)
79	96	#define CDNS_MCP_INT_RX_WL BIT(2)
80	97	#define CDNS_MCP_INT_TXE BIT(1)
	98	+#define CDNS_MCP_INT_TXF BIT(0)
	99	+#define CDNS_MCP_INT_RESERVED (CDNS_MCP_INT_RESERVED1 \| CDNS_MCP_INT_RESERVED2)
81	100
82	101	#define CDNS_MCP_INTSET 0x4C
83	102
..	..	@@ -114,8 +133,7 @@
114	133	#define CDNS_MCP_CMD_SSP_TAG BIT(31)
115	134	#define CDNS_MCP_CMD_COMMAND GENMASK(30, 28)
116	135	#define CDNS_MCP_CMD_DEV_ADDR GENMASK(27, 24)
117		-#define CDNS_MCP_CMD_REG_ADDR_H GENMASK(23, 16)
118		-#define CDNS_MCP_CMD_REG_ADDR_L GENMASK(15, 8)
	136	+#define CDNS_MCP_CMD_REG_ADDR GENMASK(23, 8)
119	137	#define CDNS_MCP_CMD_REG_DATA GENMASK(7, 0)
120	138
121	139	#define CDNS_MCP_CMD_READ 2
..	..	@@ -157,6 +175,7 @@
157	175	#define CDNS_DPN_HCTRL_LCTRL GENMASK(10, 8)
158	176
159	177	#define CDNS_PORTCTRL 0x130
	178	+#define CDNS_PORTCTRL_TEST_FAILED BIT(1)
160	179	#define CDNS_PORTCTRL_DIRN BIT(7)
161	180	#define CDNS_PORTCTRL_BANK_INVERT BIT(8)
162	181
..	..	@@ -169,14 +188,7 @@
169	188	#define CDNS_PDI_CONFIG_PORT GENMASK(4, 0)
170	189
171	190	/* Driver defaults */
172		-
173		-#define CDNS_DEFAULT_CLK_DIVIDER 0
174		-#define CDNS_DEFAULT_FRAME_SHAPE 0x30
175		-#define CDNS_DEFAULT_SSP_INTERVAL 0x18
176	191	#define CDNS_TX_TIMEOUT 2000
177		-
178		-#define CDNS_PCM_PDI_OFFSET 0x2
179		-#define CDNS_PDM_PDI_OFFSET 0x6
180	192
181	193	#define CDNS_SCP_RX_FIFOLEVEL 0x2
182	194
..	..	@@ -203,32 +215,266 @@
203	215	cdns_writel(cdns, offset, tmp);
204	216	}
205	217
206		-static int cdns_clear_bit(struct sdw_cdns *cdns, int offset, u32 value)
	218	+static int cdns_set_wait(struct sdw_cdns *cdns, int offset, u32 mask, u32 value)
207	219	{
208	220	int timeout = 10;
209	221	u32 reg_read;
210	222
211		- writel(value, cdns->registers + offset);
212		-
213		- /* Wait for bit to be self cleared */
	223	+ /* Wait for bit to be set */
214	224	do {
215	225	reg_read = readl(cdns->registers + offset);
216		- if ((reg_read & value) == 0)
	226	+ if ((reg_read & mask) == value)
217	227	return 0;
218	228
219	229	timeout--;
220		- udelay(50);
	230	+ usleep_range(50, 100);
221	231	} while (timeout != 0);
222	232
223		- return -EAGAIN;
	233	+ return -ETIMEDOUT;
224	234	}
	235	+
	236	+static int cdns_clear_bit(struct sdw_cdns *cdns, int offset, u32 value)
	237	+{
	238	+ writel(value, cdns->registers + offset);
	239	+
	240	+ /* Wait for bit to be self cleared */
	241	+ return cdns_set_wait(cdns, offset, value, 0);
	242	+}
	243	+
	244	+/*
	245	+ * all changes to the MCP_CONFIG, MCP_CONTROL, MCP_CMDCTRL and MCP_PHYCTRL
	246	+ * need to be confirmed with a write to MCP_CONFIG_UPDATE
	247	+ */
	248	+static int cdns_config_update(struct sdw_cdns *cdns)
	249	+{
	250	+ int ret;
	251	+
	252	+ if (sdw_cdns_is_clock_stop(cdns)) {
	253	+ dev_err(cdns->dev, "Cannot program MCP_CONFIG_UPDATE in ClockStopMode\n");
	254	+ return -EINVAL;
	255	+ }
	256	+
	257	+ ret = cdns_clear_bit(cdns, CDNS_MCP_CONFIG_UPDATE,
	258	+ CDNS_MCP_CONFIG_UPDATE_BIT);
	259	+ if (ret < 0)
	260	+ dev_err(cdns->dev, "Config update timedout\n");
	261	+
	262	+ return ret;
	263	+}
	264	+
	265	+/*
	266	+ * debugfs
	267	+ */
	268	+#ifdef CONFIG_DEBUG_FS
	269	+
	270	+#define RD_BUF (2 * PAGE_SIZE)
	271	+
	272	+static ssize_t cdns_sprintf(struct sdw_cdns *cdns,
	273	+ char *buf, size_t pos, unsigned int reg)
	274	+{
	275	+ return scnprintf(buf + pos, RD_BUF - pos,
	276	+ "%4x\t%8x\n", reg, cdns_readl(cdns, reg));
	277	+}
	278	+
	279	+static int cdns_reg_show(struct seq_file s, void data)
	280	+{
	281	+ struct sdw_cdns *cdns = s->private;
	282	+ char *buf;
	283	+ ssize_t ret;
	284	+ int num_ports;
	285	+ int i, j;
	286	+
	287	+ buf = kzalloc(RD_BUF, GFP_KERNEL);
	288	+ if (!buf)
	289	+ return -ENOMEM;
	290	+
	291	+ ret = scnprintf(buf, RD_BUF, "Register Value\n");
	292	+ ret += scnprintf(buf + ret, RD_BUF - ret, "\nMCP Registers\n");
	293	+ /* 8 MCP registers */
	294	+ for (i = CDNS_MCP_CONFIG; i <= CDNS_MCP_PHYCTRL; i += sizeof(u32))
	295	+ ret += cdns_sprintf(cdns, buf, ret, i);
	296	+
	297	+ ret += scnprintf(buf + ret, RD_BUF - ret,
	298	+ "\nStatus & Intr Registers\n");
	299	+ /* 13 Status & Intr registers (offsets 0x70 and 0x74 not defined) */
	300	+ for (i = CDNS_MCP_STAT; i <= CDNS_MCP_FIFOSTAT; i += sizeof(u32))
	301	+ ret += cdns_sprintf(cdns, buf, ret, i);
	302	+
	303	+ ret += scnprintf(buf + ret, RD_BUF - ret,
	304	+ "\nSSP & Clk ctrl Registers\n");
	305	+ ret += cdns_sprintf(cdns, buf, ret, CDNS_MCP_SSP_CTRL0);
	306	+ ret += cdns_sprintf(cdns, buf, ret, CDNS_MCP_SSP_CTRL1);
	307	+ ret += cdns_sprintf(cdns, buf, ret, CDNS_MCP_CLK_CTRL0);
	308	+ ret += cdns_sprintf(cdns, buf, ret, CDNS_MCP_CLK_CTRL1);
	309	+
	310	+ ret += scnprintf(buf + ret, RD_BUF - ret,
	311	+ "\nDPn B0 Registers\n");
	312	+
	313	+ num_ports = cdns->num_ports;
	314	+
	315	+ for (i = 0; i < num_ports; i++) {
	316	+ ret += scnprintf(buf + ret, RD_BUF - ret,
	317	+ "\nDP-%d\n", i);
	318	+ for (j = CDNS_DPN_B0_CONFIG(i);
	319	+ j < CDNS_DPN_B0_ASYNC_CTRL(i); j += sizeof(u32))
	320	+ ret += cdns_sprintf(cdns, buf, ret, j);
	321	+ }
	322	+
	323	+ ret += scnprintf(buf + ret, RD_BUF - ret,
	324	+ "\nDPn B1 Registers\n");
	325	+ for (i = 0; i < num_ports; i++) {
	326	+ ret += scnprintf(buf + ret, RD_BUF - ret,
	327	+ "\nDP-%d\n", i);
	328	+
	329	+ for (j = CDNS_DPN_B1_CONFIG(i);
	330	+ j < CDNS_DPN_B1_ASYNC_CTRL(i); j += sizeof(u32))
	331	+ ret += cdns_sprintf(cdns, buf, ret, j);
	332	+ }
	333	+
	334	+ ret += scnprintf(buf + ret, RD_BUF - ret,
	335	+ "\nDPn Control Registers\n");
	336	+ for (i = 0; i < num_ports; i++)
	337	+ ret += cdns_sprintf(cdns, buf, ret,
	338	+ CDNS_PORTCTRL + i * CDNS_PORT_OFFSET);
	339	+
	340	+ ret += scnprintf(buf + ret, RD_BUF - ret,
	341	+ "\nPDIn Config Registers\n");
	342	+
	343	+ /* number of PDI and ports is interchangeable */
	344	+ for (i = 0; i < num_ports; i++)
	345	+ ret += cdns_sprintf(cdns, buf, ret, CDNS_PDI_CONFIG(i));
	346	+
	347	+ seq_printf(s, "%s", buf);
	348	+ kfree(buf);
	349	+
	350	+ return 0;
	351	+}
	352	+DEFINE_SHOW_ATTRIBUTE(cdns_reg);
	353	+
	354	+static int cdns_hw_reset(void *data, u64 value)
	355	+{
	356	+ struct sdw_cdns *cdns = data;
	357	+ int ret;
	358	+
	359	+ if (value != 1)
	360	+ return -EINVAL;
	361	+
	362	+ /* Userspace changed the hardware state behind the kernel's back */
	363	+ add_taint(TAINT_USER, LOCKDEP_STILL_OK);
	364	+
	365	+ ret = sdw_cdns_exit_reset(cdns);
	366	+
	367	+ dev_dbg(cdns->dev, "link hw_reset done: %d\n", ret);
	368	+
	369	+ return ret;
	370	+}
	371	+
	372	+DEFINE_DEBUGFS_ATTRIBUTE(cdns_hw_reset_fops, NULL, cdns_hw_reset, "%llu\n");
	373	+
	374	+static int cdns_parity_error_injection(void *data, u64 value)
	375	+{
	376	+ struct sdw_cdns *cdns = data;
	377	+ struct sdw_bus *bus;
	378	+ int ret;
	379	+
	380	+ if (value != 1)
	381	+ return -EINVAL;
	382	+
	383	+ bus = &cdns->bus;
	384	+
	385	+ /*
	386	+ * Resume Master device. If this results in a bus reset, the
	387	+ * Slave devices will re-attach and be re-enumerated.
	388	+ */
	389	+ ret = pm_runtime_get_sync(bus->dev);
	390	+ if (ret < 0 && ret != -EACCES) {
	391	+ dev_err_ratelimited(cdns->dev,
	392	+ "pm_runtime_get_sync failed in %s, ret %d\n",
	393	+ __func__, ret);
	394	+ pm_runtime_put_noidle(bus->dev);
	395	+ return ret;
	396	+ }
	397	+
	398	+ /*
	399	+ * wait long enough for Slave(s) to be in steady state. This
	400	+ * does not need to be super precise.
	401	+ */
	402	+ msleep(200);
	403	+
	404	+ /*
	405	+ * Take the bus lock here to make sure that any bus transactions
	406	+ * will be queued while we inject a parity error on a dummy read
	407	+ */
	408	+ mutex_lock(&bus->bus_lock);
	409	+
	410	+ /* program hardware to inject parity error */
	411	+ cdns_updatel(cdns, CDNS_MCP_CMDCTRL,
	412	+ CDNS_MCP_CMDCTRL_INSERT_PARITY_ERR,
	413	+ CDNS_MCP_CMDCTRL_INSERT_PARITY_ERR);
	414	+
	415	+ /* commit changes */
	416	+ cdns_updatel(cdns, CDNS_MCP_CONFIG_UPDATE,
	417	+ CDNS_MCP_CONFIG_UPDATE_BIT,
	418	+ CDNS_MCP_CONFIG_UPDATE_BIT);
	419	+
	420	+ /* do a broadcast dummy read to avoid bus clashes */
	421	+ ret = sdw_bread_no_pm_unlocked(&cdns->bus, 0xf, SDW_SCP_DEVID_0);
	422	+ dev_info(cdns->dev, "parity error injection, read: %d\n", ret);
	423	+
	424	+ /* program hardware to disable parity error */
	425	+ cdns_updatel(cdns, CDNS_MCP_CMDCTRL,
	426	+ CDNS_MCP_CMDCTRL_INSERT_PARITY_ERR,
	427	+ 0);
	428	+
	429	+ /* commit changes */
	430	+ cdns_updatel(cdns, CDNS_MCP_CONFIG_UPDATE,
	431	+ CDNS_MCP_CONFIG_UPDATE_BIT,
	432	+ CDNS_MCP_CONFIG_UPDATE_BIT);
	433	+
	434	+ /* Continue bus operation with parity error injection disabled */
	435	+ mutex_unlock(&bus->bus_lock);
	436	+
	437	+ /* Userspace changed the hardware state behind the kernel's back */
	438	+ add_taint(TAINT_USER, LOCKDEP_STILL_OK);
	439	+
	440	+ /*
	441	+ * allow Master device to enter pm_runtime suspend. This may
	442	+ * also result in Slave devices suspending.
	443	+ */
	444	+ pm_runtime_mark_last_busy(bus->dev);
	445	+ pm_runtime_put_autosuspend(bus->dev);
	446	+
	447	+ return 0;
	448	+}
	449	+
	450	+DEFINE_DEBUGFS_ATTRIBUTE(cdns_parity_error_fops, NULL,
	451	+ cdns_parity_error_injection, "%llu\n");
	452	+
	453	+/**
	454	+ * sdw_cdns_debugfs_init() - Cadence debugfs init
	455	+ * @cdns: Cadence instance
	456	+ * @root: debugfs root
	457	+ */
	458	+void sdw_cdns_debugfs_init(struct sdw_cdns cdns, struct dentry root)
	459	+{
	460	+ debugfs_create_file("cdns-registers", 0400, root, cdns, &cdns_reg_fops);
	461	+
	462	+ debugfs_create_file("cdns-hw-reset", 0200, root, cdns,
	463	+ &cdns_hw_reset_fops);
	464	+
	465	+ debugfs_create_file("cdns-parity-error-injection", 0200, root, cdns,
	466	+ &cdns_parity_error_fops);
	467	+}
	468	+EXPORT_SYMBOL_GPL(sdw_cdns_debugfs_init);
	469	+
	470	+#endif /* CONFIG_DEBUG_FS */
225	471
226	472	/*
227	473	* IO Calls
228	474	*/
229		-static enum sdw_command_response cdns_fill_msg_resp(
230		- struct sdw_cdns *cdns,
231		- struct sdw_msg *msg, int count, int offset)
	475	+static enum sdw_command_response
	476	+cdns_fill_msg_resp(struct sdw_cdns *cdns,
	477	+ struct sdw_msg *msg, int count, int offset)
232	478	{
233	479	int nack = 0, no_ack = 0;
234	480	int i;
..	..	@@ -237,33 +483,60 @@
237	483	for (i = 0; i < count; i++) {
238	484	if (!(cdns->response_buf[i] & CDNS_MCP_RESP_ACK)) {
239	485	no_ack = 1;
240		- dev_dbg(cdns->dev, "Msg Ack not received\n");
241		- if (cdns->response_buf[i] & CDNS_MCP_RESP_NACK) {
242		- nack = 1;
243		- dev_err(cdns->dev, "Msg NACK received\n");
244		- }
	486	+ dev_dbg_ratelimited(cdns->dev, "Msg Ack not received\n");
	487	+ }
	488	+ if (cdns->response_buf[i] & CDNS_MCP_RESP_NACK) {
	489	+ nack = 1;
	490	+ dev_err_ratelimited(cdns->dev, "Msg NACK received\n");
245	491	}
246	492	}
247	493
248	494	if (nack) {
249		- dev_err(cdns->dev, "Msg NACKed for Slave %d\n", msg->dev_num);
	495	+ dev_err_ratelimited(cdns->dev, "Msg NACKed for Slave %d\n", msg->dev_num);
250	496	return SDW_CMD_FAIL;
251		- } else if (no_ack) {
252		- dev_dbg(cdns->dev, "Msg ignored for Slave %d\n", msg->dev_num);
	497	+ }
	498	+
	499	+ if (no_ack) {
	500	+ dev_dbg_ratelimited(cdns->dev, "Msg ignored for Slave %d\n", msg->dev_num);
253	501	return SDW_CMD_IGNORED;
254	502	}
255	503
256		- /* fill response */
257		- for (i = 0; i < count; i++)
258		- msg->buf[i + offset] = cdns->response_buf[i] >>
259		- SDW_REG_SHIFT(CDNS_MCP_RESP_RDATA);
	504	+ if (msg->flags == SDW_MSG_FLAG_READ) {
	505	+ /* fill response */
	506	+ for (i = 0; i < count; i++)
	507	+ msg->buf[i + offset] = FIELD_GET(CDNS_MCP_RESP_RDATA,
	508	+ cdns->response_buf[i]);
	509	+ }
260	510
261	511	return SDW_CMD_OK;
262	512	}
263	513
	514	+static void cdns_read_response(struct sdw_cdns *cdns)
	515	+{
	516	+ u32 num_resp, cmd_base;
	517	+ int i;
	518	+
	519	+ /* RX_FIFO_AVAIL can be 2 entries more than the FIFO size */
	520	+ BUILD_BUG_ON(ARRAY_SIZE(cdns->response_buf) < CDNS_MCP_CMD_LEN + 2);
	521	+
	522	+ num_resp = cdns_readl(cdns, CDNS_MCP_FIFOSTAT);
	523	+ num_resp &= CDNS_MCP_RX_FIFO_AVAIL;
	524	+ if (num_resp > ARRAY_SIZE(cdns->response_buf)) {
	525	+ dev_warn(cdns->dev, "RX AVAIL %d too long\n", num_resp);
	526	+ num_resp = ARRAY_SIZE(cdns->response_buf);
	527	+ }
	528	+
	529	+ cmd_base = CDNS_MCP_CMD_BASE;
	530	+
	531	+ for (i = 0; i < num_resp; i++) {
	532	+ cdns->response_buf[i] = cdns_readl(cdns, cmd_base);
	533	+ cmd_base += CDNS_MCP_CMD_WORD_LEN;
	534	+ }
	535	+}
	536	+
264	537	static enum sdw_command_response
265	538	_cdns_xfer_msg(struct sdw_cdns cdns, struct sdw_msg msg, int cmd,
266		- int offset, int count, bool defer)
	539	+ int offset, int count, bool defer)
267	540	{
268	541	unsigned long time;
269	542	u32 base, i, data;
..	..	@@ -279,14 +552,15 @@
279	552	addr = msg->addr;
280	553
281	554	for (i = 0; i < count; i++) {
282		- data = msg->dev_num << SDW_REG_SHIFT(CDNS_MCP_CMD_DEV_ADDR);
283		- data \|= cmd << SDW_REG_SHIFT(CDNS_MCP_CMD_COMMAND);
284		- data \|= addr++ << SDW_REG_SHIFT(CDNS_MCP_CMD_REG_ADDR_L);
	555	+ data = FIELD_PREP(CDNS_MCP_CMD_DEV_ADDR, msg->dev_num);
	556	+ data \|= FIELD_PREP(CDNS_MCP_CMD_COMMAND, cmd);
	557	+ data \|= FIELD_PREP(CDNS_MCP_CMD_REG_ADDR, addr);
	558	+ addr++;
285	559
286	560	if (msg->flags == SDW_MSG_FLAG_WRITE)
287	561	data \|= msg->buf[i + offset];
288	562
289		- data \|= msg->ssp_sync << SDW_REG_SHIFT(CDNS_MCP_CMD_SSP_TAG);
	563	+ data \|= FIELD_PREP(CDNS_MCP_CMD_SSP_TAG, msg->ssp_sync);
290	564	cdns_writel(cdns, base, data);
291	565	base += CDNS_MCP_CMD_WORD_LEN;
292	566	}
..	..	@@ -296,18 +570,23 @@
296	570
297	571	/* wait for timeout or response */
298	572	time = wait_for_completion_timeout(&cdns->tx_complete,
299		- msecs_to_jiffies(CDNS_TX_TIMEOUT));
	573	+ msecs_to_jiffies(CDNS_TX_TIMEOUT));
300	574	if (!time) {
301		- dev_err(cdns->dev, "IO transfer timed out\n");
	575	+ dev_err(cdns->dev, "IO transfer timed out, cmd %d device %d addr %x len %d\n",
	576	+ cmd, msg->dev_num, msg->addr, msg->len);
302	577	msg->len = 0;
	578	+
	579	+ /* Drain anything in the RX_FIFO */
	580	+ cdns_read_response(cdns);
	581	+
303	582	return SDW_CMD_TIMEOUT;
304	583	}
305	584
306	585	return cdns_fill_msg_resp(cdns, msg, count, offset);
307	586	}
308	587
309		-static enum sdw_command_response cdns_program_scp_addr(
310		- struct sdw_cdns cdns, struct sdw_msg msg)
	588	+static enum sdw_command_response
	589	+cdns_program_scp_addr(struct sdw_cdns cdns, struct sdw_msg msg)
311	590	{
312	591	int nack = 0, no_ack = 0;
313	592	unsigned long time;
..	..	@@ -320,12 +599,12 @@
320	599	cdns->msg_count = CDNS_SCP_RX_FIFOLEVEL;
321	600	}
322	601
323		- data[0] = msg->dev_num << SDW_REG_SHIFT(CDNS_MCP_CMD_DEV_ADDR);
324		- data[0] \|= 0x3 << SDW_REG_SHIFT(CDNS_MCP_CMD_COMMAND);
	602	+ data[0] = FIELD_PREP(CDNS_MCP_CMD_DEV_ADDR, msg->dev_num);
	603	+ data[0] \|= FIELD_PREP(CDNS_MCP_CMD_COMMAND, 0x3);
325	604	data[1] = data[0];
326	605
327		- data[0] \|= SDW_SCP_ADDRPAGE1 << SDW_REG_SHIFT(CDNS_MCP_CMD_REG_ADDR_L);
328		- data[1] \|= SDW_SCP_ADDRPAGE2 << SDW_REG_SHIFT(CDNS_MCP_CMD_REG_ADDR_L);
	606	+ data[0] \|= FIELD_PREP(CDNS_MCP_CMD_REG_ADDR, SDW_SCP_ADDRPAGE1);
	607	+ data[1] \|= FIELD_PREP(CDNS_MCP_CMD_REG_ADDR, SDW_SCP_ADDRPAGE2);
329	608
330	609	data[0] \|= msg->addr_page1;
331	610	data[1] \|= msg->addr_page2;
..	..	@@ -336,7 +615,7 @@
336	615	cdns_writel(cdns, base, data[1]);
337	616
338	617	time = wait_for_completion_timeout(&cdns->tx_complete,
339		- msecs_to_jiffies(CDNS_TX_TIMEOUT));
	618	+ msecs_to_jiffies(CDNS_TX_TIMEOUT));
340	619	if (!time) {
341	620	dev_err(cdns->dev, "SCP Msg trf timed out\n");
342	621	msg->len = 0;
..	..	@@ -347,22 +626,24 @@
347	626	for (i = 0; i < 2; i++) {
348	627	if (!(cdns->response_buf[i] & CDNS_MCP_RESP_ACK)) {
349	628	no_ack = 1;
350		- dev_err(cdns->dev, "Program SCP Ack not received");
	629	+ dev_err(cdns->dev, "Program SCP Ack not received\n");
351	630	if (cdns->response_buf[i] & CDNS_MCP_RESP_NACK) {
352	631	nack = 1;
353		- dev_err(cdns->dev, "Program SCP NACK received");
	632	+ dev_err(cdns->dev, "Program SCP NACK received\n");
354	633	}
355	634	}
356	635	}
357	636
358	637	/* For NACK, NO ack, don't return err if we are in Broadcast mode */
359	638	if (nack) {
360		- dev_err(cdns->dev,
361		- "SCP_addrpage NACKed for Slave %d", msg->dev_num);
	639	+ dev_err_ratelimited(cdns->dev,
	640	+ "SCP_addrpage NACKed for Slave %d\n", msg->dev_num);
362	641	return SDW_CMD_FAIL;
363		- } else if (no_ack) {
364		- dev_dbg(cdns->dev,
365		- "SCP_addrpage ignored for Slave %d", msg->dev_num);
	642	+ }
	643	+
	644	+ if (no_ack) {
	645	+ dev_dbg_ratelimited(cdns->dev,
	646	+ "SCP_addrpage ignored for Slave %d\n", msg->dev_num);
366	647	return SDW_CMD_IGNORED;
367	648	}
368	649
..	..	@@ -410,7 +691,7 @@
410	691
411	692	for (i = 0; i < msg->len / CDNS_MCP_CMD_LEN; i++) {
412	693	ret = _cdns_xfer_msg(cdns, msg, cmd, i * CDNS_MCP_CMD_LEN,
413		- CDNS_MCP_CMD_LEN, false);
	694	+ CDNS_MCP_CMD_LEN, false);
414	695	if (ret < 0)
415	696	goto exit;
416	697	}
..	..	@@ -419,7 +700,7 @@
419	700	goto exit;
420	701
421	702	ret = _cdns_xfer_msg(cdns, msg, cmd, i * CDNS_MCP_CMD_LEN,
422		- msg->len % CDNS_MCP_CMD_LEN, false);
	703	+ msg->len % CDNS_MCP_CMD_LEN, false);
423	704
424	705	exit:
425	706	return ret;
..	..	@@ -428,7 +709,7 @@
428	709
429	710	enum sdw_command_response
430	711	cdns_xfer_msg_defer(struct sdw_bus *bus,
431		- struct sdw_msg msg, struct sdw_defer defer)
	712	+ struct sdw_msg msg, struct sdw_defer defer)
432	713	{
433	714	struct sdw_cdns *cdns = bus_to_cdns(bus);
434	715	int cmd = 0, ret;
..	..	@@ -466,28 +747,13 @@
466	747	* IRQ handling
467	748	*/
468	749
469		-static void cdns_read_response(struct sdw_cdns *cdns)
470		-{
471		- u32 num_resp, cmd_base;
472		- int i;
473		-
474		- num_resp = cdns_readl(cdns, CDNS_MCP_FIFOSTAT);
475		- num_resp &= CDNS_MCP_RX_FIFO_AVAIL;
476		-
477		- cmd_base = CDNS_MCP_CMD_BASE;
478		-
479		- for (i = 0; i < num_resp; i++) {
480		- cdns->response_buf[i] = cdns_readl(cdns, cmd_base);
481		- cmd_base += CDNS_MCP_CMD_WORD_LEN;
482		- }
483		-}
484		-
485	750	static int cdns_update_slave_status(struct sdw_cdns *cdns,
486		- u32 slave0, u32 slave1)
	751	+ u32 slave0, u32 slave1)
487	752	{
488	753	enum sdw_slave_status status[SDW_MAX_DEVICES + 1];
489	754	bool is_slave = false;
490		- u64 slave, mask;
	755	+ u64 slave;
	756	+ u32 mask;
491	757	int i, set_status;
492	758
493	759	/* combine the two status */
..	..	@@ -525,13 +791,36 @@
525	791
526	792	/* first check if Slave reported multiple status */
527	793	if (set_status > 1) {
528		- dev_warn(cdns->dev,
529		- "Slave reported multiple Status: %d\n",
530		- status[i]);
531		- /*
532		- * TODO: we need to reread the status here by
533		- * issuing a PING cmd
534		- */
	794	+ u32 val;
	795	+
	796	+ dev_warn_ratelimited(cdns->dev,
	797	+ "Slave %d reported multiple Status: %d\n",
	798	+ i, mask);
	799	+
	800	+ /* check latest status extracted from PING commands */
	801	+ val = cdns_readl(cdns, CDNS_MCP_SLAVE_STAT);
	802	+ val >>= (i * 2);
	803	+
	804	+ switch (val & 0x3) {
	805	+ case 0:
	806	+ status[i] = SDW_SLAVE_UNATTACHED;
	807	+ break;
	808	+ case 1:
	809	+ status[i] = SDW_SLAVE_ATTACHED;
	810	+ break;
	811	+ case 2:
	812	+ status[i] = SDW_SLAVE_ALERT;
	813	+ break;
	814	+ case 3:
	815	+ default:
	816	+ status[i] = SDW_SLAVE_RESERVED;
	817	+ break;
	818	+ }
	819	+
	820	+ dev_warn_ratelimited(cdns->dev,
	821	+ "Slave %d status updated to %d\n",
	822	+ i, status[i]);
	823	+
535	824	}
536	825	}
537	826
..	..	@@ -558,6 +847,10 @@
558	847
559	848	int_status = cdns_readl(cdns, CDNS_MCP_INTSTAT);
560	849
	850	+ /* check for reserved values read as zero */
	851	+ if (int_status & CDNS_MCP_INT_RESERVED)
	852	+ return IRQ_NONE;
	853	+
561	854	if (!(int_status & CDNS_MCP_INT_IRQ))
562	855	return IRQ_NONE;
563	856
..	..	@@ -566,18 +859,22 @@
566	859
567	860	if (cdns->defer) {
568	861	cdns_fill_msg_resp(cdns, cdns->defer->msg,
569		- cdns->defer->length, 0);
	862	+ cdns->defer->length, 0);
570	863	complete(&cdns->defer->complete);
571	864	cdns->defer = NULL;
572		- } else
	865	+ } else {
573	866	complete(&cdns->tx_complete);
	867	+ }
	868	+ }
	869	+
	870	+ if (int_status & CDNS_MCP_INT_PARITY) {
	871	+ /* Parity error detected by Master */
	872	+ dev_err_ratelimited(cdns->dev, "Parity error\n");
574	873	}
575	874
576	875	if (int_status & CDNS_MCP_INT_CTRL_CLASH) {
577		-
578	876	/* Slave is driving bit slot during control word */
579	877	dev_err_ratelimited(cdns->dev, "Bus clash for control word\n");
580		- int_status \|= CDNS_MCP_INT_CTRL_CLASH;
581	878	}
582	879
583	880	if (int_status & CDNS_MCP_INT_DATA_CLASH) {
..	..	@@ -586,16 +883,37 @@
586	883	* ownership of data bits or Slave gone bonkers
587	884	*/
588	885	dev_err_ratelimited(cdns->dev, "Bus clash for data word\n");
589		- int_status \|= CDNS_MCP_INT_DATA_CLASH;
	886	+ }
	887	+
	888	+ if (cdns->bus.params.m_data_mode != SDW_PORT_DATA_MODE_NORMAL &&
	889	+ int_status & CDNS_MCP_INT_DPINT) {
	890	+ u32 port_intstat;
	891	+
	892	+ /* just log which ports report an error */
	893	+ port_intstat = cdns_readl(cdns, CDNS_MCP_PORT_INTSTAT);
	894	+ dev_err_ratelimited(cdns->dev, "DP interrupt: PortIntStat %8x\n",
	895	+ port_intstat);
	896	+
	897	+ /* clear status w/ write1 */
	898	+ cdns_writel(cdns, CDNS_MCP_PORT_INTSTAT, port_intstat);
590	899	}
591	900
592	901	if (int_status & CDNS_MCP_INT_SLAVE_MASK) {
593	902	/* Mask the Slave interrupt and wake thread */
594	903	cdns_updatel(cdns, CDNS_MCP_INTMASK,
595		- CDNS_MCP_INT_SLAVE_MASK, 0);
	904	+ CDNS_MCP_INT_SLAVE_MASK, 0);
596	905
597	906	int_status &= ~CDNS_MCP_INT_SLAVE_MASK;
598		- ret = IRQ_WAKE_THREAD;
	907	+
	908	+ /*
	909	+ * Deal with possible race condition between interrupt
	910	+ * handling and disabling interrupts on suspend.
	911	+ *
	912	+ * If the master is in the process of disabling
	913	+ * interrupts, don't schedule a workqueue
	914	+ */
	915	+ if (cdns->interrupt_enabled)
	916	+ schedule_work(&cdns->work);
599	917	}
600	918
601	919	cdns_writel(cdns, CDNS_MCP_INTSTAT, int_status);
..	..	@@ -604,16 +922,18 @@
604	922	EXPORT_SYMBOL(sdw_cdns_irq);
605	923
606	924	/**
607		- * sdw_cdns_thread() - Cadence irq thread handler
608		- * @irq: irq number
609		- * @dev_id: irq context
	925	+ * To update slave status in a work since we will need to handle
	926	+ * other interrupts eg. CDNS_MCP_INT_RX_WL during the update slave
	927	+ * process.
	928	+ * @work: cdns worker thread
610	929	*/
611		-irqreturn_t sdw_cdns_thread(int irq, void *dev_id)
	930	+static void cdns_update_slave_status_work(struct work_struct *work)
612	931	{
613		- struct sdw_cdns *cdns = dev_id;
	932	+ struct sdw_cdns *cdns =
	933	+ container_of(work, struct sdw_cdns, work);
614	934	u32 slave0, slave1;
615	935
616		- dev_dbg(cdns->dev, "Slave status change\n");
	936	+ dev_dbg_ratelimited(cdns->dev, "Slave status change\n");
617	937
618	938	slave0 = cdns_readl(cdns, CDNS_MCP_SLAVE_INTSTAT0);
619	939	slave1 = cdns_readl(cdns, CDNS_MCP_SLAVE_INTSTAT1);
..	..	@@ -625,55 +945,134 @@
625	945	/* clear and unmask Slave interrupt now */
626	946	cdns_writel(cdns, CDNS_MCP_INTSTAT, CDNS_MCP_INT_SLAVE_MASK);
627	947	cdns_updatel(cdns, CDNS_MCP_INTMASK,
628		- CDNS_MCP_INT_SLAVE_MASK, CDNS_MCP_INT_SLAVE_MASK);
	948	+ CDNS_MCP_INT_SLAVE_MASK, CDNS_MCP_INT_SLAVE_MASK);
629	949
630		- return IRQ_HANDLED;
631	950	}
632		-EXPORT_SYMBOL(sdw_cdns_thread);
633	951
634	952	/*
635	953	* init routines
636	954	*/
637		-static int _cdns_enable_interrupt(struct sdw_cdns *cdns)
	955	+
	956	+/**
	957	+ * sdw_cdns_exit_reset() - Program reset parameters and start bus operations
	958	+ * @cdns: Cadence instance
	959	+ */
	960	+int sdw_cdns_exit_reset(struct sdw_cdns *cdns)
	961	+{
	962	+ /* program maximum length reset to be safe */
	963	+ cdns_updatel(cdns, CDNS_MCP_CONTROL,
	964	+ CDNS_MCP_CONTROL_RST_DELAY,
	965	+ CDNS_MCP_CONTROL_RST_DELAY);
	966	+
	967	+ /* use hardware generated reset */
	968	+ cdns_updatel(cdns, CDNS_MCP_CONTROL,
	969	+ CDNS_MCP_CONTROL_HW_RST,
	970	+ CDNS_MCP_CONTROL_HW_RST);
	971	+
	972	+ /* commit changes */
	973	+ cdns_updatel(cdns, CDNS_MCP_CONFIG_UPDATE,
	974	+ CDNS_MCP_CONFIG_UPDATE_BIT,
	975	+ CDNS_MCP_CONFIG_UPDATE_BIT);
	976	+
	977	+ /* don't wait here */
	978	+ return 0;
	979	+
	980	+}
	981	+EXPORT_SYMBOL(sdw_cdns_exit_reset);
	982	+
	983	+/**
	984	+ * sdw_cdns_enable_slave_interrupt() - Enable SDW slave interrupts
	985	+ * @cdns: Cadence instance
	986	+ * @state: boolean for true/false
	987	+ */
	988	+static void cdns_enable_slave_interrupts(struct sdw_cdns *cdns, bool state)
638	989	{
639	990	u32 mask;
640	991
641		- cdns_writel(cdns, CDNS_MCP_SLAVE_INTMASK0,
642		- CDNS_MCP_SLAVE_INTMASK0_MASK);
643		- cdns_writel(cdns, CDNS_MCP_SLAVE_INTMASK1,
644		- CDNS_MCP_SLAVE_INTMASK1_MASK);
	992	+ mask = cdns_readl(cdns, CDNS_MCP_INTMASK);
	993	+ if (state)
	994	+ mask \|= CDNS_MCP_INT_SLAVE_MASK;
	995	+ else
	996	+ mask &= ~CDNS_MCP_INT_SLAVE_MASK;
645	997
646		- mask = CDNS_MCP_INT_SLAVE_RSVD \| CDNS_MCP_INT_SLAVE_ALERT \|
647		- CDNS_MCP_INT_SLAVE_ATTACH \| CDNS_MCP_INT_SLAVE_NATTACH \|
648		- CDNS_MCP_INT_CTRL_CLASH \| CDNS_MCP_INT_DATA_CLASH \|
649		- CDNS_MCP_INT_RX_WL \| CDNS_MCP_INT_IRQ \| CDNS_MCP_INT_DPINT;
	998	+ cdns_writel(cdns, CDNS_MCP_INTMASK, mask);
	999	+}
650	1000
	1001	+/**
	1002	+ * sdw_cdns_enable_interrupt() - Enable SDW interrupts
	1003	+ * @cdns: Cadence instance
	1004	+ * @state: True if we are trying to enable interrupt.
	1005	+ */
	1006	+int sdw_cdns_enable_interrupt(struct sdw_cdns *cdns, bool state)
	1007	+{
	1008	+ u32 slave_intmask0 = 0;
	1009	+ u32 slave_intmask1 = 0;
	1010	+ u32 mask = 0;
	1011	+
	1012	+ if (!state)
	1013	+ goto update_masks;
	1014	+
	1015	+ slave_intmask0 = CDNS_MCP_SLAVE_INTMASK0_MASK;
	1016	+ slave_intmask1 = CDNS_MCP_SLAVE_INTMASK1_MASK;
	1017	+
	1018	+ /* enable detection of all slave state changes */
	1019	+ mask = CDNS_MCP_INT_SLAVE_MASK;
	1020	+
	1021	+ /* enable detection of bus issues */
	1022	+ mask \|= CDNS_MCP_INT_CTRL_CLASH \| CDNS_MCP_INT_DATA_CLASH \|
	1023	+ CDNS_MCP_INT_PARITY;
	1024	+
	1025	+ /* port interrupt limited to test modes for now */
	1026	+ if (cdns->bus.params.m_data_mode != SDW_PORT_DATA_MODE_NORMAL)
	1027	+ mask \|= CDNS_MCP_INT_DPINT;
	1028	+
	1029	+ /* enable detection of RX fifo level */
	1030	+ mask \|= CDNS_MCP_INT_RX_WL;
	1031	+
	1032	+ /*
	1033	+ * CDNS_MCP_INT_IRQ needs to be set otherwise all previous
	1034	+ * settings are irrelevant
	1035	+ */
	1036	+ mask \|= CDNS_MCP_INT_IRQ;
	1037	+
	1038	+ if (interrupt_mask) /* parameter override */
	1039	+ mask = interrupt_mask;
	1040	+
	1041	+update_masks:
	1042	+ /* clear slave interrupt status before enabling interrupt */
	1043	+ if (state) {
	1044	+ u32 slave_state;
	1045	+
	1046	+ slave_state = cdns_readl(cdns, CDNS_MCP_SLAVE_INTSTAT0);
	1047	+ cdns_writel(cdns, CDNS_MCP_SLAVE_INTSTAT0, slave_state);
	1048	+ slave_state = cdns_readl(cdns, CDNS_MCP_SLAVE_INTSTAT1);
	1049	+ cdns_writel(cdns, CDNS_MCP_SLAVE_INTSTAT1, slave_state);
	1050	+ }
	1051	+ cdns->interrupt_enabled = state;
	1052	+
	1053	+ /*
	1054	+ * Complete any on-going status updates before updating masks,
	1055	+ * and cancel queued status updates.
	1056	+ *
	1057	+ * There could be a race with a new interrupt thrown before
	1058	+ * the 3 mask updates below are complete, so in the interrupt
	1059	+ * we use the 'interrupt_enabled' status to prevent new work
	1060	+ * from being queued.
	1061	+ */
	1062	+ if (!state)
	1063	+ cancel_work_sync(&cdns->work);
	1064	+
	1065	+ cdns_writel(cdns, CDNS_MCP_SLAVE_INTMASK0, slave_intmask0);
	1066	+ cdns_writel(cdns, CDNS_MCP_SLAVE_INTMASK1, slave_intmask1);
651	1067	cdns_writel(cdns, CDNS_MCP_INTMASK, mask);
652	1068
653	1069	return 0;
654	1070	}
655		-
656		-/**
657		- * sdw_cdns_enable_interrupt() - Enable SDW interrupts and update config
658		- * @cdns: Cadence instance
659		- */
660		-int sdw_cdns_enable_interrupt(struct sdw_cdns *cdns)
661		-{
662		- int ret;
663		-
664		- _cdns_enable_interrupt(cdns);
665		- ret = cdns_clear_bit(cdns, CDNS_MCP_CONFIG_UPDATE,
666		- CDNS_MCP_CONFIG_UPDATE_BIT);
667		- if (ret < 0)
668		- dev_err(cdns->dev, "Config update timedout");
669		-
670		- return ret;
671		-}
672	1071	EXPORT_SYMBOL(sdw_cdns_enable_interrupt);
673	1072
674	1073	static int cdns_allocate_pdi(struct sdw_cdns *cdns,
675		- struct sdw_cdns_pdi **stream,
676		- u32 num, u32 pdi_offset)
	1074	+ struct sdw_cdns_pdi **stream,
	1075	+ u32 num, u32 pdi_offset)
677	1076	{
678	1077	struct sdw_cdns_pdi *pdi;
679	1078	int i;
..	..	@@ -687,7 +1086,6 @@
687	1086
688	1087	for (i = 0; i < num; i++) {
689	1088	pdi[i].num = i + pdi_offset;
690		- pdi[i].assigned = false;
691	1089	}
692	1090
693	1091	*stream = pdi;
..	..	@@ -701,10 +1099,11 @@
701	1099	* @config: Stream configurations
702	1100	*/
703	1101	int sdw_cdns_pdi_init(struct sdw_cdns *cdns,
704		- struct sdw_cdns_stream_config config)
	1102	+ struct sdw_cdns_stream_config config)
705	1103	{
706	1104	struct sdw_cdns_streams *stream;
707		- int offset, i, ret;
	1105	+ int offset;
	1106	+ int ret;
708	1107
709	1108	cdns->pcm.num_bd = config.pcm_bd;
710	1109	cdns->pcm.num_in = config.pcm_in;
..	..	@@ -716,9 +1115,8 @@
716	1115	/* Allocate PDIs for PCMs */
717	1116	stream = &cdns->pcm;
718	1117
719		- /* First two PDIs are reserved for bulk transfers */
720		- stream->num_bd -= CDNS_PCM_PDI_OFFSET;
721		- offset = CDNS_PCM_PDI_OFFSET;
	1118	+ /* we allocate PDI0 and PDI1 which are used for Bulk */
	1119	+ offset = 0;
722	1120
723	1121	ret = cdns_allocate_pdi(cdns, &stream->bd,
724	1122	stream->num_bd, offset);
..	..	@@ -745,7 +1143,6 @@
745	1143
746	1144	/* Allocate PDIs for PDMs */
747	1145	stream = &cdns->pdm;
748		- offset = CDNS_PDM_PDI_OFFSET;
749	1146	ret = cdns_allocate_pdi(cdns, &stream->bd,
750	1147	stream->num_bd, offset);
751	1148	if (ret)
..	..	@@ -762,6 +1159,7 @@
762	1159
763	1160	ret = cdns_allocate_pdi(cdns, &stream->out,
764	1161	stream->num_out, offset);
	1162	+
765	1163	if (ret)
766	1164	return ret;
767	1165
..	..	@@ -769,21 +1167,54 @@
769	1167	stream->num_pdi = stream->num_bd + stream->num_in + stream->num_out;
770	1168	cdns->num_ports += stream->num_pdi;
771	1169
772		- cdns->ports = devm_kcalloc(cdns->dev, cdns->num_ports,
773		- sizeof(*cdns->ports), GFP_KERNEL);
774		- if (!cdns->ports) {
775		- ret = -ENOMEM;
776		- return ret;
777		- }
778		-
779		- for (i = 0; i < cdns->num_ports; i++) {
780		- cdns->ports[i].assigned = false;
781		- cdns->ports[i].num = i + 1; /* Port 0 reserved for bulk */
782		- }
783		-
784	1170	return 0;
785	1171	}
786	1172	EXPORT_SYMBOL(sdw_cdns_pdi_init);
	1173	+
	1174	+static u32 cdns_set_initial_frame_shape(int n_rows, int n_cols)
	1175	+{
	1176	+ u32 val;
	1177	+ int c;
	1178	+ int r;
	1179	+
	1180	+ r = sdw_find_row_index(n_rows);
	1181	+ c = sdw_find_col_index(n_cols);
	1182	+
	1183	+ val = FIELD_PREP(CDNS_MCP_FRAME_SHAPE_ROW_MASK, r);
	1184	+ val \|= FIELD_PREP(CDNS_MCP_FRAME_SHAPE_COL_MASK, c);
	1185	+
	1186	+ return val;
	1187	+}
	1188	+
	1189	+static void cdns_init_clock_ctrl(struct sdw_cdns *cdns)
	1190	+{
	1191	+ struct sdw_bus *bus = &cdns->bus;
	1192	+ struct sdw_master_prop *prop = &bus->prop;
	1193	+ u32 val;
	1194	+ u32 ssp_interval;
	1195	+ int divider;
	1196	+
	1197	+ /* Set clock divider */
	1198	+ divider = (prop->mclk_freq / prop->max_clk_freq) - 1;
	1199	+
	1200	+ cdns_updatel(cdns, CDNS_MCP_CLK_CTRL0,
	1201	+ CDNS_MCP_CLK_MCLKD_MASK, divider);
	1202	+ cdns_updatel(cdns, CDNS_MCP_CLK_CTRL1,
	1203	+ CDNS_MCP_CLK_MCLKD_MASK, divider);
	1204	+
	1205	+ /*
	1206	+ * Frame shape changes after initialization have to be done
	1207	+ * with the bank switch mechanism
	1208	+ */
	1209	+ val = cdns_set_initial_frame_shape(prop->default_row,
	1210	+ prop->default_col);
	1211	+ cdns_writel(cdns, CDNS_MCP_FRAME_SHAPE_INIT, val);
	1212	+
	1213	+ /* Set SSP interval to default value */
	1214	+ ssp_interval = prop->default_frame_rate / SDW_CADENCE_GSYNC_HZ;
	1215	+ cdns_writel(cdns, CDNS_MCP_SSP_CTRL0, ssp_interval);
	1216	+ cdns_writel(cdns, CDNS_MCP_SSP_CTRL1, ssp_interval);
	1217	+}
787	1218
788	1219	/**
789	1220	* sdw_cdns_init() - Cadence initialization
..	..	@@ -792,88 +1223,80 @@
792	1223	int sdw_cdns_init(struct sdw_cdns *cdns)
793	1224	{
794	1225	u32 val;
795		- int ret;
796	1226
797		- /* Exit clock stop */
798		- ret = cdns_clear_bit(cdns, CDNS_MCP_CONTROL,
799		- CDNS_MCP_CONTROL_CLK_STOP_CLR);
800		- if (ret < 0) {
801		- dev_err(cdns->dev, "Couldn't exit from clock stop\n");
802		- return ret;
803		- }
	1227	+ cdns_init_clock_ctrl(cdns);
804	1228
805		- /* Set clock divider */
806		- val = cdns_readl(cdns, CDNS_MCP_CLK_CTRL0);
807		- val \|= CDNS_DEFAULT_CLK_DIVIDER;
808		- cdns_writel(cdns, CDNS_MCP_CLK_CTRL0, val);
	1229	+ /* reset msg_count to default value of FIFOLEVEL */
	1230	+ cdns->msg_count = cdns_readl(cdns, CDNS_MCP_FIFOLEVEL);
809	1231
810		- /* Set the default frame shape */
811		- cdns_writel(cdns, CDNS_MCP_FRAME_SHAPE_INIT, CDNS_DEFAULT_FRAME_SHAPE);
812		-
813		- /* Set SSP interval to default value */
814		- cdns_writel(cdns, CDNS_MCP_SSP_CTRL0, CDNS_DEFAULT_SSP_INTERVAL);
815		- cdns_writel(cdns, CDNS_MCP_SSP_CTRL1, CDNS_DEFAULT_SSP_INTERVAL);
	1232	+ /* flush command FIFOs */
	1233	+ cdns_updatel(cdns, CDNS_MCP_CONTROL, CDNS_MCP_CONTROL_CMD_RST,
	1234	+ CDNS_MCP_CONTROL_CMD_RST);
816	1235
817	1236	/* Set cmd accept mode */
818	1237	cdns_updatel(cdns, CDNS_MCP_CONTROL, CDNS_MCP_CONTROL_CMD_ACCEPT,
819		- CDNS_MCP_CONTROL_CMD_ACCEPT);
	1238	+ CDNS_MCP_CONTROL_CMD_ACCEPT);
820	1239
821	1240	/* Configure mcp config */
822	1241	val = cdns_readl(cdns, CDNS_MCP_CONFIG);
823	1242
824		- /* Set Max cmd retry to 15 */
825		- val \|= CDNS_MCP_CONFIG_MCMD_RETRY;
826		-
827		- /* Set frame delay between PREQ and ping frame to 15 frames */
828		- val \|= 0xF << SDW_REG_SHIFT(CDNS_MCP_CONFIG_MPREQ_DELAY);
829		-
830		- /* Disable auto bus release */
831		- val &= ~CDNS_MCP_CONFIG_BUS_REL;
832		-
833		- /* Disable sniffer mode */
834		- val &= ~CDNS_MCP_CONFIG_SNIFFER;
	1243	+ /* enable bus operations with clock and data */
	1244	+ val &= ~CDNS_MCP_CONFIG_OP;
	1245	+ val \|= CDNS_MCP_CONFIG_OP_NORMAL;
835	1246
836	1247	/* Set cmd mode for Tx and Rx cmds */
837	1248	val &= ~CDNS_MCP_CONFIG_CMD;
838	1249
839		- /* Set operation to normal */
840		- val &= ~CDNS_MCP_CONFIG_OP;
841		- val \|= CDNS_MCP_CONFIG_OP_NORMAL;
	1250	+ /* Disable sniffer mode */
	1251	+ val &= ~CDNS_MCP_CONFIG_SNIFFER;
	1252	+
	1253	+ /* Disable auto bus release */
	1254	+ val &= ~CDNS_MCP_CONFIG_BUS_REL;
	1255	+
	1256	+ if (cdns->bus.multi_link)
	1257	+ /* Set Multi-master mode to take gsync into account */
	1258	+ val \|= CDNS_MCP_CONFIG_MMASTER;
	1259	+
	1260	+ /* leave frame delay to hardware default of 0x1F */
	1261	+
	1262	+ /* leave command retry to hardware default of 0 */
842	1263
843	1264	cdns_writel(cdns, CDNS_MCP_CONFIG, val);
844	1265
	1266	+ /* changes will be committed later */
845	1267	return 0;
846	1268	}
847	1269	EXPORT_SYMBOL(sdw_cdns_init);
848	1270
849	1271	int cdns_bus_conf(struct sdw_bus bus, struct sdw_bus_params params)
850	1272	{
	1273	+ struct sdw_master_prop *prop = &bus->prop;
851	1274	struct sdw_cdns *cdns = bus_to_cdns(bus);
852		- int mcp_clkctrl_off, mcp_clkctrl;
	1275	+ int mcp_clkctrl_off;
853	1276	int divider;
854	1277
855	1278	if (!params->curr_dr_freq) {
856		- dev_err(cdns->dev, "NULL curr_dr_freq");
	1279	+ dev_err(cdns->dev, "NULL curr_dr_freq\n");
857	1280	return -EINVAL;
858	1281	}
859	1282
860		- divider = (params->max_dr_freq / params->curr_dr_freq) - 1;
	1283	+ divider = prop->mclk_freq * SDW_DOUBLE_RATE_FACTOR /
	1284	+ params->curr_dr_freq;
	1285	+ divider--; /* divider is 1/(N+1) */
861	1286
862	1287	if (params->next_bank)
863	1288	mcp_clkctrl_off = CDNS_MCP_CLK_CTRL1;
864	1289	else
865	1290	mcp_clkctrl_off = CDNS_MCP_CLK_CTRL0;
866	1291
867		- mcp_clkctrl = cdns_readl(cdns, mcp_clkctrl_off);
868		- mcp_clkctrl \|= divider;
869		- cdns_writel(cdns, mcp_clkctrl_off, mcp_clkctrl);
	1292	+ cdns_updatel(cdns, mcp_clkctrl_off, CDNS_MCP_CLK_MCLKD_MASK, divider);
870	1293
871	1294	return 0;
872	1295	}
873	1296	EXPORT_SYMBOL(cdns_bus_conf);
874	1297
875	1298	static int cdns_port_params(struct sdw_bus *bus,
876		- struct sdw_port_params *p_params, unsigned int bank)
	1299	+ struct sdw_port_params *p_params, unsigned int bank)
877	1300	{
878	1301	struct sdw_cdns *cdns = bus_to_cdns(bus);
879	1302	int dpn_config = 0, dpn_config_off;
..	..	@@ -885,12 +1308,9 @@
885	1308
886	1309	dpn_config = cdns_readl(cdns, dpn_config_off);
887	1310
888		- dpn_config \|= ((p_params->bps - 1) <<
889		- SDW_REG_SHIFT(CDNS_DPN_CONFIG_WL));
890		- dpn_config \|= (p_params->flow_mode <<
891		- SDW_REG_SHIFT(CDNS_DPN_CONFIG_PORT_FLOW));
892		- dpn_config \|= (p_params->data_mode <<
893		- SDW_REG_SHIFT(CDNS_DPN_CONFIG_PORT_DAT));
	1311	+ u32p_replace_bits(&dpn_config, (p_params->bps - 1), CDNS_DPN_CONFIG_WL);
	1312	+ u32p_replace_bits(&dpn_config, p_params->flow_mode, CDNS_DPN_CONFIG_PORT_FLOW);
	1313	+ u32p_replace_bits(&dpn_config, p_params->data_mode, CDNS_DPN_CONFIG_PORT_DAT);
894	1314
895	1315	cdns_writel(cdns, dpn_config_off, dpn_config);
896	1316
..	..	@@ -898,8 +1318,8 @@
898	1318	}
899	1319
900	1320	static int cdns_transport_params(struct sdw_bus *bus,
901		- struct sdw_transport_params *t_params,
902		- enum sdw_reg_bank bank)
	1321	+ struct sdw_transport_params *t_params,
	1322	+ enum sdw_reg_bank bank)
903	1323	{
904	1324	struct sdw_cdns *cdns = bus_to_cdns(bus);
905	1325	int dpn_offsetctrl = 0, dpn_offsetctrl_off;
..	..	@@ -926,24 +1346,17 @@
926	1346	}
927	1347
928	1348	dpn_config = cdns_readl(cdns, dpn_config_off);
929		-
930		- dpn_config \|= (t_params->blk_grp_ctrl <<
931		- SDW_REG_SHIFT(CDNS_DPN_CONFIG_BGC));
932		- dpn_config \|= (t_params->blk_pkg_mode <<
933		- SDW_REG_SHIFT(CDNS_DPN_CONFIG_BPM));
	1349	+ u32p_replace_bits(&dpn_config, t_params->blk_grp_ctrl, CDNS_DPN_CONFIG_BGC);
	1350	+ u32p_replace_bits(&dpn_config, t_params->blk_pkg_mode, CDNS_DPN_CONFIG_BPM);
934	1351	cdns_writel(cdns, dpn_config_off, dpn_config);
935	1352
936		- dpn_offsetctrl \|= (t_params->offset1 <<
937		- SDW_REG_SHIFT(CDNS_DPN_OFFSET_CTRL_1));
938		- dpn_offsetctrl \|= (t_params->offset2 <<
939		- SDW_REG_SHIFT(CDNS_DPN_OFFSET_CTRL_2));
	1353	+ u32p_replace_bits(&dpn_offsetctrl, t_params->offset1, CDNS_DPN_OFFSET_CTRL_1);
	1354	+ u32p_replace_bits(&dpn_offsetctrl, t_params->offset2, CDNS_DPN_OFFSET_CTRL_2);
940	1355	cdns_writel(cdns, dpn_offsetctrl_off, dpn_offsetctrl);
941	1356
942		- dpn_hctrl \|= (t_params->hstart <<
943		- SDW_REG_SHIFT(CDNS_DPN_HCTRL_HSTART));
944		- dpn_hctrl \|= (t_params->hstop << SDW_REG_SHIFT(CDNS_DPN_HCTRL_HSTOP));
945		- dpn_hctrl \|= (t_params->lane_ctrl <<
946		- SDW_REG_SHIFT(CDNS_DPN_HCTRL_LCTRL));
	1357	+ u32p_replace_bits(&dpn_hctrl, t_params->hstart, CDNS_DPN_HCTRL_HSTART);
	1358	+ u32p_replace_bits(&dpn_hctrl, t_params->hstop, CDNS_DPN_HCTRL_HSTOP);
	1359	+ u32p_replace_bits(&dpn_hctrl, t_params->lane_ctrl, CDNS_DPN_HCTRL_LCTRL);
947	1360
948	1361	cdns_writel(cdns, dpn_hctrl_off, dpn_hctrl);
949	1362	cdns_writel(cdns, dpn_samplectrl_off, (t_params->sample_interval - 1));
..	..	@@ -952,7 +1365,7 @@
952	1365	}
953	1366
954	1367	static int cdns_port_enable(struct sdw_bus *bus,
955		- struct sdw_enable_ch *enable_ch, unsigned int bank)
	1368	+ struct sdw_enable_ch *enable_ch, unsigned int bank)
956	1369	{
957	1370	struct sdw_cdns *cdns = bus_to_cdns(bus);
958	1371	int dpn_chnen_off, ch_mask;
..	..	@@ -975,6 +1388,168 @@
975	1388	};
976	1389
977	1390	/**
	1391	+ * sdw_cdns_is_clock_stop: Check clock status
	1392	+ *
	1393	+ * @cdns: Cadence instance
	1394	+ */
	1395	+bool sdw_cdns_is_clock_stop(struct sdw_cdns *cdns)
	1396	+{
	1397	+ return !!(cdns_readl(cdns, CDNS_MCP_STAT) & CDNS_MCP_STAT_CLK_STOP);
	1398	+}
	1399	+EXPORT_SYMBOL(sdw_cdns_is_clock_stop);
	1400	+
	1401	+/**
	1402	+ * sdw_cdns_clock_stop: Cadence clock stop configuration routine
	1403	+ *
	1404	+ * @cdns: Cadence instance
	1405	+ * @block_wake: prevent wakes if required by the platform
	1406	+ */
	1407	+int sdw_cdns_clock_stop(struct sdw_cdns *cdns, bool block_wake)
	1408	+{
	1409	+ bool slave_present = false;
	1410	+ struct sdw_slave *slave;
	1411	+ int ret;
	1412	+
	1413	+ /* Check suspend status */
	1414	+ if (sdw_cdns_is_clock_stop(cdns)) {
	1415	+ dev_dbg(cdns->dev, "Clock is already stopped\n");
	1416	+ return 0;
	1417	+ }
	1418	+
	1419	+ /*
	1420	+ * Before entering clock stop we mask the Slave
	1421	+ * interrupts. This helps avoid having to deal with e.g. a
	1422	+ * Slave becoming UNATTACHED while the clock is being stopped
	1423	+ */
	1424	+ cdns_enable_slave_interrupts(cdns, false);
	1425	+
	1426	+ /*
	1427	+ * For specific platforms, it is required to be able to put
	1428	+ * master into a state in which it ignores wake-up trials
	1429	+ * in clock stop state
	1430	+ */
	1431	+ if (block_wake)
	1432	+ cdns_updatel(cdns, CDNS_MCP_CONTROL,
	1433	+ CDNS_MCP_CONTROL_BLOCK_WAKEUP,
	1434	+ CDNS_MCP_CONTROL_BLOCK_WAKEUP);
	1435	+
	1436	+ list_for_each_entry(slave, &cdns->bus.slaves, node) {
	1437	+ if (slave->status == SDW_SLAVE_ATTACHED \|\|
	1438	+ slave->status == SDW_SLAVE_ALERT) {
	1439	+ slave_present = true;
	1440	+ break;
	1441	+ }
	1442	+ }
	1443	+
	1444	+ /*
	1445	+ * This CMD_ACCEPT should be used when there are no devices
	1446	+ * attached on the link when entering clock stop mode. If this is
	1447	+ * not set and there is a broadcast write then the command ignored
	1448	+ * will be treated as a failure
	1449	+ */
	1450	+ if (!slave_present)
	1451	+ cdns_updatel(cdns, CDNS_MCP_CONTROL,
	1452	+ CDNS_MCP_CONTROL_CMD_ACCEPT,
	1453	+ CDNS_MCP_CONTROL_CMD_ACCEPT);
	1454	+ else
	1455	+ cdns_updatel(cdns, CDNS_MCP_CONTROL,
	1456	+ CDNS_MCP_CONTROL_CMD_ACCEPT, 0);
	1457	+
	1458	+ /* commit changes */
	1459	+ ret = cdns_config_update(cdns);
	1460	+ if (ret < 0) {
	1461	+ dev_err(cdns->dev, "%s: config_update failed\n", __func__);
	1462	+ return ret;
	1463	+ }
	1464	+
	1465	+ /* Prepare slaves for clock stop */
	1466	+ if (slave_present) {
	1467	+ ret = sdw_bus_prep_clk_stop(&cdns->bus);
	1468	+ if (ret < 0 && ret != -ENODATA) {
	1469	+ dev_err(cdns->dev, "prepare clock stop failed %d\n", ret);
	1470	+ return ret;
	1471	+ }
	1472	+ }
	1473	+
	1474	+ /*
	1475	+ * Enter clock stop mode and only report errors if there are
	1476	+ * Slave devices present (ALERT or ATTACHED)
	1477	+ */
	1478	+ ret = sdw_bus_clk_stop(&cdns->bus);
	1479	+ if (ret < 0 && slave_present && ret != -ENODATA) {
	1480	+ dev_err(cdns->dev, "bus clock stop failed %d", ret);
	1481	+ return ret;
	1482	+ }
	1483	+
	1484	+ ret = cdns_set_wait(cdns, CDNS_MCP_STAT,
	1485	+ CDNS_MCP_STAT_CLK_STOP,
	1486	+ CDNS_MCP_STAT_CLK_STOP);
	1487	+ if (ret < 0)
	1488	+ dev_err(cdns->dev, "Clock stop failed %d\n", ret);
	1489	+
	1490	+ return ret;
	1491	+}
	1492	+EXPORT_SYMBOL(sdw_cdns_clock_stop);
	1493	+
	1494	+/**
	1495	+ * sdw_cdns_clock_restart: Cadence PM clock restart configuration routine
	1496	+ *
	1497	+ * @cdns: Cadence instance
	1498	+ * @bus_reset: context may be lost while in low power modes and the bus
	1499	+ * may require a Severe Reset and re-enumeration after a wake.
	1500	+ */
	1501	+int sdw_cdns_clock_restart(struct sdw_cdns *cdns, bool bus_reset)
	1502	+{
	1503	+ int ret;
	1504	+
	1505	+ /* unmask Slave interrupts that were masked when stopping the clock */
	1506	+ cdns_enable_slave_interrupts(cdns, true);
	1507	+
	1508	+ ret = cdns_clear_bit(cdns, CDNS_MCP_CONTROL,
	1509	+ CDNS_MCP_CONTROL_CLK_STOP_CLR);
	1510	+ if (ret < 0) {
	1511	+ dev_err(cdns->dev, "Couldn't exit from clock stop\n");
	1512	+ return ret;
	1513	+ }
	1514	+
	1515	+ ret = cdns_set_wait(cdns, CDNS_MCP_STAT, CDNS_MCP_STAT_CLK_STOP, 0);
	1516	+ if (ret < 0) {
	1517	+ dev_err(cdns->dev, "clock stop exit failed %d\n", ret);
	1518	+ return ret;
	1519	+ }
	1520	+
	1521	+ cdns_updatel(cdns, CDNS_MCP_CONTROL,
	1522	+ CDNS_MCP_CONTROL_BLOCK_WAKEUP, 0);
	1523	+
	1524	+ /*
	1525	+ * clear CMD_ACCEPT so that the command ignored
	1526	+ * will be treated as a failure during a broadcast write
	1527	+ */
	1528	+ cdns_updatel(cdns, CDNS_MCP_CONTROL, CDNS_MCP_CONTROL_CMD_ACCEPT, 0);
	1529	+
	1530	+ if (!bus_reset) {
	1531	+
	1532	+ /* enable bus operations with clock and data */
	1533	+ cdns_updatel(cdns, CDNS_MCP_CONFIG,
	1534	+ CDNS_MCP_CONFIG_OP,
	1535	+ CDNS_MCP_CONFIG_OP_NORMAL);
	1536	+
	1537	+ ret = cdns_config_update(cdns);
	1538	+ if (ret < 0) {
	1539	+ dev_err(cdns->dev, "%s: config_update failed\n", __func__);
	1540	+ return ret;
	1541	+ }
	1542	+
	1543	+ ret = sdw_bus_exit_clk_stop(&cdns->bus);
	1544	+ if (ret < 0)
	1545	+ dev_err(cdns->dev, "bus failed to exit clock stop %d\n", ret);
	1546	+ }
	1547	+
	1548	+ return ret;
	1549	+}
	1550	+EXPORT_SYMBOL(sdw_cdns_clock_restart);
	1551	+
	1552	+/**
978	1553	* sdw_cdns_probe() - Cadence probe routine
979	1554	* @cdns: Cadence instance
980	1555	*/
..	..	@@ -983,35 +1558,60 @@
983	1558	init_completion(&cdns->tx_complete);
984	1559	cdns->bus.port_ops = &cdns_port_ops;
985	1560
	1561	+ INIT_WORK(&cdns->work, cdns_update_slave_status_work);
986	1562	return 0;
987	1563	}
988	1564	EXPORT_SYMBOL(sdw_cdns_probe);
989	1565
990	1566	int cdns_set_sdw_stream(struct snd_soc_dai *dai,
991		- void *stream, bool pcm, int direction)
	1567	+ void *stream, bool pcm, int direction)
992	1568	{
993	1569	struct sdw_cdns *cdns = snd_soc_dai_get_drvdata(dai);
994	1570	struct sdw_cdns_dma_data *dma;
995	1571
996		- dma = kzalloc(sizeof(*dma), GFP_KERNEL);
997		- if (!dma)
998		- return -ENOMEM;
	1572	+ if (stream) {
	1573	+ /* first paranoia check */
	1574	+ if (direction == SNDRV_PCM_STREAM_PLAYBACK)
	1575	+ dma = dai->playback_dma_data;
	1576	+ else
	1577	+ dma = dai->capture_dma_data;
999	1578
1000		- if (pcm)
1001		- dma->stream_type = SDW_STREAM_PCM;
1002		- else
1003		- dma->stream_type = SDW_STREAM_PDM;
	1579	+ if (dma) {
	1580	+ dev_err(dai->dev,
	1581	+ "dma_data already allocated for dai %s\n",
	1582	+ dai->name);
	1583	+ return -EINVAL;
	1584	+ }
1004	1585
1005		- dma->bus = &cdns->bus;
1006		- dma->link_id = cdns->instance;
	1586	+ /* allocate and set dma info */
	1587	+ dma = kzalloc(sizeof(*dma), GFP_KERNEL);
	1588	+ if (!dma)
	1589	+ return -ENOMEM;
1007	1590
1008		- dma->stream = stream;
	1591	+ if (pcm)
	1592	+ dma->stream_type = SDW_STREAM_PCM;
	1593	+ else
	1594	+ dma->stream_type = SDW_STREAM_PDM;
1009	1595
1010		- if (direction == SNDRV_PCM_STREAM_PLAYBACK)
1011		- dai->playback_dma_data = dma;
1012		- else
1013		- dai->capture_dma_data = dma;
	1596	+ dma->bus = &cdns->bus;
	1597	+ dma->link_id = cdns->instance;
1014	1598
	1599	+ dma->stream = stream;
	1600	+
	1601	+ if (direction == SNDRV_PCM_STREAM_PLAYBACK)
	1602	+ dai->playback_dma_data = dma;
	1603	+ else
	1604	+ dai->capture_dma_data = dma;
	1605	+ } else {
	1606	+ /* for NULL stream we release allocated dma_data */
	1607	+ if (direction == SNDRV_PCM_STREAM_PLAYBACK) {
	1608	+ kfree(dai->playback_dma_data);
	1609	+ dai->playback_dma_data = NULL;
	1610	+ } else {
	1611	+ kfree(dai->capture_dma_data);
	1612	+ dai->capture_dma_data = NULL;
	1613	+ }
	1614	+ }
1015	1615	return 0;
1016	1616	}
1017	1617	EXPORT_SYMBOL(cdns_set_sdw_stream);
..	..	@@ -1020,22 +1620,25 @@
1020	1620	* cdns_find_pdi() - Find a free PDI
1021	1621	*
1022	1622	* @cdns: Cadence instance
	1623	+ * @offset: Starting offset
1023	1624	* @num: Number of PDIs
1024	1625	* @pdi: PDI instances
	1626	+ * @dai_id: DAI id
1025	1627	*
1026		- * Find and return a free PDI for a given PDI array
	1628	+ * Find a PDI for a given PDI array. The PDI num and dai_id are
	1629	+ * expected to match, return NULL otherwise.
1027	1630	*/
1028	1631	static struct sdw_cdns_pdi cdns_find_pdi(struct sdw_cdns cdns,
1029		- unsigned int num, struct sdw_cdns_pdi *pdi)
	1632	+ unsigned int offset,
	1633	+ unsigned int num,
	1634	+ struct sdw_cdns_pdi *pdi,
	1635	+ int dai_id)
1030	1636	{
1031	1637	int i;
1032	1638
1033		- for (i = 0; i < num; i++) {
1034		- if (pdi[i].assigned == true)
1035		- continue;
1036		- pdi[i].assigned = true;
1037		- return &pdi[i];
1038		- }
	1639	+ for (i = offset; i < offset + num; i++)
	1640	+ if (pdi[i].num == dai_id)
	1641	+ return &pdi[i];
1039	1642
1040	1643	return NULL;
1041	1644	}
..	..	@@ -1044,141 +1647,70 @@
1044	1647	* sdw_cdns_config_stream: Configure a stream
1045	1648	*
1046	1649	* @cdns: Cadence instance
1047		- * @port: Cadence data port
1048	1650	* @ch: Channel count
1049	1651	* @dir: Data direction
1050	1652	* @pdi: PDI to be used
1051	1653	*/
1052	1654	void sdw_cdns_config_stream(struct sdw_cdns *cdns,
1053		- struct sdw_cdns_port *port,
1054		- u32 ch, u32 dir, struct sdw_cdns_pdi *pdi)
	1655	+ u32 ch, u32 dir, struct sdw_cdns_pdi *pdi)
1055	1656	{
1056	1657	u32 offset, val = 0;
1057	1658
1058		- if (dir == SDW_DATA_DIR_RX)
	1659	+ if (dir == SDW_DATA_DIR_RX) {
1059	1660	val = CDNS_PORTCTRL_DIRN;
1060	1661
1061		- offset = CDNS_PORTCTRL + port->num * CDNS_PORT_OFFSET;
1062		- cdns_updatel(cdns, offset, CDNS_PORTCTRL_DIRN, val);
	1662	+ if (cdns->bus.params.m_data_mode != SDW_PORT_DATA_MODE_NORMAL)
	1663	+ val \|= CDNS_PORTCTRL_TEST_FAILED;
	1664	+ }
	1665	+ offset = CDNS_PORTCTRL + pdi->num * CDNS_PORT_OFFSET;
	1666	+ cdns_updatel(cdns, offset,
	1667	+ CDNS_PORTCTRL_DIRN \| CDNS_PORTCTRL_TEST_FAILED,
	1668	+ val);
1063	1669
1064		- val = port->num;
1065		- val \|= ((1 << ch) - 1) << SDW_REG_SHIFT(CDNS_PDI_CONFIG_CHANNEL);
	1670	+ val = pdi->num;
	1671	+ val \|= CDNS_PDI_CONFIG_SOFT_RESET;
	1672	+ val \|= FIELD_PREP(CDNS_PDI_CONFIG_CHANNEL, (1 << ch) - 1);
1066	1673	cdns_writel(cdns, CDNS_PDI_CONFIG(pdi->num), val);
1067	1674	}
1068	1675	EXPORT_SYMBOL(sdw_cdns_config_stream);
1069	1676
1070	1677	/**
1071		- * cdns_get_num_pdi() - Get number of PDIs required
1072		- *
1073		- * @cdns: Cadence instance
1074		- * @pdi: PDI to be used
1075		- * @num: Number of PDIs
1076		- * @ch_count: Channel count
1077		- */
1078		-static int cdns_get_num_pdi(struct sdw_cdns *cdns,
1079		- struct sdw_cdns_pdi *pdi,
1080		- unsigned int num, u32 ch_count)
1081		-{
1082		- int i, pdis = 0;
1083		-
1084		- for (i = 0; i < num; i++) {
1085		- if (pdi[i].assigned == true)
1086		- continue;
1087		-
1088		- if (pdi[i].ch_count < ch_count)
1089		- ch_count -= pdi[i].ch_count;
1090		- else
1091		- ch_count = 0;
1092		-
1093		- pdis++;
1094		-
1095		- if (!ch_count)
1096		- break;
1097		- }
1098		-
1099		- if (ch_count)
1100		- return 0;
1101		-
1102		- return pdis;
1103		-}
1104		-
1105		-/**
1106		- * sdw_cdns_get_stream() - Get stream information
	1678	+ * sdw_cdns_alloc_pdi() - Allocate a PDI
1107	1679	*
1108	1680	* @cdns: Cadence instance
1109	1681	* @stream: Stream to be allocated
1110	1682	* @ch: Channel count
1111	1683	* @dir: Data direction
	1684	+ * @dai_id: DAI id
1112	1685	*/
1113		-int sdw_cdns_get_stream(struct sdw_cdns *cdns,
1114		- struct sdw_cdns_streams *stream,
1115		- u32 ch, u32 dir)
1116		-{
1117		- int pdis = 0;
1118		-
1119		- if (dir == SDW_DATA_DIR_RX)
1120		- pdis = cdns_get_num_pdi(cdns, stream->in, stream->num_in, ch);
1121		- else
1122		- pdis = cdns_get_num_pdi(cdns, stream->out, stream->num_out, ch);
1123		-
1124		- /* check if we found PDI, else find in bi-directional */
1125		- if (!pdis)
1126		- pdis = cdns_get_num_pdi(cdns, stream->bd, stream->num_bd, ch);
1127		-
1128		- return pdis;
1129		-}
1130		-EXPORT_SYMBOL(sdw_cdns_get_stream);
1131		-
1132		-/**
1133		- * sdw_cdns_alloc_stream() - Allocate a stream
1134		- *
1135		- * @cdns: Cadence instance
1136		- * @stream: Stream to be allocated
1137		- * @port: Cadence data port
1138		- * @ch: Channel count
1139		- * @dir: Data direction
1140		- */
1141		-int sdw_cdns_alloc_stream(struct sdw_cdns *cdns,
1142		- struct sdw_cdns_streams *stream,
1143		- struct sdw_cdns_port *port, u32 ch, u32 dir)
	1686	+struct sdw_cdns_pdi sdw_cdns_alloc_pdi(struct sdw_cdns cdns,
	1687	+ struct sdw_cdns_streams *stream,
	1688	+ u32 ch, u32 dir, int dai_id)
1144	1689	{
1145	1690	struct sdw_cdns_pdi *pdi = NULL;
1146	1691
1147	1692	if (dir == SDW_DATA_DIR_RX)
1148		- pdi = cdns_find_pdi(cdns, stream->num_in, stream->in);
	1693	+ pdi = cdns_find_pdi(cdns, 0, stream->num_in, stream->in,
	1694	+ dai_id);
1149	1695	else
1150		- pdi = cdns_find_pdi(cdns, stream->num_out, stream->out);
	1696	+ pdi = cdns_find_pdi(cdns, 0, stream->num_out, stream->out,
	1697	+ dai_id);
1151	1698
1152	1699	/* check if we found a PDI, else find in bi-directional */
1153	1700	if (!pdi)
1154		- pdi = cdns_find_pdi(cdns, stream->num_bd, stream->bd);
	1701	+ pdi = cdns_find_pdi(cdns, 2, stream->num_bd, stream->bd,
	1702	+ dai_id);
1155	1703
1156		- if (!pdi)
1157		- return -EIO;
	1704	+ if (pdi) {
	1705	+ pdi->l_ch_num = 0;
	1706	+ pdi->h_ch_num = ch - 1;
	1707	+ pdi->dir = dir;
	1708	+ pdi->ch_count = ch;
	1709	+ }
1158	1710
1159		- port->pdi = pdi;
1160		- pdi->l_ch_num = 0;
1161		- pdi->h_ch_num = ch - 1;
1162		- pdi->dir = dir;
1163		- pdi->ch_count = ch;
1164		-
1165		- return 0;
	1711	+ return pdi;
1166	1712	}
1167		-EXPORT_SYMBOL(sdw_cdns_alloc_stream);
1168		-
1169		-void sdw_cdns_shutdown(struct snd_pcm_substream *substream,
1170		- struct snd_soc_dai *dai)
1171		-{
1172		- struct sdw_cdns_dma_data *dma;
1173		-
1174		- dma = snd_soc_dai_get_dma_data(dai, substream);
1175		- if (!dma)
1176		- return;
1177		-
1178		- snd_soc_dai_set_dma_data(dai, substream, NULL);
1179		- kfree(dma);
1180		-}
1181		-EXPORT_SYMBOL(sdw_cdns_shutdown);
	1713	+EXPORT_SYMBOL(sdw_cdns_alloc_pdi);
1182	1714
1183	1715	MODULE_LICENSE("Dual BSD/GPL");
1184	1716	MODULE_DESCRIPTION("Cadence Soundwire Library");