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);