tg3: Create funcs for power source switching

The power source switching code is about to get a little more complex.
This patch seeks to simplify future power source switching patches by
clarifying the existing code.

Signed-off-by: Matt Carlson <mcarlson@broadcom.com>
Reviewed-by: Michael Chan <mchan@broadcom.com>
Reviewed-by: Benjamin Li <benli@broadcom.com>
Signed-off-by: David S. Miller <davem@conan.davemloft.net>

1 files changed, 119 insertions, 82 deletions

diff --git a/drivers/net/tg3.c b/drivers/net/tg3.c
index da5e9105b9f4..ca6687a94225 100644
--- a/drivers/net/tg3.c
+++ b/drivers/net/tg3.c
@@ -107,6 +107,8 @@ static inline void _tg3_flag_clear(enum TG3_FLAGS flag, unsigned long *bits)
          NETIF_MSG_RX_ERR       | \
          NETIF_MSG_TX_ERR)
 
+#define TG3_GRC_LCLCTL_PWRSW_DELAY      100
+
 /* length of time before we decide the hardware is borked,
  * and dev->tx_timeout() should be called to fix the problem
  */
@@ -2165,6 +2167,118 @@ out:
         return 0;
 }
 
+static inline int tg3_pwrsrc_switch_to_vmain(struct tg3 *tp)
+{
+        if (!tg3_flag(tp, IS_NIC))
+                return 0;
+
+        tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl,
+                    TG3_GRC_LCLCTL_PWRSW_DELAY);
+
+        return 0;
+}
+
+static void tg3_pwrsrc_die_with_vmain(struct tg3 *tp)
+{
+        u32 grc_local_ctrl;
+
+        if (!tg3_flag(tp, IS_NIC) ||
+            GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5700 ||
+            GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5701)
+                return;
+
+        grc_local_ctrl = tp->grc_local_ctrl | GRC_LCLCTRL_GPIO_OE1;
+
+        tw32_wait_f(GRC_LOCAL_CTRL,
+                    grc_local_ctrl | GRC_LCLCTRL_GPIO_OUTPUT1,
+                    TG3_GRC_LCLCTL_PWRSW_DELAY);
+
+        tw32_wait_f(GRC_LOCAL_CTRL,
+                    grc_local_ctrl,
+                    TG3_GRC_LCLCTL_PWRSW_DELAY);
+
+        tw32_wait_f(GRC_LOCAL_CTRL,
+                    grc_local_ctrl | GRC_LCLCTRL_GPIO_OUTPUT1,
+                    TG3_GRC_LCLCTL_PWRSW_DELAY);
+}
+
+static void tg3_pwrsrc_switch_to_vaux(struct tg3 *tp)
+{
+        if (!tg3_flag(tp, IS_NIC))
+                return;
+
+        if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5700 ||
+            GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5701) {
+                tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
+                            (GRC_LCLCTRL_GPIO_OE0 |
+                             GRC_LCLCTRL_GPIO_OE1 |
+                             GRC_LCLCTRL_GPIO_OE2 |
+                             GRC_LCLCTRL_GPIO_OUTPUT0 |
+                             GRC_LCLCTRL_GPIO_OUTPUT1),
+                            TG3_GRC_LCLCTL_PWRSW_DELAY);
+        } else if (tp->pdev->device == PCI_DEVICE_ID_TIGON3_5761 ||
+                   tp->pdev->device == TG3PCI_DEVICE_TIGON3_5761S) {
+                /* The 5761 non-e device swaps GPIO 0 and GPIO 2. */
+                u32 grc_local_ctrl = GRC_LCLCTRL_GPIO_OE0 |
+                                     GRC_LCLCTRL_GPIO_OE1 |
+                                     GRC_LCLCTRL_GPIO_OE2 |
+                                     GRC_LCLCTRL_GPIO_OUTPUT0 |
+                                     GRC_LCLCTRL_GPIO_OUTPUT1 |
+                                     tp->grc_local_ctrl;
+                tw32_wait_f(GRC_LOCAL_CTRL, grc_local_ctrl,
+                            TG3_GRC_LCLCTL_PWRSW_DELAY);
+
+                grc_local_ctrl |= GRC_LCLCTRL_GPIO_OUTPUT2;
+                tw32_wait_f(GRC_LOCAL_CTRL, grc_local_ctrl,
+                            TG3_GRC_LCLCTL_PWRSW_DELAY);
+
+                grc_local_ctrl &= ~GRC_LCLCTRL_GPIO_OUTPUT0;
+                tw32_wait_f(GRC_LOCAL_CTRL, grc_local_ctrl,
+                            TG3_GRC_LCLCTL_PWRSW_DELAY);
+        } else {
+                u32 no_gpio2;
+                u32 grc_local_ctrl = 0;
+
+                /* Workaround to prevent overdrawing Amps. */
+                if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5714) {
+                        grc_local_ctrl |= GRC_LCLCTRL_GPIO_OE3;
+                        tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
+                                    grc_local_ctrl,
+                                    TG3_GRC_LCLCTL_PWRSW_DELAY);
+                }
+
+                /* On 5753 and variants, GPIO2 cannot be used. */
+                no_gpio2 = tp->nic_sram_data_cfg &
+                           NIC_SRAM_DATA_CFG_NO_GPIO2;
+
+                grc_local_ctrl |= GRC_LCLCTRL_GPIO_OE0 |
+                                  GRC_LCLCTRL_GPIO_OE1 |
+                                  GRC_LCLCTRL_GPIO_OE2 |
+                                  GRC_LCLCTRL_GPIO_OUTPUT1 |
+                                  GRC_LCLCTRL_GPIO_OUTPUT2;
+                if (no_gpio2) {
+                        grc_local_ctrl &= ~(GRC_LCLCTRL_GPIO_OE2 |
+                                            GRC_LCLCTRL_GPIO_OUTPUT2);
+                }
+                tw32_wait_f(GRC_LOCAL_CTRL,
+                            tp->grc_local_ctrl | grc_local_ctrl,
+                            TG3_GRC_LCLCTL_PWRSW_DELAY);
+
+                grc_local_ctrl |= GRC_LCLCTRL_GPIO_OUTPUT0;
+
+                tw32_wait_f(GRC_LOCAL_CTRL,
+                            tp->grc_local_ctrl | grc_local_ctrl,
+                            TG3_GRC_LCLCTL_PWRSW_DELAY);
+
+                if (!no_gpio2) {
+                        grc_local_ctrl &= ~GRC_LCLCTRL_GPIO_OUTPUT2;
+                        tw32_wait_f(GRC_LOCAL_CTRL,
+                                    tp->grc_local_ctrl | grc_local_ctrl,
+                                    TG3_GRC_LCLCTL_PWRSW_DELAY);
+                }
+        }
+}
+
 static void tg3_frob_aux_power(struct tg3 *tp)
 {
         bool need_vaux = false;
@@ -2200,86 +2314,10 @@ static void tg3_frob_aux_power(struct tg3 *tp)
         if (tg3_flag(tp, WOL_ENABLE) || tg3_flag(tp, ENABLE_ASF))
                 need_vaux = true;
 
-        if (need_vaux) {
-                if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5700 ||
-                    GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5701) {
-                        tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
-                                    (GRC_LCLCTRL_GPIO_OE0 |
-                                     GRC_LCLCTRL_GPIO_OE1 |
-                                     GRC_LCLCTRL_GPIO_OE2 |
-                                     GRC_LCLCTRL_GPIO_OUTPUT0 |
-                                     GRC_LCLCTRL_GPIO_OUTPUT1),
-                                    100);
-                } else if (tp->pdev->device == PCI_DEVICE_ID_TIGON3_5761 ||
-                           tp->pdev->device == TG3PCI_DEVICE_TIGON3_5761S) {
-                        /* The 5761 non-e device swaps GPIO 0 and GPIO 2. */
-                        u32 grc_local_ctrl = GRC_LCLCTRL_GPIO_OE0 |
-                                             GRC_LCLCTRL_GPIO_OE1 |
-                                             GRC_LCLCTRL_GPIO_OE2 |
-                                             GRC_LCLCTRL_GPIO_OUTPUT0 |
-                                             GRC_LCLCTRL_GPIO_OUTPUT1 |
-                                             tp->grc_local_ctrl;
-                        tw32_wait_f(GRC_LOCAL_CTRL, grc_local_ctrl, 100);
-
-                        grc_local_ctrl |= GRC_LCLCTRL_GPIO_OUTPUT2;
-                        tw32_wait_f(GRC_LOCAL_CTRL, grc_local_ctrl, 100);
-
-                        grc_local_ctrl &= ~GRC_LCLCTRL_GPIO_OUTPUT0;
-                        tw32_wait_f(GRC_LOCAL_CTRL, grc_local_ctrl, 100);
-                } else {
-                        u32 no_gpio2;
-                        u32 grc_local_ctrl = 0;
-
-                        /* Workaround to prevent overdrawing Amps. */
-                        if (GET_ASIC_REV(tp->pci_chip_rev_id) ==
-                            ASIC_REV_5714) {
-                                grc_local_ctrl |= GRC_LCLCTRL_GPIO_OE3;
-                                tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
-                                            grc_local_ctrl, 100);
-                        }
-
-                        /* On 5753 and variants, GPIO2 cannot be used. */
-                        no_gpio2 = tp->nic_sram_data_cfg &
-                                    NIC_SRAM_DATA_CFG_NO_GPIO2;
-
-                        grc_local_ctrl |= GRC_LCLCTRL_GPIO_OE0 |
-                                         GRC_LCLCTRL_GPIO_OE1 |
-                                         GRC_LCLCTRL_GPIO_OE2 |
-                                         GRC_LCLCTRL_GPIO_OUTPUT1 |
-                                         GRC_LCLCTRL_GPIO_OUTPUT2;
-                        if (no_gpio2) {
-                                grc_local_ctrl &= ~(GRC_LCLCTRL_GPIO_OE2 |
-                                                    GRC_LCLCTRL_GPIO_OUTPUT2);
-                        }
-                        tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
-                                                    grc_local_ctrl, 100);
-
-                        grc_local_ctrl |= GRC_LCLCTRL_GPIO_OUTPUT0;
-
-                        tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
-                                                    grc_local_ctrl, 100);
-
-                        if (!no_gpio2) {
-                                grc_local_ctrl &= ~GRC_LCLCTRL_GPIO_OUTPUT2;
-                                tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
-                                            grc_local_ctrl, 100);
-                        }
-                }
-        } else {
-                if (GET_ASIC_REV(tp->pci_chip_rev_id) != ASIC_REV_5700 &&
-                    GET_ASIC_REV(tp->pci_chip_rev_id) != ASIC_REV_5701) {
-                        tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
-                                    (GRC_LCLCTRL_GPIO_OE1 |
-                                     GRC_LCLCTRL_GPIO_OUTPUT1), 100);
-
-                        tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
-                                    GRC_LCLCTRL_GPIO_OE1, 100);
-
-                        tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl |
-                                    (GRC_LCLCTRL_GPIO_OE1 |
-                                     GRC_LCLCTRL_GPIO_OUTPUT1), 100);
-                }
-        }
+        if (need_vaux)
+                tg3_pwrsrc_switch_to_vaux(tp);
+        else
+                tg3_pwrsrc_die_with_vmain(tp);
 }
 
 static int tg3_5700_link_polarity(struct tg3 *tp, u32 speed)
@@ -2624,8 +2662,7 @@ static int tg3_power_up(struct tg3 *tp)
         pci_set_power_state(tp->pdev, PCI_D0);
 
         /* Switch out of Vaux if it is a NIC */
-        if (tg3_flag(tp, IS_NIC))
-                tw32_wait_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl, 100);
+        tg3_pwrsrc_switch_to_vmain(tp);
 
         return 0;
 }