1 files changed, 26 insertions, 16 deletions

diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index b3f35a6af32b..552006853cd7 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -735,6 +735,24 @@ static inline unsigned int mv_hardport_from_port(unsigned int port)
         return port & MV_PORT_MASK;
 }
 
+/*
+ * Consolidate some rather tricky bit shift calculations.
+ * This is hot-path stuff, so not a function.
+ * Simple code, with two return values, so macro rather than inline.
+ *
+ * port is the sole input, in range 0..7.
+ * shift is one output, for use with the main_cause and main_mask registers.
+ * hardport is the other output, in range 0..3
+ *
+ * Note that port and hardport may be the same variable in some cases.
+ */
+#define MV_PORT_TO_SHIFT_AND_HARDPORT(port, shift, hardport)    \
+{                                                               \
+        shift    = mv_hc_from_port(port) * HC_SHIFT;            \
+        hardport = mv_hardport_from_port(port);                 \
+        shift   += hardport * 2;                                \
+}
+
 static inline void __iomem *mv_hc_base(void __iomem *base, unsigned int hc)
 {
         return (base + MV_SATAHC0_REG_BASE + (hc * MV_SATAHC_REG_SZ));
@@ -2412,15 +2430,13 @@ static int mv_hardreset(struct ata_link *link, unsigned int *class,
 static void mv_eh_freeze(struct ata_port *ap)
 {
         struct mv_host_priv *hpriv = ap->host->private_data;
-        unsigned int hc = (ap->port_no > 3) ? 1 : 0;
-        unsigned int shift;
+        unsigned int shift, hardport, port = ap->port_no;
         u32 main_mask;
 
         /* FIXME: handle coalescing completion events properly */
 
-        shift = ap->port_no * 2;
-        if (hc > 0)
-                shift++;
+        mv_stop_edma(ap);
+        MV_PORT_TO_SHIFT_AND_HARDPORT(port, shift, hardport);
 
         /* disable assertion of portN err, done events */
         main_mask = readl(hpriv->main_mask_reg_addr);
@@ -2431,28 +2447,22 @@ static void mv_eh_freeze(struct ata_port *ap)
 static void mv_eh_thaw(struct ata_port *ap)
 {
         struct mv_host_priv *hpriv = ap->host->private_data;
-        void __iomem *mmio = hpriv->base;
-        unsigned int hc = (ap->port_no > 3) ? 1 : 0;
-        void __iomem *hc_mmio = mv_hc_base(mmio, hc);
+        unsigned int shift, hardport, port = ap->port_no;
+        void __iomem *hc_mmio = mv_hc_base_from_port(hpriv->base, port);
         void __iomem *port_mmio = mv_ap_base(ap);
-        unsigned int shift, hc_port_no = ap->port_no;
         u32 main_mask, hc_irq_cause;
 
         /* FIXME: handle coalescing completion events properly */
 
-        shift = ap->port_no * 2;
-        if (hc > 0) {
-                shift++;
-                hc_port_no -= 4;
-        }
+        MV_PORT_TO_SHIFT_AND_HARDPORT(port, shift, hardport);
 
         /* clear EDMA errors on this port */
         writel(0, port_mmio + EDMA_ERR_IRQ_CAUSE_OFS);
 
         /* clear pending irq events */
         hc_irq_cause = readl(hc_mmio + HC_IRQ_CAUSE_OFS);
-        hc_irq_cause &= ~((DEV_IRQ | DMA_IRQ) << hc_port_no);
-        writel(hc_irq_cause, hc_mmio + HC_IRQ_CAUSE_OFS);
+        hc_irq_cause &= ~((DEV_IRQ | DMA_IRQ) << hardport);
+        writelfl(hc_irq_cause, hc_mmio + HC_IRQ_CAUSE_OFS);
 
         /* enable assertion of portN err, done events */
         main_mask = readl(hpriv->main_mask_reg_addr);