clk-smd.c 3.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Copyright (C) 2013 Boris BREZILLON <[email protected]>
  4. */
  5. #include <linux/clk-provider.h>
  6. #include <linux/clkdev.h>
  7. #include <linux/clk/at91_pmc.h>
  8. #include <linux/of.h>
  9. #include <linux/mfd/syscon.h>
  10. #include <linux/regmap.h>
  11. #include "pmc.h"
  12. #define SMD_DIV_SHIFT 8
  13. #define SMD_MAX_DIV 0xf
  14. struct at91sam9x5_clk_smd {
  15. struct clk_hw hw;
  16. struct regmap *regmap;
  17. };
  18. #define to_at91sam9x5_clk_smd(hw) \
  19. container_of(hw, struct at91sam9x5_clk_smd, hw)
  20. static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw,
  21. unsigned long parent_rate)
  22. {
  23. struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
  24. unsigned int smdr;
  25. u8 smddiv;
  26. regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
  27. smddiv = (smdr & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT;
  28. return parent_rate / (smddiv + 1);
  29. }
  30. static long at91sam9x5_clk_smd_round_rate(struct clk_hw *hw, unsigned long rate,
  31. unsigned long *parent_rate)
  32. {
  33. unsigned long div;
  34. unsigned long bestrate;
  35. unsigned long tmp;
  36. if (rate >= *parent_rate)
  37. return *parent_rate;
  38. div = *parent_rate / rate;
  39. if (div > SMD_MAX_DIV)
  40. return *parent_rate / (SMD_MAX_DIV + 1);
  41. bestrate = *parent_rate / div;
  42. tmp = *parent_rate / (div + 1);
  43. if (bestrate - rate > rate - tmp)
  44. bestrate = tmp;
  45. return bestrate;
  46. }
  47. static int at91sam9x5_clk_smd_set_parent(struct clk_hw *hw, u8 index)
  48. {
  49. struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
  50. if (index > 1)
  51. return -EINVAL;
  52. regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMDS,
  53. index ? AT91_PMC_SMDS : 0);
  54. return 0;
  55. }
  56. static u8 at91sam9x5_clk_smd_get_parent(struct clk_hw *hw)
  57. {
  58. struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
  59. unsigned int smdr;
  60. regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
  61. return smdr & AT91_PMC_SMDS;
  62. }
  63. static int at91sam9x5_clk_smd_set_rate(struct clk_hw *hw, unsigned long rate,
  64. unsigned long parent_rate)
  65. {
  66. struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
  67. unsigned long div = parent_rate / rate;
  68. if (parent_rate % rate || div < 1 || div > (SMD_MAX_DIV + 1))
  69. return -EINVAL;
  70. regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMD_DIV,
  71. (div - 1) << SMD_DIV_SHIFT);
  72. return 0;
  73. }
  74. static const struct clk_ops at91sam9x5_smd_ops = {
  75. .recalc_rate = at91sam9x5_clk_smd_recalc_rate,
  76. .round_rate = at91sam9x5_clk_smd_round_rate,
  77. .get_parent = at91sam9x5_clk_smd_get_parent,
  78. .set_parent = at91sam9x5_clk_smd_set_parent,
  79. .set_rate = at91sam9x5_clk_smd_set_rate,
  80. };
  81. struct clk_hw * __init
  82. at91sam9x5_clk_register_smd(struct regmap *regmap, const char *name,
  83. const char **parent_names, u8 num_parents)
  84. {
  85. struct at91sam9x5_clk_smd *smd;
  86. struct clk_hw *hw;
  87. struct clk_init_data init;
  88. int ret;
  89. smd = kzalloc(sizeof(*smd), GFP_KERNEL);
  90. if (!smd)
  91. return ERR_PTR(-ENOMEM);
  92. init.name = name;
  93. init.ops = &at91sam9x5_smd_ops;
  94. init.parent_names = parent_names;
  95. init.num_parents = num_parents;
  96. init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
  97. smd->hw.init = &init;
  98. smd->regmap = regmap;
  99. hw = &smd->hw;
  100. ret = clk_hw_register(NULL, &smd->hw);
  101. if (ret) {
  102. kfree(smd);
  103. hw = ERR_PTR(ret);
  104. }
  105. return hw;
  106. }