clk-mix.c 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509
  1. /*
  2. * mmp mix(div and mux) clock operation source file
  3. *
  4. * Copyright (C) 2014 Marvell
  5. * Chao Xie <chao.xie@marvell.com>
  6. *
  7. * This file is licensed under the terms of the GNU General Public
  8. * License version 2. This program is licensed "as is" without any
  9. * warranty of any kind, whether express or implied.
  10. */
  11. #include <linux/clk-provider.h>
  12. #include <linux/slab.h>
  13. #include <linux/io.h>
  14. #include <linux/err.h>
  15. #include "clk.h"
  16. /*
  17. * The mix clock is a clock combined mux and div type clock.
  18. * Because the div field and mux field need to be set at same
  19. * time, we can not divide it into 2 types of clock
  20. */
  21. #define to_clk_mix(hw) container_of(hw, struct mmp_clk_mix, hw)
  22. static unsigned int _get_maxdiv(struct mmp_clk_mix *mix)
  23. {
  24. unsigned int div_mask = (1 << mix->reg_info.width_div) - 1;
  25. unsigned int maxdiv = 0;
  26. struct clk_div_table *clkt;
  27. if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  28. return div_mask;
  29. if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  30. return 1 << div_mask;
  31. if (mix->div_table) {
  32. for (clkt = mix->div_table; clkt->div; clkt++)
  33. if (clkt->div > maxdiv)
  34. maxdiv = clkt->div;
  35. return maxdiv;
  36. }
  37. return div_mask + 1;
  38. }
  39. static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val)
  40. {
  41. struct clk_div_table *clkt;
  42. if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  43. return val;
  44. if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  45. return 1 << val;
  46. if (mix->div_table) {
  47. for (clkt = mix->div_table; clkt->div; clkt++)
  48. if (clkt->val == val)
  49. return clkt->div;
  50. if (clkt->div == 0)
  51. return 0;
  52. }
  53. return val + 1;
  54. }
  55. static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val)
  56. {
  57. int num_parents = clk_hw_get_num_parents(&mix->hw);
  58. int i;
  59. if (mix->mux_flags & CLK_MUX_INDEX_BIT)
  60. return ffs(val) - 1;
  61. if (mix->mux_flags & CLK_MUX_INDEX_ONE)
  62. return val - 1;
  63. if (mix->mux_table) {
  64. for (i = 0; i < num_parents; i++)
  65. if (mix->mux_table[i] == val)
  66. return i;
  67. if (i == num_parents)
  68. return 0;
  69. }
  70. return val;
  71. }
  72. static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div)
  73. {
  74. struct clk_div_table *clkt;
  75. if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  76. return div;
  77. if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  78. return __ffs(div);
  79. if (mix->div_table) {
  80. for (clkt = mix->div_table; clkt->div; clkt++)
  81. if (clkt->div == div)
  82. return clkt->val;
  83. if (clkt->div == 0)
  84. return 0;
  85. }
  86. return div - 1;
  87. }
  88. static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux)
  89. {
  90. if (mix->mux_table)
  91. return mix->mux_table[mux];
  92. return mux;
  93. }
  94. static void _filter_clk_table(struct mmp_clk_mix *mix,
  95. struct mmp_clk_mix_clk_table *table,
  96. unsigned int table_size)
  97. {
  98. int i;
  99. struct mmp_clk_mix_clk_table *item;
  100. struct clk_hw *parent, *hw;
  101. unsigned long parent_rate;
  102. hw = &mix->hw;
  103. for (i = 0; i < table_size; i++) {
  104. item = &table[i];
  105. parent = clk_hw_get_parent_by_index(hw, item->parent_index);
  106. parent_rate = clk_hw_get_rate(parent);
  107. if (parent_rate % item->rate) {
  108. item->valid = 0;
  109. } else {
  110. item->divisor = parent_rate / item->rate;
  111. item->valid = 1;
  112. }
  113. }
  114. }
  115. static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val,
  116. unsigned int change_mux, unsigned int change_div)
  117. {
  118. struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
  119. u8 width, shift;
  120. u32 mux_div, fc_req;
  121. int ret, timeout = 50;
  122. unsigned long flags = 0;
  123. if (!change_mux && !change_div)
  124. return -EINVAL;
  125. if (mix->lock)
  126. spin_lock_irqsave(mix->lock, flags);
  127. if (mix->type == MMP_CLK_MIX_TYPE_V1
  128. || mix->type == MMP_CLK_MIX_TYPE_V2)
  129. mux_div = readl(ri->reg_clk_ctrl);
  130. else
  131. mux_div = readl(ri->reg_clk_sel);
  132. if (change_div) {
  133. width = ri->width_div;
  134. shift = ri->shift_div;
  135. mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
  136. mux_div |= MMP_CLK_BITS_SET_VAL(div_val, width, shift);
  137. }
  138. if (change_mux) {
  139. width = ri->width_mux;
  140. shift = ri->shift_mux;
  141. mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
  142. mux_div |= MMP_CLK_BITS_SET_VAL(mux_val, width, shift);
  143. }
  144. if (mix->type == MMP_CLK_MIX_TYPE_V1) {
  145. writel(mux_div, ri->reg_clk_ctrl);
  146. } else if (mix->type == MMP_CLK_MIX_TYPE_V2) {
  147. mux_div |= (1 << ri->bit_fc);
  148. writel(mux_div, ri->reg_clk_ctrl);
  149. do {
  150. fc_req = readl(ri->reg_clk_ctrl);
  151. timeout--;
  152. if (!(fc_req & (1 << ri->bit_fc)))
  153. break;
  154. } while (timeout);
  155. if (timeout == 0) {
  156. pr_err("%s:%s cannot do frequency change\n",
  157. __func__, clk_hw_get_name(&mix->hw));
  158. ret = -EBUSY;
  159. goto error;
  160. }
  161. } else {
  162. fc_req = readl(ri->reg_clk_ctrl);
  163. fc_req |= 1 << ri->bit_fc;
  164. writel(fc_req, ri->reg_clk_ctrl);
  165. writel(mux_div, ri->reg_clk_sel);
  166. fc_req &= ~(1 << ri->bit_fc);
  167. }
  168. ret = 0;
  169. error:
  170. if (mix->lock)
  171. spin_unlock_irqrestore(mix->lock, flags);
  172. return ret;
  173. }
  174. static int mmp_clk_mix_determine_rate(struct clk_hw *hw,
  175. struct clk_rate_request *req)
  176. {
  177. struct mmp_clk_mix *mix = to_clk_mix(hw);
  178. struct mmp_clk_mix_clk_table *item;
  179. struct clk_hw *parent, *parent_best;
  180. unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best;
  181. unsigned long gap, gap_best;
  182. u32 div_val_max;
  183. unsigned int div;
  184. int i, j;
  185. mix_rate_best = 0;
  186. parent_rate_best = 0;
  187. gap_best = ULONG_MAX;
  188. parent_best = NULL;
  189. if (mix->table) {
  190. for (i = 0; i < mix->table_size; i++) {
  191. item = &mix->table[i];
  192. if (item->valid == 0)
  193. continue;
  194. parent = clk_hw_get_parent_by_index(hw,
  195. item->parent_index);
  196. parent_rate = clk_hw_get_rate(parent);
  197. mix_rate = parent_rate / item->divisor;
  198. gap = abs(mix_rate - req->rate);
  199. if (!parent_best || gap < gap_best) {
  200. parent_best = parent;
  201. parent_rate_best = parent_rate;
  202. mix_rate_best = mix_rate;
  203. gap_best = gap;
  204. if (gap_best == 0)
  205. goto found;
  206. }
  207. }
  208. } else {
  209. for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
  210. parent = clk_hw_get_parent_by_index(hw, i);
  211. parent_rate = clk_hw_get_rate(parent);
  212. div_val_max = _get_maxdiv(mix);
  213. for (j = 0; j < div_val_max; j++) {
  214. div = _get_div(mix, j);
  215. mix_rate = parent_rate / div;
  216. gap = abs(mix_rate - req->rate);
  217. if (!parent_best || gap < gap_best) {
  218. parent_best = parent;
  219. parent_rate_best = parent_rate;
  220. mix_rate_best = mix_rate;
  221. gap_best = gap;
  222. if (gap_best == 0)
  223. goto found;
  224. }
  225. }
  226. }
  227. }
  228. found:
  229. if (!parent_best)
  230. return -EINVAL;
  231. req->best_parent_rate = parent_rate_best;
  232. req->best_parent_hw = parent_best;
  233. req->rate = mix_rate_best;
  234. return 0;
  235. }
  236. static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw,
  237. unsigned long rate,
  238. unsigned long parent_rate,
  239. u8 index)
  240. {
  241. struct mmp_clk_mix *mix = to_clk_mix(hw);
  242. unsigned int div;
  243. u32 div_val, mux_val;
  244. div = parent_rate / rate;
  245. div_val = _get_div_val(mix, div);
  246. mux_val = _get_mux_val(mix, index);
  247. return _set_rate(mix, mux_val, div_val, 1, 1);
  248. }
  249. static u8 mmp_clk_mix_get_parent(struct clk_hw *hw)
  250. {
  251. struct mmp_clk_mix *mix = to_clk_mix(hw);
  252. struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
  253. unsigned long flags = 0;
  254. u32 mux_div = 0;
  255. u8 width, shift;
  256. u32 mux_val;
  257. if (mix->lock)
  258. spin_lock_irqsave(mix->lock, flags);
  259. if (mix->type == MMP_CLK_MIX_TYPE_V1
  260. || mix->type == MMP_CLK_MIX_TYPE_V2)
  261. mux_div = readl(ri->reg_clk_ctrl);
  262. else
  263. mux_div = readl(ri->reg_clk_sel);
  264. if (mix->lock)
  265. spin_unlock_irqrestore(mix->lock, flags);
  266. width = mix->reg_info.width_mux;
  267. shift = mix->reg_info.shift_mux;
  268. mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift);
  269. return _get_mux(mix, mux_val);
  270. }
  271. static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw,
  272. unsigned long parent_rate)
  273. {
  274. struct mmp_clk_mix *mix = to_clk_mix(hw);
  275. struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
  276. unsigned long flags = 0;
  277. u32 mux_div = 0;
  278. u8 width, shift;
  279. unsigned int div;
  280. if (mix->lock)
  281. spin_lock_irqsave(mix->lock, flags);
  282. if (mix->type == MMP_CLK_MIX_TYPE_V1
  283. || mix->type == MMP_CLK_MIX_TYPE_V2)
  284. mux_div = readl(ri->reg_clk_ctrl);
  285. else
  286. mux_div = readl(ri->reg_clk_sel);
  287. if (mix->lock)
  288. spin_unlock_irqrestore(mix->lock, flags);
  289. width = mix->reg_info.width_div;
  290. shift = mix->reg_info.shift_div;
  291. div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift));
  292. return parent_rate / div;
  293. }
  294. static int mmp_clk_set_parent(struct clk_hw *hw, u8 index)
  295. {
  296. struct mmp_clk_mix *mix = to_clk_mix(hw);
  297. struct mmp_clk_mix_clk_table *item;
  298. int i;
  299. u32 div_val, mux_val;
  300. if (mix->table) {
  301. for (i = 0; i < mix->table_size; i++) {
  302. item = &mix->table[i];
  303. if (item->valid == 0)
  304. continue;
  305. if (item->parent_index == index)
  306. break;
  307. }
  308. if (i < mix->table_size) {
  309. div_val = _get_div_val(mix, item->divisor);
  310. mux_val = _get_mux_val(mix, item->parent_index);
  311. } else
  312. return -EINVAL;
  313. } else {
  314. mux_val = _get_mux_val(mix, index);
  315. div_val = 0;
  316. }
  317. return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0);
  318. }
  319. static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
  320. unsigned long best_parent_rate)
  321. {
  322. struct mmp_clk_mix *mix = to_clk_mix(hw);
  323. struct mmp_clk_mix_clk_table *item;
  324. unsigned long parent_rate;
  325. unsigned int best_divisor;
  326. struct clk_hw *parent;
  327. int i;
  328. best_divisor = best_parent_rate / rate;
  329. if (mix->table) {
  330. for (i = 0; i < mix->table_size; i++) {
  331. item = &mix->table[i];
  332. if (item->valid == 0)
  333. continue;
  334. parent = clk_hw_get_parent_by_index(hw,
  335. item->parent_index);
  336. parent_rate = clk_hw_get_rate(parent);
  337. if (parent_rate == best_parent_rate
  338. && item->divisor == best_divisor)
  339. break;
  340. }
  341. if (i < mix->table_size)
  342. return _set_rate(mix,
  343. _get_mux_val(mix, item->parent_index),
  344. _get_div_val(mix, item->divisor),
  345. 1, 1);
  346. else
  347. return -EINVAL;
  348. } else {
  349. for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
  350. parent = clk_hw_get_parent_by_index(hw, i);
  351. parent_rate = clk_hw_get_rate(parent);
  352. if (parent_rate == best_parent_rate)
  353. break;
  354. }
  355. if (i < clk_hw_get_num_parents(hw))
  356. return _set_rate(mix, _get_mux_val(mix, i),
  357. _get_div_val(mix, best_divisor), 1, 1);
  358. else
  359. return -EINVAL;
  360. }
  361. }
  362. static int mmp_clk_mix_init(struct clk_hw *hw)
  363. {
  364. struct mmp_clk_mix *mix = to_clk_mix(hw);
  365. if (mix->table)
  366. _filter_clk_table(mix, mix->table, mix->table_size);
  367. return 0;
  368. }
  369. const struct clk_ops mmp_clk_mix_ops = {
  370. .determine_rate = mmp_clk_mix_determine_rate,
  371. .set_rate_and_parent = mmp_clk_mix_set_rate_and_parent,
  372. .set_rate = mmp_clk_set_rate,
  373. .set_parent = mmp_clk_set_parent,
  374. .get_parent = mmp_clk_mix_get_parent,
  375. .recalc_rate = mmp_clk_mix_recalc_rate,
  376. .init = mmp_clk_mix_init,
  377. };
  378. struct clk *mmp_clk_register_mix(struct device *dev,
  379. const char *name,
  380. const char * const *parent_names,
  381. u8 num_parents,
  382. unsigned long flags,
  383. struct mmp_clk_mix_config *config,
  384. spinlock_t *lock)
  385. {
  386. struct mmp_clk_mix *mix;
  387. struct clk *clk;
  388. struct clk_init_data init;
  389. size_t table_bytes;
  390. mix = kzalloc(sizeof(*mix), GFP_KERNEL);
  391. if (!mix)
  392. return ERR_PTR(-ENOMEM);
  393. init.name = name;
  394. init.flags = flags | CLK_GET_RATE_NOCACHE;
  395. init.parent_names = parent_names;
  396. init.num_parents = num_parents;
  397. init.ops = &mmp_clk_mix_ops;
  398. memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info));
  399. if (config->table) {
  400. table_bytes = sizeof(*config->table) * config->table_size;
  401. mix->table = kmemdup(config->table, table_bytes, GFP_KERNEL);
  402. if (!mix->table)
  403. goto free_mix;
  404. mix->table_size = config->table_size;
  405. }
  406. if (config->mux_table) {
  407. table_bytes = sizeof(u32) * num_parents;
  408. mix->mux_table = kmemdup(config->mux_table, table_bytes,
  409. GFP_KERNEL);
  410. if (!mix->mux_table) {
  411. kfree(mix->table);
  412. goto free_mix;
  413. }
  414. }
  415. mix->div_flags = config->div_flags;
  416. mix->mux_flags = config->mux_flags;
  417. mix->lock = lock;
  418. mix->hw.init = &init;
  419. if (config->reg_info.bit_fc >= 32)
  420. mix->type = MMP_CLK_MIX_TYPE_V1;
  421. else if (config->reg_info.reg_clk_sel)
  422. mix->type = MMP_CLK_MIX_TYPE_V3;
  423. else
  424. mix->type = MMP_CLK_MIX_TYPE_V2;
  425. clk = clk_register(dev, &mix->hw);
  426. if (IS_ERR(clk)) {
  427. kfree(mix->mux_table);
  428. kfree(mix->table);
  429. kfree(mix);
  430. }
  431. return clk;
  432. free_mix:
  433. kfree(mix);
  434. return ERR_PTR(-ENOMEM);
  435. }