drivers: u95xx: Add HSI channel priority

Configure channel priority when configuring HSI interface.

ST-Ericsson Linux next: N/A
ST-Ericsson ID: 365662
ST-Ericsson FOSS-OUT ID: Trivial

Change-Id: I9880ee8d118dd8fefe7dc8ee46814ee1862cfac8
Signed-off-by: Derek Morton <derek.morton@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/42229
Reviewed-by: Christopher BLAIR <chris.blair@stericsson.com>
Reviewed-by: QATOOLS
Reviewed-by: QABUILD
Reviewed-by: Pawel SZYSZUK <pawel.szyszuk@stericsson.com>
Reviewed-by: Andrew LYNN <andrew.lynn@stericsson.com>

2 files changed, 21 insertions, 1 deletions

diff --git a/drivers/hsi/clients/hsi_char.c b/drivers/hsi/clients/hsi_char.c
index 4281156657b..6d5bf8d0012 100644
--- a/drivers/hsi/clients/hsi_char.c
+++ b/drivers/hsi/clients/hsi_char.c
@@ -123,7 +123,7 @@ static unsigned int max_data_size = 0x1000;
 module_param(max_data_size, uint, 1);
 MODULE_PARM_DESC(max_data_size, "max read/write data size [4,8..65536] (^2)");
 
-static int channels_map[HSI_CHAR_DEVS] = {0, -1, -1 , -1, -1, -1, -1, -1};
+static int channels_map[HSI_CHAR_DEVS] = {0, -1, -1, -1, -1, -1, -1, -1};
 module_param_array(channels_map, int, NULL, 0);
 MODULE_PARM_DESC(channels_map, "Array of HSI channels ([0...7]) to be probed");
 
@@ -624,10 +624,14 @@ static inline void hsi_char_rx2icfg(struct hsi_config *cfg,
 static inline void hsi_char_tx2icfg(struct hsi_config *cfg,
 						struct hsc_tx_config *tx_cfg)
 {
+	int ch;
+
 	cfg->mode = tx_cfg->mode;
 	cfg->channels = tx_cfg->channels;
 	cfg->speed = tx_cfg->speed;
 	cfg->arb_mode = tx_cfg->arb_mode;
+	for (ch = 0; ch < HSI_MAX_CHANNELS; ch++)
+		cfg->ch_prio[ch] = (tx_cfg->priority >> ch) & 1;
 }
 
 static inline void hsi_char_rx2ecfg(struct hsc_rx_config *rx_cfg,
@@ -641,10 +645,16 @@ static inline void hsi_char_rx2ecfg(struct hsc_rx_config *rx_cfg,
 static inline void hsi_char_tx2ecfg(struct hsc_tx_config *tx_cfg,
 							struct hsi_config *cfg)
 {
+	int ch;
+
 	tx_cfg->mode = cfg->mode;
 	tx_cfg->channels = cfg->channels;
 	tx_cfg->speed = cfg->speed;
 	tx_cfg->arb_mode = cfg->arb_mode;
+	tx_cfg->priority = 0;
+	for (ch = 0; ch < HSI_MAX_CHANNELS; ch++)
+		if (cfg->ch_prio[ch])
+			tx_cfg->priority |= (1 << ch);
 }
 
 static ssize_t hsi_char_read(struct file *file, char __user *buf,
diff --git a/drivers/hsi/controllers/ste_hsi.c b/drivers/hsi/controllers/ste_hsi.c
index 34e249f0a91..beae4ff95b1 100644
--- a/drivers/hsi/controllers/ste_hsi.c
+++ b/drivers/hsi/controllers/ste_hsi.c
@@ -31,6 +31,7 @@ struct ste_hsi_hw_context {
 	unsigned int tx_mode;
 	unsigned int tx_divisor;
 	unsigned int tx_channels;
+	unsigned int tx_priority;
 	unsigned int rx_mode;
 	unsigned int rx_channels;
 };
@@ -191,6 +192,8 @@ static void ste_hsi_setup_registers(struct ste_hsi_controller *ste_hsi)
 	writel(pcontext->tx_mode, ste_hsi->tx_base + STE_HSI_TX_MODE);
 	writel(pcontext->tx_divisor, ste_hsi->tx_base + STE_HSI_TX_DIVISOR);
 	writel(pcontext->tx_channels, ste_hsi->tx_base + STE_HSI_TX_CHANNELS);
+	writel(pcontext->tx_priority, ste_hsi->tx_base + STE_HSI_TX_PRIORITY);
+
 	/* Calculate buffers number per channel */
 	buffers = STE_HSI_MAX_BUFFERS / pcontext->tx_channels;
 	for (i = 0; i < pcontext->tx_channels; i++) {
@@ -1347,6 +1350,7 @@ static int ste_hsi_setup(struct hsi_client *cl)
 	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
 	int err;
 	u32 div = 0;
+	int ch;
 
 	if (ste_hsi->regulator)
 		regulator_enable(ste_hsi->regulator);
@@ -1372,6 +1376,12 @@ static int ste_hsi_setup(struct hsi_client *cl)
 		ste_hsi->context->tx_mode = cl->tx_cfg.mode;
 		ste_hsi->context->tx_divisor = div;
 		ste_hsi->context->tx_channels = cl->tx_cfg.channels;
+		ste_hsi->context->tx_priority = 0;
+		if (HSI_ARB_PRIO == cl->tx_cfg.arb_mode)
+			for (ch = 0; ch < STE_HSI_MAX_CHANNELS; ch++)
+				if (cl->tx_cfg.ch_prio[ch])
+					ste_hsi->context->tx_priority |=
+								(1 << ch);
 
 		if ((HSI_FLOW_PIPE == cl->rx_cfg.flow) &&
 			(HSI_MODE_FRAME == cl->rx_cfg.mode))