Merge branch '4.14-features' of git://git.linux-mips.org/pub/scm/ralf/upstream-linus

Pull MIPS updates from Ralf Baechle:
 "This is the main pull request for 4.14 for MIPS; below a summary of
  the non-merge commits:

  CM:
   - Rename mips_cm_base to mips_gcr_base
   - Specify register size when generating accessors
   - Use BIT/GENMASK for register fields, order & drop shifts
   - Add cluster & block args to mips_cm_lock_other()

  CPC:
   - Use common CPS accessor generation macros
   - Use BIT/GENMASK for register fields, order & drop shifts
   - Introduce register modify (set/clear/change) accessors
   - Use change_*, set_* & clear_* where appropriate
   - Add CM/CPC 3.5 register definitions
   - Use GlobalNumber macros rather than magic numbers
   - Have asm/mips-cps.h include CM & CPC headers
   - Cluster support for topology functions
   - Detect CPUs in secondary clusters

  CPS:
   - Read GIC_VL_IDENT directly, not via irqchip driver

  DMA:
   - Consolidate coherent and non-coherent dma_alloc code
   - Don't use dma_cache_sync to implement fd_cacheflush

  FPU emulation / FP assist code:
   - Another series of 14 commits fixing corner cases such as NaN
     propgagation and other special input values.
   - Zero bits 32-63 of the result for a CLASS.D instruction.
   - Enhanced statics via debugfs
   - Do not use bools for arithmetic. GCC 7.1 moans about this.
   - Correct user fault_addr type

  Generic MIPS:
   - Enhancement of stack backtraces
   - Cleanup from non-existing options
   - Handle non word sized instructions when examining frame
   - Fix detection and decoding of ADDIUSP instruction
   - Fix decoding of SWSP16 instruction
   - Refactor handling of stack pointer in get_frame_info
   - Remove unreachable code from force_fcr31_sig()
   - Convert to using %pOF instead of full_name
   - Remove the R6000 support.
   - Move FP code from *_switch.S to *_fpu.S
   - Remove unused ST_OFF from r2300_switch.S
   - Allow platform to specify multiple its.S files
   - Add #includes to various files to ensure code builds reliable and
     without warning..
   - Remove __invalidate_kernel_vmap_range
   - Remove plat_timer_setup
   - Declare various variables & functions static
   - Abstract CPU core & VP(E) ID access through accessor functions
   - Store core & VP IDs in GlobalNumber-style variable
   - Unify checks for sibling CPUs
   - Add CPU cluster number accessors
   - Prevent direct use of generic_defconfig
   - Make CONFIG_MIPS_MT_SMP default y
   - Add __ioread64_copy
   - Remove unnecessary inclusions of linux/irqchip/mips-gic.h

  GIC:
   - Introduce asm/mips-gic.h with accessor functions
   - Use new GIC accessor functions in mips-gic-timer
   - Remove counter access functions from irq-mips-gic.c
   - Remove gic_read_local_vp_id() from irq-mips-gic.c
   - Simplify shared interrupt pending/mask reads in irq-mips-gic.c
   - Simplify gic_local_irq_domain_map() in irq-mips-gic.c
   - Drop gic_(re)set_mask() functions in irq-mips-gic.c
   - Remove gic_set_polarity(), gic_set_trigger(), gic_set_dual_edge(),
     gic_map_to_pin() and gic_map_to_vpe() from irq-mips-gic.c.
   - Convert remaining shared reg access, local int mask access and
     remaining local reg access to new accessors
   - Move GIC_LOCAL_INT_* to asm/mips-gic.h
   - Remove GIC_CPU_INT* macros from irq-mips-gic.c
   - Move various definitions to the driver
   - Remove gic_get_usm_range()
   - Remove __gic_irq_dispatch() forward declaration
   - Remove gic_init()
   - Use mips_gic_present() in place of gic_present and remove
     gic_present
   - Move gic_get_c0_*_int() to asm/mips-gic.h
   - Remove linux/irqchip/mips-gic.h
   - Inline __gic_init()
   - Inline gic_basic_init()
   - Make pcpu_masks a per-cpu variable
   - Use pcpu_masks to avoid reading GIC_SH_MASK*
   - Clean up mti, reserved-cpu-vectors handling
   - Use cpumask_first_and() in gic_set_affinity()
   - Let the core set struct irq_common_data affinity

  microMIPS:
   - Fix microMIPS stack unwinding on big endian systems

  MIPS-GIC:
   - SYNC after enabling GIC region

  NUMA:
   - Remove the unused parent_node() macro

  R6:
   - Constify r2_decoder_tables
   - Add accessor & bit definitions for GlobalNumber

  SMP:
   - Constify smp ops
   - Allow boot_secondary SMP op to return errors

  VDSO:
   - Drop gic_get_usm_range() usage
   - Avoid use of linux/irqchip/mips-gic.h

  Platform changes:

  Alchemy:
   - Add devboard machine type to cpuinfo
   - update cpu feature overrides
   - Threaded carddetect irqs for devboards

  AR7:
   - allow NULL clock for clk_get_rate

  BCM63xx:
   - Fix ENETDMA_6345_MAXBURST_REG offset
   - Allow NULL clock for clk_get_rate

  CI20:
   - Enable GPIO and RTC drivers in defconfig
   - Add ethernet and fixed-regulator nodes to DTS

  Generic platform:
   - Move Boston and NI 169445 FIT image source to their own files
   - Include asm/bootinfo.h for plat_fdt_relocated()
   - Include asm/time.h for get_c0_*_int()
   - Include asm/bootinfo.h for plat_fdt_relocated()
   - Include asm/time.h for get_c0_*_int()
   - Allow filtering enabled boards by requirements
   - Don't explicitly disable CONFIG_USB_SUPPORT
   - Bump default NR_CPUS to 16

  JZ4700:
   - Probe the jz4740-rtc driver from devicetree

  Lantiq:
   - Drop check of boot select from the spi-falcon driver.
   - Drop check of boot select from the lantiq-flash MTD driver.
   - Access boot cause register in the watchdog driver through regmap
   - Add device tree binding documentation for the watchdog driver
   - Add docs for the RCU DT bindings.
   - Convert the fpi bus driver to a platform_driver
   - Remove ltq_reset_cause() and ltq_boot_select(
   - Switch to a proper reset driver
   - Switch to a new drivers/soc GPHY driver
   - Add an USB PHY driver for the Lantiq SoCs using the RCU module
   - Use of_platform_default_populate instead of __dt_register_buses
   - Enable MFD_SYSCON to be able to use it for the RCU MFD
   - Replace ltq_boot_select() with dummy implementation.

  Loongson 2F:
   - Allow NULL clock for clk_get_rate

  Malta:
   - Use new GIC accessor functions

  NI 169445:
   - Add support for NI 169445 board.
   - Only include in 32r2el kernels

  Octeon:
   - Add support for watchdog of 78XX SOCs.
   - Add support for watchdog of CN68XX SOCs.
   - Expose support for mips32r1, mips32r2 and mips64r1
   - Enable more drivers in config file
   - Add support for accessing the boot vector.
   - Remove old boot vector code from watchdog driver
   - Define watchdog registers for 70xx, 73xx, 78xx, F75xx.
   - Make CSR functions node aware.
   - Allow access to CIU3 IRQ domains.
   - Misc cleanups in the watchdog driver

  Omega2+:
   - New board, add support and defconfig

  Pistachio:
   - Enable Root FS on NFS in defconfig

  Ralink:
   - Add Mediatek MT7628A SoC
   - Allow NULL clock for clk_get_rate
   - Explicitly request exclusive reset control in the pci-mt7620 PCI driver.

  SEAD3:
   - Only include in 32 bit kernels by default

  VoCore:
   - Add VoCore as a vendor t0 dt-bindings
   - Add defconfig file"

* '4.14-features' of git://git.linux-mips.org/pub/scm/ralf/upstream-linus: (167 commits)
  MIPS: Refactor handling of stack pointer in get_frame_info
  MIPS: Stacktrace: Fix microMIPS stack unwinding on big endian systems
  MIPS: microMIPS: Fix decoding of swsp16 instruction
  MIPS: microMIPS: Fix decoding of addiusp instruction
  MIPS: microMIPS: Fix detection of addiusp instruction
  MIPS: Handle non word sized instructions when examining frame
  MIPS: ralink: allow NULL clock for clk_get_rate
  MIPS: Loongson 2F: allow NULL clock for clk_get_rate
  MIPS: BCM63XX: allow NULL clock for clk_get_rate
  MIPS: AR7: allow NULL clock for clk_get_rate
  MIPS: BCM63XX: fix ENETDMA_6345_MAXBURST_REG offset
  mips: Save all registers when saving the frame
  MIPS: Add DWARF unwinding to assembly
  MIPS: Make SAVE_SOME more standard
  MIPS: Fix issues in backtraces
  MIPS: jz4780: DTS: Probe the jz4740-rtc driver from devicetree
  MIPS: Ci20: Enable RTC driver
  watchdog: octeon-wdt: Add support for 78XX SOCs.
  watchdog: octeon-wdt: Add support for cn68XX SOCs.
  watchdog: octeon-wdt: File cleaning.
  ...

21 files changed, 1346 insertions, 656 deletions

diff --git a/drivers/clocksource/mips-gic-timer.c b/drivers/clocksource/mips-gic-timer.c
index 17b861ea2626..ae3167c28b12 100644
--- a/drivers/clocksource/mips-gic-timer.c
+++ b/drivers/clocksource/mips-gic-timer.c
@@ -10,25 +10,45 @@
 #include <linux/cpu.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
-#include <linux/irqchip/mips-gic.h>
 #include <linux/notifier.h>
 #include <linux/of_irq.h>
 #include <linux/percpu.h>
 #include <linux/smp.h>
 #include <linux/time.h>
+#include <asm/mips-cps.h>
 
 static DEFINE_PER_CPU(struct clock_event_device, gic_clockevent_device);
 static int gic_timer_irq;
 static unsigned int gic_frequency;
 
+static u64 notrace gic_read_count(void)
+{
+	unsigned int hi, hi2, lo;
+
+	if (mips_cm_is64)
+		return read_gic_counter();
+
+	do {
+		hi = read_gic_counter_32h();
+		lo = read_gic_counter_32l();
+		hi2 = read_gic_counter_32h();
+	} while (hi2 != hi);
+
+	return (((u64) hi) << 32) + lo;
+}
+
 static int gic_next_event(unsigned long delta, struct clock_event_device *evt)
 {
+	unsigned long flags;
 	u64 cnt;
 	int res;
 
 	cnt = gic_read_count();
 	cnt += (u64)delta;
-	gic_write_cpu_compare(cnt, cpumask_first(evt->cpumask));
+	local_irq_save(flags);
+	write_gic_vl_other(mips_cm_vp_id(cpumask_first(evt->cpumask)));
+	write_gic_vo_compare(cnt);
+	local_irq_restore(flags);
 	res = ((int)(gic_read_count() - cnt) >= 0) ? -ETIME : 0;
 	return res;
 }
@@ -37,7 +57,7 @@ static irqreturn_t gic_compare_interrupt(int irq, void *dev_id)
 {
 	struct clock_event_device *cd = dev_id;
 
-	gic_write_compare(gic_read_compare());
+	write_gic_vl_compare(read_gic_vl_compare());
 	cd->event_handler(cd);
 	return IRQ_HANDLED;
 }
@@ -139,10 +159,15 @@ static struct clocksource gic_clocksource = {
 
 static int __init __gic_clocksource_init(void)
 {
+	unsigned int count_width;
 	int ret;
 
 	/* Set clocksource mask. */
-	gic_clocksource.mask = CLOCKSOURCE_MASK(gic_get_count_width());
+	count_width = read_gic_config() & GIC_CONFIG_COUNTBITS;
+	count_width >>= __fls(GIC_CONFIG_COUNTBITS);
+	count_width *= 4;
+	count_width += 32;
+	gic_clocksource.mask = CLOCKSOURCE_MASK(count_width);
 
 	/* Calculate a somewhat reasonable rating value. */
 	gic_clocksource.rating = 200 + gic_frequency / 10000000;
@@ -159,7 +184,7 @@ static int __init gic_clocksource_of_init(struct device_node *node)
 	struct clk *clk;
 	int ret;
 
-	if (!gic_present || !node->parent ||
+	if (!mips_gic_present() || !node->parent ||
 	    !of_device_is_compatible(node->parent, "mti,gic")) {
 		pr_warn("No DT definition for the mips gic driver\n");
 		return -ENXIO;
@@ -197,7 +222,7 @@ static int __init gic_clocksource_of_init(struct device_node *node)
 	}
 
 	/* And finally start the counter */
-	gic_start_count();
+	clear_gic_config(GIC_CONFIG_COUNTSTOP);
 
 	return 0;
 }
diff --git a/drivers/cpuidle/cpuidle-cps.c b/drivers/cpuidle/cpuidle-cps.c
index 12b9145913de..72b5e47286b4 100644
--- a/drivers/cpuidle/cpuidle-cps.c
+++ b/drivers/cpuidle/cpuidle-cps.c
@@ -37,7 +37,7 @@ static int cps_nc_enter(struct cpuidle_device *dev,
 	 * TODO: don't treat core 0 specially, just prevent the final core
 	 * TODO: remap interrupt affinity temporarily
 	 */
-	if (!cpu_data[dev->cpu].core && (index > STATE_NC_WAIT))
+	if (cpus_are_siblings(0, dev->cpu) && (index > STATE_NC_WAIT))
 		index = STATE_NC_WAIT;
 
 	/* Select the appropriate cps_pm_state */
diff --git a/drivers/irqchip/irq-mips-cpu.c b/drivers/irqchip/irq-mips-cpu.c
index 14461cbfab2f..66f97fde13d8 100644
--- a/drivers/irqchip/irq-mips-cpu.c
+++ b/drivers/irqchip/irq-mips-cpu.c
@@ -101,7 +101,7 @@ static void mips_mt_send_ipi(struct irq_data *d, unsigned int cpu)
 	local_irq_save(flags);
 
 	/* We can only send IPIs to VPEs within the local core */
-	WARN_ON(cpu_data[cpu].core != current_cpu_data.core);
+	WARN_ON(!cpus_are_siblings(smp_processor_id(), cpu));
 
 	vpflags = dvpe();
 	settc(cpu_vpe_id(&cpu_data[cpu]));
diff --git a/drivers/irqchip/irq-mips-gic.c b/drivers/irqchip/irq-mips-gic.c
index b3a60da088db..6e52a88bbd9e 100644
--- a/drivers/irqchip/irq-mips-gic.c
+++ b/drivers/irqchip/irq-mips-gic.c
@@ -12,27 +12,38 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/irqchip.h>
-#include <linux/irqchip/mips-gic.h>
 #include <linux/of_address.h>
+#include <linux/percpu.h>
 #include <linux/sched.h>
 #include <linux/smp.h>
 
-#include <asm/mips-cm.h>
+#include <asm/mips-cps.h>
 #include <asm/setup.h>
 #include <asm/traps.h>
 
 #include <dt-bindings/interrupt-controller/mips-gic.h>
 
-unsigned int gic_present;
+#define GIC_MAX_INTRS		256
+#define GIC_MAX_LONGS		BITS_TO_LONGS(GIC_MAX_INTRS)
 
-struct gic_pcpu_mask {
-	DECLARE_BITMAP(pcpu_mask, GIC_MAX_INTRS);
-};
+/* Add 2 to convert GIC CPU pin to core interrupt */
+#define GIC_CPU_PIN_OFFSET	2
+
+/* Mapped interrupt to pin X, then GIC will generate the vector (X+1). */
+#define GIC_PIN_TO_VEC_OFFSET	1
+
+/* Convert between local/shared IRQ number and GIC HW IRQ number. */
+#define GIC_LOCAL_HWIRQ_BASE	0
+#define GIC_LOCAL_TO_HWIRQ(x)	(GIC_LOCAL_HWIRQ_BASE + (x))
+#define GIC_HWIRQ_TO_LOCAL(x)	((x) - GIC_LOCAL_HWIRQ_BASE)
+#define GIC_SHARED_HWIRQ_BASE	GIC_NUM_LOCAL_INTRS
+#define GIC_SHARED_TO_HWIRQ(x)	(GIC_SHARED_HWIRQ_BASE + (x))
+#define GIC_HWIRQ_TO_SHARED(x)	((x) - GIC_SHARED_HWIRQ_BASE)
+
+void __iomem *mips_gic_base;
 
-static unsigned long __gic_base_addr;
+DEFINE_PER_CPU_READ_MOSTLY(unsigned long[GIC_MAX_LONGS], pcpu_masks);
 
-static void __iomem *gic_base;
-static struct gic_pcpu_mask pcpu_masks[NR_CPUS];
 static DEFINE_SPINLOCK(gic_lock);
 static struct irq_domain *gic_irq_domain;
 static struct irq_domain *gic_ipi_domain;
@@ -44,202 +55,13 @@ static struct irq_chip gic_level_irq_controller, gic_edge_irq_controller;
 DECLARE_BITMAP(ipi_resrv, GIC_MAX_INTRS);
 DECLARE_BITMAP(ipi_available, GIC_MAX_INTRS);
 
-static void __gic_irq_dispatch(void);
-
-static inline u32 gic_read32(unsigned int reg)
-{
-	return __raw_readl(gic_base + reg);
-}
-
-static inline u64 gic_read64(unsigned int reg)
-{
-	return __raw_readq(gic_base + reg);
-}
-
-static inline unsigned long gic_read(unsigned int reg)
-{
-	if (!mips_cm_is64)
-		return gic_read32(reg);
-	else
-		return gic_read64(reg);
-}
-
-static inline void gic_write32(unsigned int reg, u32 val)
-{
-	return __raw_writel(val, gic_base + reg);
-}
-
-static inline void gic_write64(unsigned int reg, u64 val)
-{
-	return __raw_writeq(val, gic_base + reg);
-}
-
-static inline void gic_write(unsigned int reg, unsigned long val)
-{
-	if (!mips_cm_is64)
-		return gic_write32(reg, (u32)val);
-	else
-		return gic_write64(reg, (u64)val);
-}
-
-static inline void gic_update_bits(unsigned int reg, unsigned long mask,
-				   unsigned long val)
-{
-	unsigned long regval;
-
-	regval = gic_read(reg);
-	regval &= ~mask;
-	regval |= val;
-	gic_write(reg, regval);
-}
-
-static inline void gic_reset_mask(unsigned int intr)
-{
-	gic_write(GIC_REG(SHARED, GIC_SH_RMASK) + GIC_INTR_OFS(intr),
-		  1ul << GIC_INTR_BIT(intr));
-}
-
-static inline void gic_set_mask(unsigned int intr)
-{
-	gic_write(GIC_REG(SHARED, GIC_SH_SMASK) + GIC_INTR_OFS(intr),
-		  1ul << GIC_INTR_BIT(intr));
-}
-
-static inline void gic_set_polarity(unsigned int intr, unsigned int pol)
-{
-	gic_update_bits(GIC_REG(SHARED, GIC_SH_SET_POLARITY) +
-			GIC_INTR_OFS(intr), 1ul << GIC_INTR_BIT(intr),
-			(unsigned long)pol << GIC_INTR_BIT(intr));
-}
-
-static inline void gic_set_trigger(unsigned int intr, unsigned int trig)
-{
-	gic_update_bits(GIC_REG(SHARED, GIC_SH_SET_TRIGGER) +
-			GIC_INTR_OFS(intr), 1ul << GIC_INTR_BIT(intr),
-			(unsigned long)trig << GIC_INTR_BIT(intr));
-}
-
-static inline void gic_set_dual_edge(unsigned int intr, unsigned int dual)
-{
-	gic_update_bits(GIC_REG(SHARED, GIC_SH_SET_DUAL) + GIC_INTR_OFS(intr),
-			1ul << GIC_INTR_BIT(intr),
-			(unsigned long)dual << GIC_INTR_BIT(intr));
-}
-
-static inline void gic_map_to_pin(unsigned int intr, unsigned int pin)
-{
-	gic_write32(GIC_REG(SHARED, GIC_SH_INTR_MAP_TO_PIN_BASE) +
-		    GIC_SH_MAP_TO_PIN(intr), GIC_MAP_TO_PIN_MSK | pin);
-}
-
-static inline void gic_map_to_vpe(unsigned int intr, unsigned int vpe)
-{
-	gic_write(GIC_REG(SHARED, GIC_SH_INTR_MAP_TO_VPE_BASE) +
-		  GIC_SH_MAP_TO_VPE_REG_OFF(intr, vpe),
-		  GIC_SH_MAP_TO_VPE_REG_BIT(vpe));
-}
-
-#ifdef CONFIG_CLKSRC_MIPS_GIC
-u64 notrace gic_read_count(void)
-{
-	unsigned int hi, hi2, lo;
-
-	if (mips_cm_is64)
-		return (u64)gic_read(GIC_REG(SHARED, GIC_SH_COUNTER));
-
-	do {
-		hi = gic_read32(GIC_REG(SHARED, GIC_SH_COUNTER_63_32));
-		lo = gic_read32(GIC_REG(SHARED, GIC_SH_COUNTER_31_00));
-		hi2 = gic_read32(GIC_REG(SHARED, GIC_SH_COUNTER_63_32));
-	} while (hi2 != hi);
-
-	return (((u64) hi) << 32) + lo;
-}
-
-unsigned int gic_get_count_width(void)
-{
-	unsigned int bits, config;
-
-	config = gic_read(GIC_REG(SHARED, GIC_SH_CONFIG));
-	bits = 32 + 4 * ((config & GIC_SH_CONFIG_COUNTBITS_MSK) >>
-			 GIC_SH_CONFIG_COUNTBITS_SHF);
-
-	return bits;
-}
-
-void notrace gic_write_compare(u64 cnt)
-{
-	if (mips_cm_is64) {
-		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE), cnt);
-	} else {
-		gic_write32(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE_HI),
-					(int)(cnt >> 32));
-		gic_write32(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE_LO),
-					(int)(cnt & 0xffffffff));
-	}
-}
-
-void notrace gic_write_cpu_compare(u64 cnt, int cpu)
-{
-	unsigned long flags;
-
-	local_irq_save(flags);
-
-	gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR), mips_cm_vp_id(cpu));
-
-	if (mips_cm_is64) {
-		gic_write(GIC_REG(VPE_OTHER, GIC_VPE_COMPARE), cnt);
-	} else {
-		gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_COMPARE_HI),
-					(int)(cnt >> 32));
-		gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_COMPARE_LO),
-					(int)(cnt & 0xffffffff));
-	}
-
-	local_irq_restore(flags);
-}
-
-u64 gic_read_compare(void)
-{
-	unsigned int hi, lo;
-
-	if (mips_cm_is64)
-		return (u64)gic_read(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE));
-
-	hi = gic_read32(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE_HI));
-	lo = gic_read32(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE_LO));
-
-	return (((u64) hi) << 32) + lo;
-}
-
-void gic_start_count(void)
+static void gic_clear_pcpu_masks(unsigned int intr)
 {
-	u32 gicconfig;
-
-	/* Start the counter */
-	gicconfig = gic_read(GIC_REG(SHARED, GIC_SH_CONFIG));
-	gicconfig &= ~(1 << GIC_SH_CONFIG_COUNTSTOP_SHF);
-	gic_write(GIC_REG(SHARED, GIC_SH_CONFIG), gicconfig);
-}
-
-void gic_stop_count(void)
-{
-	u32 gicconfig;
-
-	/* Stop the counter */
-	gicconfig = gic_read(GIC_REG(SHARED, GIC_SH_CONFIG));
-	gicconfig |= 1 << GIC_SH_CONFIG_COUNTSTOP_SHF;
-	gic_write(GIC_REG(SHARED, GIC_SH_CONFIG), gicconfig);
-}
-
-#endif
-
-unsigned gic_read_local_vp_id(void)
-{
-	unsigned long ident;
+	unsigned int i;
 
-	ident = gic_read(GIC_REG(VPE_LOCAL, GIC_VP_IDENT));
-	return ident & GIC_VP_IDENT_VCNUM_MSK;
+	/* Clear the interrupt's bit in all pcpu_masks */
+	for_each_possible_cpu(i)
+		clear_bit(intr, per_cpu_ptr(pcpu_masks, i));
 }
 
 static bool gic_local_irq_is_routable(int intr)
@@ -250,17 +72,17 @@ static bool gic_local_irq_is_routable(int intr)
 	if (cpu_has_veic)
 		return true;
 
-	vpe_ctl = gic_read32(GIC_REG(VPE_LOCAL, GIC_VPE_CTL));
+	vpe_ctl = read_gic_vl_ctl();
 	switch (intr) {
 	case GIC_LOCAL_INT_TIMER:
-		return vpe_ctl & GIC_VPE_CTL_TIMER_RTBL_MSK;
+		return vpe_ctl & GIC_VX_CTL_TIMER_ROUTABLE;
 	case GIC_LOCAL_INT_PERFCTR:
-		return vpe_ctl & GIC_VPE_CTL_PERFCNT_RTBL_MSK;
+		return vpe_ctl & GIC_VX_CTL_PERFCNT_ROUTABLE;
 	case GIC_LOCAL_INT_FDC:
-		return vpe_ctl & GIC_VPE_CTL_FDC_RTBL_MSK;
+		return vpe_ctl & GIC_VX_CTL_FDC_ROUTABLE;
 	case GIC_LOCAL_INT_SWINT0:
 	case GIC_LOCAL_INT_SWINT1:
-		return vpe_ctl & GIC_VPE_CTL_SWINT_RTBL_MSK;
+		return vpe_ctl & GIC_VX_CTL_SWINT_ROUTABLE;
 	default:
 		return true;
 	}
@@ -272,15 +94,14 @@ static void gic_bind_eic_interrupt(int irq, int set)
 	irq -= GIC_PIN_TO_VEC_OFFSET;
 
 	/* Set irq to use shadow set */
-	gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_EIC_SHADOW_SET_BASE) +
-		  GIC_VPE_EIC_SS(irq), set);
+	write_gic_vl_eic_shadow_set(irq, set);
 }
 
 static void gic_send_ipi(struct irq_data *d, unsigned int cpu)
 {
 	irq_hw_number_t hwirq = GIC_HWIRQ_TO_SHARED(irqd_to_hwirq(d));
 
-	gic_write(GIC_REG(SHARED, GIC_SH_WEDGE), GIC_SH_WEDGE_SET(hwirq));
+	write_gic_wedge(GIC_WEDGE_RW | hwirq);
 }
 
 int gic_get_c0_compare_int(void)
@@ -316,47 +137,22 @@ int gic_get_c0_fdc_int(void)
 				  GIC_LOCAL_TO_HWIRQ(GIC_LOCAL_INT_FDC));
 }
 
-int gic_get_usm_range(struct resource *gic_usm_res)
-{
-	if (!gic_present)
-		return -1;
-
-	gic_usm_res->start = __gic_base_addr + USM_VISIBLE_SECTION_OFS;
-	gic_usm_res->end = gic_usm_res->start + (USM_VISIBLE_SECTION_SIZE - 1);
-
-	return 0;
-}
-
 static void gic_handle_shared_int(bool chained)
 {
-	unsigned int i, intr, virq, gic_reg_step = mips_cm_is64 ? 8 : 4;
+	unsigned int intr, virq;
 	unsigned long *pcpu_mask;
-	unsigned long pending_reg, intrmask_reg;
 	DECLARE_BITMAP(pending, GIC_MAX_INTRS);
-	DECLARE_BITMAP(intrmask, GIC_MAX_INTRS);
 
 	/* Get per-cpu bitmaps */
-	pcpu_mask = pcpu_masks[smp_processor_id()].pcpu_mask;
-
-	pending_reg = GIC_REG(SHARED, GIC_SH_PEND);
-	intrmask_reg = GIC_REG(SHARED, GIC_SH_MASK);
-
-	for (i = 0; i < BITS_TO_LONGS(gic_shared_intrs); i++) {
-		pending[i] = gic_read(pending_reg);
-		intrmask[i] = gic_read(intrmask_reg);
-		pending_reg += gic_reg_step;
-		intrmask_reg += gic_reg_step;
-
-		if (!IS_ENABLED(CONFIG_64BIT) || mips_cm_is64)
-			continue;
+	pcpu_mask = this_cpu_ptr(pcpu_masks);
 
-		pending[i] |= (u64)gic_read(pending_reg) << 32;
-		intrmask[i] |= (u64)gic_read(intrmask_reg) << 32;
-		pending_reg += gic_reg_step;
-		intrmask_reg += gic_reg_step;
-	}
+	if (mips_cm_is64)
+		__ioread64_copy(pending, addr_gic_pend(),
+				DIV_ROUND_UP(gic_shared_intrs, 64));
+	else
+		__ioread32_copy(pending, addr_gic_pend(),
+				DIV_ROUND_UP(gic_shared_intrs, 32));
 
-	bitmap_and(pending, pending, intrmask, gic_shared_intrs);
 	bitmap_and(pending, pending, pcpu_mask, gic_shared_intrs);
 
 	for_each_set_bit(intr, pending, gic_shared_intrs) {
@@ -371,19 +167,30 @@ static void gic_handle_shared_int(bool chained)
 
 static void gic_mask_irq(struct irq_data *d)
 {
-	gic_reset_mask(GIC_HWIRQ_TO_SHARED(d->hwirq));
+	unsigned int intr = GIC_HWIRQ_TO_SHARED(d->hwirq);
+
+	write_gic_rmask(BIT(intr));
+	gic_clear_pcpu_masks(intr);
 }
 
 static void gic_unmask_irq(struct irq_data *d)
 {
-	gic_set_mask(GIC_HWIRQ_TO_SHARED(d->hwirq));
+	struct cpumask *affinity = irq_data_get_affinity_mask(d);
+	unsigned int intr = GIC_HWIRQ_TO_SHARED(d->hwirq);
+	unsigned int cpu;
+
+	write_gic_smask(BIT(intr));
+
+	gic_clear_pcpu_masks(intr);
+	cpu = cpumask_first_and(affinity, cpu_online_mask);
+	set_bit(intr, per_cpu_ptr(pcpu_masks, cpu));
 }
 
 static void gic_ack_irq(struct irq_data *d)
 {
 	unsigned int irq = GIC_HWIRQ_TO_SHARED(d->hwirq);
 
-	gic_write(GIC_REG(SHARED, GIC_SH_WEDGE), GIC_SH_WEDGE_CLR(irq));
+	write_gic_wedge(irq);
 }
 
 static int gic_set_type(struct irq_data *d, unsigned int type)
@@ -395,34 +202,34 @@ static int gic_set_type(struct irq_data *d, unsigned int type)
 	spin_lock_irqsave(&gic_lock, flags);
 	switch (type & IRQ_TYPE_SENSE_MASK) {
 	case IRQ_TYPE_EDGE_FALLING:
-		gic_set_polarity(irq, GIC_POL_NEG);
-		gic_set_trigger(irq, GIC_TRIG_EDGE);
-		gic_set_dual_edge(irq, GIC_TRIG_DUAL_DISABLE);
+		change_gic_pol(irq, GIC_POL_FALLING_EDGE);
+		change_gic_trig(irq, GIC_TRIG_EDGE);
+		change_gic_dual(irq, GIC_DUAL_SINGLE);
 		is_edge = true;
 		break;
 	case IRQ_TYPE_EDGE_RISING:
-		gic_set_polarity(irq, GIC_POL_POS);
-		gic_set_trigger(irq, GIC_TRIG_EDGE);
-		gic_set_dual_edge(irq, GIC_TRIG_DUAL_DISABLE);
+		change_gic_pol(irq, GIC_POL_RISING_EDGE);
+		change_gic_trig(irq, GIC_TRIG_EDGE);
+		change_gic_dual(irq, GIC_DUAL_SINGLE);
 		is_edge = true;
 		break;
 	case IRQ_TYPE_EDGE_BOTH:
 		/* polarity is irrelevant in this case */
-		gic_set_trigger(irq, GIC_TRIG_EDGE);
-		gic_set_dual_edge(irq, GIC_TRIG_DUAL_ENABLE);
+		change_gic_trig(irq, GIC_TRIG_EDGE);
+		change_gic_dual(irq, GIC_DUAL_DUAL);
 		is_edge = true;
 		break;
 	case IRQ_TYPE_LEVEL_LOW:
-		gic_set_polarity(irq, GIC_POL_NEG);
-		gic_set_trigger(irq, GIC_TRIG_LEVEL);
-		gic_set_dual_edge(irq, GIC_TRIG_DUAL_DISABLE);
+		change_gic_pol(irq, GIC_POL_ACTIVE_LOW);
+		change_gic_trig(irq, GIC_TRIG_LEVEL);
+		change_gic_dual(irq, GIC_DUAL_SINGLE);
 		is_edge = false;
 		break;
 	case IRQ_TYPE_LEVEL_HIGH:
 	default:
-		gic_set_polarity(irq, GIC_POL_POS);
-		gic_set_trigger(irq, GIC_TRIG_LEVEL);
-		gic_set_dual_edge(irq, GIC_TRIG_DUAL_DISABLE);
+		change_gic_pol(irq, GIC_POL_ACTIVE_HIGH);
+		change_gic_trig(irq, GIC_TRIG_LEVEL);
+		change_gic_dual(irq, GIC_DUAL_SINGLE);
 		is_edge = false;
 		break;
 	}
@@ -443,32 +250,28 @@ static int gic_set_affinity(struct irq_data *d, const struct cpumask *cpumask,
 			    bool force)
 {
 	unsigned int irq = GIC_HWIRQ_TO_SHARED(d->hwirq);
-	cpumask_t	tmp = CPU_MASK_NONE;
-	unsigned long	flags;
-	int		i, cpu;
+	unsigned long flags;
+	unsigned int cpu;
 
-	cpumask_and(&tmp, cpumask, cpu_online_mask);
-	if (cpumask_empty(&tmp))
+	cpu = cpumask_first_and(cpumask, cpu_online_mask);
+	if (cpu >= NR_CPUS)
 		return -EINVAL;
 
-	cpu = cpumask_first(&tmp);
-
 	/* Assumption : cpumask refers to a single CPU */
 	spin_lock_irqsave(&gic_lock, flags);
 
 	/* Re-route this IRQ */
-	gic_map_to_vpe(irq, mips_cm_vp_id(cpu));
+	write_gic_map_vp(irq, BIT(mips_cm_vp_id(cpu)));
 
 	/* Update the pcpu_masks */
-	for (i = 0; i < min(gic_vpes, NR_CPUS); i++)
-		clear_bit(irq, pcpu_masks[i].pcpu_mask);
-	set_bit(irq, pcpu_masks[cpu].pcpu_mask);
+	gic_clear_pcpu_masks(irq);
+	if (read_gic_mask(irq))
+		set_bit(irq, per_cpu_ptr(pcpu_masks, cpu));
 
-	cpumask_copy(irq_data_get_affinity_mask(d), cpumask);
 	irq_data_update_effective_affinity(d, cpumask_of(cpu));
 	spin_unlock_irqrestore(&gic_lock, flags);
 
-	return IRQ_SET_MASK_OK_NOCOPY;
+	return IRQ_SET_MASK_OK;
 }
 #endif
 
@@ -499,8 +302,8 @@ static void gic_handle_local_int(bool chained)
 	unsigned long pending, masked;
 	unsigned int intr, virq;
 
-	pending = gic_read32(GIC_REG(VPE_LOCAL, GIC_VPE_PEND));
-	masked = gic_read32(GIC_REG(VPE_LOCAL, GIC_VPE_MASK));
+	pending = read_gic_vl_pend();
+	masked = read_gic_vl_mask();
 
 	bitmap_and(&pending, &pending, &masked, GIC_NUM_LOCAL_INTRS);
 
@@ -518,14 +321,14 @@ static void gic_mask_local_irq(struct irq_data *d)
 {
 	int intr = GIC_HWIRQ_TO_LOCAL(d->hwirq);
 
-	gic_write32(GIC_REG(VPE_LOCAL, GIC_VPE_RMASK), 1 << intr);
+	write_gic_vl_rmask(BIT(intr));
 }
 
 static void gic_unmask_local_irq(struct irq_data *d)
 {
 	int intr = GIC_HWIRQ_TO_LOCAL(d->hwirq);
 
-	gic_write32(GIC_REG(VPE_LOCAL, GIC_VPE_SMASK), 1 << intr);
+	write_gic_vl_smask(BIT(intr));
 }
 
 static struct irq_chip gic_local_irq_controller = {
@@ -542,9 +345,8 @@ static void gic_mask_local_irq_all_vpes(struct irq_data *d)
 
 	spin_lock_irqsave(&gic_lock, flags);
 	for (i = 0; i < gic_vpes; i++) {
-		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR),
-			  mips_cm_vp_id(i));
-		gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_RMASK), 1 << intr);
+		write_gic_vl_other(mips_cm_vp_id(i));
+		write_gic_vo_rmask(BIT(intr));
 	}
 	spin_unlock_irqrestore(&gic_lock, flags);
 }
@@ -557,9 +359,8 @@ static void gic_unmask_local_irq_all_vpes(struct irq_data *d)
 
 	spin_lock_irqsave(&gic_lock, flags);
 	for (i = 0; i < gic_vpes; i++) {
-		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR),
-			  mips_cm_vp_id(i));
-		gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_SMASK), 1 << intr);
+		write_gic_vl_other(mips_cm_vp_id(i));
+		write_gic_vo_smask(BIT(intr));
 	}
 	spin_unlock_irqrestore(&gic_lock, flags);
 }
@@ -582,103 +383,50 @@ static void gic_irq_dispatch(struct irq_desc *desc)
 	gic_handle_shared_int(true);
 }
 
-static void __init gic_basic_init(void)
-{
-	unsigned int i;
-
-	board_bind_eic_interrupt = &gic_bind_eic_interrupt;
-
-	/* Setup defaults */
-	for (i = 0; i < gic_shared_intrs; i++) {
-		gic_set_polarity(i, GIC_POL_POS);
-		gic_set_trigger(i, GIC_TRIG_LEVEL);
-		gic_reset_mask(i);
-	}
-
-	for (i = 0; i < gic_vpes; i++) {
-		unsigned int j;
-
-		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR),
-			  mips_cm_vp_id(i));
-		for (j = 0; j < GIC_NUM_LOCAL_INTRS; j++) {
-			if (!gic_local_irq_is_routable(j))
-				continue;
-			gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_RMASK), 1 << j);
-		}
-	}
-}
-
 static int gic_local_irq_domain_map(struct irq_domain *d, unsigned int virq,
 				    irq_hw_number_t hw)
 {
 	int intr = GIC_HWIRQ_TO_LOCAL(hw);
-	int ret = 0;
 	int i;
 	unsigned long flags;
+	u32 val;
 
 	if (!gic_local_irq_is_routable(intr))
 		return -EPERM;
 
+	if (intr > GIC_LOCAL_INT_FDC) {
+		pr_err("Invalid local IRQ %d\n", intr);
+		return -EINVAL;
+	}
+
+	if (intr == GIC_LOCAL_INT_TIMER) {
+		/* CONFIG_MIPS_CMP workaround (see __gic_init) */
+		val = GIC_MAP_PIN_MAP_TO_PIN | timer_cpu_pin;
+	} else {
+		val = GIC_MAP_PIN_MAP_TO_PIN | gic_cpu_pin;
+	}
+
 	spin_lock_irqsave(&gic_lock, flags);
 	for (i = 0; i < gic_vpes; i++) {
-		u32 val = GIC_MAP_TO_PIN_MSK | gic_cpu_pin;
-
-		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR),
-			  mips_cm_vp_id(i));
-
-		switch (intr) {
-		case GIC_LOCAL_INT_WD:
-			gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_WD_MAP), val);
-			break;
-		case GIC_LOCAL_INT_COMPARE:
-			gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_COMPARE_MAP),
-				    val);
-			break;
-		case GIC_LOCAL_INT_TIMER:
-			/* CONFIG_MIPS_CMP workaround (see __gic_init) */
-			val = GIC_MAP_TO_PIN_MSK | timer_cpu_pin;
-			gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_TIMER_MAP),
-				    val);
-			break;
-		case GIC_LOCAL_INT_PERFCTR:
-			gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_PERFCTR_MAP),
-				    val);
-			break;
-		case GIC_LOCAL_INT_SWINT0:
-			gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_SWINT0_MAP),
-				    val);
-			break;
-		case GIC_LOCAL_INT_SWINT1:
-			gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_SWINT1_MAP),
-				    val);
-			break;
-		case GIC_LOCAL_INT_FDC:
-			gic_write32(GIC_REG(VPE_OTHER, GIC_VPE_FDC_MAP), val);
-			break;
-		default:
-			pr_err("Invalid local IRQ %d\n", intr);
-			ret = -EINVAL;
-			break;
-		}
+		write_gic_vl_other(mips_cm_vp_id(i));
+		write_gic_vo_map(intr, val);
 	}
 	spin_unlock_irqrestore(&gic_lock, flags);
 
-	return ret;
+	return 0;
 }
 
 static int gic_shared_irq_domain_map(struct irq_domain *d, unsigned int virq,
-				     irq_hw_number_t hw, unsigned int vpe)
+				     irq_hw_number_t hw, unsigned int cpu)
 {
 	int intr = GIC_HWIRQ_TO_SHARED(hw);
 	unsigned long flags;
-	int i;
 
 	spin_lock_irqsave(&gic_lock, flags);
-	gic_map_to_pin(intr, gic_cpu_pin);
-	gic_map_to_vpe(intr, mips_cm_vp_id(vpe));
-	for (i = 0; i < min(gic_vpes, NR_CPUS); i++)
-		clear_bit(intr, pcpu_masks[i].pcpu_mask);
-	set_bit(intr, pcpu_masks[vpe].pcpu_mask);
+	write_gic_map_pin(intr, GIC_MAP_PIN_MAP_TO_PIN | gic_cpu_pin);
+	write_gic_map_vp(intr, BIT(mips_cm_vp_id(cpu)));
+	gic_clear_pcpu_masks(intr);
+	set_bit(intr, per_cpu_ptr(pcpu_masks, cpu));
 	spin_unlock_irqrestore(&gic_lock, flags);
 
 	return 0;
@@ -885,34 +633,69 @@ static const struct irq_domain_ops gic_ipi_domain_ops = {
 	.match = gic_ipi_domain_match,
 };
 
-static void __init __gic_init(unsigned long gic_base_addr,
-			      unsigned long gic_addrspace_size,
-			      unsigned int cpu_vec, unsigned int irqbase,
-			      struct device_node *node)
+
+static int __init gic_of_init(struct device_node *node,
+			      struct device_node *parent)
 {
-	unsigned int gicconfig, cpu;
-	unsigned int v[2];
+	unsigned int cpu_vec, i, j, gicconfig, cpu, v[2];
+	unsigned long reserved;
+	phys_addr_t gic_base;
+	struct resource res;
+	size_t gic_len;
+
+	/* Find the first available CPU vector. */
+	i = 0;
+	reserved = (C_SW0 | C_SW1) >> __fls(C_SW0);
+	while (!of_property_read_u32_index(node, "mti,reserved-cpu-vectors",
+					   i++, &cpu_vec))
+		reserved |= BIT(cpu_vec);
+
+	cpu_vec = find_first_zero_bit(&reserved, hweight_long(ST0_IM));
+	if (cpu_vec == hweight_long(ST0_IM)) {
+		pr_err("No CPU vectors available for GIC\n");
+		return -ENODEV;
+	}
+
+	if (of_address_to_resource(node, 0, &res)) {
+		/*
+		 * Probe the CM for the GIC base address if not specified
+		 * in the device-tree.
+		 */
+		if (mips_cm_present()) {
+			gic_base = read_gcr_gic_base() &
+				~CM_GCR_GIC_BASE_GICEN;
+			gic_len = 0x20000;
+		} else {
+			pr_err("Failed to get GIC memory range\n");
+			return -ENODEV;
+		}
+	} else {
+		gic_base = res.start;
+		gic_len = resource_size(&res);
+	}
 
-	__gic_base_addr = gic_base_addr;
+	if (mips_cm_present()) {
+		write_gcr_gic_base(gic_base | CM_GCR_GIC_BASE_GICEN);
+		/* Ensure GIC region is enabled before trying to access it */
+		__sync();
+	}
 
-	gic_base = ioremap_nocache(gic_base_addr, gic_addrspace_size);
+	mips_gic_base = ioremap_nocache(gic_base, gic_len);
 
-	gicconfig = gic_read(GIC_REG(SHARED, GIC_SH_CONFIG));
-	gic_shared_intrs = (gicconfig & GIC_SH_CONFIG_NUMINTRS_MSK) >>
-		   GIC_SH_CONFIG_NUMINTRS_SHF;
-	gic_shared_intrs = ((gic_shared_intrs + 1) * 8);
+	gicconfig = read_gic_config();
+	gic_shared_intrs = gicconfig & GIC_CONFIG_NUMINTERRUPTS;
+	gic_shared_intrs >>= __fls(GIC_CONFIG_NUMINTERRUPTS);
+	gic_shared_intrs = (gic_shared_intrs + 1) * 8;
 
-	gic_vpes = (gicconfig & GIC_SH_CONFIG_NUMVPES_MSK) >>
-		  GIC_SH_CONFIG_NUMVPES_SHF;
+	gic_vpes = gicconfig & GIC_CONFIG_PVPS;
+	gic_vpes >>= __fls(GIC_CONFIG_PVPS);
 	gic_vpes = gic_vpes + 1;
 
 	if (cpu_has_veic) {
 		/* Set EIC mode for all VPEs */
 		for_each_present_cpu(cpu) {
-			gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR),
-				  mips_cm_vp_id(cpu));
-			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_CTL),
-				  GIC_VPE_CTL_EIC_MODE_MSK);
+			write_gic_vl_other(mips_cm_vp_id(cpu));
+			write_gic_vo_ctl(GIC_VX_CTL_EIC);
 		}
 
 		/* Always use vector 1 in EIC mode */
@@ -937,9 +720,7 @@ static void __init __gic_init(unsigned long gic_base_addr,
 		 */
 		if (IS_ENABLED(CONFIG_MIPS_CMP) &&
 		    gic_local_irq_is_routable(GIC_LOCAL_INT_TIMER)) {
-			timer_cpu_pin = gic_read32(GIC_REG(VPE_LOCAL,
-							 GIC_VPE_TIMER_MAP)) &
-					GIC_MAP_MSK;
+			timer_cpu_pin = read_gic_vl_timer_map() & GIC_MAP_PIN_MAP;
 			irq_set_chained_handler(MIPS_CPU_IRQ_BASE +
 						GIC_CPU_PIN_OFFSET +
 						timer_cpu_pin,
@@ -950,17 +731,21 @@ static void __init __gic_init(unsigned long gic_base_addr,
 	}
 
 	gic_irq_domain = irq_domain_add_simple(node, GIC_NUM_LOCAL_INTRS +
-					       gic_shared_intrs, irqbase,
+					       gic_shared_intrs, 0,
 					       &gic_irq_domain_ops, NULL);
-	if (!gic_irq_domain)
-		panic("Failed to add GIC IRQ domain");
+	if (!gic_irq_domain) {
+		pr_err("Failed to add GIC IRQ domain");
+		return -ENXIO;
+	}
 
 	gic_ipi_domain = irq_domain_add_hierarchy(gic_irq_domain,
 						  IRQ_DOMAIN_FLAG_IPI_PER_CPU,
 						  GIC_NUM_LOCAL_INTRS + gic_shared_intrs,
 						  node, &gic_ipi_domain_ops, NULL);
-	if (!gic_ipi_domain)
-		panic("Failed to add GIC IPI domain");
+	if (!gic_ipi_domain) {
+		pr_err("Failed to add GIC IPI domain");
+		return -ENXIO;
+	}
 
 	irq_domain_update_bus_token(gic_ipi_domain, DOMAIN_BUS_IPI);
 
@@ -975,64 +760,25 @@ static void __init __gic_init(unsigned long gic_base_addr,
 	}
 
 	bitmap_copy(ipi_available, ipi_resrv, GIC_MAX_INTRS);
-	gic_basic_init();
-}
-
-void __init gic_init(unsigned long gic_base_addr,
-		     unsigned long gic_addrspace_size,
-		     unsigned int cpu_vec, unsigned int irqbase)
-{
-	__gic_init(gic_base_addr, gic_addrspace_size, cpu_vec, irqbase, NULL);
-}
 
-static int __init gic_of_init(struct device_node *node,
-			      struct device_node *parent)
-{
-	struct resource res;
-	unsigned int cpu_vec, i = 0, reserved = 0;
-	phys_addr_t gic_base;
-	size_t gic_len;
+	board_bind_eic_interrupt = &gic_bind_eic_interrupt;
 
-	/* Find the first available CPU vector. */
-	while (!of_property_read_u32_index(node, "mti,reserved-cpu-vectors",
-					   i++, &cpu_vec))
-		reserved |= BIT(cpu_vec);
-	for (cpu_vec = 2; cpu_vec < 8; cpu_vec++) {
-		if (!(reserved & BIT(cpu_vec)))
-			break;
-	}
-	if (cpu_vec == 8) {
-		pr_err("No CPU vectors available for GIC\n");
-		return -ENODEV;
+	/* Setup defaults */
+	for (i = 0; i < gic_shared_intrs; i++) {
+		change_gic_pol(i, GIC_POL_ACTIVE_HIGH);
+		change_gic_trig(i, GIC_TRIG_LEVEL);
+		write_gic_rmask(BIT(i));
 	}
 
-	if (of_address_to_resource(node, 0, &res)) {
-		/*
-		 * Probe the CM for the GIC base address if not specified
-		 * in the device-tree.
-		 */
-		if (mips_cm_present()) {
-			gic_base = read_gcr_gic_base() &
-				~CM_GCR_GIC_BASE_GICEN_MSK;
-			gic_len = 0x20000;
-		} else {
-			pr_err("Failed to get GIC memory range\n");
-			return -ENODEV;
+	for (i = 0; i < gic_vpes; i++) {
+		write_gic_vl_other(mips_cm_vp_id(i));
+		for (j = 0; j < GIC_NUM_LOCAL_INTRS; j++) {
+			if (!gic_local_irq_is_routable(j))
+				continue;
+			write_gic_vo_rmask(BIT(j));
 		}
-	} else {
-		gic_base = res.start;
-		gic_len = resource_size(&res);
 	}
 
-	if (mips_cm_present()) {
-		write_gcr_gic_base(gic_base | CM_GCR_GIC_BASE_GICEN_MSK);
-		/* Ensure GIC region is enabled before trying to access it */
-		__sync();
-	}
-	gic_present = true;
-
-	__gic_init(gic_base, gic_len, cpu_vec, 0, node);
-
 	return 0;
 }
 IRQCHIP_DECLARE(mips_gic, "mti,gic", gic_of_init);
diff --git a/drivers/mtd/maps/lantiq-flash.c b/drivers/mtd/maps/lantiq-flash.c
index 3e33ab66eb24..77b1d8013295 100644
--- a/drivers/mtd/maps/lantiq-flash.c
+++ b/drivers/mtd/maps/lantiq-flash.c
@@ -114,12 +114,6 @@ ltq_mtd_probe(struct platform_device *pdev)
 	struct cfi_private *cfi;
 	int err;
 
-	if (of_machine_is_compatible("lantiq,falcon") &&
-			(ltq_boot_select() != BS_FLASH)) {
-		dev_err(&pdev->dev, "invalid bootstrap options\n");
-		return -ENODEV;
-	}
-
 	ltq_mtd = devm_kzalloc(&pdev->dev, sizeof(struct ltq_mtd), GFP_KERNEL);
 	if (!ltq_mtd)
 		return -ENOMEM;
diff --git a/drivers/pcmcia/db1xxx_ss.c b/drivers/pcmcia/db1xxx_ss.c
index 944674ee3464..19e17829f515 100644
--- a/drivers/pcmcia/db1xxx_ss.c
+++ b/drivers/pcmcia/db1xxx_ss.c
@@ -131,22 +131,27 @@ static irqreturn_t db1000_pcmcia_stschgirq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+/* Db/Pb1200 have separate per-socket insertion and ejection
+ * interrupts which stay asserted as long as the card is
+ * inserted/missing.  The one which caused us to be called
+ * needs to be disabled and the other one enabled.
+ */
 static irqreturn_t db1200_pcmcia_cdirq(int irq, void *data)
 {
+	disable_irq_nosync(irq);
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t db1200_pcmcia_cdirq_fn(int irq, void *data)
+{
 	struct db1x_pcmcia_sock *sock = data;
 
-	/* Db/Pb1200 have separate per-socket insertion and ejection
-	 * interrupts which stay asserted as long as the card is
-	 * inserted/missing.  The one which caused us to be called
-	 * needs to be disabled and the other one enabled.
-	 */
-	if (irq == sock->insert_irq) {
-		disable_irq_nosync(sock->insert_irq);
+	/* Wait a bit for the signals to stop bouncing. */
+	msleep(100);
+	if (irq == sock->insert_irq)
 		enable_irq(sock->eject_irq);
-	} else {
-		disable_irq_nosync(sock->eject_irq);
+	else
 		enable_irq(sock->insert_irq);
-	}
 
 	pcmcia_parse_events(&sock->socket, SS_DETECT);
 
@@ -172,13 +177,13 @@ static int db1x_pcmcia_setup_irqs(struct db1x_pcmcia_sock *sock)
 	 */
 	if ((sock->board_type == BOARD_TYPE_DB1200) ||
 	    (sock->board_type == BOARD_TYPE_DB1300)) {
-		ret = request_irq(sock->insert_irq, db1200_pcmcia_cdirq,
-				  0, "pcmcia_insert", sock);
+		ret = request_threaded_irq(sock->insert_irq, db1200_pcmcia_cdirq,
+			db1200_pcmcia_cdirq_fn, 0, "pcmcia_insert", sock);
 		if (ret)
 			goto out1;
 
-		ret = request_irq(sock->eject_irq, db1200_pcmcia_cdirq,
-				  0, "pcmcia_eject", sock);
+		ret = request_threaded_irq(sock->eject_irq, db1200_pcmcia_cdirq,
+			db1200_pcmcia_cdirq_fn, 0, "pcmcia_eject", sock);
 		if (ret) {
 			free_irq(sock->insert_irq, sock);
 			goto out1;
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index 441912c10b82..5c8d452e35e2 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -44,6 +44,7 @@ source "drivers/phy/allwinner/Kconfig"
 source "drivers/phy/amlogic/Kconfig"
 source "drivers/phy/broadcom/Kconfig"
 source "drivers/phy/hisilicon/Kconfig"
+source "drivers/phy/lantiq/Kconfig"
 source "drivers/phy/marvell/Kconfig"
 source "drivers/phy/mediatek/Kconfig"
 source "drivers/phy/motorola/Kconfig"
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index 06f3c500030d..3a52dcb09566 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -6,9 +6,9 @@ obj-$(CONFIG_GENERIC_PHY)		+= phy-core.o
 obj-$(CONFIG_PHY_LPC18XX_USB_OTG)	+= phy-lpc18xx-usb-otg.o
 obj-$(CONFIG_PHY_XGENE)			+= phy-xgene.o
 obj-$(CONFIG_PHY_PISTACHIO_USB)		+= phy-pistachio-usb.o
-
 obj-$(CONFIG_ARCH_SUNXI)		+= allwinner/
 obj-$(CONFIG_ARCH_MESON)		+= amlogic/
+obj-$(CONFIG_LANTIQ)			+= lantiq/
 obj-$(CONFIG_ARCH_MEDIATEK)		+= mediatek/
 obj-$(CONFIG_ARCH_RENESAS)		+= renesas/
 obj-$(CONFIG_ARCH_ROCKCHIP)		+= rockchip/
diff --git a/drivers/phy/lantiq/Kconfig b/drivers/phy/lantiq/Kconfig
new file mode 100644
index 000000000000..326d88a6417d
--- /dev/null
+++ b/drivers/phy/lantiq/Kconfig
@@ -0,0 +1,9 @@
+#
+# Phy drivers for Lantiq / Intel platforms
+#
+config PHY_LANTIQ_RCU_USB2
+	tristate "Lantiq XWAY SoC RCU based USB PHY"
+	depends on OF && (SOC_TYPE_XWAY || COMPILE_TEST)
+	select GENERIC_PHY
+	help
+	  Support for the USB PHY(s) on the Lantiq / Intel XWAY family SoCs.
diff --git a/drivers/phy/lantiq/Makefile b/drivers/phy/lantiq/Makefile
new file mode 100644
index 000000000000..f73eb56a5416
--- /dev/null
+++ b/drivers/phy/lantiq/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_PHY_LANTIQ_RCU_USB2)	+= phy-lantiq-rcu-usb2.o
diff --git a/drivers/phy/lantiq/phy-lantiq-rcu-usb2.c b/drivers/phy/lantiq/phy-lantiq-rcu-usb2.c
new file mode 100644
index 000000000000..986224fca9e9
--- /dev/null
+++ b/drivers/phy/lantiq/phy-lantiq-rcu-usb2.c
@@ -0,0 +1,254 @@
+/*
+ * Lantiq XWAY SoC RCU module based USB 1.1/2.0 PHY driver
+ *
+ * Copyright (C) 2016 Martin Blumenstingl <martin.blumenstingl@googlemail.com>
+ * Copyright (C) 2017 Hauke Mehrtens <hauke@hauke-m.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+#include <linux/reset.h>
+
+/* Transmitter HS Pre-Emphasis Enable */
+#define RCU_CFG1_TX_PEE		BIT(0)
+/* Disconnect Threshold */
+#define RCU_CFG1_DIS_THR_MASK	0x00038000
+#define RCU_CFG1_DIS_THR_SHIFT	15
+
+struct ltq_rcu_usb2_bits {
+	u8 hostmode;
+	u8 slave_endianness;
+	u8 host_endianness;
+	bool have_ana_cfg;
+};
+
+struct ltq_rcu_usb2_priv {
+	struct regmap			*regmap;
+	unsigned int			phy_reg_offset;
+	unsigned int			ana_cfg1_reg_offset;
+	const struct ltq_rcu_usb2_bits	*reg_bits;
+	struct device			*dev;
+	struct phy			*phy;
+	struct clk			*phy_gate_clk;
+	struct reset_control		*ctrl_reset;
+	struct reset_control		*phy_reset;
+};
+
+static const struct ltq_rcu_usb2_bits xway_rcu_usb2_reg_bits = {
+	.hostmode = 11,
+	.slave_endianness = 9,
+	.host_endianness = 10,
+	.have_ana_cfg = false,
+};
+
+static const struct ltq_rcu_usb2_bits xrx100_rcu_usb2_reg_bits = {
+	.hostmode = 11,
+	.slave_endianness = 17,
+	.host_endianness = 10,
+	.have_ana_cfg = false,
+};
+
+static const struct ltq_rcu_usb2_bits xrx200_rcu_usb2_reg_bits = {
+	.hostmode = 11,
+	.slave_endianness = 9,
+	.host_endianness = 10,
+	.have_ana_cfg = true,
+};
+
+static const struct of_device_id ltq_rcu_usb2_phy_of_match[] = {
+	{
+		.compatible = "lantiq,ase-usb2-phy",
+		.data = &xway_rcu_usb2_reg_bits,
+	},
+	{
+		.compatible = "lantiq,danube-usb2-phy",
+		.data = &xway_rcu_usb2_reg_bits,
+	},
+	{
+		.compatible = "lantiq,xrx100-usb2-phy",
+		.data = &xrx100_rcu_usb2_reg_bits,
+	},
+	{
+		.compatible = "lantiq,xrx200-usb2-phy",
+		.data = &xrx200_rcu_usb2_reg_bits,
+	},
+	{
+		.compatible = "lantiq,xrx300-usb2-phy",
+		.data = &xrx200_rcu_usb2_reg_bits,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, ltq_rcu_usb2_phy_of_match);
+
+static int ltq_rcu_usb2_phy_init(struct phy *phy)
+{
+	struct ltq_rcu_usb2_priv *priv = phy_get_drvdata(phy);
+
+	if (priv->reg_bits->have_ana_cfg) {
+		regmap_update_bits(priv->regmap, priv->ana_cfg1_reg_offset,
+			RCU_CFG1_TX_PEE, RCU_CFG1_TX_PEE);
+		regmap_update_bits(priv->regmap, priv->ana_cfg1_reg_offset,
+			RCU_CFG1_DIS_THR_MASK, 7 << RCU_CFG1_DIS_THR_SHIFT);
+	}
+
+	/* Configure core to host mode */
+	regmap_update_bits(priv->regmap, priv->phy_reg_offset,
+			   BIT(priv->reg_bits->hostmode), 0);
+
+	/* Select DMA endianness (Host-endian: big-endian) */
+	regmap_update_bits(priv->regmap, priv->phy_reg_offset,
+		BIT(priv->reg_bits->slave_endianness), 0);
+	regmap_update_bits(priv->regmap, priv->phy_reg_offset,
+		BIT(priv->reg_bits->host_endianness),
+		BIT(priv->reg_bits->host_endianness));
+
+	return 0;
+}
+
+static int ltq_rcu_usb2_phy_power_on(struct phy *phy)
+{
+	struct ltq_rcu_usb2_priv *priv = phy_get_drvdata(phy);
+	struct device *dev = priv->dev;
+	int ret;
+
+	reset_control_deassert(priv->phy_reset);
+
+	ret = clk_prepare_enable(priv->phy_gate_clk);
+	if (ret)
+		dev_err(dev, "failed to enable PHY gate\n");
+
+	return ret;
+}
+
+static int ltq_rcu_usb2_phy_power_off(struct phy *phy)
+{
+	struct ltq_rcu_usb2_priv *priv = phy_get_drvdata(phy);
+
+	reset_control_assert(priv->phy_reset);
+
+	clk_disable_unprepare(priv->phy_gate_clk);
+
+	return 0;
+}
+
+static struct phy_ops ltq_rcu_usb2_phy_ops = {
+	.init		= ltq_rcu_usb2_phy_init,
+	.power_on	= ltq_rcu_usb2_phy_power_on,
+	.power_off	= ltq_rcu_usb2_phy_power_off,
+	.owner		= THIS_MODULE,
+};
+
+static int ltq_rcu_usb2_of_parse(struct ltq_rcu_usb2_priv *priv,
+				 struct platform_device *pdev)
+{
+	struct device *dev = priv->dev;
+	const __be32 *offset;
+	int ret;
+
+	priv->reg_bits = of_device_get_match_data(dev);
+
+	priv->regmap = syscon_node_to_regmap(dev->of_node->parent);
+	if (IS_ERR(priv->regmap)) {
+		dev_err(dev, "Failed to lookup RCU regmap\n");
+		return PTR_ERR(priv->regmap);
+	}
+
+	offset = of_get_address(dev->of_node, 0, NULL, NULL);
+	if (!offset) {
+		dev_err(dev, "Failed to get RCU PHY reg offset\n");
+		return -ENOENT;
+	}
+	priv->phy_reg_offset = __be32_to_cpu(*offset);
+
+	if (priv->reg_bits->have_ana_cfg) {
+		offset = of_get_address(dev->of_node, 1, NULL, NULL);
+		if (!offset) {
+			dev_err(dev, "Failed to get RCU ANA CFG1 reg offset\n");
+			return -ENOENT;
+		}
+		priv->ana_cfg1_reg_offset = __be32_to_cpu(*offset);
+	}
+
+	priv->phy_gate_clk = devm_clk_get(dev, "phy");
+	if (IS_ERR(priv->phy_gate_clk)) {
+		dev_err(dev, "Unable to get USB phy gate clk\n");
+		return PTR_ERR(priv->phy_gate_clk);
+	}
+
+	priv->ctrl_reset = devm_reset_control_get_shared(dev, "ctrl");
+	if (IS_ERR(priv->ctrl_reset)) {
+		if (PTR_ERR(priv->ctrl_reset) != -EPROBE_DEFER)
+			dev_err(dev, "failed to get 'ctrl' reset\n");
+		return PTR_ERR(priv->ctrl_reset);
+	}
+
+	priv->phy_reset = devm_reset_control_get_optional(dev, "phy");
+	if (IS_ERR(priv->phy_reset))
+		return PTR_ERR(priv->phy_reset);
+
+	return 0;
+}
+
+static int ltq_rcu_usb2_phy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct ltq_rcu_usb2_priv *priv;
+	struct phy_provider *provider;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->dev = dev;
+
+	ret = ltq_rcu_usb2_of_parse(priv, pdev);
+	if (ret)
+		return ret;
+
+	/* Reset USB core through reset controller */
+	reset_control_deassert(priv->ctrl_reset);
+
+	reset_control_assert(priv->phy_reset);
+
+	priv->phy = devm_phy_create(dev, dev->of_node, &ltq_rcu_usb2_phy_ops);
+	if (IS_ERR(priv->phy)) {
+		dev_err(dev, "failed to create PHY\n");
+		return PTR_ERR(priv->phy);
+	}
+
+	phy_set_drvdata(priv->phy, priv);
+
+	provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	if (IS_ERR(provider))
+		return PTR_ERR(provider);
+
+	dev_set_drvdata(priv->dev, priv);
+	return 0;
+}
+
+static struct platform_driver ltq_rcu_usb2_phy_driver = {
+	.probe	= ltq_rcu_usb2_phy_probe,
+	.driver = {
+		.name	= "lantiq-rcu-usb2-phy",
+		.of_match_table	= ltq_rcu_usb2_phy_of_match,
+	}
+};
+module_platform_driver(ltq_rcu_usb2_phy_driver);
+
+MODULE_AUTHOR("Martin Blumenstingl <martin.blumenstingl@googlemail.com>");
+MODULE_DESCRIPTION("Lantiq XWAY USB2 PHY driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/reset/Kconfig b/drivers/reset/Kconfig
index 52d5251660b9..e0c393214264 100644
--- a/drivers/reset/Kconfig
+++ b/drivers/reset/Kconfig
@@ -47,6 +47,12 @@ config RESET_IMX7
 	help
 	  This enables the reset controller driver for i.MX7 SoCs.
 
+config RESET_LANTIQ
+	bool "Lantiq XWAY Reset Driver" if COMPILE_TEST
+	default SOC_TYPE_XWAY
+	help
+	  This enables the reset controller driver for Lantiq / Intel XWAY SoCs.
+
 config RESET_LPC18XX
 	bool "LPC18xx/43xx Reset Driver" if COMPILE_TEST
 	default ARCH_LPC18XX
diff --git a/drivers/reset/Makefile b/drivers/reset/Makefile
index b62783f50fe5..d368367110e5 100644
--- a/drivers/reset/Makefile
+++ b/drivers/reset/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_RESET_ATH79) += reset-ath79.o
 obj-$(CONFIG_RESET_BERLIN) += reset-berlin.o
 obj-$(CONFIG_RESET_HSDK_V1) += reset-hsdk-v1.o
 obj-$(CONFIG_RESET_IMX7) += reset-imx7.o
+obj-$(CONFIG_RESET_LANTIQ) += reset-lantiq.o
 obj-$(CONFIG_RESET_LPC18XX) += reset-lpc18xx.o
 obj-$(CONFIG_RESET_MESON) += reset-meson.o
 obj-$(CONFIG_RESET_OXNAS) += reset-oxnas.o
diff --git a/drivers/reset/reset-lantiq.c b/drivers/reset/reset-lantiq.c
new file mode 100644
index 000000000000..11a582e50d30
--- /dev/null
+++ b/drivers/reset/reset-lantiq.c
@@ -0,0 +1,212 @@
+/*
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under the terms of the GNU General Public License version 2 as published
+ *  by the Free Software Foundation.
+ *
+ *  Copyright (C) 2010 John Crispin <blogic@phrozen.org>
+ *  Copyright (C) 2013-2015 Lantiq Beteiligungs-GmbH & Co.KG
+ *  Copyright (C) 2016 Martin Blumenstingl <martin.blumenstingl@googlemail.com>
+ *  Copyright (C) 2017 Hauke Mehrtens <hauke@hauke-m.de>
+ */
+
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/reset-controller.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+
+#define LANTIQ_RCU_RESET_TIMEOUT	10000
+
+struct lantiq_rcu_reset_priv {
+	struct reset_controller_dev rcdev;
+	struct device *dev;
+	struct regmap *regmap;
+	u32 reset_offset;
+	u32 status_offset;
+};
+
+static struct lantiq_rcu_reset_priv *to_lantiq_rcu_reset_priv(
+	struct reset_controller_dev *rcdev)
+{
+	return container_of(rcdev, struct lantiq_rcu_reset_priv, rcdev);
+}
+
+static int lantiq_rcu_reset_status(struct reset_controller_dev *rcdev,
+				   unsigned long id)
+{
+	struct lantiq_rcu_reset_priv *priv = to_lantiq_rcu_reset_priv(rcdev);
+	unsigned int status = (id >> 8) & 0x1f;
+	u32 val;
+	int ret;
+
+	ret = regmap_read(priv->regmap, priv->status_offset, &val);
+	if (ret)
+		return ret;
+
+	return !!(val & BIT(status));
+}
+
+static int lantiq_rcu_reset_status_timeout(struct reset_controller_dev *rcdev,
+					   unsigned long id, bool assert)
+{
+	int ret;
+	int retry = LANTIQ_RCU_RESET_TIMEOUT;
+
+	do {
+		ret = lantiq_rcu_reset_status(rcdev, id);
+		if (ret < 0)
+			return ret;
+		if (ret == assert)
+			return 0;
+		usleep_range(20, 40);
+	} while (--retry);
+
+	return -ETIMEDOUT;
+}
+
+static int lantiq_rcu_reset_update(struct reset_controller_dev *rcdev,
+				   unsigned long id, bool assert)
+{
+	struct lantiq_rcu_reset_priv *priv = to_lantiq_rcu_reset_priv(rcdev);
+	unsigned int set = id & 0x1f;
+	u32 val = assert ? BIT(set) : 0;
+	int ret;
+
+	ret = regmap_update_bits(priv->regmap, priv->reset_offset, BIT(set),
+				 val);
+	if (ret) {
+		dev_err(priv->dev, "Failed to set reset bit %u\n", set);
+		return ret;
+	}
+
+
+	ret = lantiq_rcu_reset_status_timeout(rcdev, id, assert);
+	if (ret)
+		dev_err(priv->dev, "Failed to %s bit %u\n",
+			assert ? "assert" : "deassert", set);
+
+	return ret;
+}
+
+static int lantiq_rcu_reset_assert(struct reset_controller_dev *rcdev,
+			     unsigned long id)
+{
+	return lantiq_rcu_reset_update(rcdev, id, true);
+}
+
+static int lantiq_rcu_reset_deassert(struct reset_controller_dev *rcdev,
+			       unsigned long id)
+{
+	return lantiq_rcu_reset_update(rcdev, id, false);
+}
+
+static int lantiq_rcu_reset_reset(struct reset_controller_dev *rcdev,
+			    unsigned long id)
+{
+	int ret;
+
+	ret = lantiq_rcu_reset_assert(rcdev, id);
+	if (ret)
+		return ret;
+
+	return lantiq_rcu_reset_deassert(rcdev, id);
+}
+
+static const struct reset_control_ops lantiq_rcu_reset_ops = {
+	.assert = lantiq_rcu_reset_assert,
+	.deassert = lantiq_rcu_reset_deassert,
+	.status = lantiq_rcu_reset_status,
+	.reset	= lantiq_rcu_reset_reset,
+};
+
+static int lantiq_rcu_reset_of_parse(struct platform_device *pdev,
+			       struct lantiq_rcu_reset_priv *priv)
+{
+	struct device *dev = &pdev->dev;
+	const __be32 *offset;
+
+	priv->regmap = syscon_node_to_regmap(dev->of_node->parent);
+	if (IS_ERR(priv->regmap)) {
+		dev_err(&pdev->dev, "Failed to lookup RCU regmap\n");
+		return PTR_ERR(priv->regmap);
+	}
+
+	offset = of_get_address(dev->of_node, 0, NULL, NULL);
+	if (!offset) {
+		dev_err(&pdev->dev, "Failed to get RCU reset offset\n");
+		return -ENOENT;
+	}
+	priv->reset_offset = __be32_to_cpu(*offset);
+
+	offset = of_get_address(dev->of_node, 1, NULL, NULL);
+	if (!offset) {
+		dev_err(&pdev->dev, "Failed to get RCU status offset\n");
+		return -ENOENT;
+	}
+	priv->status_offset = __be32_to_cpu(*offset);
+
+	return 0;
+}
+
+static int lantiq_rcu_reset_xlate(struct reset_controller_dev *rcdev,
+				  const struct of_phandle_args *reset_spec)
+{
+	unsigned int status, set;
+
+	set = reset_spec->args[0];
+	status = reset_spec->args[1];
+
+	if (set >= rcdev->nr_resets || status >= rcdev->nr_resets)
+		return -EINVAL;
+
+	return (status << 8) | set;
+}
+
+static int lantiq_rcu_reset_probe(struct platform_device *pdev)
+{
+	struct lantiq_rcu_reset_priv *priv;
+	int err;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->dev = &pdev->dev;
+	platform_set_drvdata(pdev, priv);
+
+	err = lantiq_rcu_reset_of_parse(pdev, priv);
+	if (err)
+		return err;
+
+	priv->rcdev.ops = &lantiq_rcu_reset_ops;
+	priv->rcdev.owner = THIS_MODULE;
+	priv->rcdev.of_node = pdev->dev.of_node;
+	priv->rcdev.nr_resets = 32;
+	priv->rcdev.of_xlate = lantiq_rcu_reset_xlate;
+	priv->rcdev.of_reset_n_cells = 2;
+
+	return reset_controller_register(&priv->rcdev);
+}
+
+static const struct of_device_id lantiq_rcu_reset_dt_ids[] = {
+	{ .compatible = "lantiq,danube-reset", },
+	{ .compatible = "lantiq,xrx200-reset", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, lantiq_rcu_reset_dt_ids);
+
+static struct platform_driver lantiq_rcu_reset_driver = {
+	.probe	= lantiq_rcu_reset_probe,
+	.driver = {
+		.name		= "lantiq-reset",
+		.of_match_table	= lantiq_rcu_reset_dt_ids,
+	},
+};
+module_platform_driver(lantiq_rcu_reset_driver);
+
+MODULE_AUTHOR("Martin Blumenstingl <martin.blumenstingl@googlemail.com>");
+MODULE_DESCRIPTION("Lantiq XWAY RCU Reset Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/soc/Makefile b/drivers/soc/Makefile
index 280a6a91a9e2..2fcaff864584 100644
--- a/drivers/soc/Makefile
+++ b/drivers/soc/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_ARCH_DOVE)		+= dove/
 obj-$(CONFIG_MACH_DOVE)		+= dove/
 obj-y				+= fsl/
 obj-$(CONFIG_ARCH_MXC)		+= imx/
+obj-$(CONFIG_SOC_XWAY)		+= lantiq/
 obj-$(CONFIG_ARCH_MEDIATEK)	+= mediatek/
 obj-$(CONFIG_ARCH_MESON)	+= amlogic/
 obj-$(CONFIG_ARCH_QCOM)		+= qcom/
diff --git a/drivers/soc/lantiq/Makefile b/drivers/soc/lantiq/Makefile
new file mode 100644
index 000000000000..be9e866d53e5
--- /dev/null
+++ b/drivers/soc/lantiq/Makefile
@@ -0,0 +1,2 @@
+obj-y				+= fpi-bus.o
+obj-$(CONFIG_XRX200_PHY_FW)	+= gphy.o
diff --git a/drivers/soc/lantiq/fpi-bus.c b/drivers/soc/lantiq/fpi-bus.c
new file mode 100644
index 000000000000..a671c9984c4c
--- /dev/null
+++ b/drivers/soc/lantiq/fpi-bus.c
@@ -0,0 +1,87 @@
+/*
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under the terms of the GNU General Public License version 2 as published
+ *  by the Free Software Foundation.
+ *
+ *  Copyright (C) 2011-2015 John Crispin <blogic@phrozen.org>
+ *  Copyright (C) 2015 Martin Blumenstingl <martin.blumenstingl@googlemail.com>
+ *  Copyright (C) 2017 Hauke Mehrtens <hauke@hauke-m.de>
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+
+#include <lantiq_soc.h>
+
+#define XBAR_ALWAYS_LAST	0x430
+#define XBAR_FPI_BURST_EN	BIT(1)
+#define XBAR_AHB_BURST_EN	BIT(2)
+
+#define RCU_VR9_BE_AHB1S	0x00000008
+
+static int ltq_fpi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct resource *res_xbar;
+	struct regmap *rcu_regmap;
+	void __iomem *xbar_membase;
+	u32 rcu_ahb_endianness_reg_offset;
+	int ret;
+
+	res_xbar = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	xbar_membase = devm_ioremap_resource(dev, res_xbar);
+	if (IS_ERR(xbar_membase))
+		return PTR_ERR(xbar_membase);
+
+	/* RCU configuration is optional */
+	rcu_regmap = syscon_regmap_lookup_by_phandle(np, "lantiq,rcu");
+	if (IS_ERR(rcu_regmap))
+		return PTR_ERR(rcu_regmap);
+
+	ret = device_property_read_u32(dev, "lantiq,offset-endianness",
+				       &rcu_ahb_endianness_reg_offset);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to get RCU reg offset\n");
+		return ret;
+	}
+
+	ret = regmap_update_bits(rcu_regmap, rcu_ahb_endianness_reg_offset,
+				 RCU_VR9_BE_AHB1S, RCU_VR9_BE_AHB1S);
+	if (ret) {
+		dev_warn(&pdev->dev,
+			 "Failed to configure RCU AHB endianness\n");
+		return ret;
+	}
+
+	/* disable fpi burst */
+	ltq_w32_mask(XBAR_FPI_BURST_EN, 0, xbar_membase + XBAR_ALWAYS_LAST);
+
+	return of_platform_populate(dev->of_node, NULL, NULL, dev);
+}
+
+static const struct of_device_id ltq_fpi_match[] = {
+	{ .compatible = "lantiq,xrx200-fpi" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ltq_fpi_match);
+
+static struct platform_driver ltq_fpi_driver = {
+	.probe = ltq_fpi_probe,
+	.driver = {
+		.name = "fpi-xway",
+		.of_match_table = ltq_fpi_match,
+	},
+};
+
+module_platform_driver(ltq_fpi_driver);
+
+MODULE_DESCRIPTION("Lantiq FPI bus driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/soc/lantiq/gphy.c b/drivers/soc/lantiq/gphy.c
new file mode 100644
index 000000000000..8d8659463b3e
--- /dev/null
+++ b/drivers/soc/lantiq/gphy.c
@@ -0,0 +1,260 @@
+/*
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under the terms of the GNU General Public License version 2 as published
+ *  by the Free Software Foundation.
+ *
+ *  Copyright (C) 2012 John Crispin <blogic@phrozen.org>
+ *  Copyright (C) 2016 Martin Blumenstingl <martin.blumenstingl@googlemail.com>
+ *  Copyright (C) 2017 Hauke Mehrtens <hauke@hauke-m.de>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/firmware.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+#include <linux/reset.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/property.h>
+#include <dt-bindings/mips/lantiq_rcu_gphy.h>
+
+#include <lantiq_soc.h>
+
+#define XRX200_GPHY_FW_ALIGN	(16 * 1024)
+
+struct xway_gphy_priv {
+	struct clk *gphy_clk_gate;
+	struct reset_control *gphy_reset;
+	struct reset_control *gphy_reset2;
+	struct notifier_block gphy_reboot_nb;
+	void __iomem *membase;
+	char *fw_name;
+};
+
+struct xway_gphy_match_data {
+	char *fe_firmware_name;
+	char *ge_firmware_name;
+};
+
+static const struct xway_gphy_match_data xrx200a1x_gphy_data = {
+	.fe_firmware_name = "lantiq/xrx200_phy22f_a14.bin",
+	.ge_firmware_name = "lantiq/xrx200_phy11g_a14.bin",
+};
+
+static const struct xway_gphy_match_data xrx200a2x_gphy_data = {
+	.fe_firmware_name = "lantiq/xrx200_phy22f_a22.bin",
+	.ge_firmware_name = "lantiq/xrx200_phy11g_a22.bin",
+};
+
+static const struct xway_gphy_match_data xrx300_gphy_data = {
+	.fe_firmware_name = "lantiq/xrx300_phy22f_a21.bin",
+	.ge_firmware_name = "lantiq/xrx300_phy11g_a21.bin",
+};
+
+static const struct of_device_id xway_gphy_match[] = {
+	{ .compatible = "lantiq,xrx200a1x-gphy", .data = &xrx200a1x_gphy_data },
+	{ .compatible = "lantiq,xrx200a2x-gphy", .data = &xrx200a2x_gphy_data },
+	{ .compatible = "lantiq,xrx300-gphy", .data = &xrx300_gphy_data },
+	{ .compatible = "lantiq,xrx330-gphy", .data = &xrx300_gphy_data },
+	{},
+};
+MODULE_DEVICE_TABLE(of, xway_gphy_match);
+
+static struct xway_gphy_priv *to_xway_gphy_priv(struct notifier_block *nb)
+{
+	return container_of(nb, struct xway_gphy_priv, gphy_reboot_nb);
+}
+
+static int xway_gphy_reboot_notify(struct notifier_block *reboot_nb,
+				   unsigned long code, void *unused)
+{
+	struct xway_gphy_priv *priv = to_xway_gphy_priv(reboot_nb);
+
+	if (priv) {
+		reset_control_assert(priv->gphy_reset);
+		reset_control_assert(priv->gphy_reset2);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int xway_gphy_load(struct device *dev, struct xway_gphy_priv *priv,
+			  dma_addr_t *dev_addr)
+{
+	const struct firmware *fw;
+	void *fw_addr;
+	dma_addr_t dma_addr;
+	size_t size;
+	int ret;
+
+	ret = request_firmware(&fw, priv->fw_name, dev);
+	if (ret) {
+		dev_err(dev, "failed to load firmware: %s, error: %i\n",
+			priv->fw_name, ret);
+		return ret;
+	}
+
+	/*
+	 * GPHY cores need the firmware code in a persistent and contiguous
+	 * memory area with a 16 kB boundary aligned start address.
+	 */
+	size = fw->size + XRX200_GPHY_FW_ALIGN;
+
+	fw_addr = dmam_alloc_coherent(dev, size, &dma_addr, GFP_KERNEL);
+	if (fw_addr) {
+		fw_addr = PTR_ALIGN(fw_addr, XRX200_GPHY_FW_ALIGN);
+		*dev_addr = ALIGN(dma_addr, XRX200_GPHY_FW_ALIGN);
+		memcpy(fw_addr, fw->data, fw->size);
+	} else {
+		dev_err(dev, "failed to alloc firmware memory\n");
+		ret = -ENOMEM;
+	}
+
+	release_firmware(fw);
+
+	return ret;
+}
+
+static int xway_gphy_of_probe(struct platform_device *pdev,
+			      struct xway_gphy_priv *priv)
+{
+	struct device *dev = &pdev->dev;
+	const struct xway_gphy_match_data *gphy_fw_name_cfg;
+	u32 gphy_mode;
+	int ret;
+	struct resource *res_gphy;
+
+	gphy_fw_name_cfg = of_device_get_match_data(dev);
+
+	priv->gphy_clk_gate = devm_clk_get(dev, NULL);
+	if (IS_ERR(priv->gphy_clk_gate)) {
+		dev_err(dev, "Failed to lookup gate clock\n");
+		return PTR_ERR(priv->gphy_clk_gate);
+	}
+
+	res_gphy = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->membase = devm_ioremap_resource(dev, res_gphy);
+	if (IS_ERR(priv->membase))
+		return PTR_ERR(priv->membase);
+
+	priv->gphy_reset = devm_reset_control_get(dev, "gphy");
+	if (IS_ERR(priv->gphy_reset)) {
+		if (PTR_ERR(priv->gphy_reset) != -EPROBE_DEFER)
+			dev_err(dev, "Failed to lookup gphy reset\n");
+		return PTR_ERR(priv->gphy_reset);
+	}
+
+	priv->gphy_reset2 = devm_reset_control_get_optional(dev, "gphy2");
+	if (IS_ERR(priv->gphy_reset2))
+		return PTR_ERR(priv->gphy_reset2);
+
+	ret = device_property_read_u32(dev, "lantiq,gphy-mode", &gphy_mode);
+	/* Default to GE mode */
+	if (ret)
+		gphy_mode = GPHY_MODE_GE;
+
+	switch (gphy_mode) {
+	case GPHY_MODE_FE:
+		priv->fw_name = gphy_fw_name_cfg->fe_firmware_name;
+		break;
+	case GPHY_MODE_GE:
+		priv->fw_name = gphy_fw_name_cfg->ge_firmware_name;
+		break;
+	default:
+		dev_err(dev, "Unknown GPHY mode %d\n", gphy_mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int xway_gphy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct xway_gphy_priv *priv;
+	dma_addr_t fw_addr = 0;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	ret = xway_gphy_of_probe(pdev, priv);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(priv->gphy_clk_gate);
+	if (ret)
+		return ret;
+
+	ret = xway_gphy_load(dev, priv, &fw_addr);
+	if (ret) {
+		clk_disable_unprepare(priv->gphy_clk_gate);
+		return ret;
+	}
+
+	reset_control_assert(priv->gphy_reset);
+	reset_control_assert(priv->gphy_reset2);
+
+	iowrite32be(fw_addr, priv->membase);
+
+	reset_control_deassert(priv->gphy_reset);
+	reset_control_deassert(priv->gphy_reset2);
+
+	/* assert the gphy reset because it can hang after a reboot: */
+	priv->gphy_reboot_nb.notifier_call = xway_gphy_reboot_notify;
+	priv->gphy_reboot_nb.priority = -1;
+
+	ret = register_reboot_notifier(&priv->gphy_reboot_nb);
+	if (ret)
+		dev_warn(dev, "Failed to register reboot notifier\n");
+
+	platform_set_drvdata(pdev, priv);
+
+	return ret;
+}
+
+static int xway_gphy_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct xway_gphy_priv *priv = platform_get_drvdata(pdev);
+	int ret;
+
+	reset_control_assert(priv->gphy_reset);
+	reset_control_assert(priv->gphy_reset2);
+
+	iowrite32be(0, priv->membase);
+
+	clk_disable_unprepare(priv->gphy_clk_gate);
+
+	ret = unregister_reboot_notifier(&priv->gphy_reboot_nb);
+	if (ret)
+		dev_warn(dev, "Failed to unregister reboot notifier\n");
+
+	return 0;
+}
+
+static struct platform_driver xway_gphy_driver = {
+	.probe = xway_gphy_probe,
+	.remove = xway_gphy_remove,
+	.driver = {
+		.name = "xway-rcu-gphy",
+		.of_match_table = xway_gphy_match,
+	},
+};
+
+module_platform_driver(xway_gphy_driver);
+
+MODULE_FIRMWARE("lantiq/xrx300_phy11g_a21.bin");
+MODULE_FIRMWARE("lantiq/xrx300_phy22f_a21.bin");
+MODULE_FIRMWARE("lantiq/xrx200_phy11g_a14.bin");
+MODULE_FIRMWARE("lantiq/xrx200_phy11g_a22.bin");
+MODULE_FIRMWARE("lantiq/xrx200_phy22f_a14.bin");
+MODULE_FIRMWARE("lantiq/xrx200_phy22f_a22.bin");
+MODULE_AUTHOR("Martin Blumenstingl <martin.blumenstingl@googlemail.com>");
+MODULE_DESCRIPTION("Lantiq XWAY GPHY Firmware Loader");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/lantiq_wdt.c b/drivers/watchdog/lantiq_wdt.c
index e0823677d8c1..7f43cefa0eae 100644
--- a/drivers/watchdog/lantiq_wdt.c
+++ b/drivers/watchdog/lantiq_wdt.c
@@ -4,6 +4,7 @@
  *  by the Free Software Foundation.
  *
  *  Copyright (C) 2010 John Crispin <john@phrozen.org>
+ *  Copyright (C) 2017 Hauke Mehrtens <hauke@hauke-m.de>
  *  Based on EP93xx wdt driver
  */
 
@@ -17,9 +18,20 @@
 #include <linux/uaccess.h>
 #include <linux/clk.h>
 #include <linux/io.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
 
 #include <lantiq_soc.h>
 
+#define LTQ_XRX_RCU_RST_STAT		0x0014
+#define LTQ_XRX_RCU_RST_STAT_WDT	BIT(31)
+
+/* CPU0 Reset Source Register */
+#define LTQ_FALCON_SYS1_CPU0RS		0x0060
+/* reset cause mask */
+#define LTQ_FALCON_SYS1_CPU0RS_MASK	0x0007
+#define LTQ_FALCON_SYS1_CPU0RS_WDT	0x02
+
 /*
  * Section 3.4 of the datasheet
  * The password sequence protects the WDT control register from unintended
@@ -186,16 +198,70 @@ static struct miscdevice ltq_wdt_miscdev = {
 	.fops	= &ltq_wdt_fops,
 };
 
+typedef int (*ltq_wdt_bootstatus_set)(struct platform_device *pdev);
+
+static int ltq_wdt_bootstatus_xrx(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct regmap *rcu_regmap;
+	u32 val;
+	int err;
+
+	rcu_regmap = syscon_regmap_lookup_by_phandle(dev->of_node, "regmap");
+	if (IS_ERR(rcu_regmap))
+		return PTR_ERR(rcu_regmap);
+
+	err = regmap_read(rcu_regmap, LTQ_XRX_RCU_RST_STAT, &val);
+	if (err)
+		return err;
+
+	if (val & LTQ_XRX_RCU_RST_STAT_WDT)
+		ltq_wdt_bootstatus = WDIOF_CARDRESET;
+
+	return 0;
+}
+
+static int ltq_wdt_bootstatus_falcon(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct regmap *rcu_regmap;
+	u32 val;
+	int err;
+
+	rcu_regmap = syscon_regmap_lookup_by_phandle(dev->of_node,
+						     "lantiq,rcu");
+	if (IS_ERR(rcu_regmap))
+		return PTR_ERR(rcu_regmap);
+
+	err = regmap_read(rcu_regmap, LTQ_FALCON_SYS1_CPU0RS, &val);
+	if (err)
+		return err;
+
+	if ((val & LTQ_FALCON_SYS1_CPU0RS_MASK) == LTQ_FALCON_SYS1_CPU0RS_WDT)
+		ltq_wdt_bootstatus = WDIOF_CARDRESET;
+
+	return 0;
+}
+
 static int
 ltq_wdt_probe(struct platform_device *pdev)
 {
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	struct clk *clk;
+	ltq_wdt_bootstatus_set ltq_wdt_bootstatus_set;
+	int ret;
 
 	ltq_wdt_membase = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(ltq_wdt_membase))
 		return PTR_ERR(ltq_wdt_membase);
 
+	ltq_wdt_bootstatus_set = of_device_get_match_data(&pdev->dev);
+	if (ltq_wdt_bootstatus_set) {
+		ret = ltq_wdt_bootstatus_set(pdev);
+		if (ret)
+			return ret;
+	}
+
 	/* we do not need to enable the clock as it is always running */
 	clk = clk_get_io();
 	if (IS_ERR(clk)) {
@@ -205,10 +271,6 @@ ltq_wdt_probe(struct platform_device *pdev)
 	ltq_io_region_clk_rate = clk_get_rate(clk);
 	clk_put(clk);
 
-	/* find out if the watchdog caused the last reboot */
-	if (ltq_reset_cause() == LTQ_RST_CAUSE_WDTRST)
-		ltq_wdt_bootstatus = WDIOF_CARDRESET;
-
 	dev_info(&pdev->dev, "Init done\n");
 	return misc_register(&ltq_wdt_miscdev);
 }
@@ -222,7 +284,9 @@ ltq_wdt_remove(struct platform_device *pdev)
 }
 
 static const struct of_device_id ltq_wdt_match[] = {
-	{ .compatible = "lantiq,wdt" },
+	{ .compatible = "lantiq,wdt", .data = NULL},
+	{ .compatible = "lantiq,xrx100-wdt", .data = ltq_wdt_bootstatus_xrx },
+	{ .compatible = "lantiq,falcon-wdt", .data = ltq_wdt_bootstatus_falcon },
 	{},
 };
 MODULE_DEVICE_TABLE(of, ltq_wdt_match);
diff --git a/drivers/watchdog/octeon-wdt-main.c b/drivers/watchdog/octeon-wdt-main.c
index b5cdceb36cff..0ec419a3f7ed 100644
--- a/drivers/watchdog/octeon-wdt-main.c
+++ b/drivers/watchdog/octeon-wdt-main.c
@@ -1,7 +1,7 @@
 /*
  * Octeon Watchdog driver
  *
- * Copyright (C) 2007, 2008, 2009, 2010 Cavium Networks
+ * Copyright (C) 2007-2017 Cavium, Inc.
  *
  * Converted to use WATCHDOG_CORE by Aaro Koskinen <aaro.koskinen@iki.fi>.
  *
@@ -59,20 +59,23 @@
 #include <linux/interrupt.h>
 #include <linux/watchdog.h>
 #include <linux/cpumask.h>
-#include <linux/bitops.h>
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/string.h>
 #include <linux/delay.h>
 #include <linux/cpu.h>
-#include <linux/smp.h>
-#include <linux/fs.h>
 #include <linux/irq.h>
 
 #include <asm/mipsregs.h>
 #include <asm/uasm.h>
 
 #include <asm/octeon/octeon.h>
+#include <asm/octeon/cvmx-boot-vector.h>
+#include <asm/octeon/cvmx-ciu2-defs.h>
+#include <asm/octeon/cvmx-rst-defs.h>
+
+/* Watchdog interrupt major block number (8 MSBs of intsn) */
+#define WD_BLOCK_NUMBER		0x01
+
+static int divisor;
 
 /* The count needed to achieve timeout_sec. */
 static unsigned int timeout_cnt;
@@ -84,7 +87,7 @@ static unsigned int max_timeout_sec;
 static unsigned int timeout_sec;
 
 /* Set to non-zero when userspace countdown mode active */
-static int do_coundown;
+static bool do_countdown;
 static unsigned int countdown_reset;
 static unsigned int per_cpu_countdown[NR_CPUS];
 
@@ -92,152 +95,38 @@ static cpumask_t irq_enabled_cpus;
 
 #define WD_TIMO 60			/* Default heartbeat = 60 seconds */
 
+#define CVMX_GSERX_SCRATCH(offset) (CVMX_ADD_IO_SEG(0x0001180090000020ull) + ((offset) & 15) * 0x1000000ull)
+
 static int heartbeat = WD_TIMO;
-module_param(heartbeat, int, S_IRUGO);
+module_param(heartbeat, int, 0444);
 MODULE_PARM_DESC(heartbeat,
 	"Watchdog heartbeat in seconds. (0 < heartbeat, default="
 				__MODULE_STRING(WD_TIMO) ")");
 
 static bool nowayout = WATCHDOG_NOWAYOUT;
-module_param(nowayout, bool, S_IRUGO);
+module_param(nowayout, bool, 0444);
 MODULE_PARM_DESC(nowayout,
 	"Watchdog cannot be stopped once started (default="
 				__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
 
-static u32 nmi_stage1_insns[64] __initdata;
-/* We need one branch and therefore one relocation per target label. */
-static struct uasm_label labels[5] __initdata;
-static struct uasm_reloc relocs[5] __initdata;
-
-enum lable_id {
-	label_enter_bootloader = 1
-};
+static int disable;
+module_param(disable, int, 0444);
+MODULE_PARM_DESC(disable,
+	"Disable the watchdog entirely (default=0)");
 
-/* Some CP0 registers */
-#define K0		26
-#define C0_CVMMEMCTL 11, 7
-#define C0_STATUS 12, 0
-#define C0_EBASE 15, 1
-#define C0_DESAVE 31, 0
+static struct cvmx_boot_vector_element *octeon_wdt_bootvector;
 
 void octeon_wdt_nmi_stage2(void);
 
-static void __init octeon_wdt_build_stage1(void)
-{
-	int i;
-	int len;
-	u32 *p = nmi_stage1_insns;
-#ifdef CONFIG_HOTPLUG_CPU
-	struct uasm_label *l = labels;
-	struct uasm_reloc *r = relocs;
-#endif
-
-	/*
-	 * For the next few instructions running the debugger may
-	 * cause corruption of k0 in the saved registers. Since we're
-	 * about to crash, nobody probably cares.
-	 *
-	 * Save K0 into the debug scratch register
-	 */
-	uasm_i_dmtc0(&p, K0, C0_DESAVE);
-
-	uasm_i_mfc0(&p, K0, C0_STATUS);
-#ifdef CONFIG_HOTPLUG_CPU
-	if (octeon_bootloader_entry_addr)
-		uasm_il_bbit0(&p, &r, K0, ilog2(ST0_NMI),
-			      label_enter_bootloader);
-#endif
-	/* Force 64-bit addressing enabled */
-	uasm_i_ori(&p, K0, K0, ST0_UX | ST0_SX | ST0_KX);
-	uasm_i_mtc0(&p, K0, C0_STATUS);
-
-#ifdef CONFIG_HOTPLUG_CPU
-	if (octeon_bootloader_entry_addr) {
-		uasm_i_mfc0(&p, K0, C0_EBASE);
-		/* Coreid number in K0 */
-		uasm_i_andi(&p, K0, K0, 0xf);
-		/* 8 * coreid in bits 16-31 */
-		uasm_i_dsll_safe(&p, K0, K0, 3 + 16);
-		uasm_i_ori(&p, K0, K0, 0x8001);
-		uasm_i_dsll_safe(&p, K0, K0, 16);
-		uasm_i_ori(&p, K0, K0, 0x0700);
-		uasm_i_drotr_safe(&p, K0, K0, 32);
-		/*
-		 * Should result in: 0x8001,0700,0000,8*coreid which is
-		 * CVMX_CIU_WDOGX(coreid) - 0x0500
-		 *
-		 * Now ld K0, CVMX_CIU_WDOGX(coreid)
-		 */
-		uasm_i_ld(&p, K0, 0x500, K0);
-		/*
-		 * If bit one set handle the NMI as a watchdog event.
-		 * otherwise transfer control to bootloader.
-		 */
-		uasm_il_bbit0(&p, &r, K0, 1, label_enter_bootloader);
-		uasm_i_nop(&p);
-	}
-#endif
-
-	/* Clear Dcache so cvmseg works right. */
-	uasm_i_cache(&p, 1, 0, 0);
-
-	/* Use K0 to do a read/modify/write of CVMMEMCTL */
-	uasm_i_dmfc0(&p, K0, C0_CVMMEMCTL);
-	/* Clear out the size of CVMSEG	*/
-	uasm_i_dins(&p, K0, 0, 0, 6);
-	/* Set CVMSEG to its largest value */
-	uasm_i_ori(&p, K0, K0, 0x1c0 | 54);
-	/* Store the CVMMEMCTL value */
-	uasm_i_dmtc0(&p, K0, C0_CVMMEMCTL);
-
-	/* Load the address of the second stage handler */
-	UASM_i_LA(&p, K0, (long)octeon_wdt_nmi_stage2);
-	uasm_i_jr(&p, K0);
-	uasm_i_dmfc0(&p, K0, C0_DESAVE);
-
-#ifdef CONFIG_HOTPLUG_CPU
-	if (octeon_bootloader_entry_addr) {
-		uasm_build_label(&l, p, label_enter_bootloader);
-		/* Jump to the bootloader and restore K0 */
-		UASM_i_LA(&p, K0, (long)octeon_bootloader_entry_addr);
-		uasm_i_jr(&p, K0);
-		uasm_i_dmfc0(&p, K0, C0_DESAVE);
-	}
-#endif
-	uasm_resolve_relocs(relocs, labels);
-
-	len = (int)(p - nmi_stage1_insns);
-	pr_debug("Synthesized NMI stage 1 handler (%d instructions)\n", len);
-
-	pr_debug("\t.set push\n");
-	pr_debug("\t.set noreorder\n");
-	for (i = 0; i < len; i++)
-		pr_debug("\t.word 0x%08x\n", nmi_stage1_insns[i]);
-	pr_debug("\t.set pop\n");
-
-	if (len > 32)
-		panic("NMI stage 1 handler exceeds 32 instructions, was %d\n",
-		      len);
-}
-
 static int cpu2core(int cpu)
 {
 #ifdef CONFIG_SMP
-	return cpu_logical_map(cpu);
+	return cpu_logical_map(cpu) & 0x3f;
 #else
 	return cvmx_get_core_num();
 #endif
 }
 
-static int core2cpu(int coreid)
-{
-#ifdef CONFIG_SMP
-	return cpu_number_map(coreid);
-#else
-	return 0;
-#endif
-}
-
 /**
  * Poke the watchdog when an interrupt is received
  *
@@ -248,13 +137,14 @@ static int core2cpu(int coreid)
  */
 static irqreturn_t octeon_wdt_poke_irq(int cpl, void *dev_id)
 {
-	unsigned int core = cvmx_get_core_num();
-	int cpu = core2cpu(core);
+	int cpu = raw_smp_processor_id();
+	unsigned int core = cpu2core(cpu);
+	int node = cpu_to_node(cpu);
 
-	if (do_coundown) {
+	if (do_countdown) {
 		if (per_cpu_countdown[cpu] > 0) {
 			/* We're alive, poke the watchdog */
-			cvmx_write_csr(CVMX_CIU_PP_POKEX(core), 1);
+			cvmx_write_csr_node(node, CVMX_CIU_PP_POKEX(core), 1);
 			per_cpu_countdown[cpu]--;
 		} else {
 			/* Bad news, you are about to reboot. */
@@ -263,7 +153,7 @@ static irqreturn_t octeon_wdt_poke_irq(int cpl, void *dev_id)
 		}
 	} else {
 		/* Not open, just ping away... */
-		cvmx_write_csr(CVMX_CIU_PP_POKEX(core), 1);
+		cvmx_write_csr_node(node, CVMX_CIU_PP_POKEX(core), 1);
 	}
 	return IRQ_HANDLED;
 }
@@ -338,10 +228,10 @@ void octeon_wdt_nmi_stage3(u64 reg[32])
 	u64 cp0_epc = read_c0_epc();
 
 	/* Delay so output from all cores output is not jumbled together. */
-	__delay(100000000ull * coreid);
+	udelay(85000 * coreid);
 
 	octeon_wdt_write_string("\r\n*** NMI Watchdog interrupt on Core 0x");
-	octeon_wdt_write_hex(coreid, 1);
+	octeon_wdt_write_hex(coreid, 2);
 	octeon_wdt_write_string(" ***\r\n");
 	for (i = 0; i < 32; i++) {
 		octeon_wdt_write_string("\t");
@@ -364,33 +254,98 @@ void octeon_wdt_nmi_stage3(u64 reg[32])
 	octeon_wdt_write_hex(cp0_cause, 16);
 	octeon_wdt_write_string("\r\n");
 
-	octeon_wdt_write_string("\tsum0\t0x");
-	octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU_INTX_SUM0(coreid * 2)), 16);
-	octeon_wdt_write_string("\ten0\t0x");
-	octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU_INTX_EN0(coreid * 2)), 16);
-	octeon_wdt_write_string("\r\n");
+	/* The CIU register is different for each Octeon model. */
+	if (OCTEON_IS_MODEL(OCTEON_CN68XX)) {
+		octeon_wdt_write_string("\tsrc_wd\t0x");
+		octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU2_SRC_PPX_IP2_WDOG(coreid)), 16);
+		octeon_wdt_write_string("\ten_wd\t0x");
+		octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU2_EN_PPX_IP2_WDOG(coreid)), 16);
+		octeon_wdt_write_string("\r\n");
+		octeon_wdt_write_string("\tsrc_rml\t0x");
+		octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU2_SRC_PPX_IP2_RML(coreid)), 16);
+		octeon_wdt_write_string("\ten_rml\t0x");
+		octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU2_EN_PPX_IP2_RML(coreid)), 16);
+		octeon_wdt_write_string("\r\n");
+		octeon_wdt_write_string("\tsum\t0x");
+		octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU2_SUM_PPX_IP2(coreid)), 16);
+		octeon_wdt_write_string("\r\n");
+	} else if (!octeon_has_feature(OCTEON_FEATURE_CIU3)) {
+		octeon_wdt_write_string("\tsum0\t0x");
+		octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU_INTX_SUM0(coreid * 2)), 16);
+		octeon_wdt_write_string("\ten0\t0x");
+		octeon_wdt_write_hex(cvmx_read_csr(CVMX_CIU_INTX_EN0(coreid * 2)), 16);
+		octeon_wdt_write_string("\r\n");
+	}
 
 	octeon_wdt_write_string("*** Chip soft reset soon ***\r\n");
+
+	/*
+	 * G-30204: We must trigger a soft reset before watchdog
+	 * does an incomplete job of doing it.
+	 */
+	if (OCTEON_IS_OCTEON3() && !OCTEON_IS_MODEL(OCTEON_CN70XX)) {
+		u64 scr;
+		unsigned int node = cvmx_get_node_num();
+		unsigned int lcore = cvmx_get_local_core_num();
+		union cvmx_ciu_wdogx ciu_wdog;
+
+		/*
+		 * Wait for other cores to print out information, but
+		 * not too long.  Do the soft reset before watchdog
+		 * can trigger it.
+		 */
+		do {
+			ciu_wdog.u64 = cvmx_read_csr_node(node, CVMX_CIU_WDOGX(lcore));
+		} while (ciu_wdog.s.cnt > 0x10000);
+
+		scr = cvmx_read_csr_node(0, CVMX_GSERX_SCRATCH(0));
+		scr |= 1 << 11; /* Indicate watchdog in bit 11 */
+		cvmx_write_csr_node(0, CVMX_GSERX_SCRATCH(0), scr);
+		cvmx_write_csr_node(0, CVMX_RST_SOFT_RST, 1);
+	}
+}
+
+static int octeon_wdt_cpu_to_irq(int cpu)
+{
+	unsigned int coreid;
+	int node;
+	int irq;
+
+	coreid = cpu2core(cpu);
+	node = cpu_to_node(cpu);
+
+	if (octeon_has_feature(OCTEON_FEATURE_CIU3)) {
+		struct irq_domain *domain;
+		int hwirq;
+
+		domain = octeon_irq_get_block_domain(node,
+						     WD_BLOCK_NUMBER);
+		hwirq = WD_BLOCK_NUMBER << 12 | 0x200 | coreid;
+		irq = irq_find_mapping(domain, hwirq);
+	} else {
+		irq = OCTEON_IRQ_WDOG0 + coreid;
+	}
+	return irq;
 }
 
 static int octeon_wdt_cpu_pre_down(unsigned int cpu)
 {
 	unsigned int core;
-	unsigned int irq;
+	int node;
 	union cvmx_ciu_wdogx ciu_wdog;
 
 	core = cpu2core(cpu);
 
-	irq = OCTEON_IRQ_WDOG0 + core;
+	node = cpu_to_node(cpu);
 
 	/* Poke the watchdog to clear out its state */
-	cvmx_write_csr(CVMX_CIU_PP_POKEX(core), 1);
+	cvmx_write_csr_node(node, CVMX_CIU_PP_POKEX(core), 1);
 
 	/* Disable the hardware. */
 	ciu_wdog.u64 = 0;
-	cvmx_write_csr(CVMX_CIU_WDOGX(core), ciu_wdog.u64);
+	cvmx_write_csr_node(node, CVMX_CIU_WDOGX(core), ciu_wdog.u64);
 
-	free_irq(irq, octeon_wdt_poke_irq);
+	free_irq(octeon_wdt_cpu_to_irq(cpu), octeon_wdt_poke_irq);
 	return 0;
 }
 
@@ -399,31 +354,56 @@ static int octeon_wdt_cpu_online(unsigned int cpu)
 	unsigned int core;
 	unsigned int irq;
 	union cvmx_ciu_wdogx ciu_wdog;
+	int node;
+	struct irq_domain *domain;
+	int hwirq;
 
 	core = cpu2core(cpu);
+	node = cpu_to_node(cpu);
+
+	octeon_wdt_bootvector[core].target_ptr = (u64)octeon_wdt_nmi_stage2;
 
 	/* Disable it before doing anything with the interrupts. */
 	ciu_wdog.u64 = 0;
-	cvmx_write_csr(CVMX_CIU_WDOGX(core), ciu_wdog.u64);
+	cvmx_write_csr_node(node, CVMX_CIU_WDOGX(core), ciu_wdog.u64);
 
 	per_cpu_countdown[cpu] = countdown_reset;
 
-	irq = OCTEON_IRQ_WDOG0 + core;
+	if (octeon_has_feature(OCTEON_FEATURE_CIU3)) {
+		/* Must get the domain for the watchdog block */
+		domain = octeon_irq_get_block_domain(node, WD_BLOCK_NUMBER);
+
+		/* Get a irq for the wd intsn (hardware interrupt) */
+		hwirq = WD_BLOCK_NUMBER << 12 | 0x200 | core;
+		irq = irq_create_mapping(domain, hwirq);
+		irqd_set_trigger_type(irq_get_irq_data(irq),
+				      IRQ_TYPE_EDGE_RISING);
+	} else
+		irq = OCTEON_IRQ_WDOG0 + core;
 
 	if (request_irq(irq, octeon_wdt_poke_irq,
 			IRQF_NO_THREAD, "octeon_wdt", octeon_wdt_poke_irq))
 		panic("octeon_wdt: Couldn't obtain irq %d", irq);
 
+	/* Must set the irq affinity here */
+	if (octeon_has_feature(OCTEON_FEATURE_CIU3)) {
+		cpumask_t mask;
+
+		cpumask_clear(&mask);
+		cpumask_set_cpu(cpu, &mask);
+		irq_set_affinity(irq, &mask);
+	}
+
 	cpumask_set_cpu(cpu, &irq_enabled_cpus);
 
 	/* Poke the watchdog to clear out its state */
-	cvmx_write_csr(CVMX_CIU_PP_POKEX(core), 1);
+	cvmx_write_csr_node(node, CVMX_CIU_PP_POKEX(core), 1);
 
 	/* Finally enable the watchdog now that all handlers are installed */
 	ciu_wdog.u64 = 0;
 	ciu_wdog.s.len = timeout_cnt;
 	ciu_wdog.s.mode = 3;	/* 3 = Interrupt + NMI + Soft-Reset */
-	cvmx_write_csr(CVMX_CIU_WDOGX(core), ciu_wdog.u64);
+	cvmx_write_csr_node(node, CVMX_CIU_WDOGX(core), ciu_wdog.u64);
 
 	return 0;
 }
@@ -432,17 +412,20 @@ static int octeon_wdt_ping(struct watchdog_device __always_unused *wdog)
 {
 	int cpu;
 	int coreid;
+	int node;
+
+	if (disable)
+		return 0;
 
 	for_each_online_cpu(cpu) {
 		coreid = cpu2core(cpu);
-		cvmx_write_csr(CVMX_CIU_PP_POKEX(coreid), 1);
+		node = cpu_to_node(cpu);
+		cvmx_write_csr_node(node, CVMX_CIU_PP_POKEX(coreid), 1);
 		per_cpu_countdown[cpu] = countdown_reset;
-		if ((countdown_reset || !do_coundown) &&
+		if ((countdown_reset || !do_countdown) &&
 		    !cpumask_test_cpu(cpu, &irq_enabled_cpus)) {
 			/* We have to enable the irq */
-			int irq = OCTEON_IRQ_WDOG0 + coreid;
-
-			enable_irq(irq);
+			enable_irq(octeon_wdt_cpu_to_irq(cpu));
 			cpumask_set_cpu(cpu, &irq_enabled_cpus);
 		}
 	}
@@ -472,7 +455,7 @@ static void octeon_wdt_calc_parameters(int t)
 
 	countdown_reset = periods > 2 ? periods - 2 : 0;
 	heartbeat = t;
-	timeout_cnt = ((octeon_get_io_clock_rate() >> 8) * timeout_sec) >> 8;
+	timeout_cnt = ((octeon_get_io_clock_rate() / divisor) * timeout_sec) >> 8;
 }
 
 static int octeon_wdt_set_timeout(struct watchdog_device *wdog,
@@ -481,20 +464,25 @@ static int octeon_wdt_set_timeout(struct watchdog_device *wdog,
 	int cpu;
 	int coreid;
 	union cvmx_ciu_wdogx ciu_wdog;
+	int node;
 
 	if (t <= 0)
 		return -1;
 
 	octeon_wdt_calc_parameters(t);
 
+	if (disable)
+		return 0;
+
 	for_each_online_cpu(cpu) {
 		coreid = cpu2core(cpu);
-		cvmx_write_csr(CVMX_CIU_PP_POKEX(coreid), 1);
+		node = cpu_to_node(cpu);
+		cvmx_write_csr_node(node, CVMX_CIU_PP_POKEX(coreid), 1);
 		ciu_wdog.u64 = 0;
 		ciu_wdog.s.len = timeout_cnt;
 		ciu_wdog.s.mode = 3;	/* 3 = Interrupt + NMI + Soft-Reset */
-		cvmx_write_csr(CVMX_CIU_WDOGX(coreid), ciu_wdog.u64);
-		cvmx_write_csr(CVMX_CIU_PP_POKEX(coreid), 1);
+		cvmx_write_csr_node(node, CVMX_CIU_WDOGX(coreid), ciu_wdog.u64);
+		cvmx_write_csr_node(node, CVMX_CIU_PP_POKEX(coreid), 1);
 	}
 	octeon_wdt_ping(wdog); /* Get the irqs back on. */
 	return 0;
@@ -503,13 +491,13 @@ static int octeon_wdt_set_timeout(struct watchdog_device *wdog,
 static int octeon_wdt_start(struct watchdog_device *wdog)
 {
 	octeon_wdt_ping(wdog);
-	do_coundown = 1;
+	do_countdown = 1;
 	return 0;
 }
 
 static int octeon_wdt_stop(struct watchdog_device *wdog)
 {
-	do_coundown = 0;
+	do_countdown = 0;
 	octeon_wdt_ping(wdog);
 	return 0;
 }
@@ -540,14 +528,25 @@ static enum cpuhp_state octeon_wdt_online;
  */
 static int __init octeon_wdt_init(void)
 {
-	int i;
 	int ret;
-	u64 *ptr;
+
+	octeon_wdt_bootvector = cvmx_boot_vector_get();
+	if (!octeon_wdt_bootvector) {
+		pr_err("Error: Cannot allocate boot vector.\n");
+		return -ENOMEM;
+	}
+
+	if (OCTEON_IS_MODEL(OCTEON_CN68XX))
+		divisor = 0x200;
+	else if (OCTEON_IS_MODEL(OCTEON_CN78XX))
+		divisor = 0x400;
+	else
+		divisor = 0x100;
 
 	/*
 	 * Watchdog time expiration length = The 16 bits of LEN
 	 * represent the most significant bits of a 24 bit decrementer
-	 * that decrements every 256 cycles.
+	 * that decrements every divisor cycle.
 	 *
 	 * Try for a timeout of 5 sec, if that fails a smaller number
 	 * of even seconds,
@@ -555,8 +554,7 @@ static int __init octeon_wdt_init(void)
 	max_timeout_sec = 6;
 	do {
 		max_timeout_sec--;
-		timeout_cnt = ((octeon_get_io_clock_rate() >> 8) *
-			      max_timeout_sec) >> 8;
+		timeout_cnt = ((octeon_get_io_clock_rate() / divisor) * max_timeout_sec) >> 8;
 	} while (timeout_cnt > 65535);
 
 	BUG_ON(timeout_cnt == 0);
@@ -576,16 +574,10 @@ static int __init octeon_wdt_init(void)
 		return ret;
 	}
 
-	/* Build the NMI handler ... */
-	octeon_wdt_build_stage1();
-
-	/* ... and install it. */
-	ptr = (u64 *) nmi_stage1_insns;
-	for (i = 0; i < 16; i++) {
-		cvmx_write_csr(CVMX_MIO_BOOT_LOC_ADR, i * 8);
-		cvmx_write_csr(CVMX_MIO_BOOT_LOC_DAT, ptr[i]);
+	if (disable) {
+		pr_notice("disabled\n");
+		return 0;
 	}
-	cvmx_write_csr(CVMX_MIO_BOOT_LOC_CFGX(0), 0x81fc0000);
 
 	cpumask_clear(&irq_enabled_cpus);
 
@@ -607,6 +599,10 @@ err:
 static void __exit octeon_wdt_cleanup(void)
 {
 	watchdog_unregister_device(&octeon_wdt);
+
+	if (disable)
+		return;
+
 	cpuhp_remove_state(octeon_wdt_online);
 
 	/*
@@ -617,7 +613,7 @@ static void __exit octeon_wdt_cleanup(void)
 }
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Cavium Networks <support@caviumnetworks.com>");
-MODULE_DESCRIPTION("Cavium Networks Octeon Watchdog driver.");
+MODULE_AUTHOR("Cavium Inc. <support@cavium.com>");
+MODULE_DESCRIPTION("Cavium Inc. OCTEON Watchdog driver.");
 module_init(octeon_wdt_init);
 module_exit(octeon_wdt_cleanup);
diff --git a/drivers/watchdog/octeon-wdt-nmi.S b/drivers/watchdog/octeon-wdt-nmi.S
index 8a900a5e3233..97f6eb7b5a8e 100644
--- a/drivers/watchdog/octeon-wdt-nmi.S
+++ b/drivers/watchdog/octeon-wdt-nmi.S
@@ -3,20 +3,40 @@
  * License.  See the file "COPYING" in the main directory of this archive
  * for more details.
  *
- * Copyright (C) 2007 Cavium Networks
+ * Copyright (C) 2007-2017 Cavium, Inc.
  */
 #include <asm/asm.h>
 #include <asm/regdef.h>
 
-#define SAVE_REG(r)	sd $r, -32768+6912-(32-r)*8($0)
+#define CVMSEG_BASE	-32768
+#define CVMSEG_SIZE	6912
+#define SAVE_REG(r)	sd $r, CVMSEG_BASE + CVMSEG_SIZE - ((32 - r) * 8)($0)
 
         NESTED(octeon_wdt_nmi_stage2, 0, sp)
 	.set 	push
 	.set 	noreorder
 	.set 	noat
-	/* Save all registers to the top CVMSEG. This shouldn't
+	/* Clear Dcache so cvmseg works right. */
+	cache	1,0($0)
+	/* Use K0 to do a read/modify/write of CVMMEMCTL */
+	dmfc0	k0, $11, 7
+	/* Clear out the size of CVMSEG	*/
+	dins	k0, $0, 0, 6
+	/* Set CVMSEG to its largest value */
+	ori	k0, k0, 0x1c0 | 54
+	/* Store the CVMMEMCTL value */
+	dmtc0	k0, $11, 7
+	/*
+	 * Restore K0 from the debug scratch register, it was saved in
+	 * the boot-vector code.
+	 */
+	dmfc0	k0, $31
+
+	/*
+	 * Save all registers to the top CVMSEG. This shouldn't
 	 * corrupt any state used by the kernel. Also all registers
-	 * should have the value right before the NMI. */
+	 * should have the value right before the NMI.
+	 */
 	SAVE_REG(0)
 	SAVE_REG(1)
 	SAVE_REG(2)
@@ -49,16 +69,22 @@
 	SAVE_REG(29)
 	SAVE_REG(30)
 	SAVE_REG(31)
+	/* Write zero to all CVMSEG locations per Core-15169 */
+	dli	a0, CVMSEG_SIZE - (33 * 8)
+1:	sd	zero, CVMSEG_BASE(a0)
+	daddiu	a0, a0, -8
+	bgez	a0, 1b
+	nop
 	/* Set the stack to begin right below the registers */
-	li	sp, -32768+6912-32*8
+	dli	sp, CVMSEG_BASE + CVMSEG_SIZE - (32 * 8)
 	/* Load the address of the third stage handler */
-	dla	a0, octeon_wdt_nmi_stage3
+	dla	$25, octeon_wdt_nmi_stage3
 	/* Call the third stage handler */
-	jal	a0
+	jal	$25
 	/* a0 is the address of the saved registers */
 	 move	a0, sp
 	/* Loop forvever if we get here. */
-1:	b	1b
+2:	b	2b
 	nop
 	.set pop
 	END(octeon_wdt_nmi_stage2)