clk-mux.c 5.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copyright (C) 2019 DENX Software Engineering
  4. * Lukasz Majewski, DENX Software Engineering, lukma@denx.de
  5. *
  6. * Copyright (C) 2011 Sascha Hauer, Pengutronix <s.hauer@pengutronix.de>
  7. * Copyright (C) 2011 Richard Zhao, Linaro <richard.zhao@linaro.org>
  8. * Copyright (C) 2011-2012 Mike Turquette, Linaro Ltd <mturquette@linaro.org>
  9. *
  10. * Simple multiplexer clock implementation
  11. */
  12. /*
  13. * U-Boot CCF porting node:
  14. *
  15. * The Linux kernel - as of tag: 5.0-rc3 is using also the imx_clk_fixup_mux()
  16. * version of CCF mux. It is used on e.g. imx6q to provide fixes (like
  17. * imx_cscmr1_fixup) for broken HW.
  18. *
  19. * At least for IMX6Q (but NOT IMX6QP) it is important when we set the parent
  20. * clock.
  21. */
  22. #include <common.h>
  23. #include <asm/io.h>
  24. #include <malloc.h>
  25. #include <clk-uclass.h>
  26. #include <dm/device.h>
  27. #include <dm/devres.h>
  28. #include <linux/bitops.h>
  29. #include <linux/clk-provider.h>
  30. #include <clk.h>
  31. #include "clk.h"
  32. #include <linux/err.h>
  33. #define UBOOT_DM_CLK_CCF_MUX "ccf_clk_mux"
  34. int clk_mux_val_to_index(struct clk *clk, u32 *table, unsigned int flags,
  35. unsigned int val)
  36. {
  37. struct clk_mux *mux = to_clk_mux(clk);
  38. int num_parents = mux->num_parents;
  39. if (table) {
  40. int i;
  41. for (i = 0; i < num_parents; i++)
  42. if (table[i] == val)
  43. return i;
  44. return -EINVAL;
  45. }
  46. if (val && (flags & CLK_MUX_INDEX_BIT))
  47. val = ffs(val) - 1;
  48. if (val && (flags & CLK_MUX_INDEX_ONE))
  49. val--;
  50. if (val >= num_parents)
  51. return -EINVAL;
  52. return val;
  53. }
  54. unsigned int clk_mux_index_to_val(u32 *table, unsigned int flags, u8 index)
  55. {
  56. unsigned int val = index;
  57. if (table) {
  58. val = table[index];
  59. } else {
  60. if (flags & CLK_MUX_INDEX_BIT)
  61. val = 1 << index;
  62. if (flags & CLK_MUX_INDEX_ONE)
  63. val++;
  64. }
  65. return val;
  66. }
  67. u8 clk_mux_get_parent(struct clk *clk)
  68. {
  69. struct clk_mux *mux = to_clk_mux(clk);
  70. u32 val;
  71. #if CONFIG_IS_ENABLED(SANDBOX_CLK_CCF)
  72. val = mux->io_mux_val;
  73. #else
  74. val = readl(mux->reg);
  75. #endif
  76. val >>= mux->shift;
  77. val &= mux->mask;
  78. return clk_mux_val_to_index(clk, mux->table, mux->flags, val);
  79. }
  80. static int clk_fetch_parent_index(struct clk *clk,
  81. struct clk *parent)
  82. {
  83. struct clk_mux *mux = to_clk_mux(clk);
  84. int i;
  85. if (!parent)
  86. return -EINVAL;
  87. for (i = 0; i < mux->num_parents; i++) {
  88. if (!strcmp(parent->dev->name, mux->parent_names[i]))
  89. return i;
  90. }
  91. return -EINVAL;
  92. }
  93. static int clk_mux_set_parent(struct clk *clk, struct clk *parent)
  94. {
  95. struct clk_mux *mux = to_clk_mux(clk);
  96. int index;
  97. u32 val;
  98. u32 reg;
  99. index = clk_fetch_parent_index(clk, parent);
  100. if (index < 0) {
  101. printf("Could not fetch index\n");
  102. return index;
  103. }
  104. val = clk_mux_index_to_val(mux->table, mux->flags, index);
  105. if (mux->flags & CLK_MUX_HIWORD_MASK) {
  106. reg = mux->mask << (mux->shift + 16);
  107. } else {
  108. reg = readl(mux->reg);
  109. reg &= ~(mux->mask << mux->shift);
  110. }
  111. val = val << mux->shift;
  112. reg |= val;
  113. writel(reg, mux->reg);
  114. return 0;
  115. }
  116. const struct clk_ops clk_mux_ops = {
  117. .get_rate = clk_generic_get_rate,
  118. .set_parent = clk_mux_set_parent,
  119. };
  120. struct clk *clk_hw_register_mux_table(struct device *dev, const char *name,
  121. const char * const *parent_names, u8 num_parents,
  122. unsigned long flags,
  123. void __iomem *reg, u8 shift, u32 mask,
  124. u8 clk_mux_flags, u32 *table)
  125. {
  126. struct clk_mux *mux;
  127. struct clk *clk;
  128. u8 width = 0;
  129. int ret;
  130. if (clk_mux_flags & CLK_MUX_HIWORD_MASK) {
  131. width = fls(mask) - ffs(mask) + 1;
  132. if (width + shift > 16) {
  133. pr_err("mux value exceeds LOWORD field\n");
  134. return ERR_PTR(-EINVAL);
  135. }
  136. }
  137. /* allocate the mux */
  138. mux = kzalloc(sizeof(*mux), GFP_KERNEL);
  139. if (!mux)
  140. return ERR_PTR(-ENOMEM);
  141. /* U-boot specific assignments */
  142. mux->parent_names = parent_names;
  143. mux->num_parents = num_parents;
  144. /* struct clk_mux assignments */
  145. mux->reg = reg;
  146. mux->shift = shift;
  147. mux->mask = mask;
  148. mux->flags = clk_mux_flags;
  149. mux->table = table;
  150. #if CONFIG_IS_ENABLED(SANDBOX_CLK_CCF)
  151. mux->io_mux_val = *(u32 *)reg;
  152. #endif
  153. clk = &mux->clk;
  154. /*
  155. * Read the current mux setup - so we assign correct parent.
  156. *
  157. * Changing parent would require changing internals of udevice struct
  158. * for the corresponding clock (to do that define .set_parent() method.
  159. */
  160. ret = clk_register(clk, UBOOT_DM_CLK_CCF_MUX, name,
  161. parent_names[clk_mux_get_parent(clk)]);
  162. if (ret) {
  163. kfree(mux);
  164. return ERR_PTR(ret);
  165. }
  166. return clk;
  167. }
  168. struct clk *clk_register_mux_table(struct device *dev, const char *name,
  169. const char * const *parent_names, u8 num_parents,
  170. unsigned long flags,
  171. void __iomem *reg, u8 shift, u32 mask,
  172. u8 clk_mux_flags, u32 *table)
  173. {
  174. struct clk *clk;
  175. clk = clk_hw_register_mux_table(dev, name, parent_names, num_parents,
  176. flags, reg, shift, mask, clk_mux_flags,
  177. table);
  178. if (IS_ERR(clk))
  179. return ERR_CAST(clk);
  180. return clk;
  181. }
  182. struct clk *clk_register_mux(struct device *dev, const char *name,
  183. const char * const *parent_names, u8 num_parents,
  184. unsigned long flags,
  185. void __iomem *reg, u8 shift, u8 width,
  186. u8 clk_mux_flags)
  187. {
  188. u32 mask = BIT(width) - 1;
  189. return clk_register_mux_table(dev, name, parent_names, num_parents,
  190. flags, reg, shift, mask, clk_mux_flags,
  191. NULL);
  192. }
  193. U_BOOT_DRIVER(ccf_clk_mux) = {
  194. .name = UBOOT_DM_CLK_CCF_MUX,
  195. .id = UCLASS_CLK,
  196. .ops = &clk_mux_ops,
  197. .flags = DM_FLAG_PRE_RELOC,
  198. };