--- linux-4.9.37/drivers/clk/goke/reset.c 1970-01-01 03:00:00.000000000 +0300 +++ linux-4.9.y/drivers/clk/goke/reset.c 2021-06-07 13:01:33.000000000 +0300 @@ -0,0 +1,113 @@ +/* + * Copyright (c) Hunan Goke,Chengdu Goke,Shandong Goke. 2021. All rights reserved. + */ + +#include +#include +#include +#include +#include +#include +#include "reset.h" + +#define GOKE_RESET_BIT_MASK 0x1f +#define GOKE_RESET_OFFSET_SHIFT 8 +#define GOKE_RESET_OFFSET_MASK 0xffff00 + +struct gk_reset_controller { + spinlock_t lock; + void __iomem *membase; + struct reset_controller_dev rcdev; +}; + + +#define to_gk_reset_controller(rcdev) \ + container_of(rcdev, struct gk_reset_controller, rcdev) + +static int gk_reset_of_xlate(struct reset_controller_dev *rcdev, + const struct of_phandle_args *reset_spec) +{ + u32 offset; + u8 bit; + + offset = (reset_spec->args[0] << GOKE_RESET_OFFSET_SHIFT) + & GOKE_RESET_OFFSET_MASK; + bit = reset_spec->args[1] & GOKE_RESET_BIT_MASK; + + return (offset | bit); +} + +static int gk_reset_assert(struct reset_controller_dev *rcdev, + unsigned long id) +{ + struct gk_reset_controller *rstc = to_gk_reset_controller(rcdev); + unsigned long flags; + u32 offset, reg; + u8 bit; + + offset = (id & GOKE_RESET_OFFSET_MASK) >> GOKE_RESET_OFFSET_SHIFT; + bit = id & GOKE_RESET_BIT_MASK; + + spin_lock_irqsave(&rstc->lock, flags); + + reg = readl(rstc->membase + offset); + writel(reg | BIT(bit), rstc->membase + offset); + + spin_unlock_irqrestore(&rstc->lock, flags); + + return 0; +} + +static int gk_reset_deassert(struct reset_controller_dev *rcdev, + unsigned long id) +{ + struct gk_reset_controller *rstc = to_gk_reset_controller(rcdev); + unsigned long flags; + u32 offset, reg; + u8 bit; + + offset = (id & GOKE_RESET_OFFSET_MASK) >> GOKE_RESET_OFFSET_SHIFT; + bit = id & GOKE_RESET_BIT_MASK; + + spin_lock_irqsave(&rstc->lock, flags); + + reg = readl(rstc->membase + offset); + writel(reg & ~BIT(bit), rstc->membase + offset); + + spin_unlock_irqrestore(&rstc->lock, flags); + + return 0; +} + +static const struct reset_control_ops gk_reset_ops = { + .assert = gk_reset_assert, + .deassert = gk_reset_deassert, +}; + +int __init gk_reset_init(struct device_node *np, + int nr_rsts) +{ + struct gk_reset_controller *rstc; + + rstc = kzalloc(sizeof(*rstc), GFP_KERNEL); + if (!rstc) + return -ENOMEM; + + rstc->membase = of_iomap(np, 0); + if (!rstc->membase){ + kfree(rstc); + return -EINVAL; + } + + spin_lock_init(&rstc->lock); + + rstc->rcdev.owner = THIS_MODULE; + rstc->rcdev.nr_resets = nr_rsts; + rstc->rcdev.ops = &gk_reset_ops; + rstc->rcdev.of_node = np; + rstc->rcdev.of_reset_n_cells = 2; + rstc->rcdev.of_xlate = gk_reset_of_xlate; + + return reset_controller_register(&rstc->rcdev); +} +EXPORT_SYMBOL_GPL(gk_reset_init);