From b2c90217392ecdf05bc98f73cce41c2008f9b0df Mon Sep 17 00:00:00 2001
From: Micky Ching <micky_ching@realsil.com.cn>
Date: Mon, 11 Aug 2014 14:12:42 +0800
Subject: [PATCH] mfd: rtsx: fix rts5227 suspend/resume setting

Signed-off-by: Micky Ching <micky_ching@realsil.com.cn>
---
 drivers/mfd/rts5227.c        |   13 +++++++++++++
 drivers/mfd/rtsx_pcr.c       |   16 ++++++++++++----
 include/linux/mfd/rtsx_pci.h |   18 ++++++++++++++++++
 3 files changed, 43 insertions(+), 4 deletions(-)

diff --git a/drivers/mfd/rts5227.c b/drivers/mfd/rts5227.c
index 9c8eec8..65f236b 100644
--- a/drivers/mfd/rts5227.c
+++ b/drivers/mfd/rts5227.c
@@ -128,8 +128,21 @@ static int rts5227_extra_init_hw(struct rtsx_pcr *pcr)
 	return rtsx_pci_send_cmd(pcr, 100);
 }
 
+static int rts5227_reset_first(struct rtsx_pcr *pcr)
+{
+	/* init aspm */
+	if (rtsx_pci_update_cfg_byte(pcr, LCTLR, 0xFC, 0))
+		pcr_err(pcr, "update LCTLR failed\n");
+	/* reset PM before send buffer cmd */
+	if (rtsx_pci_write_register(pcr, PM_CTRL3, 0x10, 0x00))
+		pcr_err(pcr, "set PM_CTRL3 failed\n");
+
+	return 0;
+}
+
 static int rts5227_optimize_phy(struct rtsx_pcr *pcr)
 {
+	rts5227_reset_first(pcr);
 	/* Optimize RX sensitivity */
 	return rtsx_pci_write_phy_register(pcr, 0x00, 0xBA42);
 }
diff --git a/drivers/mfd/rtsx_pcr.c b/drivers/mfd/rtsx_pcr.c
index 1d15735..a61739e 100644
--- a/drivers/mfd/rtsx_pcr.c
+++ b/drivers/mfd/rtsx_pcr.c
@@ -952,16 +952,20 @@ static int rtsx_pci_init_hw(struct rtsx_pcr *pcr)
 
 	/* Power on SSC */
 	err = rtsx_pci_write_register(pcr, FPDCTL, SSC_POWER_DOWN, 0);
-	if (err < 0)
+	if (err < 0) {
+		pcr_err(pcr, "power on ssc\n");
 		return err;
+	}
 
 	/* Wait SSC power stable */
 	udelay(200);
 
 	if (pcr->ops->optimize_phy) {
 		err = pcr->ops->optimize_phy(pcr);
-		if (err < 0)
+		if (err < 0) {
+			pcr_err(pcr, "optimize phy\n");
 			return err;
+		}
 	}
 
 	rtsx_pci_init_cmd(pcr);
@@ -1003,8 +1007,10 @@ static int rtsx_pci_init_hw(struct rtsx_pcr *pcr)
 	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, NFTS_TX_CTRL, 0x02, 0);
 
 	err = rtsx_pci_send_cmd(pcr, 100);
-	if (err < 0)
+	if (err < 0) {
+		pcr_err(pcr, "send init hw cmd\n");
 		return err;
+	}
 
 	rtsx_pci_write_config_byte(pcr, LCTLR, 0);
 
@@ -1015,8 +1021,10 @@ static int rtsx_pci_init_hw(struct rtsx_pcr *pcr)
 
 	if (pcr->ops->extra_init_hw) {
 		err = pcr->ops->extra_init_hw(pcr);
-		if (err < 0)
+		if (err < 0) {
+			pcr_err(pcr, "init extra hw\n");
 			return err;
+		}
 	}
 
 	/* No CD interrupt if probing driver with card inserted.
diff --git a/include/linux/mfd/rtsx_pci.h b/include/linux/mfd/rtsx_pci.h
index 0ce7721..a294951 100644
--- a/include/linux/mfd/rtsx_pci.h
+++ b/include/linux/mfd/rtsx_pci.h
@@ -915,6 +915,11 @@ struct rtsx_pcr {
 	struct rtsx_slot		*slots;
 };
 
+#define pcr_err(pcr, fmt, arg...)			\
+	dev_err(&(pcr)->pci->dev, "%s error: " fmt, __func__, ##arg)
+#define pcr_dbg(pcr, fmt, arg...)			\
+	dev_dbg(&(pcr)->pci->dev, "%s: " fmt, __func__, ##arg)
+
 #define CHK_PCI_PID(pcr, pid)		((pcr)->pci->device == (pid))
 #define PCI_VID(pcr)			((pcr)->pci->vendor)
 #define PCI_PID(pcr)			((pcr)->pci->device)
@@ -961,4 +966,17 @@ static inline u8 *rtsx_pci_get_cmd_data(struct rtsx_pcr *pcr)
 	return (u8 *)(pcr->host_cmds_ptr);
 }
 
+static inline int rtsx_pci_update_cfg_byte(struct rtsx_pcr *pcr, int addr,
+		u8 mask, u8 append)
+{
+	int err;
+	u8 val;
+
+	err = pci_read_config_byte(pcr->pci, addr, &val);
+	if (err)
+		return err;
+	err = pci_write_config_byte(pcr->pci, addr, (val & mask) | append);
+	return err;
+}
+
 #endif
-- 
1.7.9.5