diff --git a/drivers/tegra/hwpm/include/tegra_hwpm_io.h b/drivers/tegra/hwpm/include/tegra_hwpm_io.h index 68d49cb..2175dd4 100644 --- a/drivers/tegra/hwpm/include/tegra_hwpm_io.h +++ b/drivers/tegra/hwpm/include/tegra_hwpm_io.h @@ -71,6 +71,9 @@ static inline u32 get_field(u32 input_data, u32 mask) #define tegra_hwpm_read_sticky_bits(hwpm, reg_base, reg_offset, val) \ tegra_hwpm_read_sticky_bits_impl(hwpm, reg_base, reg_offset, val) +#define tegra_hwpm_write_sticky_bits(hwpm, reg_base, reg_offset, val) \ + tegra_hwpm_write_sticky_bits_impl(hwpm, reg_base, reg_offset, val) + #define tegra_hwpm_fake_readl(hwpm, aperture, addr, val) \ tegra_hwpm_fake_readl_impl(hwpm, aperture, addr, val) diff --git a/drivers/tegra/hwpm/os/linux/io_utils.c b/drivers/tegra/hwpm/os/linux/io_utils.c index fb90f23..394160b 100644 --- a/drivers/tegra/hwpm/os/linux/io_utils.c +++ b/drivers/tegra/hwpm/os/linux/io_utils.c @@ -26,16 +26,12 @@ #include #include -int tegra_hwpm_read_sticky_bits_impl(struct tegra_soc_hwpm *hwpm, - u64 reg_base, u64 reg_offset, u32 *val) +static int hwpm_remap_readl(struct tegra_soc_hwpm *hwpm, u64 addr, u32 *val) { - void __iomem *ptr = NULL; - u64 reg_addr = tegra_hwpm_safe_add_u64(reg_base, reg_offset); + void __iomem *ptr = ioremap(addr, 0x4); - ptr = ioremap(reg_addr, 0x4); if (!ptr) { - tegra_hwpm_err(hwpm, "Failed to map register(0x%llx)", - reg_addr); + tegra_hwpm_err(hwpm, "Failed to map address(0x%llx)", addr); return -ENODEV; } *val = __raw_readl(ptr); @@ -44,6 +40,36 @@ int tegra_hwpm_read_sticky_bits_impl(struct tegra_soc_hwpm *hwpm, return 0; } +static int hwpm_remap_writel(struct tegra_soc_hwpm *hwpm, u64 addr, u32 val) +{ + void __iomem *ptr = ioremap(addr, 0x4); + + if (!ptr) { + tegra_hwpm_err(hwpm, "Failed to map address(0x%llx)", addr); + return -ENODEV; + } + __raw_writel(val, ptr); + iounmap(ptr); + + return 0; +} + +int tegra_hwpm_read_sticky_bits_impl(struct tegra_soc_hwpm *hwpm, + u64 reg_base, u64 reg_offset, u32 *val) +{ + u64 reg_addr = tegra_hwpm_safe_add_u64(reg_base, reg_offset); + + return hwpm_remap_readl(hwpm, reg_addr, val); +} + +int tegra_hwpm_write_sticky_bits_impl(struct tegra_soc_hwpm *hwpm, + u64 reg_base, u64 reg_offset, u32 val) +{ + u64 reg_addr = tegra_hwpm_safe_add_u64(reg_base, reg_offset); + + return hwpm_remap_writel(hwpm, reg_addr, val); +} + int tegra_hwpm_fake_readl_impl(struct tegra_soc_hwpm *hwpm, struct hwpm_ip_aperture *aperture, u64 offset, u32 *val) { @@ -179,7 +205,7 @@ static int ip_writel(struct tegra_soc_hwpm *hwpm, struct hwpm_ip_inst *ip_inst, * PERFMONs, PMA and RTR registers fall in this category */ static int hwpm_readl(struct tegra_soc_hwpm *hwpm, - struct hwpm_ip_aperture *aperture, u64 offset, u32 *val) + struct hwpm_ip_aperture *aperture, u64 offset, u32 *val) { tegra_hwpm_dbg(hwpm, hwpm_register, "Aperture (0x%llx-0x%llx) offset(0x%llx)", diff --git a/drivers/tegra/hwpm/os/linux/io_utils.h b/drivers/tegra/hwpm/os/linux/io_utils.h index cce342b..82986ac 100644 --- a/drivers/tegra/hwpm/os/linux/io_utils.h +++ b/drivers/tegra/hwpm/os/linux/io_utils.h @@ -23,6 +23,8 @@ struct hwpm_ip_aperture; int tegra_hwpm_read_sticky_bits_impl(struct tegra_soc_hwpm *hwpm, u64 reg_base, u64 reg_offset, u32 *val); +int tegra_hwpm_write_sticky_bits_impl(struct tegra_soc_hwpm *hwpm, + u64 reg_base, u64 reg_offset, u32 val); int tegra_hwpm_fake_readl_impl(struct tegra_soc_hwpm *hwpm, struct hwpm_ip_aperture *aperture, u64 offset, u32 *val); int tegra_hwpm_fake_writel_impl(struct tegra_soc_hwpm *hwpm,