3 files changed, 232 insertions, 17 deletions

diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.h b/drivers/scsi/mpt2sas/mpt2sas_base.h
index 014318fa3b0e..93c067f5dbbc 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.h
@@ -853,6 +853,8 @@ int mpt2sas_config_set_iounit_pg1(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigReply_t
     *mpi_reply, Mpi2IOUnitPage1_t *config_page);
 int mpt2sas_config_get_sas_iounit_pg1(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigReply_t
     *mpi_reply, Mpi2SasIOUnitPage1_t *config_page, u16 sz);
+int mpt2sas_config_set_sas_iounit_pg1(struct MPT2SAS_ADAPTER *ioc,
+    Mpi2ConfigReply_t *mpi_reply, Mpi2SasIOUnitPage1_t *config_page, u16 sz);
 int mpt2sas_config_get_ioc_pg8(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigReply_t
     *mpi_reply, Mpi2IOCPage8_t *config_page);
 int mpt2sas_config_get_expander_pg0(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigReply_t
diff --git a/drivers/scsi/mpt2sas/mpt2sas_config.c b/drivers/scsi/mpt2sas/mpt2sas_config.c
index 594a389c6526..411c27d7f787 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_config.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_config.c
@@ -324,7 +324,9 @@ _config_request(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigRequest_t
                 if (r != 0)
                         goto out;
                 if (mpi_request->Action ==
-                    MPI2_CONFIG_ACTION_PAGE_WRITE_CURRENT) {
+                    MPI2_CONFIG_ACTION_PAGE_WRITE_CURRENT ||
+                    mpi_request->Action ==
+                    MPI2_CONFIG_ACTION_PAGE_WRITE_NVRAM) {
                         ioc->base_add_sg_single(&mpi_request->PageBufferSGE,
                             MPT2_CONFIG_COMMON_WRITE_SGLFLAGS | mem.sz,
                             mem.page_dma);
@@ -882,7 +884,7 @@ mpt2sas_config_get_sas_iounit_pg0(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigReply_t
 }
 
 /**
- * mpt2sas_config_get_sas_iounit_pg1 - obtain sas iounit page 0
+ * mpt2sas_config_get_sas_iounit_pg1 - obtain sas iounit page 1
  * @ioc: per adapter object
  * @mpi_reply: reply mf payload returned from firmware
  * @config_page: contents of the config page
@@ -907,7 +909,7 @@ mpt2sas_config_get_sas_iounit_pg1(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigReply_t
         mpi_request.Header.PageType = MPI2_CONFIG_PAGETYPE_EXTENDED;
         mpi_request.ExtPageType = MPI2_CONFIG_EXTPAGETYPE_SAS_IO_UNIT;
         mpi_request.Header.PageNumber = 1;
-        mpi_request.Header.PageVersion = MPI2_SASIOUNITPAGE0_PAGEVERSION;
+        mpi_request.Header.PageVersion = MPI2_SASIOUNITPAGE1_PAGEVERSION;
         mpt2sas_base_build_zero_len_sge(ioc, &mpi_request.PageBufferSGE);
         r = _config_request(ioc, &mpi_request, mpi_reply,
             MPT2_CONFIG_PAGE_DEFAULT_TIMEOUT, NULL, 0);
@@ -922,6 +924,49 @@ mpt2sas_config_get_sas_iounit_pg1(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigReply_t
 }
 
 /**
+ * mpt2sas_config_set_sas_iounit_pg1 - send sas iounit page 1
+ * @ioc: per adapter object
+ * @mpi_reply: reply mf payload returned from firmware
+ * @config_page: contents of the config page
+ * @sz: size of buffer passed in config_page
+ * Context: sleep.
+ *
+ * Calling function should call config_get_number_hba_phys prior to
+ * this function, so enough memory is allocated for config_page.
+ *
+ * Returns 0 for success, non-zero for failure.
+ */
+int
+mpt2sas_config_set_sas_iounit_pg1(struct MPT2SAS_ADAPTER *ioc, Mpi2ConfigReply_t
+    *mpi_reply, Mpi2SasIOUnitPage1_t *config_page, u16 sz)
+{
+        Mpi2ConfigRequest_t mpi_request;
+        int r;
+
+        memset(&mpi_request, 0, sizeof(Mpi2ConfigRequest_t));
+        mpi_request.Function = MPI2_FUNCTION_CONFIG;
+        mpi_request.Action = MPI2_CONFIG_ACTION_PAGE_HEADER;
+        mpi_request.Header.PageType = MPI2_CONFIG_PAGETYPE_EXTENDED;
+        mpi_request.ExtPageType = MPI2_CONFIG_EXTPAGETYPE_SAS_IO_UNIT;
+        mpi_request.Header.PageNumber = 1;
+        mpi_request.Header.PageVersion = MPI2_SASIOUNITPAGE1_PAGEVERSION;
+        mpt2sas_base_build_zero_len_sge(ioc, &mpi_request.PageBufferSGE);
+        r = _config_request(ioc, &mpi_request, mpi_reply,
+            MPT2_CONFIG_PAGE_DEFAULT_TIMEOUT, NULL, 0);
+        if (r)
+                goto out;
+
+        mpi_request.Action = MPI2_CONFIG_ACTION_PAGE_WRITE_CURRENT;
+        _config_request(ioc, &mpi_request, mpi_reply,
+            MPT2_CONFIG_PAGE_DEFAULT_TIMEOUT, config_page, sz);
+        mpi_request.Action = MPI2_CONFIG_ACTION_PAGE_WRITE_NVRAM;
+        r = _config_request(ioc, &mpi_request, mpi_reply,
+            MPT2_CONFIG_PAGE_DEFAULT_TIMEOUT, config_page, sz);
+ out:
+        return r;
+}
+
+/**
  * mpt2sas_config_get_expander_pg0 - obtain expander page 0
  * @ioc: per adapter object
  * @mpi_reply: reply mf payload returned from firmware
diff --git a/drivers/scsi/mpt2sas/mpt2sas_transport.c b/drivers/scsi/mpt2sas/mpt2sas_transport.c
index 3a82872bad44..789f9ee7f001 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_transport.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_transport.c
@@ -855,6 +855,17 @@ rphy_to_ioc(struct sas_rphy *rphy)
         return shost_priv(shost);
 }
 
+static struct _sas_phy *
+_transport_find_local_phy(struct MPT2SAS_ADAPTER *ioc, struct sas_phy *phy)
+{
+        int i;
+
+        for (i = 0; i < ioc->sas_hba.num_phys; i++)
+                if (ioc->sas_hba.phy[i].phy == phy)
+                        return(&ioc->sas_hba.phy[i]);
+        return NULL;
+}
+
 /**
  * _transport_get_linkerrors -
  * @phy: The sas phy object
@@ -870,14 +881,8 @@ _transport_get_linkerrors(struct sas_phy *phy)
         struct _sas_phy *mpt2sas_phy;
         Mpi2ConfigReply_t mpi_reply;
         Mpi2SasPhyPage1_t phy_pg1;
-        int i;
 
-        for (i = 0, mpt2sas_phy = NULL; i < ioc->sas_hba.num_phys &&
-            !mpt2sas_phy; i++) {
-                if (ioc->sas_hba.phy[i].phy != phy)
-                        continue;
-                mpt2sas_phy = &ioc->sas_hba.phy[i];
-        }
+        mpt2sas_phy = _transport_find_local_phy(ioc, phy);
 
         if (!mpt2sas_phy) /* this phy not on sas_host */
                 return -EINVAL;
@@ -971,14 +976,8 @@ _transport_phy_reset(struct sas_phy *phy, int hard_reset)
         struct _sas_phy *mpt2sas_phy;
         Mpi2SasIoUnitControlReply_t mpi_reply;
         Mpi2SasIoUnitControlRequest_t mpi_request;
-        int i;
 
-        for (i = 0, mpt2sas_phy = NULL; i < ioc->sas_hba.num_phys &&
-            !mpt2sas_phy; i++) {
-                if (ioc->sas_hba.phy[i].phy != phy)
-                        continue;
-                mpt2sas_phy = &ioc->sas_hba.phy[i];
-        }
+        mpt2sas_phy = _transport_find_local_phy(ioc, phy);
 
         if (!mpt2sas_phy) /* this phy not on sas_host */
                 return -EINVAL;
@@ -1006,6 +1005,173 @@ _transport_phy_reset(struct sas_phy *phy, int hard_reset)
 }
 
 /**
+ * _transport_phy_enable - enable/disable phys
+ * @phy: The sas phy object
+ * @enable: enable phy when true
+ *
+ * Only support sas_host direct attached phys.
+ * Returns 0 for success, non-zero for failure.
+ */
+static int
+_transport_phy_enable(struct sas_phy *phy, int enable)
+{
+        struct MPT2SAS_ADAPTER *ioc = phy_to_ioc(phy);
+        struct _sas_phy *mpt2sas_phy;
+        Mpi2SasIOUnitPage1_t *sas_iounit_pg1 = NULL;
+        Mpi2ConfigReply_t mpi_reply;
+        u16 ioc_status;
+        u16 sz;
+        int rc = 0;
+
+        mpt2sas_phy = _transport_find_local_phy(ioc, phy);
+
+        if (!mpt2sas_phy) /* this phy not on sas_host */
+                return -EINVAL;
+
+        /* sas_iounit page 1 */
+        sz = offsetof(Mpi2SasIOUnitPage1_t, PhyData) + (ioc->sas_hba.num_phys *
+            sizeof(Mpi2SasIOUnit1PhyData_t));
+        sas_iounit_pg1 = kzalloc(sz, GFP_KERNEL);
+        if (!sas_iounit_pg1) {
+                printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+                    ioc->name, __FILE__, __LINE__, __func__);
+                rc = -ENOMEM;
+                goto out;
+        }
+        if ((mpt2sas_config_get_sas_iounit_pg1(ioc, &mpi_reply,
+            sas_iounit_pg1, sz))) {
+                printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+                    ioc->name, __FILE__, __LINE__, __func__);
+                rc = -ENXIO;
+                goto out;
+        }
+        ioc_status = le16_to_cpu(mpi_reply.IOCStatus) &
+            MPI2_IOCSTATUS_MASK;
+        if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
+                printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+                    ioc->name, __FILE__, __LINE__, __func__);
+                rc = -EIO;
+                goto out;
+        }
+
+        if (enable)
+                sas_iounit_pg1->PhyData[mpt2sas_phy->phy_id].PhyFlags
+                    &= ~MPI2_SASIOUNIT1_PHYFLAGS_PHY_DISABLE;
+        else
+                sas_iounit_pg1->PhyData[mpt2sas_phy->phy_id].PhyFlags
+                    |= MPI2_SASIOUNIT1_PHYFLAGS_PHY_DISABLE;
+
+        mpt2sas_config_set_sas_iounit_pg1(ioc, &mpi_reply, sas_iounit_pg1, sz);
+
+ out:
+        kfree(sas_iounit_pg1);
+        return rc;
+}
+
+/**
+ * _transport_phy_speed - set phy min/max link rates
+ * @phy: The sas phy object
+ * @rates: rates defined in sas_phy_linkrates
+ *
+ * Only support sas_host direct attached phys.
+ * Returns 0 for success, non-zero for failure.
+ */
+static int
+_transport_phy_speed(struct sas_phy *phy, struct sas_phy_linkrates *rates)
+{
+        struct MPT2SAS_ADAPTER *ioc = phy_to_ioc(phy);
+        struct _sas_phy *mpt2sas_phy;
+        Mpi2SasIOUnitPage1_t *sas_iounit_pg1 = NULL;
+        Mpi2SasPhyPage0_t phy_pg0;
+        Mpi2ConfigReply_t mpi_reply;
+        u16 ioc_status;
+        u16 sz;
+        int i;
+        int rc = 0;
+
+        mpt2sas_phy = _transport_find_local_phy(ioc, phy);
+
+        if (!mpt2sas_phy) /* this phy not on sas_host */
+                return -EINVAL;
+
+        if (!rates->minimum_linkrate)
+                rates->minimum_linkrate = phy->minimum_linkrate;
+        else if (rates->minimum_linkrate < phy->minimum_linkrate_hw)
+                rates->minimum_linkrate = phy->minimum_linkrate_hw;
+
+        if (!rates->maximum_linkrate)
+                rates->maximum_linkrate = phy->maximum_linkrate;
+        else if (rates->maximum_linkrate > phy->maximum_linkrate_hw)
+                rates->maximum_linkrate = phy->maximum_linkrate_hw;
+
+        /* sas_iounit page 1 */
+        sz = offsetof(Mpi2SasIOUnitPage1_t, PhyData) + (ioc->sas_hba.num_phys *
+            sizeof(Mpi2SasIOUnit1PhyData_t));
+        sas_iounit_pg1 = kzalloc(sz, GFP_KERNEL);
+        if (!sas_iounit_pg1) {
+                printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+                    ioc->name, __FILE__, __LINE__, __func__);
+                rc = -ENOMEM;
+                goto out;
+        }
+        if ((mpt2sas_config_get_sas_iounit_pg1(ioc, &mpi_reply,
+            sas_iounit_pg1, sz))) {
+                printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+                    ioc->name, __FILE__, __LINE__, __func__);
+                rc = -ENXIO;
+                goto out;
+        }
+        ioc_status = le16_to_cpu(mpi_reply.IOCStatus) &
+            MPI2_IOCSTATUS_MASK;
+        if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
+                printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+                    ioc->name, __FILE__, __LINE__, __func__);
+                rc = -EIO;
+                goto out;
+        }
+
+        for (i = 0; i < ioc->sas_hba.num_phys; i++) {
+                if (mpt2sas_phy->phy_id != i) {
+                        sas_iounit_pg1->PhyData[i].MaxMinLinkRate =
+                            (ioc->sas_hba.phy[i].phy->minimum_linkrate +
+                            (ioc->sas_hba.phy[i].phy->maximum_linkrate << 4));
+                } else {
+                        sas_iounit_pg1->PhyData[i].MaxMinLinkRate =
+                            (rates->minimum_linkrate +
+                            (rates->maximum_linkrate << 4));
+                }
+        }
+
+        if (mpt2sas_config_set_sas_iounit_pg1(ioc, &mpi_reply, sas_iounit_pg1,
+            sz)) {
+                printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+                    ioc->name, __FILE__, __LINE__, __func__);
+                rc = -ENXIO;
+                goto out;
+        }
+
+        /* link reset */
+        _transport_phy_reset(phy, 0);
+
+        /* read phy page 0, then update the rates in the sas transport phy */
+        if (!mpt2sas_config_get_phy_pg0(ioc, &mpi_reply, &phy_pg0,
+            mpt2sas_phy->phy_id)) {
+                phy->minimum_linkrate = _transport_convert_phy_link_rate(
+                    phy_pg0.ProgrammedLinkRate & MPI2_SAS_PRATE_MIN_RATE_MASK);
+                phy->maximum_linkrate = _transport_convert_phy_link_rate(
+                    phy_pg0.ProgrammedLinkRate >> 4);
+                phy->negotiated_linkrate = _transport_convert_phy_link_rate(
+                    phy_pg0.NegotiatedLinkRate &
+                    MPI2_SAS_NEG_LINK_RATE_MASK_PHYSICAL);
+        }
+
+ out:
+        kfree(sas_iounit_pg1);
+        return rc;
+}
+
+
+/**
  * _transport_smp_handler - transport portal for smp passthru
  * @shost: shost object
  * @rphy: sas transport rphy object
@@ -1207,6 +1373,8 @@ struct sas_function_template mpt2sas_transport_functions = {
         .get_enclosure_identifier = _transport_get_enclosure_identifier,
         .get_bay_identifier     = _transport_get_bay_identifier,
         .phy_reset              = _transport_phy_reset,
+        .phy_enable             = _transport_phy_enable,
+        .set_phy_speed          = _transport_phy_speed,
         .smp_handler            = _transport_smp_handler,
 };