diff --git a/drivers/pinctrl/tegra/pinctrl-tegra.c b/drivers/pinctrl/tegra/pinctrl-tegra.c index 186ef98e7b2b8..e3a2375342819 100644 --- a/drivers/pinctrl/tegra/pinctrl-tegra.c +++ b/drivers/pinctrl/tegra/pinctrl-tegra.c @@ -631,6 +631,58 @@ static void tegra_pinctrl_clear_parked_bits(struct tegra_pmx *pmx) } } +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 *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 *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++); + } + + return 0; +} + +const struct dev_pm_ops tegra_pinctrl_pm = { + .suspend = &tegra_pinctrl_suspend, + .resume = &tegra_pinctrl_resume +}; + static bool gpio_node_has_range(const char *compatible) { struct device_node *np; @@ -655,6 +707,7 @@ int tegra_pinctrl_probe(struct platform_device *pdev, int i; const char **group_pins; int fn, gn, gfn; + unsigned long backup_regs_size = 0; pmx = devm_kzalloc(&pdev->dev, sizeof(*pmx), GFP_KERNEL); if (!pmx) @@ -707,6 +760,7 @@ int tegra_pinctrl_probe(struct platform_device *pdev, res = platform_get_resource(pdev, IORESOURCE_MEM, i); if (!res) break; + backup_regs_size += resource_size(res); } pmx->nbanks = i; @@ -715,6 +769,11 @@ int tegra_pinctrl_probe(struct platform_device *pdev, 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++) { res = platform_get_resource(pdev, IORESOURCE_MEM, i); pmx->regs[i] = devm_ioremap_resource(&pdev->dev, res); diff --git a/drivers/pinctrl/tegra/pinctrl-tegra.h b/drivers/pinctrl/tegra/pinctrl-tegra.h index 1053097740798..0fc82eea9cf12 100644 --- a/drivers/pinctrl/tegra/pinctrl-tegra.h +++ b/drivers/pinctrl/tegra/pinctrl-tegra.h @@ -17,6 +17,7 @@ struct tegra_pmx { int nbanks; void __iomem **regs; + u32 *backup_regs; }; enum tegra_pinconf_param { @@ -193,6 +194,8 @@ struct tegra_pinctrl_soc_data { bool drvtype_in_mux; }; +extern const struct dev_pm_ops tegra_pinctrl_pm; + int tegra_pinctrl_probe(struct platform_device *pdev, const struct tegra_pinctrl_soc_data *soc_data); #endif