hisi_sas: add the bist loopback feature.

driver inclusion
category: feature
bugzilla: NA
CVE: NA

add the bist loopback feature.
Signed-off-by: NYupeng Zhou <zhouyupeng1@huawei.com>
Reviewed-by: Nluojian <luojian5@huawei.com>
Reviewed-by: Nchenxiang <chenxiang66@hisilicon.com>
Reviewed-by: NYang Yingliang <yangyingliang@huawei.com>
Signed-off-by: NYang Yingliang <yangyingliang@huawei.com>

drivers/scsi/hisi_sas/hisi_sas.h

@@ -245,6 +245,28 @@ struct hisi_sas_debugfs_reg {
 	};
 };
 
+enum {
+	HISI_SAS_BIST_LOOPBACK_MODE_DIGITAL = 0,
+	HISI_SAS_BIST_LOOPBACK_MODE_SERDES,
+	HISI_SAS_BIST_LOOPBACK_MODE_REMOTE,
+};
+
+enum {
+	HISI_SAS_BIST_CODE_MODE_PRBS7 = 0,
+	HISI_SAS_BIST_CODE_MODE_PRBS23,
+	HISI_SAS_BIST_CODE_MODE_PRBS31,
+	HISI_SAS_BIST_CODE_MODE_JTPAT,
+	HISI_SAS_BIST_CODE_MODE_CJTPAT,
+	HISI_SAS_BIST_CODE_MODE_SCRAMBED_0,
+	HISI_SAS_BIST_CODE_MODE_TRAIN,
+	HISI_SAS_BIST_CODE_MODE_TRAIN_DONE,
+	HISI_SAS_BIST_CODE_MODE_HFTP,
+	HISI_SAS_BIST_CODE_MODE_MFTP,
+	HISI_SAS_BIST_CODE_MODE_LFTP,
+	HISI_SAS_BIST_CODE_MODE_FIXED_DATA,
+};
+
+
 struct hisi_sas_hw {
 	int (*hw_init)(struct hisi_hba *hisi_hba);
 	void (*setup_itct)(struct hisi_hba *hisi_hba,
@@ -297,6 +319,7 @@ struct hisi_sas_hw {
 
 	const struct hisi_sas_debugfs_reg *debugfs_reg_global;
 	const struct hisi_sas_debugfs_reg *debugfs_reg_port;
+	int (*set_bist)(struct hisi_hba *hisi_hba, bool enable);
 };
 
 struct hisi_hba {
@@ -364,6 +387,14 @@ struct hisi_hba {
 	u32 phy_state;
 	u32 intr_coal_ticks;	/* time of interrupt coalesce, unit:1us */
 	u32 intr_coal_count;	/* count of interrupt coalesce */
+	/* bist */
+	int bist_loopback_linkrate;
+	int bist_loopback_code_mode;
+	int bist_loopback_phy_id;
+	int bist_loopback_mode;
+	u32 bist_loopback_cnt;
+	int bist_loopback_enable;
+
 	int enable_dix_dif;
 
 	/* debugfs memories */
@@ -376,6 +407,7 @@ struct hisi_hba {
 
 	struct dentry *debugfs_dir;
 	struct dentry *debugfs_dump_dentry;
+	struct dentry *debugfs_bist_dentry;
 
 	bool user_ctl_irq;
 	unsigned int reply_map[NR_CPUS];

drivers/scsi/hisi_sas/hisi_sas_main.c

@@ -2857,6 +2857,388 @@ static const struct file_operations hisi_sas_debugfs_global_fops = {
 	.owner = THIS_MODULE,
 };
 
+static struct {
+	int		value;
+	char		*name;
+} hisi_sas_debugfs_loop_linkrate[] = {
+	{ SAS_LINK_RATE_1_5_GBPS,		"1.5 Gbit" },
+	{ SAS_LINK_RATE_3_0_GBPS,		"3.0 Gbit" },
+	{ SAS_LINK_RATE_6_0_GBPS,	"6.0 Gbit" },
+	{ SAS_LINK_RATE_12_0_GBPS,	"12.0 Gbit"},
+};
+
+static int hisi_sas_debugfs_bist_linkrate_show(struct seq_file *s, void *p)
+{
+	struct hisi_hba *hisi_hba = s->private;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hisi_sas_debugfs_loop_linkrate); i++) {
+		int match = (hisi_hba->bist_loopback_linkrate ==
+			     hisi_sas_debugfs_loop_linkrate[i].value);
+
+		seq_printf(s, "%s%s%s ", match ? "[" : "",
+			   hisi_sas_debugfs_loop_linkrate[i].name,
+			   match ? "]" : "");
+	}
+	seq_puts(s, "\n");
+
+	return 0;
+}
+
+ssize_t hisi_sas_debugfs_bist_linkrate_write(struct file *filp,
+					 const char __user *buf,
+					 size_t count, loff_t *ppos)
+{
+	struct seq_file *m = filp->private_data;
+	struct hisi_hba *hisi_hba = m->private;
+	char kbuf[16] = {}, *pkbuf;
+	bool found = false;
+	int i;
+
+	if (hisi_hba->bist_loopback_enable)
+		return -EINVAL;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kbuf, buf, count))
+		return -EINVAL;
+
+	pkbuf = strstrip(kbuf);
+
+	for (i = 0; i < ARRAY_SIZE(hisi_sas_debugfs_loop_linkrate); i++) {
+		if (!strncmp(hisi_sas_debugfs_loop_linkrate[i].name,
+			pkbuf, 16)) {
+			hisi_hba->bist_loopback_linkrate =
+				hisi_sas_debugfs_loop_linkrate[i].value;
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		dev_err(hisi_hba->dev, "unknown mode\n");
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static int hisi_sas_debugfs_bist_linkrate_open(struct inode *inode,
+					   struct file *filp)
+{
+	return single_open(filp, hisi_sas_debugfs_bist_linkrate_show,
+			   inode->i_private);
+}
+
+static const struct file_operations hisi_sas_debugfs_bist_linkrate_ops = {
+	.open = hisi_sas_debugfs_bist_linkrate_open,
+	.read = seq_read,
+	.write = hisi_sas_debugfs_bist_linkrate_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static struct {
+	int		value;
+	char		*name;
+} hisi_sas_debugfs_loop_code_mode[] = {
+	{ HISI_SAS_BIST_CODE_MODE_PRBS7,    "PRBS7" },
+	{ HISI_SAS_BIST_CODE_MODE_PRBS23,    "PRBS23" },
+	{ HISI_SAS_BIST_CODE_MODE_PRBS31,    "PRBS31" },
+	{ HISI_SAS_BIST_CODE_MODE_JTPAT,    "JTPAT" },
+	{ HISI_SAS_BIST_CODE_MODE_CJTPAT,    "CJTPAT" },
+	{ HISI_SAS_BIST_CODE_MODE_SCRAMBED_0,    "SCRAMBED_0" },
+	{ HISI_SAS_BIST_CODE_MODE_TRAIN,    "TRAIN" },
+	{ HISI_SAS_BIST_CODE_MODE_TRAIN_DONE,    "TRAIN_DONE" },
+	{ HISI_SAS_BIST_CODE_MODE_HFTP,    "HFTP" },
+	{ HISI_SAS_BIST_CODE_MODE_MFTP,    "MFTP" },
+	{ HISI_SAS_BIST_CODE_MODE_LFTP,    "LFTP" },
+	{ HISI_SAS_BIST_CODE_MODE_FIXED_DATA,    "FIXED_DATA" },
+};
+
+static int hisi_sas_debugfs_bist_code_mode_show(struct seq_file *s, void *p)
+{
+	struct hisi_hba *hisi_hba = s->private;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hisi_sas_debugfs_loop_code_mode); i++) {
+		int match = (hisi_hba->bist_loopback_code_mode ==
+			     hisi_sas_debugfs_loop_code_mode[i].value);
+
+		seq_printf(s, "%s%s%s ", match ? "[" : "",
+			   hisi_sas_debugfs_loop_code_mode[i].name,
+			   match ? "]" : "");
+	}
+	seq_puts(s, "\n");
+
+	return 0;
+}
+
+ssize_t hisi_sas_debugfs_bist_code_mode_write(struct file *filp,
+					 const char __user *buf,
+					 size_t count, loff_t *ppos)
+{
+	struct seq_file *m = filp->private_data;
+	struct hisi_hba *hisi_hba = m->private;
+	char kbuf[16] = {}, *pkbuf;
+	bool found = false;
+	int i;
+
+	if (hisi_hba->bist_loopback_enable)
+		return -EINVAL;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kbuf, buf, count))
+		return -EINVAL;
+
+	pkbuf = strstrip(kbuf);
+
+	for (i = 0; i < ARRAY_SIZE(hisi_sas_debugfs_loop_code_mode); i++) {
+		if (!strncmp(hisi_sas_debugfs_loop_code_mode[i].name,
+			pkbuf, 16)) {
+			hisi_hba->bist_loopback_code_mode =
+				hisi_sas_debugfs_loop_code_mode[i].value;
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		dev_err(hisi_hba->dev, "unknown mode\n");
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static int hisi_sas_debugfs_bist_code_mode_open(struct inode *inode,
+					   struct file *filp)
+{
+	return single_open(filp, hisi_sas_debugfs_bist_code_mode_show,
+			   inode->i_private);
+}
+
+static const struct file_operations hisi_sas_debugfs_bist_code_mode_ops = {
+	.open = hisi_sas_debugfs_bist_code_mode_open,
+	.read = seq_read,
+	.write = hisi_sas_debugfs_bist_code_mode_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+ssize_t hisi_sas_debugfs_bist_phy_write(struct file *filp,
+					const char __user *buf,
+					size_t count, loff_t *ppos)
+{
+	struct seq_file *m = filp->private_data;
+	struct hisi_hba *hisi_hba = m->private;
+	char kbuf[16] = {}, *pkbuf;
+	int val, phy;
+
+	if (hisi_hba->bist_loopback_enable)
+		return -EINVAL;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kbuf, buf, count))
+		return -EINVAL;
+
+	pkbuf = strstrip(kbuf);
+
+	val = kstrtoint(pkbuf, 0, &phy);
+	if (val < 0)
+		return val;
+
+	if (phy >= hisi_hba->n_phy) {
+		dev_err(hisi_hba->dev, "phy index %d exceeds limit\n", phy);
+		return -EINVAL;
+	}
+
+	hisi_hba->bist_loopback_phy_id = phy;
+
+	return count;
+}
+
+static int hisi_sas_debugfs_bist_phy_show(struct seq_file *s, void *p)
+{
+	struct hisi_hba *hisi_hba = s->private;
+
+	seq_printf(s, "%d\n", hisi_hba->bist_loopback_phy_id);
+
+	return 0;
+}
+
+static int hisi_sas_debugfs_bist_phy_open(struct inode *inode,
+					  struct file *filp)
+{
+	return single_open(filp, hisi_sas_debugfs_bist_phy_show,
+			   inode->i_private);
+}
+
+static const struct file_operations hisi_sas_debugfs_bist_phy_ops = {
+	.open = hisi_sas_debugfs_bist_phy_open,
+	.read = seq_read,
+	.write = hisi_sas_debugfs_bist_phy_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static struct {
+	int		value;
+	char		*name;
+} hisi_sas_debugfs_loop_modes[] = {
+	{ HISI_SAS_BIST_LOOPBACK_MODE_DIGITAL,		"digial" },
+	{ HISI_SAS_BIST_LOOPBACK_MODE_SERDES,		"serdes" },
+	{ HISI_SAS_BIST_LOOPBACK_MODE_REMOTE,	"remote" },
+};
+
+static int hisi_sas_debugfs_bist_mode_show(struct seq_file *s, void *p)
+{
+	struct hisi_hba *hisi_hba = s->private;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hisi_sas_debugfs_loop_modes); i++) {
+		int match = (hisi_hba->bist_loopback_mode ==
+			     hisi_sas_debugfs_loop_modes[i].value);
+
+		seq_printf(s, "%s%s%s ", match ? "[" : "",
+			   hisi_sas_debugfs_loop_modes[i].name,
+			   match ? "]" : "");
+	}
+	seq_puts(s, "\n");
+
+	return 0;
+}
+
+ssize_t hisi_sas_debugfs_bist_mode_write(struct file *filp,
+					 const char __user *buf,
+					 size_t count, loff_t *ppos)
+{
+	struct seq_file *m = filp->private_data;
+	struct hisi_hba *hisi_hba = m->private;
+	char kbuf[16] = {}, *pkbuf;
+	bool found = false;
+	int i;
+
+	if (hisi_hba->bist_loopback_enable)
+		return -EINVAL;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kbuf, buf, count))
+		return -EINVAL;
+
+	pkbuf = strstrip(kbuf);
+
+	for (i = 0; i < ARRAY_SIZE(hisi_sas_debugfs_loop_modes); i++) {
+		if (!strncmp(hisi_sas_debugfs_loop_modes[i].name, pkbuf, 16)) {
+			hisi_hba->bist_loopback_mode =
+				hisi_sas_debugfs_loop_modes[i].value;
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		dev_err(hisi_hba->dev, "unknown mode\n");
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static int hisi_sas_debugfs_bist_mode_open(struct inode *inode,
+					   struct file *filp)
+{
+	return single_open(filp, hisi_sas_debugfs_bist_mode_show,
+			   inode->i_private);
+}
+
+static const struct file_operations hisi_sas_debugfs_bist_mode_ops = {
+	.open = hisi_sas_debugfs_bist_mode_open,
+	.read = seq_read,
+	.write = hisi_sas_debugfs_bist_mode_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+ssize_t hisi_sas_debugfs_bist_enable_write(struct file *filp,
+					   const char __user *buf,
+					   size_t count, loff_t *ppos)
+{
+	struct seq_file *m = filp->private_data;
+	struct hisi_hba *hisi_hba = m->private;
+	char kbuf[16] = {}, *pkbuf;
+	int val;
+	unsigned int enable;
+
+	if (count >= sizeof(kbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kbuf, buf, count))
+		return -EINVAL;
+
+	pkbuf = strstrip(kbuf);
+
+	val = kstrtoint(pkbuf, 0, &enable);
+	if (val < 0)
+		return val;
+
+	if (enable > 1) {
+		dev_err(hisi_hba->dev, "must be 0 or 1\n");
+		return -EINVAL;
+	}
+
+	if (enable == hisi_hba->bist_loopback_enable)
+		return count;
+
+	if (!hisi_hba->hw->set_bist)
+		return -EPERM;
+
+	val = hisi_hba->hw->set_bist(hisi_hba, (bool)enable);
+	if (val < 0)
+		return val;
+
+	hisi_hba->bist_loopback_enable = enable;
+
+	return count;
+}
+
+static int hisi_sas_debugfs_bist_enable_show(struct seq_file *s, void *p)
+{
+	struct hisi_hba *hisi_hba = s->private;
+
+	seq_printf(s, "%d\n", hisi_hba->bist_loopback_enable);
+
+	return 0;
+}
+
+static int hisi_sas_debugfs_bist_enable_open(struct inode *inode,
+					     struct file *filp)
+{
+	return single_open(filp, hisi_sas_debugfs_bist_enable_show,
+			   inode->i_private);
+}
+
+static const struct file_operations hisi_sas_debugfs_bist_enable_ops = {
+	.open = hisi_sas_debugfs_bist_enable_open,
+	.read = seq_read,
+	.write = hisi_sas_debugfs_bist_enable_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+
 static int hisi_sas_debugfs_port_show(struct seq_file *s, void *p)
 {
 	struct hisi_sas_phy *phy = s->private;
@@ -3207,6 +3589,38 @@ void hisi_sas_debugfs_init(struct hisi_hba *hisi_hba)
 			    hisi_hba->debugfs_dir,
 			    hisi_hba,
 			    &hisi_sas_debugfs_trigger_dump_fops);
+	/* create bist structures */
+	hisi_hba->debugfs_bist_dentry = debugfs_create_dir("bist",
+						hisi_hba->debugfs_dir);
+	if (!hisi_hba->debugfs_bist_dentry)
+		goto fail_global;
+
+	if (!debugfs_create_file("link_rate", 0644,
+				 hisi_hba->debugfs_bist_dentry, hisi_hba,
+				 &hisi_sas_debugfs_bist_linkrate_ops))
+		goto fail_global;
+
+	if (!debugfs_create_file("code_mode", 0644,
+				 hisi_hba->debugfs_bist_dentry, hisi_hba,
+				 &hisi_sas_debugfs_bist_code_mode_ops))
+		goto fail_global;
+
+	if (!debugfs_create_file("phy_id", 0644, hisi_hba->debugfs_bist_dentry,
+				 hisi_hba, &hisi_sas_debugfs_bist_phy_ops))
+		goto fail_global;
+
+	if (!debugfs_create_u32("cnt", 0644, hisi_hba->debugfs_bist_dentry,
+			&hisi_hba->bist_loopback_cnt))
+		goto fail_global;
+
+	if (!debugfs_create_file("loopback mode", 0400,
+				 hisi_hba->debugfs_bist_dentry,
+				 hisi_hba, &hisi_sas_debugfs_bist_mode_ops))
+		goto fail_global;
+
+	if (!debugfs_create_file("enable", 0644, hisi_hba->debugfs_bist_dentry,
+				 hisi_hba, &hisi_sas_debugfs_bist_enable_ops))
+		goto fail_global;
 
 	/* Alloc buffer for global */
 	sz = hisi_hba->hw->debugfs_reg_global->count * 4;

drivers/scsi/hisi_sas/hisi_sas_v3_hw.c

@@ -196,12 +196,28 @@
 #define PHY_CFG_PHY_RST_OFF		3
 #define PHY_CFG_PHY_RST_MSK		(0x1 << PHY_CFG_PHY_RST_OFF)
 #define PROG_PHY_LINK_RATE		(PORT_BASE + 0x8)
+#define CFG_PROG_PHY_LINK_RATE_OFF	8
+#define CFG_PROG_PHY_LINK_RATE_MSK	(0xf << CFG_PROG_PHY_LINK_RATE_OFF)
 #define PHY_CTRL			(PORT_BASE + 0x14)
 #define PHY_CTRL_RESET_OFF		0
 #define PHY_CTRL_RESET_MSK		(0x1 << PHY_CTRL_RESET_OFF)
 #define CMD_HDR_PIR_OFF			8
 #define CMD_HDR_PIR_MSK			(0x1 << CMD_HDR_PIR_OFF)
 #define SERDES_CFG			(PORT_BASE + 0x1c)
+#define CFG_ALOS_CHK_DISABLE_OFF	9
+#define CFG_ALOS_CHK_DISABLE_MSK	(0x1 << CFG_ALOS_CHK_DISABLE_OFF)
+#define SAS_PHY_BIST_CTRL		(PORT_BASE + 0x2c)
+#define CFG_BIST_MODE_SEL_OFF		0
+#define CFG_BIST_MODE_SEL_MSK		(0xf << CFG_BIST_MODE_SEL_OFF)
+#define CFG_LOOP_TEST_MODE_OFF		14
+#define CFG_LOOP_TEST_MODE_MSK		(0x3 << CFG_LOOP_TEST_MODE_OFF)
+#define CFG_RX_BIST_EN_OFF		16
+#define CFG_RX_BIST_EN_MSK		(0x1 << CFG_RX_BIST_EN_OFF)
+#define CFG_TX_BIST_EN_OFF		17
+#define CFG_TX_BIST_EN_MSK		(0x1 << CFG_TX_BIST_EN_OFF)
+#define CFG_BIST_TEST_OFF		18
+#define CFG_BIST_TEST_MSK		(0x1 << CFG_BIST_TEST_OFF)
+#define SAS_BIST_ERR_CNT		(PORT_BASE + 0x38)
 #define SL_CFG				(PORT_BASE + 0x84)
 #define AIP_LIMIT			(PORT_BASE + 0x90)
 #define SL_CONTROL			(PORT_BASE + 0x94)
@@ -3031,6 +3047,106 @@ static void debugfs_snapshot_restore_v3_hw(struct hisi_hba *hisi_hba)
 	clear_bit(HISI_SAS_REJECT_CMD_BIT, &hisi_hba->flags);
 }
 
+static void hisi_sas_phy_enable(struct hisi_hba *hisi_hba,
+	u32 phy_id, const u32 phy_state)
+{
+	u32 phy_val = 0;
+	u32 temp = 0;
+
+	phy_val = hisi_sas_phy_read32(hisi_hba, phy_id, PHY_CFG);
+	temp = (phy_val & (~PHY_CFG_ENA_MSK)) | (phy_state << PHY_CFG_ENA_OFF);
+	hisi_sas_phy_write32(hisi_hba, phy_id, PHY_CFG, temp);
+}
+
+static void hisi_sas_bist_test_prep_v3_hw(struct hisi_hba *hisi_hba)
+{
+	u32 reg_val;
+	int phy_id = hisi_hba->bist_loopback_phy_id;
+
+	/* disable PHY */
+	hisi_sas_phy_enable(hisi_hba, phy_id, 0);
+
+	/* disable ALOS */
+	reg_val = hisi_sas_phy_read32(hisi_hba, phy_id, SERDES_CFG);
+	reg_val |= CFG_ALOS_CHK_DISABLE_MSK;
+	hisi_sas_phy_write32(hisi_hba, phy_id, SERDES_CFG, reg_val);
+}
+
+static void hisi_sas_bist_test_restore_v3_hw(struct hisi_hba *hisi_hba)
+{
+	u32 reg_val;
+	int phy_id = hisi_hba->bist_loopback_phy_id;
+
+	/* disable loopback */
+	reg_val = hisi_sas_phy_read32(hisi_hba, phy_id, SAS_PHY_BIST_CTRL);
+	reg_val &= ~(CFG_RX_BIST_EN_MSK | CFG_TX_BIST_EN_MSK |
+		     CFG_BIST_TEST_MSK);
+	hisi_sas_phy_write32(hisi_hba, phy_id, SAS_PHY_BIST_CTRL, reg_val);
+
+	/* enable ALOS*/
+	reg_val = hisi_sas_phy_read32(hisi_hba, phy_id, SERDES_CFG);
+	reg_val &= ~CFG_ALOS_CHK_DISABLE_MSK;
+	hisi_sas_phy_write32(hisi_hba, phy_id, SERDES_CFG, reg_val);
+
+	/* enable PHY */
+	hisi_sas_phy_enable(hisi_hba, phy_id, 1);
+}
+
+static int debugfs_set_bist_v3_hw(struct hisi_hba *hisi_hba, bool enable)
+{
+	u32 reg_val, mode_tmp;
+	u32 linkrate = hisi_hba->bist_loopback_linkrate;
+	u32 phy_id = hisi_hba->bist_loopback_phy_id;
+	u32 code_mode = hisi_hba->bist_loopback_code_mode;
+	u32 path_mode = hisi_hba->bist_loopback_mode;
+
+	pr_err("linkrate=%d phy_id=%d code_mode=%d path_mode=%d\n", linkrate,
+			phy_id, code_mode, path_mode);
+	mode_tmp = path_mode ? 2 : 1;
+	if (enable) {
+		/* some preparations before bist test */
+		hisi_sas_bist_test_prep_v3_hw(hisi_hba);
+
+		/* set linkrate of bit test*/
+		reg_val = hisi_sas_phy_read32(hisi_hba, phy_id,
+					      PROG_PHY_LINK_RATE);
+		reg_val &= ~CFG_PROG_PHY_LINK_RATE_MSK;
+		reg_val |= (linkrate << CFG_PROG_PHY_LINK_RATE_OFF);
+		hisi_sas_phy_write32(hisi_hba, phy_id,
+				     PROG_PHY_LINK_RATE, reg_val);
+
+		/* set code mode of bit test */
+		reg_val = hisi_sas_phy_read32(hisi_hba, phy_id,
+					      SAS_PHY_BIST_CTRL);
+		reg_val &= ~(CFG_BIST_MODE_SEL_MSK |
+				CFG_LOOP_TEST_MODE_MSK |
+				CFG_RX_BIST_EN_MSK |
+				CFG_TX_BIST_EN_MSK |
+				CFG_BIST_TEST_MSK);
+		reg_val |= ((code_mode << CFG_BIST_MODE_SEL_OFF) |
+			    (mode_tmp << CFG_LOOP_TEST_MODE_OFF) |
+			    CFG_BIST_TEST_MSK);
+		hisi_sas_phy_write32(hisi_hba, phy_id,
+				     SAS_PHY_BIST_CTRL, reg_val);
+
+		mdelay(100);
+		reg_val |= (CFG_RX_BIST_EN_MSK | CFG_TX_BIST_EN_MSK);
+		hisi_sas_phy_write32(hisi_hba, phy_id,
+				     SAS_PHY_BIST_CTRL, reg_val);
+
+		/* clear error bit */
+		mdelay(100);
+		hisi_sas_phy_read32(hisi_hba, phy_id, SAS_BIST_ERR_CNT);
+	} else {
+		/* disable bist test and recover it */
+		hisi_hba->bist_loopback_cnt += hisi_sas_phy_read32(hisi_hba,
+				phy_id, SAS_BIST_ERR_CNT);
+		hisi_sas_bist_test_restore_v3_hw(hisi_hba);
+	}
+
+	return 0;
+}
+
 const struct cpumask *
 get_managed_irq_aff_v3_hw(struct hisi_hba *hisi_hba, int queue)
 {
@@ -3104,6 +3220,7 @@ static const struct hisi_sas_hw hisi_sas_v3_hw = {
 	.debugfs_reg_port = &debugfs_port_reg,
 	.snapshot_prepare = debugfs_snapshot_prepare_v3_hw,
 	.snapshot_restore = debugfs_snapshot_restore_v3_hw,
+	.set_bist = debugfs_set_bist_v3_hw,
 	.get_managed_irq_aff = get_managed_irq_aff_v3_hw,
 };