x86/kprobes: Fix crash when probing CS CALL instructionsgrafted

When using eBPF to probe CS CALL instructions within a function,
a crash can be triggered.

The eBPF tool probes offset 257 of the __hrtimer_run_queues()
function:

<__hrtimer_run_queues+249>:  nopl   0x0(%rax,%rax,1)
<__hrtimer_run_queues+254>:  mov    %r14,%rdi
<__hrtimer_run_queues+257>:  cs call <__x86_indirect_thunk_r12>
<__hrtimer_run_queues+263>:  mov    %eax,%r12d
<__hrtimer_run_queues+266>:  xchg   %ax,%ax
<__hrtimer_run_queues+268>:  mov    %r13,%rdi

Which triggers this crash:

  BUG: unable to handle page fault for address: 00000000000f41c9
  #PF: supervisor write access in kernel mode
  #PF: error_code(0x0002) - not-present page
  PGD 0 P4D 0
  Oops: 0002 [#1] SMP NOPTI
  CPU: 1 PID: 0 Comm: swapper/1 Kdump: loaded Tainted: P
  RIP: 0010:__hrtimer_run_queues+0x106/0x230

Note that __hrtimer_run_queues+0x106 is __hrtimer_run_queues+262, which is
at the 6th byte of the above CS CALL instruction. Since the CS CALL
instruction occupies 6 bytes, the exception occurred in the middle of that
call instruction.

The root cause is that when using eBPF tools to probe in the middle of a
function, a kprobe with INT3 is used as the underlying implementation.

During single-step emulation of the original CALL instruction,
int3_emulate_call() assumes that the probed CALL instruction is 5 bytes
long. However, the actual CS-prefixed CALL instruction occupies 6 bytes,
so it constructs an incorrect exception return address. When the CPU
returns from the kprobe handler, the next instruction to be executed is at
the address of the last byte of that CS CALL instruction. Coincidentally,
starting from that address, the CPU fetches and decodes a completely
different instruction, which ultimately triggers a kernel crash.

Fix the issue by using the actual instruction length obtained from
the instruction decoder when constructing the exception return
address, rather than relying on the hardcoded CALL_INSN_SIZE macro.

[ mingo: Refined the changelog ]

Fixes: 6256e668b7af ("x86/kprobes: Use int3 instead of debug trap for single-step")
Suggested-by: Masami Hiramatsu (Google) <mhiramat@kernel.org>
Signed-off-by: Jinke Han <jinkehan@didiglobal.com>
Signed-off-by: Ingo Molnar <mingo@kernel.org>
Reviewed-by: Masami Hiramatsu (Google) <mhiramat@kernel.org>
Acked-by: Yafang Shao <laoar.shao@gmail.com>
Acked-by: Borislav Petkov <bp@alien8.de>
Cc: Peter Zijlstra <peterz@infradead.org>
Link: https://patch.msgid.link/20260908073742.GA10517@didi-ThinkCentre-M920t-N000

11 files changed, 1385 insertions, 0 deletions

diff --git a/drivers/soc/bcm/Kconfig b/drivers/soc/bcm/Kconfig
new file mode 100644
index 000000000..c921a22f6
--- /dev/null
+++ b/drivers/soc/bcm/Kconfig
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0-only
+menu "Broadcom SoC drivers"
+
+config SOC_BRCMSTB
+	bool "Broadcom STB SoC drivers"
+	depends on ARCH_BRCMSTB || BMIPS_GENERIC || COMPILE_TEST
+	select SOC_BUS
+	help
+	  Enables drivers for the Broadcom Set-Top Box (STB) series of chips.
+	  This option alone enables only some support code, while the drivers
+	  can be enabled individually within this menu.
+
+	  If unsure, say N.
+
+source "drivers/soc/bcm/brcmstb/Kconfig"
+
+endmenu
diff --git a/drivers/soc/bcm/Makefile b/drivers/soc/bcm/Makefile
new file mode 100644
index 000000000..32424b103
--- /dev/null
+++ b/drivers/soc/bcm/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-$(CONFIG_SOC_BRCMSTB)	+= brcmstb/
diff --git a/drivers/soc/bcm/brcmstb/Kconfig b/drivers/soc/bcm/brcmstb/Kconfig
new file mode 100644
index 000000000..c68d0e526
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/Kconfig
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0-only
+if SOC_BRCMSTB
+
+config BRCMSTB_PM
+	bool "Support suspend/resume for STB platforms"
+	default y
+	depends on PM && BMIPS_GENERIC
+
+endif # SOC_BRCMSTB
diff --git a/drivers/soc/bcm/brcmstb/Makefile b/drivers/soc/bcm/brcmstb/Makefile
new file mode 100644
index 000000000..fe5c43d26
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-y				+= common.o biuctrl.o
+obj-$(CONFIG_BRCMSTB_PM)	+= pm/
diff --git a/drivers/soc/bcm/brcmstb/biuctrl.c b/drivers/soc/bcm/brcmstb/biuctrl.c
new file mode 100644
index 000000000..bd830649b
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/biuctrl.c
@@ -0,0 +1,368 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Broadcom STB SoCs Bus Unit Interface controls
+ *
+ * Copyright (C) 2015, Broadcom Corporation
+ */
+
+#define pr_fmt(fmt)	"brcmstb: " KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+#include <linux/soc/brcmstb/brcmstb.h>
+
+#define RACENPREF_MASK			0x3
+#define RACPREFINST_SHIFT		0
+#define RACENINST_SHIFT			2
+#define RACPREFDATA_SHIFT		4
+#define RACENDATA_SHIFT			6
+#define RAC_CPU_SHIFT			8
+#define RACCFG_MASK			0xff
+#define DPREF_LINE_2_SHIFT		24
+#define DPREF_LINE_2_MASK		0xff
+
+/* Bitmask to enable instruction and data prefetching with a 256-bytes stride */
+#define RAC_DATA_INST_EN_MASK		(1 << RACPREFINST_SHIFT | \
+					 RACENPREF_MASK << RACENINST_SHIFT | \
+					 1 << RACPREFDATA_SHIFT | \
+					 RACENPREF_MASK << RACENDATA_SHIFT)
+
+#define  CPU_CREDIT_REG_MCPx_WR_PAIRING_EN_MASK	0x70000000
+#define CPU_CREDIT_REG_MCPx_READ_CRED_MASK	0xf
+#define CPU_CREDIT_REG_MCPx_WRITE_CRED_MASK	0xf
+#define CPU_CREDIT_REG_MCPx_READ_CRED_SHIFT(x)	((x) * 8)
+#define CPU_CREDIT_REG_MCPx_WRITE_CRED_SHIFT(x)	(((x) * 8) + 4)
+
+#define CPU_MCP_FLOW_REG_MCPx_RDBUFF_CRED_SHIFT(x)	((x) * 8)
+#define CPU_MCP_FLOW_REG_MCPx_RDBUFF_CRED_MASK		0xff
+
+#define CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_THRESHOLD_MASK	0xf
+#define CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_TIMEOUT_MASK		0xf
+#define CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_TIMEOUT_SHIFT	4
+#define CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_ENABLE		BIT(8)
+
+static void __iomem *cpubiuctrl_base;
+static bool mcp_wr_pairing_en;
+static const int *cpubiuctrl_regs;
+
+enum cpubiuctrl_regs {
+	CPU_CREDIT_REG = 0,
+	CPU_MCP_FLOW_REG,
+	CPU_WRITEBACK_CTRL_REG,
+	RAC_CONFIG0_REG,
+	RAC_CONFIG1_REG,
+	NUM_CPU_BIUCTRL_REGS,
+};
+
+static inline u32 cbc_readl(int reg)
+{
+	int offset = cpubiuctrl_regs[reg];
+
+	if (offset == -1 ||
+	    (IS_ENABLED(CONFIG_CACHE_B15_RAC) && reg >= RAC_CONFIG0_REG))
+		return (u32)-1;
+
+	return readl_relaxed(cpubiuctrl_base + offset);
+}
+
+static inline void cbc_writel(u32 val, int reg)
+{
+	int offset = cpubiuctrl_regs[reg];
+
+	if (offset == -1 ||
+	    (IS_ENABLED(CONFIG_CACHE_B15_RAC) && reg >= RAC_CONFIG0_REG))
+		return;
+
+	writel(val, cpubiuctrl_base + offset);
+}
+
+static const int b15_cpubiuctrl_regs[] = {
+	[CPU_CREDIT_REG] = 0x184,
+	[CPU_MCP_FLOW_REG] = -1,
+	[CPU_WRITEBACK_CTRL_REG] = -1,
+	[RAC_CONFIG0_REG] = -1,
+	[RAC_CONFIG1_REG] = -1,
+};
+
+/* Odd cases, e.g: 7260A0 */
+static const int b53_cpubiuctrl_no_wb_regs[] = {
+	[CPU_CREDIT_REG] = 0x0b0,
+	[CPU_MCP_FLOW_REG] = 0x0b4,
+	[CPU_WRITEBACK_CTRL_REG] = -1,
+	[RAC_CONFIG0_REG] = 0x78,
+	[RAC_CONFIG1_REG] = 0x7c,
+};
+
+static const int b53_cpubiuctrl_regs[] = {
+	[CPU_CREDIT_REG] = 0x0b0,
+	[CPU_MCP_FLOW_REG] = 0x0b4,
+	[CPU_WRITEBACK_CTRL_REG] = 0x22c,
+	[RAC_CONFIG0_REG] = 0x78,
+	[RAC_CONFIG1_REG] = 0x7c,
+};
+
+static const int a72_cpubiuctrl_regs[] = {
+	[CPU_CREDIT_REG] = 0x18,
+	[CPU_MCP_FLOW_REG] = 0x1c,
+	[CPU_WRITEBACK_CTRL_REG] = 0x20,
+	[RAC_CONFIG0_REG] = 0x08,
+	[RAC_CONFIG1_REG] = 0x0c,
+};
+
+static int __init mcp_write_pairing_set(void)
+{
+	u32 creds = 0;
+
+	if (!cpubiuctrl_base)
+		return -1;
+
+	creds = cbc_readl(CPU_CREDIT_REG);
+	if (mcp_wr_pairing_en) {
+		pr_info("MCP: Enabling write pairing\n");
+		cbc_writel(creds | CPU_CREDIT_REG_MCPx_WR_PAIRING_EN_MASK,
+			   CPU_CREDIT_REG);
+	} else if (creds & CPU_CREDIT_REG_MCPx_WR_PAIRING_EN_MASK) {
+		pr_info("MCP: Disabling write pairing\n");
+		cbc_writel(creds & ~CPU_CREDIT_REG_MCPx_WR_PAIRING_EN_MASK,
+			   CPU_CREDIT_REG);
+	} else {
+		pr_info("MCP: Write pairing already disabled\n");
+	}
+
+	return 0;
+}
+
+static const u32 a72_b53_mach_compat[] = {
+	0x7211,
+	0x72113,
+	0x72116,
+	0x7216,
+	0x72164,
+	0x72165,
+	0x7255,
+	0x7260,
+	0x7268,
+	0x7271,
+	0x7278,
+};
+
+/* The read-ahead cache present in the Brahma-B53 CPU is a special piece of
+ * hardware after the integrated L2 cache of the B53 CPU complex whose purpose
+ * is to prefetch instruction and/or data with a line size of either 64 bytes
+ * or 256 bytes. The rationale is that the data-bus of the CPU interface is
+ * optimized for 256-byte transactions, and enabling the read-ahead cache
+ * provides a significant performance boost (typically twice the performance
+ * for a memcpy benchmark application).
+ *
+ * The read-ahead cache is transparent for Virtual Address cache maintenance
+ * operations: IC IVAU, DC IVAC, DC CVAC, DC CVAU and DC CIVAC.  So no special
+ * handling is needed for the DMA API above and beyond what is included in the
+ * arm64 implementation.
+ *
+ * In addition, since the Point of Unification is typically between L1 and L2
+ * for the Brahma-B53 processor no special read-ahead cache handling is needed
+ * for the IC IALLU and IC IALLUIS cache maintenance operations.
+ *
+ * However, it is not possible to specify the cache level (L3) for the cache
+ * maintenance instructions operating by set/way to operate on the read-ahead
+ * cache.  The read-ahead cache will maintain coherency when inner cache lines
+ * are cleaned by set/way, but if it is necessary to invalidate inner cache
+ * lines by set/way to maintain coherency with system masters operating on
+ * shared memory that does not have hardware support for coherency, then it
+ * will also be necessary to explicitly invalidate the read-ahead cache.
+ */
+static void __init a72_b53_rac_enable_all(struct device_node *np)
+{
+	unsigned int cpu;
+	u32 enable = 0, pref_dist, shift;
+
+	if (IS_ENABLED(CONFIG_CACHE_B15_RAC))
+		return;
+
+	if (WARN(num_possible_cpus() > 4, "RAC only supports 4 CPUs\n"))
+		return;
+
+	pref_dist = cbc_readl(RAC_CONFIG1_REG);
+	for_each_possible_cpu(cpu) {
+		shift = cpu * RAC_CPU_SHIFT + RACPREFDATA_SHIFT;
+		enable |= RAC_DATA_INST_EN_MASK << (cpu * RAC_CPU_SHIFT);
+		if (cpubiuctrl_regs == a72_cpubiuctrl_regs) {
+			enable &= ~(RACENPREF_MASK << shift);
+			enable |= 3 << shift;
+			pref_dist |= 1 << (cpu + DPREF_LINE_2_SHIFT);
+		}
+	}
+
+	cbc_writel(enable, RAC_CONFIG0_REG);
+	cbc_writel(pref_dist, RAC_CONFIG1_REG);
+
+	pr_info("%pOF: Broadcom %s read-ahead cache\n",
+		np, cpubiuctrl_regs == a72_cpubiuctrl_regs ?
+		"Cortex-A72" : "Brahma-B53");
+}
+
+static void __init mcp_a72_b53_set(void)
+{
+	unsigned int i;
+	u32 reg;
+
+	reg = brcmstb_get_family_id();
+
+	for (i = 0; i < ARRAY_SIZE(a72_b53_mach_compat); i++) {
+		if (BRCM_ID(reg) == a72_b53_mach_compat[i])
+			break;
+	}
+
+	if (i == ARRAY_SIZE(a72_b53_mach_compat))
+		return;
+
+	/* Set all 3 MCP interfaces to 8 credits */
+	reg = cbc_readl(CPU_CREDIT_REG);
+	for (i = 0; i < 3; i++) {
+		reg &= ~(CPU_CREDIT_REG_MCPx_WRITE_CRED_MASK <<
+			 CPU_CREDIT_REG_MCPx_WRITE_CRED_SHIFT(i));
+		reg &= ~(CPU_CREDIT_REG_MCPx_READ_CRED_MASK <<
+			 CPU_CREDIT_REG_MCPx_READ_CRED_SHIFT(i));
+		reg |= 8 << CPU_CREDIT_REG_MCPx_WRITE_CRED_SHIFT(i);
+		reg |= 8 << CPU_CREDIT_REG_MCPx_READ_CRED_SHIFT(i);
+	}
+	cbc_writel(reg, CPU_CREDIT_REG);
+
+	/* Max out the number of in-flight Jwords reads on the MCP interface */
+	reg = cbc_readl(CPU_MCP_FLOW_REG);
+	for (i = 0; i < 3; i++)
+		reg |= CPU_MCP_FLOW_REG_MCPx_RDBUFF_CRED_MASK <<
+			CPU_MCP_FLOW_REG_MCPx_RDBUFF_CRED_SHIFT(i);
+	cbc_writel(reg, CPU_MCP_FLOW_REG);
+
+	/* Enable writeback throttling, set timeout to 128 cycles, 256 cycles
+	 * threshold
+	 */
+	reg = cbc_readl(CPU_WRITEBACK_CTRL_REG);
+	reg |= CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_ENABLE;
+	reg &= ~CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_THRESHOLD_MASK;
+	reg &= ~(CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_TIMEOUT_MASK <<
+		 CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_TIMEOUT_SHIFT);
+	reg |= 8;
+	reg |= 7 << CPU_WRITEBACK_CTRL_REG_WB_THROTTLE_TIMEOUT_SHIFT;
+	cbc_writel(reg, CPU_WRITEBACK_CTRL_REG);
+}
+
+static int __init setup_hifcpubiuctrl_regs(struct device_node *np)
+{
+	struct device_node *cpu_dn;
+	u32 family_id;
+	int ret = 0;
+
+	cpubiuctrl_base = of_iomap(np, 0);
+	if (!cpubiuctrl_base) {
+		pr_err("failed to remap BIU control base\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mcp_wr_pairing_en = of_property_read_bool(np, "brcm,write-pairing");
+
+	cpu_dn = of_get_cpu_node(0, NULL);
+	if (!cpu_dn) {
+		pr_err("failed to obtain CPU device node\n");
+		ret = -ENODEV;
+		goto out;
+	}
+
+	if (of_device_is_compatible(cpu_dn, "brcm,brahma-b15"))
+		cpubiuctrl_regs = b15_cpubiuctrl_regs;
+	else if (of_device_is_compatible(cpu_dn, "brcm,brahma-b53"))
+		cpubiuctrl_regs = b53_cpubiuctrl_regs;
+	else if (of_device_is_compatible(cpu_dn, "arm,cortex-a72"))
+		cpubiuctrl_regs = a72_cpubiuctrl_regs;
+	else {
+		pr_err("unsupported CPU\n");
+		ret = -EINVAL;
+	}
+	of_node_put(cpu_dn);
+
+	family_id = brcmstb_get_family_id();
+	if (BRCM_ID(family_id) == 0x7260 && BRCM_REV(family_id) == 0)
+		cpubiuctrl_regs = b53_cpubiuctrl_no_wb_regs;
+out:
+	if (ret && cpubiuctrl_base) {
+		iounmap(cpubiuctrl_base);
+		cpubiuctrl_base = NULL;
+	}
+	return ret;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static u32 cpubiuctrl_reg_save[NUM_CPU_BIUCTRL_REGS];
+
+static int brcmstb_cpu_credit_reg_suspend(void *data)
+{
+	unsigned int i;
+
+	if (!cpubiuctrl_base)
+		return 0;
+
+	for (i = 0; i < NUM_CPU_BIUCTRL_REGS; i++)
+		cpubiuctrl_reg_save[i] = cbc_readl(i);
+
+	return 0;
+}
+
+static void brcmstb_cpu_credit_reg_resume(void *data)
+{
+	unsigned int i;
+
+	if (!cpubiuctrl_base)
+		return;
+
+	for (i = 0; i < NUM_CPU_BIUCTRL_REGS; i++)
+		cbc_writel(cpubiuctrl_reg_save[i], i);
+}
+
+static const struct syscore_ops brcmstb_cpu_credit_syscore_ops = {
+	.suspend = brcmstb_cpu_credit_reg_suspend,
+	.resume = brcmstb_cpu_credit_reg_resume,
+};
+
+static struct syscore brcmstb_cpu_credit_syscore = {
+	.ops = &brcmstb_cpu_credit_syscore_ops,
+};
+#endif
+
+
+static int __init brcmstb_biuctrl_init(void)
+{
+	struct device_node *np;
+	int ret;
+
+	/* We might be running on a multi-platform kernel, don't make this a
+	 * fatal error, just bail out early
+	 */
+	np = of_find_compatible_node(NULL, NULL, "brcm,brcmstb-cpu-biu-ctrl");
+	if (!np)
+		return 0;
+
+	ret = setup_hifcpubiuctrl_regs(np);
+	if (ret)
+		goto out_put;
+
+	ret = mcp_write_pairing_set();
+	if (ret) {
+		pr_err("MCP: Unable to disable write pairing!\n");
+		goto out_put;
+	}
+
+	a72_b53_rac_enable_all(np);
+	mcp_a72_b53_set();
+#ifdef CONFIG_PM_SLEEP
+	register_syscore(&brcmstb_cpu_credit_syscore);
+#endif
+	ret = 0;
+out_put:
+	of_node_put(np);
+	return ret;
+}
+early_initcall(brcmstb_biuctrl_init);
diff --git a/drivers/soc/bcm/brcmstb/common.c b/drivers/soc/bcm/brcmstb/common.c
new file mode 100644
index 000000000..a903fa44e
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/common.c
@@ -0,0 +1,117 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright © 2014 NVIDIA Corporation
+ * Copyright © 2015 Broadcom Corporation
+ */
+
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/soc/brcmstb/brcmstb.h>
+#include <linux/sys_soc.h>
+
+struct brcmstb_soc_info {
+	u32 family_id;
+	u32 product_id;
+};
+
+static struct brcmstb_soc_info *soc_info;
+
+u32 brcmstb_get_family_id(void)
+{
+	return soc_info ? soc_info->family_id : 0;
+}
+EXPORT_SYMBOL(brcmstb_get_family_id);
+
+u32 brcmstb_get_product_id(void)
+{
+	return soc_info ? soc_info->product_id : 0;
+}
+EXPORT_SYMBOL(brcmstb_get_product_id);
+
+static const struct of_device_id sun_top_ctrl_match[] = {
+	{ .compatible = "brcm,bcm7125-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7346-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7358-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7360-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7362-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7420-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7425-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7429-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7435-sun-top-ctrl", },
+	{ .compatible = "brcm,brcmstb-sun-top-ctrl", },
+	{ }
+};
+
+static int __init brcmstb_soc_device_init(void)
+{
+	struct soc_device_attribute *soc_dev_attr;
+	struct device_node *sun_top_ctrl;
+	void __iomem *sun_top_ctrl_base;
+	struct soc_device *soc_dev;
+	int ret = 0;
+
+	/* We could be on a multi-platform kernel, don't make this fatal but
+	 * bail out early
+	 */
+	sun_top_ctrl = of_find_matching_node(NULL, sun_top_ctrl_match);
+	if (!sun_top_ctrl)
+		return 0;
+
+	sun_top_ctrl_base = of_iomap(sun_top_ctrl, 0);
+	if (!sun_top_ctrl_base) {
+		ret = -ENODEV;
+		goto out_put_node;
+	}
+
+	soc_info = kzalloc_obj(*soc_info);
+	if (!soc_info) {
+		ret = -ENOMEM;
+		goto out_unmap;
+	}
+
+	soc_info->family_id = readl(sun_top_ctrl_base);
+	soc_info->product_id = readl(sun_top_ctrl_base + 0x4);
+
+	soc_dev_attr = kzalloc_obj(*soc_dev_attr);
+	if (!soc_dev_attr) {
+		ret = -ENOMEM;
+		goto out_free_info;
+	}
+
+	soc_dev_attr->family = kasprintf(GFP_KERNEL, "%x",
+					 soc_info->family_id >> 28 ?
+					 soc_info->family_id >> 16 : soc_info->family_id >> 8);
+	soc_dev_attr->soc_id = kasprintf(GFP_KERNEL, "%x",
+					 soc_info->product_id >> 28 ?
+					 soc_info->product_id >> 16 : soc_info->product_id >> 8);
+	soc_dev_attr->revision = kasprintf(GFP_KERNEL, "%c%d",
+					 ((soc_info->product_id & 0xf0) >> 4) + 'A',
+					   soc_info->product_id & 0xf);
+
+	soc_dev = soc_device_register(soc_dev_attr);
+	if (IS_ERR(soc_dev)) {
+		ret = PTR_ERR(soc_dev);
+		goto out_free_attr;
+	}
+
+	iounmap(sun_top_ctrl_base);
+	of_node_put(sun_top_ctrl);
+	return 0;
+
+out_free_attr:
+	kfree(soc_dev_attr->revision);
+	kfree(soc_dev_attr->soc_id);
+	kfree(soc_dev_attr->family);
+	kfree(soc_dev_attr);
+out_free_info:
+	kfree(soc_info);
+	soc_info = NULL;
+out_unmap:
+	iounmap(sun_top_ctrl_base);
+out_put_node:
+	of_node_put(sun_top_ctrl);
+	return ret;
+}
+early_initcall(brcmstb_soc_device_init);
diff --git a/drivers/soc/bcm/brcmstb/pm/Makefile b/drivers/soc/bcm/brcmstb/pm/Makefile
new file mode 100644
index 000000000..9133a9ee0
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/pm/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-$(CONFIG_BMIPS_GENERIC)	+= s2-mips.o s3-mips.o pm-mips.o
diff --git a/drivers/soc/bcm/brcmstb/pm/pm-mips.c b/drivers/soc/bcm/brcmstb/pm/pm-mips.c
new file mode 100644
index 000000000..4dfb5a850
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/pm/pm-mips.c
@@ -0,0 +1,456 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * MIPS-specific support for Broadcom STB S2/S3/S5 power management
+ *
+ * Copyright (C) 2016-2017 Broadcom
+ */
+
+#include <linux/kernel.h>
+#include <linux/printk.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/delay.h>
+#include <linux/suspend.h>
+#include <asm/bmips.h>
+#include <asm/tlbflush.h>
+
+#include "pm.h"
+
+#define S2_NUM_PARAMS		6
+#define MAX_NUM_MEMC		3
+
+/* S3 constants */
+#define MAX_GP_REGS		16
+#define MAX_CP0_REGS		32
+#define NUM_MEMC_CLIENTS	128
+#define AON_CTRL_RAM_SIZE	128
+#define BRCMSTB_S3_MAGIC	0x5AFEB007
+
+#define CLEAR_RESET_MASK	0x01
+
+/* Index each CP0 register that needs to be saved */
+#define CONTEXT		0
+#define USER_LOCAL	1
+#define PGMK		2
+#define HWRENA		3
+#define COMPARE		4
+#define STATUS		5
+#define CONFIG		6
+#define MODE		7
+#define EDSP		8
+#define BOOT_VEC	9
+#define EBASE		10
+
+struct brcmstb_memc {
+	void __iomem *ddr_phy_base;
+	void __iomem *arb_base;
+};
+
+struct brcmstb_pm_control {
+	void __iomem *aon_ctrl_base;
+	void __iomem *aon_sram_base;
+	void __iomem *timers_base;
+	struct brcmstb_memc memcs[MAX_NUM_MEMC];
+	int num_memc;
+};
+
+struct brcm_pm_s3_context {
+	u32			cp0_regs[MAX_CP0_REGS];
+	u32			memc0_rts[NUM_MEMC_CLIENTS];
+	u32			sc_boot_vec;
+};
+
+struct brcmstb_mem_transfer;
+
+struct brcmstb_mem_transfer {
+	struct brcmstb_mem_transfer	*next;
+	void				*src;
+	void				*dst;
+	dma_addr_t			pa_src;
+	dma_addr_t			pa_dst;
+	u32				len;
+	u8				key;
+	u8				mode;
+	u8				src_remapped;
+	u8				dst_remapped;
+	u8				src_dst_remapped;
+};
+
+#define AON_SAVE_SRAM(base, idx, val) \
+	__raw_writel(val, base + (idx << 2))
+
+/* Used for saving registers in asm */
+u32 gp_regs[MAX_GP_REGS];
+
+#define	BSP_CLOCK_STOP		0x00
+#define PM_INITIATE		0x01
+
+static struct brcmstb_pm_control ctrl;
+
+static void brcm_pm_save_cp0_context(struct brcm_pm_s3_context *ctx)
+{
+	/* Generic MIPS */
+	ctx->cp0_regs[CONTEXT] = read_c0_context();
+	ctx->cp0_regs[USER_LOCAL] = read_c0_userlocal();
+	ctx->cp0_regs[PGMK] = read_c0_pagemask();
+	ctx->cp0_regs[HWRENA] = read_c0_cache();
+	ctx->cp0_regs[COMPARE] = read_c0_compare();
+	ctx->cp0_regs[STATUS] = read_c0_status();
+
+	/* Broadcom specific */
+	ctx->cp0_regs[CONFIG] = read_c0_brcm_config();
+	ctx->cp0_regs[MODE] = read_c0_brcm_mode();
+	ctx->cp0_regs[EDSP] = read_c0_brcm_edsp();
+	ctx->cp0_regs[BOOT_VEC] = read_c0_brcm_bootvec();
+	ctx->cp0_regs[EBASE] = read_c0_ebase();
+
+	ctx->sc_boot_vec = bmips_read_zscm_reg(0xa0);
+}
+
+static void brcm_pm_restore_cp0_context(struct brcm_pm_s3_context *ctx)
+{
+	/* Restore cp0 state */
+	bmips_write_zscm_reg(0xa0, ctx->sc_boot_vec);
+
+	/* Generic MIPS */
+	write_c0_context(ctx->cp0_regs[CONTEXT]);
+	write_c0_userlocal(ctx->cp0_regs[USER_LOCAL]);
+	write_c0_pagemask(ctx->cp0_regs[PGMK]);
+	write_c0_cache(ctx->cp0_regs[HWRENA]);
+	write_c0_compare(ctx->cp0_regs[COMPARE]);
+	write_c0_status(ctx->cp0_regs[STATUS]);
+
+	/* Broadcom specific */
+	write_c0_brcm_config(ctx->cp0_regs[CONFIG]);
+	write_c0_brcm_mode(ctx->cp0_regs[MODE]);
+	write_c0_brcm_edsp(ctx->cp0_regs[EDSP]);
+	write_c0_brcm_bootvec(ctx->cp0_regs[BOOT_VEC]);
+	write_c0_ebase(ctx->cp0_regs[EBASE]);
+}
+
+static void  brcmstb_pm_handshake(void)
+{
+	void __iomem *base = ctrl.aon_ctrl_base;
+	u32 tmp;
+
+	/* BSP power handshake, v1 */
+	tmp = __raw_readl(base + AON_CTRL_HOST_MISC_CMDS);
+	tmp &= ~1UL;
+	__raw_writel(tmp, base + AON_CTRL_HOST_MISC_CMDS);
+	(void)__raw_readl(base + AON_CTRL_HOST_MISC_CMDS);
+
+	__raw_writel(0, base + AON_CTRL_PM_INITIATE);
+	(void)__raw_readl(base + AON_CTRL_PM_INITIATE);
+	__raw_writel(BSP_CLOCK_STOP | PM_INITIATE,
+		     base + AON_CTRL_PM_INITIATE);
+	/*
+	 * HACK: BSP may have internal race on the CLOCK_STOP command.
+	 * Avoid touching the BSP for a few milliseconds.
+	 */
+	mdelay(3);
+}
+
+static void brcmstb_pm_s5(void)
+{
+	void __iomem *base = ctrl.aon_ctrl_base;
+
+	brcmstb_pm_handshake();
+
+	/* Clear magic s3 warm-boot value */
+	AON_SAVE_SRAM(ctrl.aon_sram_base, 0, 0);
+
+	/* Set the countdown */
+	__raw_writel(0x10, base + AON_CTRL_PM_CPU_WAIT_COUNT);
+	(void)__raw_readl(base + AON_CTRL_PM_CPU_WAIT_COUNT);
+
+	/* Prepare to S5 cold boot */
+	__raw_writel(PM_COLD_CONFIG, base + AON_CTRL_PM_CTRL);
+	(void)__raw_readl(base + AON_CTRL_PM_CTRL);
+
+	__raw_writel((PM_COLD_CONFIG | PM_PWR_DOWN), base +
+		      AON_CTRL_PM_CTRL);
+	(void)__raw_readl(base + AON_CTRL_PM_CTRL);
+
+	__asm__ __volatile__(
+	"	wait\n"
+	: : : "memory");
+}
+
+static int brcmstb_pm_s3(void)
+{
+	struct brcm_pm_s3_context s3_context;
+	void __iomem *memc_arb_base;
+	unsigned long flags;
+	u32 tmp;
+	int i;
+
+	/* Prepare for s3 */
+	AON_SAVE_SRAM(ctrl.aon_sram_base, 0, BRCMSTB_S3_MAGIC);
+	AON_SAVE_SRAM(ctrl.aon_sram_base, 1, (u32)&s3_reentry);
+	AON_SAVE_SRAM(ctrl.aon_sram_base, 2, 0);
+
+	/* Clear RESET_HISTORY */
+	tmp = __raw_readl(ctrl.aon_ctrl_base + AON_CTRL_RESET_CTRL);
+	tmp &= ~CLEAR_RESET_MASK;
+	__raw_writel(tmp, ctrl.aon_ctrl_base + AON_CTRL_RESET_CTRL);
+
+	local_irq_save(flags);
+
+	/* Inhibit DDR_RSTb pulse for both MMCs*/
+	for (i = 0; i < ctrl.num_memc; i++) {
+		tmp = __raw_readl(ctrl.memcs[i].ddr_phy_base +
+			DDR40_PHY_CONTROL_REGS_0_STANDBY_CTRL);
+
+		tmp &= ~0x0f;
+		__raw_writel(tmp, ctrl.memcs[i].ddr_phy_base +
+			DDR40_PHY_CONTROL_REGS_0_STANDBY_CTRL);
+		tmp |= (0x05 | BIT(5));
+		__raw_writel(tmp, ctrl.memcs[i].ddr_phy_base +
+			DDR40_PHY_CONTROL_REGS_0_STANDBY_CTRL);
+	}
+
+	/* Save CP0 context */
+	brcm_pm_save_cp0_context(&s3_context);
+
+	/* Save RTS(skip debug register) */
+	memc_arb_base = ctrl.memcs[0].arb_base + 4;
+	for (i = 0; i < NUM_MEMC_CLIENTS; i++) {
+		s3_context.memc0_rts[i] = __raw_readl(memc_arb_base);
+		memc_arb_base += 4;
+	}
+
+	/* Save I/O context */
+	local_flush_tlb_all();
+	_dma_cache_wback_inv(0, ~0);
+
+	brcm_pm_do_s3(ctrl.aon_ctrl_base, current_cpu_data.dcache.linesz);
+
+	/* CPU reconfiguration */
+	local_flush_tlb_all();
+	bmips_cpu_setup();
+	cpumask_clear(&bmips_booted_mask);
+
+	/* Restore RTS (skip debug register) */
+	memc_arb_base = ctrl.memcs[0].arb_base + 4;
+	for (i = 0; i < NUM_MEMC_CLIENTS; i++) {
+		__raw_writel(s3_context.memc0_rts[i], memc_arb_base);
+		memc_arb_base += 4;
+	}
+
+	/* restore CP0 context */
+	brcm_pm_restore_cp0_context(&s3_context);
+
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static int brcmstb_pm_s2(void)
+{
+	/*
+	 * We need to pass 6 arguments to an assembly function. Lets avoid the
+	 * stack and pass arguments in a explicit 4 byte array. The assembly
+	 * code assumes all arguments are 4 bytes and arguments are ordered
+	 * like so:
+	 *
+	 * 0: AON_CTRl base register
+	 * 1: DDR_PHY base register
+	 * 2: TIMERS base resgister
+	 * 3: I-Cache line size
+	 * 4: Restart vector address
+	 * 5: Restart vector size
+	 */
+	u32 s2_params[6];
+
+	/* Prepare s2 parameters */
+	s2_params[0] = (u32)ctrl.aon_ctrl_base;
+	s2_params[1] = (u32)ctrl.memcs[0].ddr_phy_base;
+	s2_params[2] = (u32)ctrl.timers_base;
+	s2_params[3] = (u32)current_cpu_data.icache.linesz;
+	s2_params[4] = (u32)BMIPS_WARM_RESTART_VEC;
+	s2_params[5] = (u32)(bmips_smp_int_vec_end -
+		bmips_smp_int_vec);
+
+	/* Drop to standby */
+	brcm_pm_do_s2(s2_params);
+
+	return 0;
+}
+
+static int brcmstb_pm_standby(bool deep_standby)
+{
+	brcmstb_pm_handshake();
+
+	/* Send IRQs to BMIPS_WARM_RESTART_VEC */
+	clear_c0_cause(CAUSEF_IV);
+	irq_disable_hazard();
+	set_c0_status(ST0_BEV);
+	irq_disable_hazard();
+
+	if (deep_standby)
+		brcmstb_pm_s3();
+	else
+		brcmstb_pm_s2();
+
+	/* Send IRQs to normal runtime vectors */
+	clear_c0_status(ST0_BEV);
+	irq_disable_hazard();
+	set_c0_cause(CAUSEF_IV);
+	irq_disable_hazard();
+
+	return 0;
+}
+
+static int brcmstb_pm_enter(suspend_state_t state)
+{
+	int ret = -EINVAL;
+
+	switch (state) {
+	case PM_SUSPEND_STANDBY:
+		ret = brcmstb_pm_standby(false);
+		break;
+	case PM_SUSPEND_MEM:
+		ret = brcmstb_pm_standby(true);
+		break;
+	}
+
+	return ret;
+}
+
+static int brcmstb_pm_valid(suspend_state_t state)
+{
+	switch (state) {
+	case PM_SUSPEND_STANDBY:
+		return true;
+	case PM_SUSPEND_MEM:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct platform_suspend_ops brcmstb_pm_ops = {
+	.enter		= brcmstb_pm_enter,
+	.valid		= brcmstb_pm_valid,
+};
+
+static const struct of_device_id aon_ctrl_dt_ids[] = {
+	{ .compatible = "brcm,brcmstb-aon-ctrl" },
+	{ /* sentinel */ }
+};
+
+static const struct of_device_id ddr_phy_dt_ids[] = {
+	{ .compatible = "brcm,brcmstb-ddr-phy" },
+	{ /* sentinel */ }
+};
+
+static const struct of_device_id arb_dt_ids[] = {
+	{ .compatible = "brcm,brcmstb-memc-arb" },
+	{ /* sentinel */ }
+};
+
+static const struct of_device_id timers_ids[] = {
+	{ .compatible = "brcm,brcmstb-timers" },
+	{ /* sentinel */ }
+};
+
+static inline void __iomem *brcmstb_ioremap_node(struct device_node *dn,
+						 int index)
+{
+	return of_io_request_and_map(dn, index, dn->full_name);
+}
+
+static void __iomem *brcmstb_ioremap_match(const struct of_device_id *matches,
+					   int index, const void **ofdata)
+{
+	struct device_node *dn;
+	const struct of_device_id *match;
+
+	dn = of_find_matching_node_and_match(NULL, matches, &match);
+	if (!dn)
+		return ERR_PTR(-EINVAL);
+
+	if (ofdata)
+		*ofdata = match->data;
+
+	return brcmstb_ioremap_node(dn, index);
+}
+
+static int brcmstb_pm_init(void)
+{
+	struct device_node *dn;
+	void __iomem *base;
+	int i;
+
+	/* AON ctrl registers */
+	base = brcmstb_ioremap_match(aon_ctrl_dt_ids, 0, NULL);
+	if (IS_ERR(base)) {
+		pr_err("error mapping AON_CTRL\n");
+		goto aon_err;
+	}
+	ctrl.aon_ctrl_base = base;
+
+	/* AON SRAM registers */
+	base = brcmstb_ioremap_match(aon_ctrl_dt_ids, 1, NULL);
+	if (IS_ERR(base)) {
+		pr_err("error mapping AON_SRAM\n");
+		goto sram_err;
+	}
+	ctrl.aon_sram_base = base;
+
+	ctrl.num_memc = 0;
+	/* Map MEMC DDR PHY registers */
+	for_each_matching_node(dn, ddr_phy_dt_ids) {
+		i = ctrl.num_memc;
+		if (i >= MAX_NUM_MEMC) {
+			pr_warn("Too many MEMCs (max %d)\n", MAX_NUM_MEMC);
+			of_node_put(dn);
+			break;
+		}
+		base = brcmstb_ioremap_node(dn, 0);
+		if (IS_ERR(base)) {
+			of_node_put(dn);
+			goto ddr_err;
+		}
+
+		ctrl.memcs[i].ddr_phy_base = base;
+		ctrl.num_memc++;
+	}
+
+	/* MEMC ARB registers */
+	base = brcmstb_ioremap_match(arb_dt_ids, 0, NULL);
+	if (IS_ERR(base)) {
+		pr_err("error mapping MEMC ARB\n");
+		goto ddr_err;
+	}
+	ctrl.memcs[0].arb_base = base;
+
+	/* Timer registers */
+	base = brcmstb_ioremap_match(timers_ids, 0, NULL);
+	if (IS_ERR(base)) {
+		pr_err("error mapping timers\n");
+		goto tmr_err;
+	}
+	ctrl.timers_base = base;
+
+	/* s3 cold boot aka s5 */
+	pm_power_off = brcmstb_pm_s5;
+
+	suspend_set_ops(&brcmstb_pm_ops);
+
+	return 0;
+
+tmr_err:
+	iounmap(ctrl.memcs[0].arb_base);
+ddr_err:
+	for (i = 0; i < ctrl.num_memc; i++)
+		iounmap(ctrl.memcs[i].ddr_phy_base);
+
+	iounmap(ctrl.aon_sram_base);
+sram_err:
+	iounmap(ctrl.aon_ctrl_base);
+aon_err:
+	return PTR_ERR(base);
+}
+arch_initcall(brcmstb_pm_init);
diff --git a/drivers/soc/bcm/brcmstb/pm/pm.h b/drivers/soc/bcm/brcmstb/pm/pm.h
new file mode 100644
index 000000000..17f7a06a7
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/pm/pm.h
@@ -0,0 +1,81 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Definitions for Broadcom STB power management / Always ON (AON) block
+ *
+ * Copyright © 2016-2017 Broadcom
+ */
+
+#ifndef __BRCMSTB_PM_H__
+#define __BRCMSTB_PM_H__
+
+#define AON_CTRL_RESET_CTRL		0x00
+#define AON_CTRL_PM_CTRL		0x04
+#define AON_CTRL_PM_STATUS		0x08
+#define AON_CTRL_PM_CPU_WAIT_COUNT	0x10
+#define AON_CTRL_PM_INITIATE		0x88
+#define AON_CTRL_HOST_MISC_CMDS		0x8c
+#define AON_CTRL_SYSTEM_DATA_RAM_OFS	0x200
+
+/* MIPS PM constants */
+/* MEMC0 offsets */
+#define DDR40_PHY_CONTROL_REGS_0_PLL_STATUS	0x10
+#define DDR40_PHY_CONTROL_REGS_0_STANDBY_CTRL	0xa4
+
+/* TIMER offsets */
+#define TIMER_TIMER1_CTRL		0x0c
+#define TIMER_TIMER1_STAT		0x1c
+
+/* TIMER defines */
+#define RESET_TIMER			0x0
+#define START_TIMER			0xbfffffff
+#define TIMER_MASK			0x3fffffff
+
+/* PM_CTRL bitfield (Method #0) */
+#define PM_FAST_PWRDOWN			(1 << 6)
+#define PM_WARM_BOOT			(1 << 5)
+#define PM_DEEP_STANDBY			(1 << 4)
+#define PM_CPU_PWR			(1 << 3)
+#define PM_USE_CPU_RDY			(1 << 2)
+#define PM_PLL_PWRDOWN			(1 << 1)
+#define PM_PWR_DOWN			(1 << 0)
+
+/* PM_CTRL bitfield (Method #1) */
+#define PM_DPHY_STANDBY_CLEAR		(1 << 20)
+#define PM_MIN_S3_WIDTH_TIMER_BYPASS	(1 << 7)
+
+#define PM_S2_COMMAND	(PM_PLL_PWRDOWN | PM_USE_CPU_RDY | PM_PWR_DOWN)
+
+/* Method 0 bitmasks */
+#define PM_COLD_CONFIG	(PM_PLL_PWRDOWN | PM_DEEP_STANDBY)
+#define PM_WARM_CONFIG	(PM_COLD_CONFIG | PM_USE_CPU_RDY | PM_WARM_BOOT)
+
+/* Method 1 bitmask */
+#define M1_PM_WARM_CONFIG (PM_DPHY_STANDBY_CLEAR | \
+			   PM_MIN_S3_WIDTH_TIMER_BYPASS | \
+			   PM_WARM_BOOT | PM_DEEP_STANDBY | \
+			   PM_PLL_PWRDOWN | PM_PWR_DOWN)
+
+#define M1_PM_COLD_CONFIG (PM_DPHY_STANDBY_CLEAR | \
+			   PM_MIN_S3_WIDTH_TIMER_BYPASS | \
+			   PM_DEEP_STANDBY | \
+			   PM_PLL_PWRDOWN | PM_PWR_DOWN)
+
+#ifndef __ASSEMBLER__
+
+#ifndef CONFIG_MIPS
+extern const unsigned long brcmstb_pm_do_s2_sz;
+extern asmlinkage int brcmstb_pm_do_s2(void __iomem *aon_ctrl_base,
+		void __iomem *ddr_phy_pll_status);
+#else
+/* s2 asm */
+extern asmlinkage int brcm_pm_do_s2(u32 *s2_params);
+
+/* s3 asm */
+extern asmlinkage int brcm_pm_do_s3(void __iomem *aon_ctrl_base,
+		int dcache_linesz);
+extern int s3_reentry;
+#endif /* CONFIG_MIPS */
+
+#endif 
+
+#endif /* __BRCMSTB_PM_H__ */
diff --git a/drivers/soc/bcm/brcmstb/pm/s2-mips.S b/drivers/soc/bcm/brcmstb/pm/s2-mips.S
new file mode 100644
index 000000000..2a26a94eb
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/pm/s2-mips.S
@@ -0,0 +1,192 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2016 Broadcom Corporation
+ */
+
+#include <asm/asm.h>
+#include <asm/regdef.h>
+#include <asm/mipsregs.h>
+#include <asm/stackframe.h>
+
+#include "pm.h"
+
+	.text
+	.set	noreorder
+	.align	5
+
+/*
+ * a0: u32 params array
+ */
+LEAF(brcm_pm_do_s2)
+
+	subu	sp, 64
+	sw	ra, 0(sp)
+	sw	s0, 4(sp)
+	sw	s1, 8(sp)
+	sw	s2, 12(sp)
+	sw	s3, 16(sp)
+	sw	s4, 20(sp)
+	sw	s5, 24(sp)
+	sw	s6, 28(sp)
+	sw	s7, 32(sp)
+
+	/*
+	 * Dereference the params array
+	 * s0: AON_CTRL base register
+	 * s1: DDR_PHY base register
+	 * s2: TIMERS base register
+	 * s3: I-Cache line size
+	 * s4: Restart vector address
+	 * s5: Restart vector size
+	 */
+	move	t0, a0
+
+	lw	s0, 0(t0)
+	lw	s1, 4(t0)
+	lw	s2, 8(t0)
+	lw	s3, 12(t0)
+	lw	s4, 16(t0)
+	lw	s5, 20(t0)
+
+	/* Lock this asm section into the I-cache */
+	addiu	t1, s3, -1
+	not	t1
+
+	la	t0, brcm_pm_do_s2
+	and	t0, t1
+
+	la	t2, asm_end
+	and	t2, t1
+
+1:	cache	0x1c, 0(t0)
+	bne	t0, t2, 1b
+	addu	t0, s3
+
+	/* Lock the interrupt vector into the I-cache */
+	move	t0, zero
+
+2:	move	t1, s4
+	cache 	0x1c, 0(t1)
+	addu	t1, s3
+	addu	t0, s3
+	ble	t0, s5, 2b
+	nop
+
+	sync
+
+	/* Power down request */
+	li	t0, PM_S2_COMMAND
+	sw	zero, AON_CTRL_PM_CTRL(s0)
+	lw	zero, AON_CTRL_PM_CTRL(s0)
+	sw	t0, AON_CTRL_PM_CTRL(s0)
+	lw	t0, AON_CTRL_PM_CTRL(s0)
+
+	/* Enable CP0 interrupt 2 and wait for interrupt */
+	mfc0	t0, CP0_STATUS
+	/* Save cp0 sr for restoring later */
+	move	s6, t0
+
+	li	t1, ~(ST0_IM | ST0_IE)
+	and	t0, t1
+	ori	t0, STATUSF_IP2
+	mtc0	t0, CP0_STATUS
+	nop
+	nop
+	nop
+	ori	t0, ST0_IE
+	mtc0	t0, CP0_STATUS
+
+	/* Wait for interrupt */
+	wait
+	nop
+
+	/* Wait for memc0 */
+1:	lw	t0, DDR40_PHY_CONTROL_REGS_0_PLL_STATUS(s1)
+	andi	t0, 1
+	beqz	t0, 1b
+	nop
+
+	/* 1ms delay needed for stable recovery */
+	/* Use TIMER1 to count 1 ms */
+	li	t0, RESET_TIMER
+	sw	t0, TIMER_TIMER1_CTRL(s2)
+	lw	t0, TIMER_TIMER1_CTRL(s2)
+
+	li	t0, START_TIMER
+	sw	t0, TIMER_TIMER1_CTRL(s2)
+	lw	t0, TIMER_TIMER1_CTRL(s2)
+
+	/* Prepare delay */
+	li	t0, TIMER_MASK
+	lw	t1, TIMER_TIMER1_STAT(s2)
+	and	t1, t0
+	/* 1ms delay */
+	addi	t1, 27000
+
+	/* Wait for the timer value to exceed t1 */
+1:	lw	t0, TIMER_TIMER1_STAT(s2)
+	sgtu	t2, t1, t0
+	bnez	t2, 1b
+	nop
+
+	/* Power back up */
+	li	t1, 1
+	sw	t1, AON_CTRL_HOST_MISC_CMDS(s0)
+	lw	t1, AON_CTRL_HOST_MISC_CMDS(s0)
+
+	sw	zero, AON_CTRL_PM_CTRL(s0)
+	lw	zero, AON_CTRL_PM_CTRL(s0)
+
+	/* Unlock I-cache */
+	addiu	t1, s3, -1
+	not	t1
+
+	la	t0, brcm_pm_do_s2
+	and 	t0, t1
+
+	la	t2, asm_end
+	and	t2, t1
+
+1:	cache	0x00, 0(t0)
+	bne	t0, t2, 1b
+	addu	t0, s3
+
+	/* Unlock interrupt vector */
+	move	t0, zero
+
+2:	move	t1, s4
+	cache 	0x00, 0(t1)
+	addu	t1, s3
+	addu	t0, s3
+	ble	t0, s5, 2b
+	nop
+
+	/* Restore cp0 sr */
+	sync
+	nop
+	mtc0	s6, CP0_STATUS
+	nop
+
+	/* Set return value to success */
+	li	v0, 0
+
+	/* Return to caller */
+	lw	s7, 32(sp)
+	lw	s6, 28(sp)
+	lw	s5, 24(sp)
+	lw	s4, 20(sp)
+	lw	s3, 16(sp)
+	lw	s2, 12(sp)
+	lw	s1, 8(sp)
+	lw	s0, 4(sp)
+	lw	ra, 0(sp)
+	addiu	sp, 64
+
+	jr ra
+	nop
+END(brcm_pm_do_s2)
+
+	.globl asm_end
+asm_end:
+	nop
+
diff --git a/drivers/soc/bcm/brcmstb/pm/s3-mips.S b/drivers/soc/bcm/brcmstb/pm/s3-mips.S
new file mode 100644
index 000000000..ecfcfd34c
--- /dev/null
+++ b/drivers/soc/bcm/brcmstb/pm/s3-mips.S
@@ -0,0 +1,138 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2016 Broadcom Corporation
+ */
+
+#include <asm/asm.h>
+#include <asm/regdef.h>
+#include <asm/mipsregs.h>
+#include <asm/bmips.h>
+
+#include "pm.h"
+
+	.text
+	.set		noreorder
+	.align		5
+	.global		s3_reentry
+
+/*
+ * a0: AON_CTRL base register
+ * a1: D-Cache line size
+ */
+LEAF(brcm_pm_do_s3)
+
+	/* Get the address of s3_context */
+	la	t0, gp_regs
+	sw	ra, 0(t0)
+	sw	s0, 4(t0)
+	sw	s1, 8(t0)
+	sw	s2, 12(t0)
+	sw	s3, 16(t0)
+	sw	s4, 20(t0)
+	sw	s5, 24(t0)
+	sw	s6, 28(t0)
+	sw	s7, 32(t0)
+	sw	gp, 36(t0)
+	sw	sp, 40(t0)
+	sw	fp, 44(t0)
+
+	/* Save CP0 Status */
+	mfc0	t1, CP0_STATUS
+	sw	t1, 48(t0)
+
+	/* Write-back gp registers - cache will be gone */
+	addiu	t1, a1, -1
+	not	t1
+	and	t0, t1
+
+	/* Flush at least 64 bytes */
+	addiu	t2, t0, 64
+	and	t2, t1
+
+1:	cache	0x17, 0(t0)
+	bne	t0, t2, 1b
+	addu	t0, a1
+
+	/* Drop to deep standby */
+	li	t1, PM_WARM_CONFIG
+	sw	zero, AON_CTRL_PM_CTRL(a0)
+	lw	zero, AON_CTRL_PM_CTRL(a0)
+	sw	t1, AON_CTRL_PM_CTRL(a0)
+	lw	t1, AON_CTRL_PM_CTRL(a0)
+
+	li	t1, (PM_WARM_CONFIG | PM_PWR_DOWN)
+	sw	t1, AON_CTRL_PM_CTRL(a0)
+	lw	t1, AON_CTRL_PM_CTRL(a0)
+
+	/* Enable CP0 interrupt 2 and wait for interrupt */
+	mfc0	t0, CP0_STATUS
+
+	li	t1, ~(ST0_IM | ST0_IE)
+	and	t0, t1
+	ori	t0, STATUSF_IP2
+	mtc0	t0, CP0_STATUS
+	nop
+	nop
+	nop
+	ori	t0, ST0_IE
+	mtc0	t0, CP0_STATUS
+
+        /* Wait for interrupt */
+        wait
+        nop
+
+s3_reentry:
+
+	/* Clear call/return stack */
+	li	t0, (0x06 << 16)
+	mtc0	t0, $22, 2
+	ssnop
+	ssnop
+	ssnop
+
+	/* Clear jump target buffer */
+	li	t0, (0x04 << 16)
+	mtc0	t0, $22, 2
+	ssnop
+	ssnop
+	ssnop
+
+	sync
+	nop
+
+	/* Setup mmu defaults */
+	mtc0	zero, CP0_WIRED
+	mtc0	zero, CP0_ENTRYHI
+	li	k0, PM_DEFAULT_MASK
+	mtc0	k0, CP0_PAGEMASK
+
+	li	sp, BMIPS_WARM_RESTART_VEC
+	la	k0, plat_wired_tlb_setup
+	jalr	k0
+	nop
+
+	/* Restore general purpose registers */
+	la	t0, gp_regs
+	lw	fp, 44(t0)
+	lw	sp, 40(t0)
+	lw	gp, 36(t0)
+	lw	s7, 32(t0)
+	lw	s6, 28(t0)
+	lw	s5, 24(t0)
+	lw	s4, 20(t0)
+	lw	s3, 16(t0)
+	lw	s2, 12(t0)
+	lw	s1, 8(t0)
+	lw	s0, 4(t0)
+	lw	ra, 0(t0)
+
+	/* Restore CP0 status */
+	lw	t1, 48(t0)
+	mtc0	t1, CP0_STATUS
+
+	/* Return to caller */
+	li	v0, 0
+	jr      ra
+	nop
+
+END(brcm_pm_do_s3)