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

1 files changed, 940 insertions, 0 deletions

diff --git a/drivers/pinctrl/tegra/pinctrl-tegra.c b/drivers/pinctrl/tegra/pinctrl-tegra.c
new file mode 100644
index 000000000..3f58f7db5
--- /dev/null
+++ b/drivers/pinctrl/tegra/pinctrl-tegra.c
@@ -0,0 +1,940 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Driver for the NVIDIA Tegra pinmux
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Derived from code:
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2010 NVIDIA Corporation
+ * Copyright (C) 2009-2011 ST-Ericsson AB
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+
+#include <linux/pinctrl/machine.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+
+#include "../core.h"
+#include "../pinctrl-utils.h"
+#include "pinctrl-tegra.h"
+
+static inline u32 pmx_readl(struct tegra_pmx *pmx, u32 bank, u32 reg)
+{
+	return readl(pmx->regs[bank] + reg);
+}
+
+static inline void pmx_writel(struct tegra_pmx *pmx, u32 val, u32 bank, u32 reg)
+{
+	writel_relaxed(val, pmx->regs[bank] + reg);
+	/* make sure pinmux register write completed */
+	pmx_readl(pmx, bank, reg);
+}
+
+static int tegra_pinctrl_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->soc->ngroups;
+}
+
+static const char *tegra_pinctrl_get_group_name(struct pinctrl_dev *pctldev,
+						unsigned group)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->soc->groups[group].name;
+}
+
+static int tegra_pinctrl_get_group_pins(struct pinctrl_dev *pctldev,
+					unsigned group,
+					const unsigned **pins,
+					unsigned *num_pins)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = pmx->soc->groups[group].pins;
+	*num_pins = pmx->soc->groups[group].npins;
+
+	return 0;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void tegra_pinctrl_pin_dbg_show(struct pinctrl_dev *pctldev,
+				       struct seq_file *s,
+				       unsigned offset)
+{
+	seq_printf(s, " %s", dev_name(pctldev->dev));
+}
+#endif
+
+static const struct cfg_param {
+	const char *property;
+	enum tegra_pinconf_param param;
+} cfg_params[] = {
+	{"nvidia,pull",			TEGRA_PINCONF_PARAM_PULL},
+	{"nvidia,tristate",		TEGRA_PINCONF_PARAM_TRISTATE},
+	{"nvidia,enable-input",		TEGRA_PINCONF_PARAM_ENABLE_INPUT},
+	{"nvidia,open-drain",		TEGRA_PINCONF_PARAM_OPEN_DRAIN},
+	{"nvidia,lock",			TEGRA_PINCONF_PARAM_LOCK},
+	{"nvidia,io-reset",		TEGRA_PINCONF_PARAM_IORESET},
+	{"nvidia,rcv-sel",		TEGRA_PINCONF_PARAM_RCV_SEL},
+	{"nvidia,io-hv",		TEGRA_PINCONF_PARAM_RCV_SEL},
+	{"nvidia,high-speed-mode",	TEGRA_PINCONF_PARAM_HIGH_SPEED_MODE},
+	{"nvidia,schmitt",		TEGRA_PINCONF_PARAM_SCHMITT},
+	{"nvidia,low-power-mode",	TEGRA_PINCONF_PARAM_LOW_POWER_MODE},
+	{"nvidia,pull-down-strength",	TEGRA_PINCONF_PARAM_DRIVE_DOWN_STRENGTH},
+	{"nvidia,pull-up-strength",	TEGRA_PINCONF_PARAM_DRIVE_UP_STRENGTH},
+	{"nvidia,slew-rate-falling",	TEGRA_PINCONF_PARAM_SLEW_RATE_FALLING},
+	{"nvidia,slew-rate-rising",	TEGRA_PINCONF_PARAM_SLEW_RATE_RISING},
+	{"nvidia,drive-type",		TEGRA_PINCONF_PARAM_DRIVE_TYPE},
+	{"nvidia,gpio-mode",		TEGRA_PINCONF_PARAM_GPIO_MODE},
+};
+
+static int tegra_pinctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
+					   struct device_node *np,
+					   struct pinctrl_map **map,
+					   unsigned *reserved_maps,
+					   unsigned *num_maps)
+{
+	struct device *dev = pctldev->dev;
+	int ret, i;
+	const char *function;
+	u32 val;
+	unsigned long config;
+	unsigned long *configs = NULL;
+	unsigned num_configs = 0;
+	unsigned reserve;
+	struct property *prop;
+	const char *group;
+
+	ret = of_property_read_string(np, "nvidia,function", &function);
+	if (ret < 0) {
+		/* EINVAL=missing, which is fine since it's optional */
+		if (ret != -EINVAL)
+			dev_err(dev,
+				"%pOF: could not parse property nvidia,function\n", np);
+		function = NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+		ret = of_property_read_u32(np, cfg_params[i].property, &val);
+		if (!ret) {
+			config = TEGRA_PINCONF_PACK(cfg_params[i].param, val);
+			ret = pinctrl_utils_add_config(pctldev, &configs,
+					&num_configs, config);
+			if (ret < 0)
+				goto exit;
+		/* EINVAL=missing, which is fine since it's optional */
+		} else if (ret != -EINVAL) {
+			dev_err(dev, "%pOF: could not parse property %s\n",
+				np, cfg_params[i].property);
+		}
+	}
+
+	reserve = 0;
+	if (function != NULL)
+		reserve++;
+	if (num_configs)
+		reserve++;
+	ret = of_property_count_strings(np, "nvidia,pins");
+	if (ret < 0) {
+		dev_err(dev, "%pOF: could not parse property nvidia,pins\n", np);
+		goto exit;
+	}
+	reserve *= ret;
+
+	ret = pinctrl_utils_reserve_map(pctldev, map, reserved_maps,
+					num_maps, reserve);
+	if (ret < 0)
+		goto exit;
+
+	of_property_for_each_string(np, "nvidia,pins", prop, group) {
+		if (function) {
+			ret = pinctrl_utils_add_map_mux(pctldev, map,
+					reserved_maps, num_maps, group,
+					function);
+			if (ret < 0)
+				goto exit;
+		}
+
+		if (num_configs) {
+			ret = pinctrl_utils_add_map_configs(pctldev, map,
+					reserved_maps, num_maps, group,
+					configs, num_configs,
+					PIN_MAP_TYPE_CONFIGS_GROUP);
+			if (ret < 0)
+				goto exit;
+		}
+	}
+
+	ret = 0;
+
+exit:
+	kfree(configs);
+	return ret;
+}
+
+static int tegra_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
+					struct device_node *np_config,
+					struct pinctrl_map **map,
+					unsigned *num_maps)
+{
+	unsigned reserved_maps;
+	int ret;
+
+	reserved_maps = 0;
+	*map = NULL;
+	*num_maps = 0;
+
+	for_each_child_of_node_scoped(np_config, np) {
+		ret = tegra_pinctrl_dt_subnode_to_map(pctldev, np, map,
+						      &reserved_maps, num_maps);
+		if (ret < 0) {
+			pinctrl_utils_free_map(pctldev, *map,
+				*num_maps);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static const struct pinctrl_ops tegra_pinctrl_ops = {
+	.get_groups_count = tegra_pinctrl_get_groups_count,
+	.get_group_name = tegra_pinctrl_get_group_name,
+	.get_group_pins = tegra_pinctrl_get_group_pins,
+#ifdef CONFIG_DEBUG_FS
+	.pin_dbg_show = tegra_pinctrl_pin_dbg_show,
+#endif
+	.dt_node_to_map = tegra_pinctrl_dt_node_to_map,
+	.dt_free_map = pinctrl_utils_free_map,
+};
+
+static int tegra_pinctrl_get_funcs_count(struct pinctrl_dev *pctldev)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->soc->nfunctions;
+}
+
+static const char *tegra_pinctrl_get_func_name(struct pinctrl_dev *pctldev,
+					       unsigned function)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->functions[function].name;
+}
+
+static int tegra_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
+					 unsigned function,
+					 const char * const **groups,
+					 unsigned * const num_groups)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = pmx->functions[function].groups;
+	*num_groups = pmx->functions[function].ngroups;
+
+	return 0;
+}
+
+static int tegra_pinctrl_set_mux(struct pinctrl_dev *pctldev,
+				 unsigned function,
+				 unsigned group)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	const struct tegra_pingroup *g;
+	int i;
+	u32 val;
+
+	g = &pmx->soc->groups[group];
+
+	if (WARN_ON(g->mux_reg < 0))
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(g->funcs); i++) {
+		if (g->funcs[i] == function)
+			break;
+	}
+	if (WARN_ON(i == ARRAY_SIZE(g->funcs)))
+		return -EINVAL;
+
+	val = pmx_readl(pmx, g->mux_bank, g->mux_reg);
+	val &= ~(0x3 << g->mux_bit);
+	val |= i << g->mux_bit;
+	/* Set the SFIO/GPIO selection to SFIO when under pinmux control*/
+	if (pmx->soc->sfsel_in_mux)
+		val |= (1 << g->sfsel_bit);
+	pmx_writel(pmx, val, g->mux_bank, g->mux_reg);
+
+	return 0;
+}
+
+static int tegra_pinctrl_get_group_index(struct pinctrl_dev *pctldev,
+					 unsigned int offset)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	unsigned int group, num_pins, j;
+	const unsigned int *pins;
+	int ret;
+
+	for (group = 0; group < pmx->soc->ngroups; ++group) {
+		ret = tegra_pinctrl_get_group_pins(pctldev, group, &pins, &num_pins);
+		if (ret < 0)
+			continue;
+		for (j = 0; j < num_pins; j++) {
+			if (offset == pins[j])
+				return group;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static const struct tegra_pingroup *tegra_pinctrl_get_group(struct pinctrl_dev *pctldev,
+							    unsigned int offset,
+							    int group_index)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	if (group_index < 0 || group_index >= pmx->soc->ngroups)
+		return NULL;
+
+	return &pmx->soc->groups[group_index];
+}
+
+static struct tegra_pingroup_config *tegra_pinctrl_get_group_config(struct pinctrl_dev *pctldev,
+								    unsigned int offset,
+								    int group_index)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	if (group_index < 0)
+		return NULL;
+
+	return &pmx->pingroup_configs[group_index];
+}
+
+static int tegra_pinctrl_gpio_request_enable(struct pinctrl_dev *pctldev,
+					     struct pinctrl_gpio_range *range,
+					     unsigned int offset)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	const struct tegra_pingroup *group;
+	struct tegra_pingroup_config *config;
+	int group_index;
+	u32 value;
+
+	if (!pmx->soc->sfsel_in_mux)
+		return 0;
+
+	group_index = tegra_pinctrl_get_group_index(pctldev, offset);
+	group = tegra_pinctrl_get_group(pctldev, offset, group_index);
+
+	if (!group)
+		return -EINVAL;
+
+	if (group->mux_reg < 0 || group->sfsel_bit < 0)
+		return -EINVAL;
+
+	config = tegra_pinctrl_get_group_config(pctldev, offset, group_index);
+	if (!config)
+		return -EINVAL;
+	value = pmx_readl(pmx, group->mux_bank, group->mux_reg);
+	config->is_sfsel = (value & BIT(group->sfsel_bit)) != 0;
+	value &= ~BIT(group->sfsel_bit);
+	pmx_writel(pmx, value, group->mux_bank, group->mux_reg);
+
+	return 0;
+}
+
+static void tegra_pinctrl_gpio_disable_free(struct pinctrl_dev *pctldev,
+					    struct pinctrl_gpio_range *range,
+					    unsigned int offset)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	const struct tegra_pingroup *group;
+	struct tegra_pingroup_config *config;
+	int group_index;
+	u32 value;
+
+	if (!pmx->soc->sfsel_in_mux)
+		return;
+
+	group_index = tegra_pinctrl_get_group_index(pctldev, offset);
+	group = tegra_pinctrl_get_group(pctldev, offset, group_index);
+
+	if (!group)
+		return;
+
+	if (group->mux_reg < 0 || group->sfsel_bit < 0)
+		return;
+
+	config = tegra_pinctrl_get_group_config(pctldev, offset, group_index);
+	if (!config)
+		return;
+	value = pmx_readl(pmx, group->mux_bank, group->mux_reg);
+	if (config->is_sfsel)
+		value |= BIT(group->sfsel_bit);
+	pmx_writel(pmx, value, group->mux_bank, group->mux_reg);
+}
+
+static const struct pinmux_ops tegra_pinmux_ops = {
+	.get_functions_count = tegra_pinctrl_get_funcs_count,
+	.get_function_name = tegra_pinctrl_get_func_name,
+	.get_function_groups = tegra_pinctrl_get_func_groups,
+	.set_mux = tegra_pinctrl_set_mux,
+	.gpio_request_enable = tegra_pinctrl_gpio_request_enable,
+	.gpio_disable_free = tegra_pinctrl_gpio_disable_free,
+};
+
+static int tegra_pinconf_reg(struct tegra_pmx *pmx,
+			     const struct tegra_pingroup *g,
+			     enum tegra_pinconf_param param,
+			     bool report_err,
+			     s8 *bank, s32 *reg, s8 *bit, s8 *width)
+{
+	switch (param) {
+	case TEGRA_PINCONF_PARAM_PULL:
+		*bank = g->pupd_bank;
+		*reg = g->pupd_reg;
+		*bit = g->pupd_bit;
+		*width = 2;
+		break;
+	case TEGRA_PINCONF_PARAM_TRISTATE:
+		*bank = g->tri_bank;
+		*reg = g->tri_reg;
+		*bit = g->tri_bit;
+		*width = 1;
+		break;
+	case TEGRA_PINCONF_PARAM_ENABLE_INPUT:
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
+		*bit = g->einput_bit;
+		*width = 1;
+		break;
+	case TEGRA_PINCONF_PARAM_OPEN_DRAIN:
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
+		*bit = g->odrain_bit;
+		*width = 1;
+		break;
+	case TEGRA_PINCONF_PARAM_LOCK:
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
+		*bit = g->lock_bit;
+		*width = 1;
+		break;
+	case TEGRA_PINCONF_PARAM_IORESET:
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
+		*bit = g->ioreset_bit;
+		*width = 1;
+		break;
+	case TEGRA_PINCONF_PARAM_RCV_SEL:
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
+		*bit = g->rcv_sel_bit;
+		*width = 1;
+		break;
+	case TEGRA_PINCONF_PARAM_HIGH_SPEED_MODE:
+		if (pmx->soc->hsm_in_mux) {
+			*bank = g->mux_bank;
+			*reg = g->mux_reg;
+		} else {
+			*bank = g->drv_bank;
+			*reg = g->drv_reg;
+		}
+		*bit = g->hsm_bit;
+		*width = 1;
+		break;
+	case TEGRA_PINCONF_PARAM_SCHMITT:
+		if (pmx->soc->schmitt_in_mux) {
+			*bank = g->mux_bank;
+			*reg = g->mux_reg;
+		} else {
+			*bank = g->drv_bank;
+			*reg = g->drv_reg;
+		}
+		*bit = g->schmitt_bit;
+		*width = 1;
+		break;
+	case TEGRA_PINCONF_PARAM_LOW_POWER_MODE:
+		*bank = g->drv_bank;
+		*reg = g->drv_reg;
+		*bit = g->lpmd_bit;
+		*width = 2;
+		break;
+	case TEGRA_PINCONF_PARAM_DRIVE_DOWN_STRENGTH:
+		*bank = g->drv_bank;
+		*reg = g->drv_reg;
+		*bit = g->drvdn_bit;
+		*width = g->drvdn_width;
+		break;
+	case TEGRA_PINCONF_PARAM_DRIVE_UP_STRENGTH:
+		*bank = g->drv_bank;
+		*reg = g->drv_reg;
+		*bit = g->drvup_bit;
+		*width = g->drvup_width;
+		break;
+	case TEGRA_PINCONF_PARAM_SLEW_RATE_FALLING:
+		*bank = g->drv_bank;
+		*reg = g->drv_reg;
+		*bit = g->slwf_bit;
+		*width = g->slwf_width;
+		break;
+	case TEGRA_PINCONF_PARAM_SLEW_RATE_RISING:
+		*bank = g->drv_bank;
+		*reg = g->drv_reg;
+		*bit = g->slwr_bit;
+		*width = g->slwr_width;
+		break;
+	case TEGRA_PINCONF_PARAM_DRIVE_TYPE:
+		if (pmx->soc->drvtype_in_mux) {
+			*bank = g->mux_bank;
+			*reg = g->mux_reg;
+		} else {
+			*bank = g->drv_bank;
+			*reg = g->drv_reg;
+		}
+		*bit = g->drvtype_bit;
+		*width = 2;
+		break;
+	case TEGRA_PINCONF_PARAM_GPIO_MODE:
+		if (pmx->soc->sfsel_in_mux) {
+			*bank = g->mux_bank;
+			*reg = g->mux_reg;
+			*bit = g->sfsel_bit;
+			*width = 1;
+		} else {
+			*reg = -EINVAL;
+		}
+		break;
+	default:
+		dev_err(pmx->dev, "Invalid config param %04x\n", param);
+		return -ENOTSUPP;
+	}
+
+	if (*reg < 0 || *bit < 0)  {
+		if (report_err) {
+			const char *prop = "unknown";
+			int i;
+
+			for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+				if (cfg_params[i].param == param) {
+					prop = cfg_params[i].property;
+					break;
+				}
+			}
+
+			dev_err(pmx->dev,
+				"Config param %04x (%s) not supported on group %s\n",
+				param, prop, g->name);
+		}
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+static int tegra_pinconf_get(struct pinctrl_dev *pctldev,
+			     unsigned pin, unsigned long *config)
+{
+	dev_err(pctldev->dev, "pin_config_get op not supported\n");
+	return -ENOTSUPP;
+}
+
+static int tegra_pinconf_set(struct pinctrl_dev *pctldev,
+			     unsigned pin, unsigned long *configs,
+			     unsigned num_configs)
+{
+	dev_err(pctldev->dev, "pin_config_set op not supported\n");
+	return -ENOTSUPP;
+}
+
+static int tegra_pinconf_group_get(struct pinctrl_dev *pctldev,
+				   unsigned group, unsigned long *config)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	enum tegra_pinconf_param param = TEGRA_PINCONF_UNPACK_PARAM(*config);
+	u16 arg;
+	const struct tegra_pingroup *g;
+	int ret;
+	s8 bank, bit, width;
+	s32 reg;
+	u32 val, mask;
+
+	g = &pmx->soc->groups[group];
+
+	ret = tegra_pinconf_reg(pmx, g, param, true, &bank, &reg, &bit,
+				&width);
+	if (ret < 0)
+		return ret;
+
+	val = pmx_readl(pmx, bank, reg);
+	mask = (1 << width) - 1;
+	arg = (val >> bit) & mask;
+
+	*config = TEGRA_PINCONF_PACK(param, arg);
+
+	return 0;
+}
+
+static int tegra_pinconf_group_set(struct pinctrl_dev *pctldev,
+				   unsigned group, unsigned long *configs,
+				   unsigned num_configs)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	enum tegra_pinconf_param param;
+	u16 arg;
+	const struct tegra_pingroup *g;
+	int ret, i;
+	s8 bank, bit, width;
+	s32 reg;
+	u32 val, mask;
+
+	g = &pmx->soc->groups[group];
+
+	for (i = 0; i < num_configs; i++) {
+		param = TEGRA_PINCONF_UNPACK_PARAM(configs[i]);
+		arg = TEGRA_PINCONF_UNPACK_ARG(configs[i]);
+
+		ret = tegra_pinconf_reg(pmx, g, param, true, &bank, &reg, &bit,
+					&width);
+		if (ret < 0)
+			return ret;
+
+		val = pmx_readl(pmx, bank, reg);
+
+		/* LOCK can't be cleared */
+		if (param == TEGRA_PINCONF_PARAM_LOCK) {
+			if ((val & BIT(bit)) && !arg) {
+				dev_err(pctldev->dev, "LOCK bit cannot be cleared\n");
+				return -EINVAL;
+			}
+		}
+
+		/* Special-case Boolean values; allow any non-zero as true */
+		if (width == 1)
+			arg = !!arg;
+
+		/* Range-check user-supplied value */
+		mask = (1 << width) - 1;
+		if (arg & ~mask) {
+			dev_err(pctldev->dev,
+				"config %lx: %x too big for %d bit register\n",
+				configs[i], arg, width);
+			return -EINVAL;
+		}
+
+		/* Update register */
+		val &= ~(mask << bit);
+		val |= arg << bit;
+		pmx_writel(pmx, val, bank, reg);
+	} /* for each config */
+
+	return 0;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void tegra_pinconf_dbg_show(struct pinctrl_dev *pctldev,
+				   struct seq_file *s, unsigned offset)
+{
+}
+
+static const char *strip_prefix(const char *s)
+{
+	const char *comma = strchr(s, ',');
+	if (!comma)
+		return s;
+
+	return comma + 1;
+}
+
+static void tegra_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
+					 struct seq_file *s, unsigned group)
+{
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	const struct tegra_pingroup *g;
+	int i, ret;
+	s8 bank, bit, width;
+	s32 reg;
+	u32 val;
+
+	g = &pmx->soc->groups[group];
+
+	for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+		ret = tegra_pinconf_reg(pmx, g, cfg_params[i].param, false,
+					&bank, &reg, &bit, &width);
+		if (ret < 0)
+			continue;
+
+		val = pmx_readl(pmx, bank, reg);
+		val >>= bit;
+		val &= (1 << width) - 1;
+
+		seq_printf(s, "\n\t%s=%u",
+			   strip_prefix(cfg_params[i].property), val);
+	}
+
+	if (g->mux_reg >= 0) {
+		/* read pinmux function and dump to seq_file */
+		val = pmx_readl(pmx, g->mux_bank, g->mux_reg);
+		val = g->funcs[(val >> g->mux_bit) & 0x3];
+
+		seq_printf(s, "\n\tfunction=%s", pmx->functions[val].name);
+	}
+}
+
+static void tegra_pinconf_config_dbg_show(struct pinctrl_dev *pctldev,
+					  struct seq_file *s,
+					  unsigned long config)
+{
+	enum tegra_pinconf_param param = TEGRA_PINCONF_UNPACK_PARAM(config);
+	u16 arg = TEGRA_PINCONF_UNPACK_ARG(config);
+	const char *pname = "unknown";
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+		if (cfg_params[i].param == param) {
+			pname = cfg_params[i].property;
+			break;
+		}
+	}
+
+	seq_printf(s, "%s=%d", strip_prefix(pname), arg);
+}
+#endif
+
+static const struct pinconf_ops tegra_pinconf_ops = {
+	.pin_config_get = tegra_pinconf_get,
+	.pin_config_set = tegra_pinconf_set,
+	.pin_config_group_get = tegra_pinconf_group_get,
+	.pin_config_group_set = tegra_pinconf_group_set,
+#ifdef CONFIG_DEBUG_FS
+	.pin_config_dbg_show = tegra_pinconf_dbg_show,
+	.pin_config_group_dbg_show = tegra_pinconf_group_dbg_show,
+	.pin_config_config_dbg_show = tegra_pinconf_config_dbg_show,
+#endif
+};
+
+static void tegra_pinctrl_clear_parked_bits(struct tegra_pmx *pmx)
+{
+	int i = 0;
+	const struct tegra_pingroup *g;
+	u32 val;
+
+	for (i = 0; i < pmx->soc->ngroups; ++i) {
+		g = &pmx->soc->groups[i];
+		if (g->parked_bitmask > 0) {
+			unsigned int bank, reg;
+
+			if (g->mux_reg != -1) {
+				bank = g->mux_bank;
+				reg = g->mux_reg;
+			} else {
+				bank = g->drv_bank;
+				reg = g->drv_reg;
+			}
+
+			val = pmx_readl(pmx, bank, reg);
+			val &= ~g->parked_bitmask;
+			pmx_writel(pmx, val, bank, reg);
+		}
+	}
+}
+
+static size_t tegra_pinctrl_get_bank_size(struct device *dev,
+					  unsigned int bank_id)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, bank_id);
+
+	return resource_size(res) / 4;
+}
+
+static int tegra_pinctrl_suspend(struct device *dev)
+{
+	struct tegra_pmx *pmx = dev_get_drvdata(dev);
+	u32 *backup_regs = pmx->backup_regs;
+	u32 __iomem *regs;
+	size_t bank_size;
+	unsigned int i, k;
+
+	for (i = 0; i < pmx->nbanks; i++) {
+		bank_size = tegra_pinctrl_get_bank_size(dev, i);
+		regs = pmx->regs[i];
+		for (k = 0; k < bank_size; k++)
+			*backup_regs++ = readl_relaxed(regs++);
+	}
+
+	return pinctrl_force_sleep(pmx->pctl);
+}
+
+static int tegra_pinctrl_resume(struct device *dev)
+{
+	struct tegra_pmx *pmx = dev_get_drvdata(dev);
+	u32 *backup_regs = pmx->backup_regs;
+	u32 __iomem *regs;
+	size_t bank_size;
+	unsigned int i, k;
+
+	for (i = 0; i < pmx->nbanks; i++) {
+		bank_size = tegra_pinctrl_get_bank_size(dev, i);
+		regs = pmx->regs[i];
+		for (k = 0; k < bank_size; k++)
+			writel_relaxed(*backup_regs++, regs++);
+	}
+
+	/* flush all the prior writes */
+	readl_relaxed(pmx->regs[0]);
+	/* wait for pinctrl register read to complete */
+	rmb();
+	return 0;
+}
+
+DEFINE_NOIRQ_DEV_PM_OPS(tegra_pinctrl_pm, tegra_pinctrl_suspend, tegra_pinctrl_resume);
+
+static bool tegra_pinctrl_gpio_node_has_range(struct tegra_pmx *pmx)
+{
+	struct device_node *np;
+	bool has_prop = false;
+
+	np = of_find_compatible_node(NULL, NULL, pmx->soc->gpio_compatible);
+	if (!np)
+		return has_prop;
+
+	has_prop = of_find_property(np, "gpio-ranges", NULL);
+
+	of_node_put(np);
+
+	return has_prop;
+}
+
+int tegra_pinctrl_probe(struct platform_device *pdev,
+			const struct tegra_pinctrl_soc_data *soc_data)
+{
+	struct tegra_pmx *pmx;
+	struct resource *res;
+	int i;
+	const char **group_pins;
+	int fn, gn, gfn;
+	unsigned long backup_regs_size = 0;
+
+	pmx = devm_kzalloc(&pdev->dev,
+			struct_size(pmx, pingroup_configs, soc_data->ngroups), GFP_KERNEL);
+	if (!pmx)
+		return -ENOMEM;
+
+	pmx->dev = &pdev->dev;
+	pmx->soc = soc_data;
+	pmx->num_pingroup_configs = soc_data->ngroups;
+
+	/*
+	 * Each mux group will appear in 4 functions' list of groups.
+	 * This over-allocates slightly, since not all groups are mux groups.
+	 */
+	pmx->group_pins = devm_kcalloc(&pdev->dev, pmx->soc->ngroups * 4,
+				       sizeof(*pmx->group_pins), GFP_KERNEL);
+	if (!pmx->group_pins)
+		return -ENOMEM;
+
+	pmx->functions = devm_kcalloc(&pdev->dev, pmx->soc->nfunctions,
+				      sizeof(*pmx->functions), GFP_KERNEL);
+	if (!pmx->functions)
+		return -ENOMEM;
+
+	group_pins = pmx->group_pins;
+
+	for (fn = 0; fn < pmx->soc->nfunctions; fn++) {
+		struct tegra_function *func = &pmx->functions[fn];
+
+		func->name = pmx->soc->functions[fn];
+		func->groups = group_pins;
+
+		for (gn = 0; gn < pmx->soc->ngroups; gn++) {
+			const struct tegra_pingroup *g = &pmx->soc->groups[gn];
+
+			if (g->mux_reg == -1)
+				continue;
+
+			for (gfn = 0; gfn < 4; gfn++)
+				if (g->funcs[gfn] == fn)
+					break;
+			if (gfn == 4)
+				continue;
+
+			BUG_ON(group_pins - pmx->group_pins >=
+				pmx->soc->ngroups * 4);
+			*group_pins++ = g->name;
+			func->ngroups++;
+		}
+	}
+
+	pmx->gpio_range.name = "Tegra GPIOs";
+	pmx->gpio_range.id = 0;
+	pmx->gpio_range.base = 0;
+	pmx->gpio_range.npins = pmx->soc->ngpios;
+
+	pmx->desc.pctlops = &tegra_pinctrl_ops;
+	pmx->desc.pmxops = &tegra_pinmux_ops;
+	pmx->desc.confops = &tegra_pinconf_ops;
+	pmx->desc.owner = THIS_MODULE;
+	pmx->desc.name = dev_name(&pdev->dev);
+	pmx->desc.pins = pmx->soc->pins;
+	pmx->desc.npins = pmx->soc->npins;
+
+	for (i = 0; ; i++) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res)
+			break;
+		backup_regs_size += resource_size(res);
+	}
+	pmx->nbanks = i;
+
+	pmx->regs = devm_kcalloc(&pdev->dev, pmx->nbanks, sizeof(*pmx->regs),
+				 GFP_KERNEL);
+	if (!pmx->regs)
+		return -ENOMEM;
+
+	pmx->backup_regs = devm_kzalloc(&pdev->dev, backup_regs_size,
+					GFP_KERNEL);
+	if (!pmx->backup_regs)
+		return -ENOMEM;
+
+	for (i = 0; i < pmx->nbanks; i++) {
+		pmx->regs[i] = devm_platform_ioremap_resource(pdev, i);
+		if (IS_ERR(pmx->regs[i]))
+			return PTR_ERR(pmx->regs[i]);
+	}
+
+	pmx->pctl = devm_pinctrl_register(&pdev->dev, &pmx->desc, pmx);
+	if (IS_ERR(pmx->pctl)) {
+		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
+		return PTR_ERR(pmx->pctl);
+	}
+
+	tegra_pinctrl_clear_parked_bits(pmx);
+
+	if (pmx->soc->ngpios > 0 && !tegra_pinctrl_gpio_node_has_range(pmx))
+		pinctrl_add_gpio_range(pmx->pctl, &pmx->gpio_range);
+
+	platform_set_drvdata(pdev, pmx);
+
+	dev_dbg(&pdev->dev, "Probed Tegra pinctrl driver\n");
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(tegra_pinctrl_probe);