Signed-off-by: Russell King (Oracle) <rmk+kernel@armlinux.org.uk>
---
 drivers/net/dsa/mv88e6xxx/chip.h     |  2 +
 drivers/net/dsa/mv88e6xxx/hwtstamp.c | 60 +++++++++++++++++++++++++++-
 2 files changed, 60 insertions(+), 2 deletions(-)

diff --git a/drivers/net/dsa/mv88e6xxx/chip.h b/drivers/net/dsa/mv88e6xxx/chip.h
index db95265efa02..361c21aaf64b 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.h
+++ b/drivers/net/dsa/mv88e6xxx/chip.h
@@ -399,6 +399,8 @@ struct mv88e6xxx_chip {
 	struct marvell_ts ptp_ts[DSA_MAX_PORTS];
 	struct marvell_ts_caps ptp_caps;
 	u16 ptp_ts_enable_count;
+	int avb_irq;
+	char avb_irq_name[64];
 
 	/* Array of port structures. */
 	struct mv88e6xxx_port ports[DSA_MAX_PORTS];
diff --git a/drivers/net/dsa/mv88e6xxx/hwtstamp.c b/drivers/net/dsa/mv88e6xxx/hwtstamp.c
index 4f6b2706a8be..d8c675886ea5 100644
--- a/drivers/net/dsa/mv88e6xxx/hwtstamp.c
+++ b/drivers/net/dsa/mv88e6xxx/hwtstamp.c
@@ -11,9 +11,12 @@
  */
 
 #include "chip.h"
+#include "global1.h"
 #include "global2.h"
 #include "hwtstamp.h"
 #include "ptp.h"
+#include <linux/interrupt.h>
+#include <linux/irqdomain.h>
 #include <linux/ptp_classify.h>
 
 #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
@@ -163,6 +166,34 @@ void mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
 		kfree_skb(clone);
 }
 
+static irqreturn_t mv88e6xxx_ptp_irq_thread_fn(int irq, void *dev_id)
+{
+	struct mv88e6xxx_chip *chip = dev_id;
+	irqreturn_t r, ret = IRQ_NONE;
+	unsigned int n, max;
+	unsigned long mask;
+	u16 status;
+	int err;
+
+	mv88e6xxx_reg_lock(chip);
+	err = mv88e6xxx_ptp_read(chip, MV88E6XXX_PTP_IRQ_STATUS, &status);
+	mv88e6xxx_reg_unlock(chip);
+	if (err || !status)
+		return IRQ_NONE;
+
+	dev_printk(KERN_DEBUG, chip->dev, "ptp irq, status=%04x\n", status);
+
+	mask = status;
+	max = chip->ds->num_ports;
+	for_each_set_bit(n, &mask, max) {
+		r = marvell_ts_irq(&chip->ptp_ts[n]);
+		if (r != IRQ_NONE)
+			ret = r;
+	}
+
+	return ret;
+}
+
 int mv88e6165_global_disable(struct mv88e6xxx_chip *chip)
 {
 	u16 val;
@@ -409,7 +440,21 @@ int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
 int mv88e6xxx_hwtstamp_setup_unlocked(struct mv88e6xxx_chip *chip)
 {
 	unsigned int n_ports = mv88e6xxx_num_ports(chip);
-	int i, err;
+	int irq, i, err;
+
+	irq = irq_find_mapping(chip->g1_irq.domain, MV88E6XXX_G1_STS_IRQ_AVB);
+	if (irq > 0) {
+		chip->avb_irq = irq;
+		snprintf(chip->avb_irq_name, sizeof(chip->avb_irq_name),
+			 "mv88e6xxx-%s-g1-avb", dev_name(chip->dev));
+
+		if (request_threaded_irq(irq, NULL, mv88e6xxx_ptp_irq_thread_fn,
+					 IRQF_ONESHOT, chip->avb_irq_name,
+					 chip)) {
+			irq_dispose_mapping(irq);
+			chip->avb_irq = 0;
+		}
+	}
 
 	for (i = err = 0; i < n_ports; ++i) {
 		err = marvell_ts_probe(&chip->ptp_ts[i], chip->dev, chip->tai,
@@ -418,10 +463,16 @@ int mv88e6xxx_hwtstamp_setup_unlocked(struct mv88e6xxx_chip *chip)
 			break;
 	}
 
-	if (err)
+	if (err) {
 		while (i--)
 			marvell_ts_remove(&chip->ptp_ts[i]);
 
+		if (chip->avb_irq) {
+			irq_dispose_mapping(chip->avb_irq);
+			chip->avb_irq = 0;
+		}
+	}
+
 	return err;
 }
 
@@ -432,4 +483,9 @@ void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)
 
 	for (i = 0; i < n_ports; i++)
 		marvell_ts_remove(&chip->ptp_ts[i]);
+
+	if (chip->avb_irq) {
+		free_irq(chip->avb_irq, chip);
+		irq_dispose_mapping(chip->avb_irq);
+	}
 }
-- 
2.47.3