soc: tegra: introduce reboot2payload

Some Tegra124 and Tegra21x boards have open-source bootloaders injected via RCM rather than onboard. These boards need special reboot handling, which we call `Reboot2Payload`. Co-developed-by: Thomas Makin <halorocker89@gmail.com> Co-developed-by: Ezekiel Bethel <zek@9net.org>
soc: tegra: add support for brom cmds
2026-10-13 17:02:57 -05:00 · 2026-10-13 17:02:56 -05:00 · 2026-10-13 16:59:29 -05:00
5 changed files with 923 additions and 16 deletions
@@ -195,3 +195,13 @@ config SOC_TEGRA_CBB
 	  Support for handling error from Tegra Control Backbone(CBB).
 	  This driver handles the errors from CBB and prints debug
 	  information about the failed transactions.
+
+config SOC_TEGRA_R2P
+	tristate "Tegra Reboot 2 Payload support"
+	depends on ARCH_TEGRA_124_SOC || ARCH_TEGRA_210_SOC
+	default n
+	help
+	  This enables a special reboot path which allows the board to reboot
+	  to a specified bootloader other than the one in boot storage.
+
+	  If in doubt, say N.
@@ -8,3 +8,4 @@ obj-$(CONFIG_SOC_TEGRA_PMC) += pmc.o
 obj-$(CONFIG_SOC_TEGRA20_VOLTAGE_COUPLER) += regulators-tegra20.o
 obj-$(CONFIG_SOC_TEGRA30_VOLTAGE_COUPLER) += regulators-tegra30.o
 obj-$(CONFIG_ARCH_TEGRA_186_SOC) += ari-tegra186.o
+obj-$(CONFIG_SOC_TEGRA_R2P) += r2p-tegra.o
@@ -35,6 +35,7 @@
 #include <linux/of.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
+#include <linux/panic_notifier.h>
 #include <linux/pinctrl/pinconf-generic.h>
 #include <linux/pinctrl/pinconf.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -111,6 +112,8 @@

 #define PMC_PWR_DET_VALUE		0xe4

+#define KERNEL_PANIC_MAGIC		0x4E415054
+
 #define PMC_USB_DEBOUNCE_DEL		0xec
 #define PMC_USB_AO			0xf0

@@ -177,6 +180,26 @@
 #define PMC_UTMIP_PAD_CFG0		0x4c0
 #define PMC_UTMIP_UHSIC_SLEEP_CFG1	0x4d0
 #define PMC_UTMIP_SLEEPWALK_P3		0x4e0
+
+/* Scratch 250: Bootrom i2c command base */
+#define PMC_BR_COMMAND_BASE		0x908
+
+/* Bootrom comand register */
+#define PMC_REG_8bit_MASK			0xFF
+#define PMC_REG_16bit_MASK			0xFFFF
+#define PMC_BR_COMMAND_I2C_ADD_MASK		0x7F
+#define PMC_BR_COMMAND_WR_COMMANDS_MASK		0x3F
+#define PMC_BR_COMMAND_WR_COMMANDS_SHIFT	8
+#define PMC_BR_COMMAND_OPERAND_SHIFT		15
+#define PMC_BR_COMMAND_CSUM_MASK		0xFF
+#define PMC_BR_COMMAND_CSUM_SHIFT		16
+#define PMC_BR_COMMAND_PMUX_MASK		0x7
+#define PMC_BR_COMMAND_PMUX_SHIFT		24
+#define PMC_BR_COMMAND_CTRL_ID_MASK		0x7
+#define PMC_BR_COMMAND_CTRL_ID_SHIFT		27
+#define PMC_BR_COMMAND_CTRL_TYPE_SHIFT		30
+#define PMC_BR_COMMAND_RST_EN_SHIFT		31
+
 /* Tegra186 and later */
 #define WAKE_AOWAKE_CNTRL(x) (0x000 + ((x) << 2))
 #define WAKE_AOWAKE_CNTRL_LEVEL (1 << 3)
@@ -292,6 +315,29 @@ struct tegra_powergate {
 	struct reset_control *reset;
 };

+struct tegra_bootrom_block {
+	const char *name;
+	int address;
+	bool reg_8bits;
+	bool data_8bits;
+	bool i2c_controller;
+	int controller_id;
+	bool enable_reset;
+	int ncommands;
+	u32 *commands;
+};
+
+struct tegra_bootrom_commands {
+	u32 command_retry_count;
+	u32 delay_between_commands;
+	u32 wait_before_bus_clear;
+	struct tegra_bootrom_block *blocks;
+	int nblocks;
+};
+
+static struct tegra_bootrom_commands *br_rst_commands;
+static struct tegra_bootrom_commands *br_off_commands;
+
 struct tegra_io_pad_soc {
 	enum tegra_io_pad id;
 	unsigned int dpd;
@@ -378,6 +424,7 @@ struct tegra_pmc_soc {
 	bool needs_mbist_war;
 	bool has_io_pad_wren;
 	bool maybe_tz_only;
+	bool has_bootrom_command;

 	const struct tegra_io_pad_soc *io_pads;
 	unsigned int num_io_pads;
@@ -419,6 +466,7 @@ struct tegra_pmc_soc {
 	bool has_usb_sleepwalk;
 	bool supports_core_domain;
 	bool has_single_mmio_aperture;
+	bool has_panic_notifier;
 };

 /**
@@ -505,6 +553,7 @@ struct tegra_pmc {
 	unsigned long *wake_cntrl_level_map;

 	struct notifier_block reboot_notifier;
+	struct notifier_block panic_notifier;
 	struct syscore syscore;

 	/* Pending wake IRQ processing */
@@ -523,7 +572,8 @@ to_powergate(struct generic_pm_domain *domain)
 	return container_of(domain, struct tegra_powergate, genpd);
 }

-static u32 tegra_pmc_readl(struct tegra_pmc *pmc, unsigned long offset)
+// FIXME this prolly should not be public
+u32 tegra_pmc_readl(struct tegra_pmc *pmc, unsigned long offset)
 {
 	struct arm_smccc_res res;

@@ -544,6 +594,7 @@ static u32 tegra_pmc_readl(struct tegra_pmc *pmc, unsigned long offset)

 	return readl(pmc->base + offset);
 }
+EXPORT_SYMBOL(tegra_pmc_readl);

 static void tegra_pmc_writel(struct tegra_pmc *pmc, u32 value,
 			     unsigned long offset)
@@ -1310,22 +1361,10 @@ static int tegra_pmc_restart_handler(struct sys_off_data *data)
 	return NOTIFY_DONE;
 }

-static int tegra_pmc_power_off_handler(struct sys_off_data *data)
+static int tegra_pmc_panic_handler(struct notifier_block *this,
+				    unsigned long action, void *data)
 {
-	struct tegra_pmc *pmc = data->cb_data;
-
-	/*
-	 * Reboot Nexus 7 into special bootloader mode if USB cable is
-	 * connected in order to display battery status and power off.
-	 */
-	if (of_machine_is_compatible("asus,grouper") &&
-	    power_supply_is_system_supplied()) {
-		const u32 go_to_charger_mode = 0xa5a55a5a;
-
-		tegra_pmc_writel(pmc, go_to_charger_mode, PMC_SCRATCH37);
-		tegra_pmc_restart(pmc);
-	}
-
+	tegra_pmc_writel(pmc, KERNEL_PANIC_MAGIC, PMC_SCRATCH37);
 	return NOTIFY_DONE;
 }

@@ -1698,6 +1737,330 @@ static void tegra_powergate_remove_all(struct device_node *parent)
 	}
 }

+static int tegra_pmc_parse_bootrom_cmd(struct device *dev,
+				       struct device_node *np,
+				       struct tegra_bootrom_commands **br_cmds)
+{
+	struct device_node *child;
+	struct tegra_bootrom_commands *bcommands;
+	int *command_ptr;
+	struct tegra_bootrom_block *block;
+	int nblocks;
+	u32 reg, data, pval;
+	u32 *wr_commands;
+	int count, nblock, ncommands, i, data_shift;
+	int ret;
+	int sz_bcommand, sz_blocks;
+
+	nblocks = of_get_available_child_count(np);
+	if (!nblocks) {
+		dev_info(dev, "PMC: No Bootrom Command\n");
+		return -ENOENT;
+	}
+
+	count = 0;
+	for_each_available_child_of_node(np, child) {
+		ret = of_property_count_u32_elems(child,
+						  "nvidia,write-commands");
+		if (ret < 0) {
+			dev_err(dev, "PMC: Node %s does not have write-commnds\n",
+				child->full_name);
+			return -EINVAL;
+		}
+		count += ret / 2;
+	}
+
+	sz_bcommand = (sizeof(*bcommands) + 0x3) & ~0x3;
+	sz_blocks = (sizeof(*block) + 0x3) & ~0x3;
+	bcommands = devm_kzalloc(dev,  sz_bcommand + nblocks * sz_blocks +
+				 count * sizeof(u32), GFP_KERNEL);
+	if (!bcommands)
+		return -ENOMEM;
+
+	bcommands->nblocks = nblocks;
+	bcommands->blocks = (void *)bcommands + sz_bcommand;
+	command_ptr = (void *)bcommands->blocks + nblocks * sz_blocks;
+
+	of_property_read_u32(np, "nvidia,command-retries-count",
+			     &bcommands->command_retry_count);
+	of_property_read_u32(np, "nvidia,delay-between-commands-us",
+			     &bcommands->delay_between_commands);
+
+	ret = of_property_read_u32(np, "nvidia,wait-before-start-bus-clear-us",
+				   &bcommands->wait_before_bus_clear);
+	if (ret < 0)
+		of_property_read_u32(np, "nvidia,wait-start-bus-clear-us",
+				     &bcommands->wait_before_bus_clear);
+
+	nblock = 0;
+	for_each_available_child_of_node(np, child) {
+		block = &bcommands->blocks[nblock];
+		ret = of_property_read_u32(child, "reg", &pval);
+		if (ret) {
+			dev_err(dev, "PMC: Reg property missing on block %s\n",
+				child->name);
+			return ret;
+		}
+		block->address = pval;
+		of_property_read_string(child, "nvidia,command-names",
+					&block->name);
+		block->reg_8bits = !of_property_read_bool(child,
+					"nvidia,enable-16bit-register");
+		block->data_8bits = !of_property_read_bool(child,
+					"nvidia,enable-16bit-data");
+		block->i2c_controller = of_property_read_bool(child,
+					"nvidia,controller-type-i2c");
+		block->enable_reset = of_property_read_bool(child,
+					"nvidia,enable-controller-reset");
+		count = of_property_count_u32_elems(child,
+						    "nvidia,write-commands");
+		ncommands = count / 2;
+
+		block->commands = command_ptr;
+		command_ptr += ncommands;
+		wr_commands = block->commands;
+		data_shift = (block->data_8bits) ? 8 : 16;
+		for (i = 0; i < ncommands; ++i) {
+			of_property_read_u32_index(child,
+						   "nvidia,write-commands",
+						   i * 2, &reg);
+			of_property_read_u32_index(child,
+						   "nvidia,write-commands",
+						   i * 2 + 1, &data);
+
+			wr_commands[i] = (data << data_shift) | reg;
+		}
+		block->ncommands = ncommands;
+		nblock++;
+	}
+	*br_cmds = bcommands;
+
+	return 0;
+}
+
+static int tegra_pmc_read_bootrom_cmd(struct device *dev,
+		      struct tegra_bootrom_commands **br_rst_cmds,
+		      struct tegra_bootrom_commands **br_off_cmds)
+{
+	struct device_node *np = dev->of_node;
+	struct device_node *br_np, *rst_np, *off_np;
+	int ret;
+
+	*br_rst_cmds = NULL;
+	*br_off_cmds = NULL;
+
+	br_np = of_find_node_by_name(np, "bootrom-commands");
+	if (!br_np) {
+		dev_info(dev, "PMC: Bootrom commmands not found\n");
+		return -ENOENT;
+	}
+
+	rst_np = of_find_node_by_name(br_np, "reset-commands");
+	if (!rst_np) {
+		dev_info(dev, "PMC: bootrom-commands used for reset\n");
+		rst_np = br_np;
+	}
+
+	ret = tegra_pmc_parse_bootrom_cmd(dev, rst_np, br_rst_cmds);
+	if (ret < 0)
+		return ret;
+
+	if (rst_np == br_np)
+		return 0;
+
+	off_np = of_find_node_by_name(br_np, "power-off-commands");
+	if (!off_np)
+		return 0;
+	ret = tegra_pmc_parse_bootrom_cmd(dev, off_np, br_off_cmds);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int tegra_pmc_configure_bootrom_scratch(struct tegra_pmc *pmc,
+		struct tegra_bootrom_commands *br_commands,
+		struct tegra_br_cmd_cfg *bcfg, u32 bcfg_size)
+{
+	struct tegra_bootrom_block *block;
+	int i, j, k;
+	u32 cmd, tmp_cmd;
+	int reg_offset = 1;
+	int bcfg_idx = 0;
+	u32 reg_data_mask, edit_data_mask;
+	int cmd_pw;
+	u32 block_add, block_val, csum;
+
+	for (i = 0; i < br_commands->nblocks; ++i) {
+		block = &br_commands->blocks[i];
+
+		cmd = block->address & PMC_BR_COMMAND_I2C_ADD_MASK;
+		cmd |= block->ncommands << PMC_BR_COMMAND_WR_COMMANDS_SHIFT;
+		if (!block->reg_8bits || !block->data_8bits)
+			cmd |= BIT(PMC_BR_COMMAND_OPERAND_SHIFT);
+
+		if (block->enable_reset)
+			cmd |= BIT(PMC_BR_COMMAND_RST_EN_SHIFT);
+
+		cmd |= (block->controller_id & PMC_BR_COMMAND_CTRL_ID_MASK) <<
+					PMC_BR_COMMAND_CTRL_ID_SHIFT;
+
+		/* Checksum will be added after parsing from reg/data */
+		tegra_pmc_writel(pmc, cmd, PMC_BR_COMMAND_BASE + (reg_offset * 4));
+		block_add = reg_offset * 4;
+		block_val = cmd;
+		reg_offset++;
+
+		cmd_pw = (block->reg_8bits && block->data_8bits) ? 2 : 1;
+		reg_data_mask = (cmd_pw == 2) ? 0xFFFF : 0xFFFFFFFFUL;
+		csum = 0;
+
+		for (j = 0; j < block->ncommands; j++) {
+			tmp_cmd = block->commands[j] & reg_data_mask;
+			if (bcfg_idx < bcfg_size &&
+			    bcfg[bcfg_idx].dev == i &&
+			    bcfg[bcfg_idx].idx == j) {
+			    	edit_data_mask = (cmd_pw == 2) ?
+						 0xFF00UL : 0xFFFF0000UL;
+				tmp_cmd &= ~edit_data_mask;
+				tmp_cmd |= (bcfg[bcfg_idx].val <<
+					   (cmd_pw == 2 ? 8 : 16)) & edit_data_mask;
+				bcfg_idx++;
+			}
+			cmd = tmp_cmd;
+			if (cmd_pw == 2) {
+				j++;
+				if (j == block->ncommands)
+					goto reg_update;
+
+				tmp_cmd = (block->commands[j] & reg_data_mask) << 16;
+				if (bcfg_idx < bcfg_size &&
+				    bcfg[bcfg_idx].dev == i &&
+				    bcfg[bcfg_idx].idx == j) {
+					edit_data_mask = 0xFF000000UL;
+					tmp_cmd &= ~edit_data_mask;
+					tmp_cmd |= (bcfg[bcfg_idx].val << 24) &
+						   edit_data_mask;
+					bcfg_idx++;
+				}
+				cmd |= tmp_cmd;
+			}
+reg_update:
+			tegra_pmc_writel(pmc, cmd, PMC_BR_COMMAND_BASE + (reg_offset * 4));
+			for (k = 0; k < 4; ++k)
+				csum += (cmd >> (k * 8)) & 0xFF;
+			reg_offset++;
+		}
+		for (k = 0; k < 4; ++k)
+			csum += (block_val >> (k * 8)) & 0xFF;
+		csum = 0x100 - csum;
+		block_val = (block_val & 0xFF00FFFF) | ((csum & 0xFF) << 16);
+		tegra_pmc_writel(pmc, block_val, PMC_BR_COMMAND_BASE + block_add);
+	}
+
+	cmd = br_commands->command_retry_count & 0x7;
+	cmd |= (br_commands->delay_between_commands & 0x1F) << 3;
+	cmd |= (br_commands->nblocks & 0x7) << 8;
+	cmd |= (br_commands->wait_before_bus_clear & 0x1F) << 11;
+	tegra_pmc_writel(pmc, cmd, PMC_BR_COMMAND_BASE);
+
+	return 0;
+}
+
+/**
+ * tegra_pmc_edit_bootrom_scratch_reset() - configure brom reset commands
+ * @pmc: power management controller
+ * @bcfg: bootrom command config structure
+ * @bcfg_size: size of bcfg
+ *
+ * Returns: 0 on success or a negative error code on failure.
+ */
+int tegra_pmc_edit_bootrom_scratch_reset(struct tegra_pmc *pmc,
+	struct tegra_br_cmd_cfg *bcfg, u32 bcfg_size)
+{
+	if (br_rst_commands) {
+		tegra_pmc_configure_bootrom_scratch(pmc, br_rst_commands,
+						    bcfg, bcfg_size);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL(tegra_pmc_edit_bootrom_scratch_reset);
+
+static int tegra_pmc_init_bootrom_power_off_cmd(struct tegra_pmc *pmc)
+{
+	int ret;
+
+	if (!br_off_commands) {
+		dev_info(pmc->dev, "PMC: Power Off Command not available\n");
+		return 0;
+	}
+
+	ret = tegra_pmc_configure_bootrom_scratch(pmc, br_off_commands, NULL, 0);
+	if (ret < 0) {
+		dev_err(pmc->dev, "PMC: Failed to configure power-off command: %d\n",
+			ret);
+		return ret;
+	}
+
+	dev_info(pmc->dev, "PMC: Successfully configure power-off commands\n");
+
+	return 0;
+}
+
+static int tegra_pmc_init_bootrom_cmds(struct tegra_pmc *pmc, struct device *dev)
+{
+	int ret;
+
+	ret = tegra_pmc_read_bootrom_cmd(dev, &br_rst_commands,
+					 &br_off_commands);
+	if (ret < 0) {
+		if (ret == -ENOENT)
+			ret = 0;
+		else
+			dev_info(dev,
+				 "PMC: Failed to read bootrom cmd: %d\n", ret);
+
+		return ret;
+	}
+
+	ret = tegra_pmc_configure_bootrom_scratch(pmc, br_rst_commands, NULL, 0);
+	if (ret < 0) {
+		dev_info(dev, "PMC: Failed to write bootrom scratch register: %d\n",
+			 ret);
+		return ret;
+	}
+
+	dev_info(dev, "PMC: Successfully configure bootrom reset commands\n");
+
+	return 0;
+}
+
+static int tegra_pmc_power_off_handler(struct sys_off_data *data)
+{
+	struct tegra_pmc *pmc = data->cb_data;
+
+	if (br_off_commands) {
+		tegra_pmc_init_bootrom_power_off_cmd(pmc);
+		tegra_pmc_restart(pmc);
+	}
+
+	/*
+	 * Reboot Nexus 7 into special bootloader mode if USB cable is
+	 * connected in order to display battery status and power off.
+	 */
+	if (of_machine_is_compatible("asus,grouper") &&
+	    power_supply_is_system_supplied()) {
+		const u32 go_to_charger_mode = 0xa5a55a5a;
+
+		tegra_pmc_writel(pmc, go_to_charger_mode, PMC_SCRATCH37);
+		tegra_pmc_restart(pmc);
+	}
+
+	return NOTIFY_DONE;
+}
+
 static const struct tegra_io_pad_soc *
 tegra_io_pad_find(struct tegra_pmc *pmc, enum tegra_io_pad id)
 {
@@ -3182,6 +3545,9 @@ static int tegra_pmc_probe(struct platform_device *pdev)
 		return dev_err_probe(&pdev->dev, PTR_ERR(pmc->clk),
 				     "failed to get pclk\n");

+	if (pmc->soc->has_bootrom_command)
+		tegra_pmc_init_bootrom_cmds(pmc, &pdev->dev);
+
 	/*
 	 * PMC should be last resort for restarting since it soft-resets
 	 * CPU without resetting everything else.
@@ -3199,6 +3565,20 @@ static int tegra_pmc_probe(struct platform_device *pdev)
 		}
 	}

+	if (pmc->soc->has_panic_notifier) {
+		pmc->panic_notifier.notifier_call = tegra_pmc_panic_handler;
+		pmc->panic_notifier.priority = INT_MAX-1;
+
+		err = atomic_notifier_chain_register(&panic_notifier_list,
+					&pmc->panic_notifier);
+		if (err) {
+			dev_err(&pdev->dev,
+				"unable to register panic notifier, %d\n",
+				err);
+			return err;
+		}
+	}
+
 	err = devm_register_sys_off_handler(&pdev->dev,
 					    SYS_OFF_MODE_RESTART,
 					    SYS_OFF_PRIO_LOW,
@@ -3566,6 +3946,7 @@ static const struct tegra_pmc_soc tegra20_pmc_soc = {
 	.has_blink_output = true,
 	.has_usb_sleepwalk = true,
 	.has_single_mmio_aperture = true,
+	.has_bootrom_command = false,
 };

 static const char * const tegra30_powergates[] = {
@@ -3628,6 +4009,7 @@ static const struct tegra_pmc_soc tegra30_pmc_soc = {
 	.has_blink_output = true,
 	.has_usb_sleepwalk = true,
 	.has_single_mmio_aperture = true,
+	.has_bootrom_command = false,
 };

 static const char * const tegra114_powergates[] = {
@@ -3686,6 +4068,7 @@ static const struct tegra_pmc_soc tegra114_pmc_soc = {
 	.has_blink_output = true,
 	.has_usb_sleepwalk = true,
 	.has_single_mmio_aperture = true,
+	.has_bootrom_command = false,
 };

 static const char * const tegra124_powergates[] = {
@@ -3838,6 +4221,7 @@ static const struct tegra_pmc_soc tegra124_pmc_soc = {
 	.has_blink_output = true,
 	.has_usb_sleepwalk = true,
 	.has_single_mmio_aperture = true,
+	.has_bootrom_command = false,
 };

 static const char * const tegra210_powergates[] = {
@@ -4018,6 +4402,8 @@ static const struct tegra_pmc_soc tegra210_pmc_soc = {
 	.has_blink_output = true,
 	.has_usb_sleepwalk = true,
 	.has_single_mmio_aperture = true,
+	.has_panic_notifier = true,
+	.has_bootrom_command = true,
 };

 static const char * const tegra210b01_powergates[] = {
@@ -4079,6 +4465,8 @@ static const struct tegra_pmc_soc tegra210b01_pmc_soc = {
 	.has_blink_output = true,
 	.has_usb_sleepwalk = true,
 	.has_single_mmio_aperture = true,
+	.has_panic_notifier = true,
+	.has_bootrom_command = true,
 };

 static const struct tegra_io_pad_soc tegra186_io_pads[] = {
@@ -0,0 +1,485 @@
+/*
+ * Copyright (c) 2019, Ezekiel Bethel <zek@9net.org>
+ * Copyright (c) 2021-2022, CTCaer <ctcaer@gmail.com>
+ * Copyright (c) 2026 Thomas Makin <halorocker89@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/firmware.h>
+#include <linux/notifier.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/moduleparam.h>
+#include <soc/tegra/pmc.h>
+
+#define TEGRA_SIP_R2P_COPY_TO_IRAM 0xC2FFFE02u
+#define  R2P_WRITE_IRAM            (0U << 0)
+#define  R2P_READ_IRAM             (1U << 0)
+#define TEGRA_SIP_R2P_DO_REBOOT    0xC2FFFE03u
+#define TEGRA_SIP_R2P_SET_BIN_CFG  0xC2FFFE04u
+
+#define IRAM_PAYLOAD_BASE    0x40010000u
+#define IRAM_CHUNK_SIZE      0x4000
+
+#define KERNEL_PANIC_MAGIC	0x4E415054
+#define RTC_REBOOT_REASON_MAGIC	0x77
+#define PMC_SCRATCH37		0x130
+
+/* hekate/Nyx */
+#define BOOT_CFG_AUTOBOOT_EN BIT(0)
+#define BOOT_CFG_FROM_ID     BIT(2)
+
+#define EXTRA_CFG_NYX_UMS    BIT(5)
+#define EXTRA_CFG_NYX_RELOAD BIT(6)
+
+enum {
+	NYX_UMS_SD_CARD = 0,
+	NYX_UMS_EMMC_BOOT0,
+	NYX_UMS_EMMC_BOOT1,
+	NYX_UMS_EMMC_GPP,
+	NYX_UMS_EMUMMC_BOOT0,
+	NYX_UMS_EMUMMC_BOOT1,
+	NYX_UMS_EMUMMC_GPP
+};
+
+enum {
+	REBOOT_REASON_NOP   = 0, /* Use [config]. */
+	REBOOT_REASON_SELF  = 1, /* Use autoboot_idx/autoboot_list. */
+	REBOOT_REASON_MENU  = 2, /* Force menu. */
+	REBOOT_REASON_UMS   = 3, /* Force selected UMS partition. */
+	REBOOT_REASON_REC   = 4, /* Set PMC_SCRATCH0_MODE_RECOVERY and reboot to self. */
+	REBOOT_REASON_PANIC = 5  /* Inform bootloader that panic occured if T210B01. */
+};
+
+struct __attribute__((__packed__)) hekate_boot_cfg
+{
+	u8 boot_cfg;
+	u8 autoboot;
+	u8 autoboot_list;
+	u8 extra_cfg;
+	union {
+		struct {
+			char id[8];
+			char emummc_path[0x78];
+		};
+		u8 ums;
+		u8 xt_str[0x80];
+	};
+};
+
+struct rtc_rr_dec
+{
+	u16 reason:4;
+	u16 autoboot_idx:4;
+	u16 autoboot_list:1;
+	u16 ums_idx:3;
+};
+
+struct rtc_rr_enc
+{
+	u16 val1:6; /* 6-bit reg. */
+	u16 val2:6; /* 6-bit reg. */
+};
+
+struct rtc_reboot_reason
+{
+	union {
+		struct rtc_rr_dec dec;
+		struct rtc_rr_enc enc;
+	};
+};
+
+struct tegra_r2p_data
+{
+	struct device *dev;
+	struct tegra_pmc *pmc;
+	struct notifier_block reboot_notifier;
+
+	char action[16];
+	int  param1;
+	int  param2;
+	char entry_id[8];
+};
+
+static int enabled = 0;
+static int param1 = 0;
+static int param2 = 0;
+static char* action = NULL;
+static char* entry_id = NULL;
+
+module_param(enabled, int, 0664);
+module_param(action, charp, 0664);
+module_param(param1, int, 0664);
+module_param(param2, int, 0664);
+module_param(entry_id, charp, 0664);
+
+MODULE_PARM_DESC(enabled, "Enable Reboot 2 Payload function");
+MODULE_PARM_DESC(action, "Reboot action to take");
+MODULE_PARM_DESC(param1, "Autoboot entry or UMS device index");
+MODULE_PARM_DESC(param2, "Autooboot entry is ini list");
+MODULE_PARM_DESC(entry_id, "Autoboot entry id");
+
+struct platform_device *r2p_device = NULL;
+
+static u32 tegra_r2p_set_cfg(struct hekate_boot_cfg *hekate_bcfg)
+{
+	struct arm_smccc_res res;
+	u64 hekate_boot_smc_cfg[4];
+
+	memcpy(hekate_boot_smc_cfg, hekate_bcfg, sizeof(hekate_boot_smc_cfg));
+	arm_smccc_smc(TEGRA_SIP_R2P_SET_BIN_CFG, hekate_boot_smc_cfg[0], hekate_boot_smc_cfg[1],
+	 		hekate_boot_smc_cfg[2], hekate_boot_smc_cfg[3], 0, 0, 0, &res);
+	return (u32)res.a1;
+}
+
+static void tegra_r2p_setup(struct tegra_r2p_data *state, const char *cmd)
+{
+	struct rtc_reboot_reason rr = {};
+	struct hekate_boot_cfg hekate_bcfg = {};
+	struct tegra_pmc *pmc = state->pmc;
+
+	uint32_t reboot_reason = REBOOT_REASON_NOP;
+	bool tegra210b01 = of_device_is_compatible(state->dev->of_node, "nvidia,tegra210b01-r2p");
+
+	u32 panic_code = tegra_pmc_readl(pmc, PMC_SCRATCH37);
+	bool panic_occurred = (panic_code == KERNEL_PANIC_MAGIC);
+
+	if (r2p_device == NULL) {
+		dev_err(&r2p_device->dev, "r2p device is null\n");
+		return;
+	}
+
+	/* Linux reboot reason */
+	if (cmd) {
+		if (!strcmp(cmd, "recovery")) {
+			reboot_reason = REBOOT_REASON_REC;
+		} else if (!strcmp(cmd, "bootloader")) {
+			reboot_reason = REBOOT_REASON_MENU;
+		} else if (!strcmp(cmd, "forced-recovery")) { /* RCM */
+			if (tegra210b01) return;
+		} else {
+			dev_err(&r2p_device->dev,
+				"Command '%s' not recognized\n", cmd);
+		}
+	}
+
+	/* Missing reboot reason or not matching, use r2p action. */
+	if (reboot_reason == REBOOT_REASON_NOP && strlen(state->action)) {
+		if (!strcmp(state->action, "self") ||
+		    !strcmp(state->action, "via-payload")) { /* Deprecated */
+			reboot_reason = REBOOT_REASON_SELF;
+		} else if (!strcmp(state->action, "bootloader")) {
+			reboot_reason = REBOOT_REASON_MENU;
+		} else if (!strcmp(state->action, "ums")) {
+			reboot_reason = REBOOT_REASON_UMS;
+		} else if (!strcmp(state->action, "normal")) {
+			reboot_reason = REBOOT_REASON_NOP;
+		} else {
+			dev_err(&r2p_device->dev,
+				"Action '%s' not recognized\n", state->action);
+		}
+	}
+
+	/* If T210B01 and panic happened override reboot reason */
+	if (tegra210b01 && panic_occurred) {
+		reboot_reason = REBOOT_REASON_PANIC;
+	}
+
+	/* Prepare boot config data */
+	switch (reboot_reason) {
+	case REBOOT_REASON_NOP:
+		if (tegra210b01) return;
+		break;
+	case REBOOT_REASON_REC:
+	case REBOOT_REASON_SELF:
+		hekate_bcfg.boot_cfg = BOOT_CFG_AUTOBOOT_EN;
+		if (strlen(state->entry_id) != 0) {
+			hekate_bcfg.boot_cfg |= BOOT_CFG_FROM_ID;
+			strcpy(hekate_bcfg.id, state->entry_id);
+		}
+		hekate_bcfg.autoboot = state->param1;
+		hekate_bcfg.autoboot_list = state->param2;
+
+		rr.dec.reason = reboot_reason;
+		rr.dec.autoboot_idx = state->param1;
+		rr.dec.autoboot_list = state->param2;
+		break;
+	case REBOOT_REASON_MENU:
+		hekate_bcfg.boot_cfg = BOOT_CFG_AUTOBOOT_EN;
+
+		rr.dec.reason = reboot_reason;
+		break;
+	case REBOOT_REASON_UMS:
+		hekate_bcfg.boot_cfg = BOOT_CFG_AUTOBOOT_EN;
+		hekate_bcfg.extra_cfg = EXTRA_CFG_NYX_UMS;
+		hekate_bcfg.ums = state->param1;
+
+		rr.dec.reason = reboot_reason;
+		rr.dec.ums_idx = state->param1;
+		break;
+	case REBOOT_REASON_PANIC:
+		rr.dec.reason = reboot_reason;
+		break;
+	}
+
+	/* Notify TZ to use its preloaded payload */
+	if (!tegra210b01) {
+		/* Set hekate boot config. */
+		tegra_r2p_set_cfg(&hekate_bcfg);
+	} else {
+		struct tegra_br_cmd_cfg bcfg[4] = {
+			{ 0, 0, rr.enc.val1 },
+			{ 0, 1, rr.enc.val2 },
+			{ 0, 2, RTC_REBOOT_REASON_MAGIC },
+			{ 0, 3, RTC_REBOOT_REASON_MAGIC }
+		};
+		tegra_pmc_edit_bootrom_scratch_reset(pmc, bcfg, 4);
+	}
+}
+
+static int tegra_r2p_reboot_notify(struct notifier_block *this,
+				   unsigned long action, void *data)
+{
+	struct tegra_r2p_data *state = container_of(this, struct tegra_r2p_data,
+					     reboot_notifier);
+	if (action == SYS_RESTART)
+		tegra_r2p_setup(state, data);
+
+	return NOTIFY_DONE;
+}
+
+static ssize_t action_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct tegra_r2p_data *state = dev_get_drvdata(&r2p_device->dev);
+	int res;
+
+	if (strlen(state->action))
+		res = sprintf(buf, "%s\n", state->action);
+	else
+		res = sprintf(buf, "Not-set\n");
+
+	return res;
+}
+
+static ssize_t action_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct tegra_r2p_data *state = dev_get_drvdata(&r2p_device->dev);
+
+	if (count > sizeof(state->action) - 1)
+		return -EINVAL;
+
+	strncpy(state->action, buf, sizeof(state->action) - 1);
+	state->action[sizeof(state->action) - 1] = 0;
+
+	/* Strip newline if it exists */
+	if (state->action[count - 1] == '\n')
+		state->action[count - 1] = 0;
+
+	return count;
+}
+
+static ssize_t entry_id_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct tegra_r2p_data *state = dev_get_drvdata(&r2p_device->dev);
+	int res;
+
+	if (strlen(state->entry_id))
+		res = sprintf(buf, "%s\n", state->entry_id);
+	else
+		res = sprintf(buf, "Not-set\n");
+
+	return res;
+}
+
+static ssize_t entry_id_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct tegra_r2p_data *state = dev_get_drvdata(&r2p_device->dev);
+
+	if (count > sizeof(state->entry_id) - 1)
+		return -EINVAL;
+
+	strncpy(state->entry_id, buf, sizeof(state->entry_id) - 1);
+	state->entry_id[sizeof(state->entry_id) - 1] = 0;
+
+	/* Strip newline if it exists */
+	if (state->entry_id[count - 1] == '\n')
+		state->entry_id[count - 1] = 0;
+
+	return count;
+}
+
+static ssize_t param1_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct tegra_r2p_data *state = dev_get_drvdata(&r2p_device->dev);
+
+	return sprintf(buf, "%d\n", (int)state->param1);
+}
+
+static ssize_t param1_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct tegra_r2p_data *state = dev_get_drvdata(&r2p_device->dev);
+	ssize_t res;
+	long value;
+
+	res = kstrtol(buf, 0, &value);
+	if (!res)
+		state->param1 = value;
+
+	return res ? res : count;
+}
+
+static ssize_t param2_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct tegra_r2p_data *state = dev_get_drvdata(&r2p_device->dev);
+
+	return sprintf(buf, "%d\n", (int)state->param2);
+}
+
+static ssize_t param2_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct tegra_r2p_data *state = dev_get_drvdata(&r2p_device->dev);
+	ssize_t res;
+	long value;
+
+	res = kstrtol(buf, 0, &value);
+	if (!res)
+		state->param2 = value;
+
+	return res ? res : count;
+}
+
+static DEVICE_ATTR_RW(action);
+static DEVICE_ATTR_RW(entry_id);
+static DEVICE_ATTR_RW(param1);
+static DEVICE_ATTR_RW(param2);
+
+static struct attribute *r2p_sysfs_attrs[] = {
+	&dev_attr_action.attr,
+	&dev_attr_entry_id.attr,
+	&dev_attr_param1.attr,
+	&dev_attr_param2.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(r2p_sysfs);
+
+static int tegra_r2p_driver_probe(struct platform_device *pdev)
+{
+	struct tegra_r2p_data *state;
+	struct tegra_pmc *pmc;
+	int err;
+
+	if (!enabled)
+		return 0;
+
+	r2p_device = pdev;
+
+	state = devm_kzalloc(&pdev->dev, sizeof(struct tegra_r2p_data), GFP_KERNEL);
+
+	/* Use nvidia,pmc to specify */
+	pmc = devm_tegra_pmc_get(&pdev->dev);
+	if (IS_ERR(pmc)) {
+		dev_err(&pdev->dev, "failed to get PMC\n");
+		return -EPROBE_DEFER;
+	}
+
+	/* Copy reboot action if valid */
+	if (action) {
+		strncpy(state->action, action, sizeof(state->action) - 1);
+		state->action[sizeof(state->action) - 1] = 0;
+	}
+
+	/* Copy entry ID if valid */
+	if (entry_id) {
+		strncpy(state->entry_id, entry_id, sizeof(state->entry_id) - 1);
+		state->entry_id[sizeof(state->entry_id) - 1] = 0;
+	}
+
+	/* Set reboot reason parameters. */
+	state->param1 = param1;
+	state->param2 = param2;
+
+	state->dev = &pdev->dev;
+	state->pmc = pmc;
+
+	dev_set_drvdata(&pdev->dev, state);
+
+	err = sysfs_create_groups(&pdev->dev.kobj, r2p_sysfs_groups);
+	if (err) {
+		dev_err(&pdev->dev, "sysfs creation failed\n");
+		return err;
+	}
+
+	state->reboot_notifier.notifier_call = tegra_r2p_reboot_notify;
+	err = devm_register_reboot_notifier(&pdev->dev,
+					    &state->reboot_notifier);
+	if (err) {
+		dev_err(&pdev->dev,
+			"unable to register reboot notifier, %d\n",
+			err);
+		return err;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id tegra_r2p_match[] = {
+	{ .compatible = "nvidia,tegra124-r2p", },
+	{ .compatible = "nvidia,tegra210-r2p", },
+	{ .compatible = "nvidia,tegra210b01-r2p", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, tegra_r2p_match);
+
+static struct platform_driver tegra_r2p_driver = {
+	.probe   = tegra_r2p_driver_probe,
+	.driver  = {
+		.name  = "tegra-r2p",
+		.owner = THIS_MODULE,
+		.of_match_table = tegra_r2p_match
+	},
+};
+
+static int __init tegra_r2p_init(void)
+{
+	return platform_driver_register(&tegra_r2p_driver);
+}
+core_initcall(tegra_r2p_init);
+
+static void __exit tegra_r2p_exit(void)
+{
+	platform_driver_unregister(&tegra_r2p_driver);
+}
+module_exit(tegra_r2p_exit);
+
+MODULE_AUTHOR("Thomas Makin <halorocker89@gmail.com>");
+MODULE_DESCRIPTION("Reboot2Payload driver for Tegra124/210/210b01");
+MODULE_LICENSE("GPL v2");
@@ -149,6 +149,12 @@ enum tegra_io_pad {
 	TEGRA_IO_PAD_AO_HV,
 };

+struct tegra_br_cmd_cfg {
+	u32 dev;
+	u32 idx;
+	u32 val;
+};
+
 #ifdef CONFIG_SOC_TEGRA_PMC
 struct tegra_pmc *devm_tegra_pmc_get(struct device *dev);

@@ -156,6 +162,10 @@ int tegra_pmc_powergate_power_on(struct tegra_pmc *pmc, unsigned int id);
 int tegra_pmc_powergate_power_off(struct tegra_pmc *pmc, unsigned int id);
 int tegra_pmc_powergate_remove_clamping(struct tegra_pmc *pmc, unsigned int id);

+int tegra_pmc_edit_bootrom_scratch_reset(struct tegra_pmc *pmc,
+	struct tegra_br_cmd_cfg *bcfg, u32 bcfg_size);
+u32 tegra_pmc_readl(struct tegra_pmc *pmc, unsigned long offset);
+
 /* Must be called with clk disabled, and returns with clk enabled */
 int tegra_pmc_powergate_sequence_power_up(struct tegra_pmc *pmc,
 					  unsigned int id, struct clk *clk,
@@ -212,6 +222,19 @@ tegra_pmc_powergate_sequence_power_up(struct tegra_pmc *pmc, unsigned int id,
 	return -ENOSYS;
 }

+static inline int
+tegra_pmc_edit_bootrom_scratch_reset(struct tegra_pmc *pmc,
+	struct tegra_br_cmd_cfg *bcfg, u32 bcfg_size)
+{
+	return -ENOSYS;
+}
+
+static inline u32
+tegra_pmc_readl(struct tegra_pmc *pmc, unsigned long offset)
+{
+	return -ENOSYS;
+}
+
 static inline int
 tegra_pmc_io_pad_power_enable(struct tegra_pmc *pmc, enum tegra_io_pad id)
 {