clkgen-fsyn.c 24 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (C) 2014 STMicroelectronics R&D Ltd
  4. */
  5. /*
  6. * Authors:
  7. * Stephen Gallimore <stephen.gallimore@st.com>,
  8. * Pankaj Dev <pankaj.dev@st.com>.
  9. */
  10. #include <linux/slab.h>
  11. #include <linux/of_address.h>
  12. #include <linux/clk.h>
  13. #include <linux/clk-provider.h>
  14. #include "clkgen.h"
  15. /*
  16. * Maximum input clock to the PLL before we divide it down by 2
  17. * although in reality in actual systems this has never been seen to
  18. * be used.
  19. */
  20. #define QUADFS_NDIV_THRESHOLD 30000000
  21. #define PLL_BW_GOODREF (0L)
  22. #define PLL_BW_VBADREF (1L)
  23. #define PLL_BW_BADREF (2L)
  24. #define PLL_BW_VGOODREF (3L)
  25. #define QUADFS_MAX_CHAN 4
  26. struct stm_fs {
  27. unsigned long ndiv;
  28. unsigned long mdiv;
  29. unsigned long pe;
  30. unsigned long sdiv;
  31. unsigned long nsdiv;
  32. };
  33. struct clkgen_quadfs_data {
  34. bool reset_present;
  35. bool bwfilter_present;
  36. bool lockstatus_present;
  37. bool powerup_polarity;
  38. bool standby_polarity;
  39. bool nsdiv_present;
  40. bool nrst_present;
  41. struct clkgen_field ndiv;
  42. struct clkgen_field ref_bw;
  43. struct clkgen_field nreset;
  44. struct clkgen_field npda;
  45. struct clkgen_field lock_status;
  46. struct clkgen_field nrst[QUADFS_MAX_CHAN];
  47. struct clkgen_field nsb[QUADFS_MAX_CHAN];
  48. struct clkgen_field en[QUADFS_MAX_CHAN];
  49. struct clkgen_field mdiv[QUADFS_MAX_CHAN];
  50. struct clkgen_field pe[QUADFS_MAX_CHAN];
  51. struct clkgen_field sdiv[QUADFS_MAX_CHAN];
  52. struct clkgen_field nsdiv[QUADFS_MAX_CHAN];
  53. const struct clk_ops *pll_ops;
  54. int (*get_params)(unsigned long, unsigned long, struct stm_fs *);
  55. int (*get_rate)(unsigned long , const struct stm_fs *,
  56. unsigned long *);
  57. };
  58. static const struct clk_ops st_quadfs_pll_c32_ops;
  59. static int clk_fs660c32_dig_get_params(unsigned long input,
  60. unsigned long output, struct stm_fs *fs);
  61. static int clk_fs660c32_dig_get_rate(unsigned long, const struct stm_fs *,
  62. unsigned long *);
  63. static const struct clkgen_quadfs_data st_fs660c32_C = {
  64. .nrst_present = true,
  65. .nrst = { CLKGEN_FIELD(0x2f0, 0x1, 0),
  66. CLKGEN_FIELD(0x2f0, 0x1, 1),
  67. CLKGEN_FIELD(0x2f0, 0x1, 2),
  68. CLKGEN_FIELD(0x2f0, 0x1, 3) },
  69. .npda = CLKGEN_FIELD(0x2f0, 0x1, 12),
  70. .nsb = { CLKGEN_FIELD(0x2f0, 0x1, 8),
  71. CLKGEN_FIELD(0x2f0, 0x1, 9),
  72. CLKGEN_FIELD(0x2f0, 0x1, 10),
  73. CLKGEN_FIELD(0x2f0, 0x1, 11) },
  74. .nsdiv_present = true,
  75. .nsdiv = { CLKGEN_FIELD(0x304, 0x1, 24),
  76. CLKGEN_FIELD(0x308, 0x1, 24),
  77. CLKGEN_FIELD(0x30c, 0x1, 24),
  78. CLKGEN_FIELD(0x310, 0x1, 24) },
  79. .mdiv = { CLKGEN_FIELD(0x304, 0x1f, 15),
  80. CLKGEN_FIELD(0x308, 0x1f, 15),
  81. CLKGEN_FIELD(0x30c, 0x1f, 15),
  82. CLKGEN_FIELD(0x310, 0x1f, 15) },
  83. .en = { CLKGEN_FIELD(0x2fc, 0x1, 0),
  84. CLKGEN_FIELD(0x2fc, 0x1, 1),
  85. CLKGEN_FIELD(0x2fc, 0x1, 2),
  86. CLKGEN_FIELD(0x2fc, 0x1, 3) },
  87. .ndiv = CLKGEN_FIELD(0x2f4, 0x7, 16),
  88. .pe = { CLKGEN_FIELD(0x304, 0x7fff, 0),
  89. CLKGEN_FIELD(0x308, 0x7fff, 0),
  90. CLKGEN_FIELD(0x30c, 0x7fff, 0),
  91. CLKGEN_FIELD(0x310, 0x7fff, 0) },
  92. .sdiv = { CLKGEN_FIELD(0x304, 0xf, 20),
  93. CLKGEN_FIELD(0x308, 0xf, 20),
  94. CLKGEN_FIELD(0x30c, 0xf, 20),
  95. CLKGEN_FIELD(0x310, 0xf, 20) },
  96. .lockstatus_present = true,
  97. .lock_status = CLKGEN_FIELD(0x2f0, 0x1, 24),
  98. .powerup_polarity = 1,
  99. .standby_polarity = 1,
  100. .pll_ops = &st_quadfs_pll_c32_ops,
  101. .get_params = clk_fs660c32_dig_get_params,
  102. .get_rate = clk_fs660c32_dig_get_rate,
  103. };
  104. static const struct clkgen_quadfs_data st_fs660c32_D = {
  105. .nrst_present = true,
  106. .nrst = { CLKGEN_FIELD(0x2a0, 0x1, 0),
  107. CLKGEN_FIELD(0x2a0, 0x1, 1),
  108. CLKGEN_FIELD(0x2a0, 0x1, 2),
  109. CLKGEN_FIELD(0x2a0, 0x1, 3) },
  110. .ndiv = CLKGEN_FIELD(0x2a4, 0x7, 16),
  111. .pe = { CLKGEN_FIELD(0x2b4, 0x7fff, 0),
  112. CLKGEN_FIELD(0x2b8, 0x7fff, 0),
  113. CLKGEN_FIELD(0x2bc, 0x7fff, 0),
  114. CLKGEN_FIELD(0x2c0, 0x7fff, 0) },
  115. .sdiv = { CLKGEN_FIELD(0x2b4, 0xf, 20),
  116. CLKGEN_FIELD(0x2b8, 0xf, 20),
  117. CLKGEN_FIELD(0x2bc, 0xf, 20),
  118. CLKGEN_FIELD(0x2c0, 0xf, 20) },
  119. .npda = CLKGEN_FIELD(0x2a0, 0x1, 12),
  120. .nsb = { CLKGEN_FIELD(0x2a0, 0x1, 8),
  121. CLKGEN_FIELD(0x2a0, 0x1, 9),
  122. CLKGEN_FIELD(0x2a0, 0x1, 10),
  123. CLKGEN_FIELD(0x2a0, 0x1, 11) },
  124. .nsdiv_present = true,
  125. .nsdiv = { CLKGEN_FIELD(0x2b4, 0x1, 24),
  126. CLKGEN_FIELD(0x2b8, 0x1, 24),
  127. CLKGEN_FIELD(0x2bc, 0x1, 24),
  128. CLKGEN_FIELD(0x2c0, 0x1, 24) },
  129. .mdiv = { CLKGEN_FIELD(0x2b4, 0x1f, 15),
  130. CLKGEN_FIELD(0x2b8, 0x1f, 15),
  131. CLKGEN_FIELD(0x2bc, 0x1f, 15),
  132. CLKGEN_FIELD(0x2c0, 0x1f, 15) },
  133. .en = { CLKGEN_FIELD(0x2ac, 0x1, 0),
  134. CLKGEN_FIELD(0x2ac, 0x1, 1),
  135. CLKGEN_FIELD(0x2ac, 0x1, 2),
  136. CLKGEN_FIELD(0x2ac, 0x1, 3) },
  137. .lockstatus_present = true,
  138. .lock_status = CLKGEN_FIELD(0x2A0, 0x1, 24),
  139. .powerup_polarity = 1,
  140. .standby_polarity = 1,
  141. .pll_ops = &st_quadfs_pll_c32_ops,
  142. .get_params = clk_fs660c32_dig_get_params,
  143. .get_rate = clk_fs660c32_dig_get_rate,};
  144. /**
  145. * DOC: A Frequency Synthesizer that multiples its input clock by a fixed factor
  146. *
  147. * Traits of this clock:
  148. * prepare - clk_(un)prepare only ensures parent is (un)prepared
  149. * enable - clk_enable and clk_disable are functional & control the Fsyn
  150. * rate - inherits rate from parent. set_rate/round_rate/recalc_rate
  151. * parent - fixed parent. No clk_set_parent support
  152. */
  153. /**
  154. * struct st_clk_quadfs_pll - A pll which outputs a fixed multiplier of
  155. * its parent clock, found inside a type of
  156. * ST quad channel frequency synthesizer block
  157. *
  158. * @hw: handle between common and hardware-specific interfaces.
  159. * @ndiv: regmap field for the ndiv control.
  160. * @regs_base: base address of the configuration registers.
  161. * @lock: spinlock.
  162. *
  163. */
  164. struct st_clk_quadfs_pll {
  165. struct clk_hw hw;
  166. void __iomem *regs_base;
  167. spinlock_t *lock;
  168. struct clkgen_quadfs_data *data;
  169. u32 ndiv;
  170. };
  171. #define to_quadfs_pll(_hw) container_of(_hw, struct st_clk_quadfs_pll, hw)
  172. static int quadfs_pll_enable(struct clk_hw *hw)
  173. {
  174. struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
  175. unsigned long flags = 0, timeout = jiffies + msecs_to_jiffies(10);
  176. if (pll->lock)
  177. spin_lock_irqsave(pll->lock, flags);
  178. /*
  179. * Bring block out of reset if we have reset control.
  180. */
  181. if (pll->data->reset_present)
  182. CLKGEN_WRITE(pll, nreset, 1);
  183. /*
  184. * Use a fixed input clock noise bandwidth filter for the moment
  185. */
  186. if (pll->data->bwfilter_present)
  187. CLKGEN_WRITE(pll, ref_bw, PLL_BW_GOODREF);
  188. CLKGEN_WRITE(pll, ndiv, pll->ndiv);
  189. /*
  190. * Power up the PLL
  191. */
  192. CLKGEN_WRITE(pll, npda, !pll->data->powerup_polarity);
  193. if (pll->lock)
  194. spin_unlock_irqrestore(pll->lock, flags);
  195. if (pll->data->lockstatus_present)
  196. while (!CLKGEN_READ(pll, lock_status)) {
  197. if (time_after(jiffies, timeout))
  198. return -ETIMEDOUT;
  199. cpu_relax();
  200. }
  201. return 0;
  202. }
  203. static void quadfs_pll_disable(struct clk_hw *hw)
  204. {
  205. struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
  206. unsigned long flags = 0;
  207. if (pll->lock)
  208. spin_lock_irqsave(pll->lock, flags);
  209. /*
  210. * Powerdown the PLL and then put block into soft reset if we have
  211. * reset control.
  212. */
  213. CLKGEN_WRITE(pll, npda, pll->data->powerup_polarity);
  214. if (pll->data->reset_present)
  215. CLKGEN_WRITE(pll, nreset, 0);
  216. if (pll->lock)
  217. spin_unlock_irqrestore(pll->lock, flags);
  218. }
  219. static int quadfs_pll_is_enabled(struct clk_hw *hw)
  220. {
  221. struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
  222. u32 npda = CLKGEN_READ(pll, npda);
  223. return pll->data->powerup_polarity ? !npda : !!npda;
  224. }
  225. static int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs,
  226. unsigned long *rate)
  227. {
  228. unsigned long nd = fs->ndiv + 16; /* ndiv value */
  229. *rate = input * nd;
  230. return 0;
  231. }
  232. static unsigned long quadfs_pll_fs660c32_recalc_rate(struct clk_hw *hw,
  233. unsigned long parent_rate)
  234. {
  235. struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
  236. unsigned long rate = 0;
  237. struct stm_fs params;
  238. params.ndiv = CLKGEN_READ(pll, ndiv);
  239. if (clk_fs660c32_vco_get_rate(parent_rate, &params, &rate))
  240. pr_err("%s:%s error calculating rate\n",
  241. clk_hw_get_name(hw), __func__);
  242. pll->ndiv = params.ndiv;
  243. return rate;
  244. }
  245. static int clk_fs660c32_vco_get_params(unsigned long input,
  246. unsigned long output, struct stm_fs *fs)
  247. {
  248. /* Formula
  249. VCO frequency = (fin x ndiv) / pdiv
  250. ndiv = VCOfreq * pdiv / fin
  251. */
  252. unsigned long pdiv = 1, n;
  253. /* Output clock range: 384Mhz to 660Mhz */
  254. if (output < 384000000 || output > 660000000)
  255. return -EINVAL;
  256. if (input > 40000000)
  257. /* This means that PDIV would be 2 instead of 1.
  258. Not supported today. */
  259. return -EINVAL;
  260. input /= 1000;
  261. output /= 1000;
  262. n = output * pdiv / input;
  263. if (n < 16)
  264. n = 16;
  265. fs->ndiv = n - 16; /* Converting formula value to reg value */
  266. return 0;
  267. }
  268. static long quadfs_pll_fs660c32_round_rate(struct clk_hw *hw,
  269. unsigned long rate,
  270. unsigned long *prate)
  271. {
  272. struct stm_fs params;
  273. if (clk_fs660c32_vco_get_params(*prate, rate, &params))
  274. return rate;
  275. clk_fs660c32_vco_get_rate(*prate, &params, &rate);
  276. pr_debug("%s: %s new rate %ld [ndiv=%u]\n",
  277. __func__, clk_hw_get_name(hw),
  278. rate, (unsigned int)params.ndiv);
  279. return rate;
  280. }
  281. static int quadfs_pll_fs660c32_set_rate(struct clk_hw *hw, unsigned long rate,
  282. unsigned long parent_rate)
  283. {
  284. struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
  285. struct stm_fs params;
  286. long hwrate = 0;
  287. unsigned long flags = 0;
  288. int ret;
  289. if (!rate || !parent_rate)
  290. return -EINVAL;
  291. ret = clk_fs660c32_vco_get_params(parent_rate, rate, &params);
  292. if (ret)
  293. return ret;
  294. clk_fs660c32_vco_get_rate(parent_rate, &params, &hwrate);
  295. pr_debug("%s: %s new rate %ld [ndiv=0x%x]\n",
  296. __func__, clk_hw_get_name(hw),
  297. hwrate, (unsigned int)params.ndiv);
  298. if (!hwrate)
  299. return -EINVAL;
  300. pll->ndiv = params.ndiv;
  301. if (pll->lock)
  302. spin_lock_irqsave(pll->lock, flags);
  303. CLKGEN_WRITE(pll, ndiv, pll->ndiv);
  304. if (pll->lock)
  305. spin_unlock_irqrestore(pll->lock, flags);
  306. return 0;
  307. }
  308. static const struct clk_ops st_quadfs_pll_c32_ops = {
  309. .enable = quadfs_pll_enable,
  310. .disable = quadfs_pll_disable,
  311. .is_enabled = quadfs_pll_is_enabled,
  312. .recalc_rate = quadfs_pll_fs660c32_recalc_rate,
  313. .round_rate = quadfs_pll_fs660c32_round_rate,
  314. .set_rate = quadfs_pll_fs660c32_set_rate,
  315. };
  316. static struct clk * __init st_clk_register_quadfs_pll(
  317. const char *name, const char *parent_name,
  318. struct clkgen_quadfs_data *quadfs, void __iomem *reg,
  319. spinlock_t *lock)
  320. {
  321. struct st_clk_quadfs_pll *pll;
  322. struct clk *clk;
  323. struct clk_init_data init;
  324. /*
  325. * Sanity check required pointers.
  326. */
  327. if (WARN_ON(!name || !parent_name))
  328. return ERR_PTR(-EINVAL);
  329. pll = kzalloc(sizeof(*pll), GFP_KERNEL);
  330. if (!pll)
  331. return ERR_PTR(-ENOMEM);
  332. init.name = name;
  333. init.ops = quadfs->pll_ops;
  334. init.flags = CLK_GET_RATE_NOCACHE;
  335. init.parent_names = &parent_name;
  336. init.num_parents = 1;
  337. pll->data = quadfs;
  338. pll->regs_base = reg;
  339. pll->lock = lock;
  340. pll->hw.init = &init;
  341. clk = clk_register(NULL, &pll->hw);
  342. if (IS_ERR(clk))
  343. kfree(pll);
  344. return clk;
  345. }
  346. /**
  347. * DOC: A digital frequency synthesizer
  348. *
  349. * Traits of this clock:
  350. * prepare - clk_(un)prepare only ensures parent is (un)prepared
  351. * enable - clk_enable and clk_disable are functional
  352. * rate - set rate is functional
  353. * parent - fixed parent. No clk_set_parent support
  354. */
  355. /**
  356. * struct st_clk_quadfs_fsynth - One clock output from a four channel digital
  357. * frequency synthesizer (fsynth) block.
  358. *
  359. * @hw: handle between common and hardware-specific interfaces
  360. *
  361. * @nsb: regmap field in the output control register for the digital
  362. * standby of this fsynth channel. This control is active low so
  363. * the channel is in standby when the control bit is cleared.
  364. *
  365. * @nsdiv: regmap field in the output control register for
  366. * for the optional divide by 3 of this fsynth channel. This control
  367. * is active low so the divide by 3 is active when the control bit is
  368. * cleared and the divide is bypassed when the bit is set.
  369. */
  370. struct st_clk_quadfs_fsynth {
  371. struct clk_hw hw;
  372. void __iomem *regs_base;
  373. spinlock_t *lock;
  374. struct clkgen_quadfs_data *data;
  375. u32 chan;
  376. /*
  377. * Cached hardware values from set_rate so we can program the
  378. * hardware in enable. There are two reasons for this:
  379. *
  380. * 1. The registers may not be writable until the parent has been
  381. * enabled.
  382. *
  383. * 2. It restores the clock rate when a driver does an enable
  384. * on PM restore, after a suspend to RAM has lost the hardware
  385. * setup.
  386. */
  387. u32 md;
  388. u32 pe;
  389. u32 sdiv;
  390. u32 nsdiv;
  391. };
  392. #define to_quadfs_fsynth(_hw) \
  393. container_of(_hw, struct st_clk_quadfs_fsynth, hw)
  394. static void quadfs_fsynth_program_enable(struct st_clk_quadfs_fsynth *fs)
  395. {
  396. /*
  397. * Pulse the program enable register lsb to make the hardware take
  398. * notice of the new md/pe values with a glitchless transition.
  399. */
  400. CLKGEN_WRITE(fs, en[fs->chan], 1);
  401. CLKGEN_WRITE(fs, en[fs->chan], 0);
  402. }
  403. static void quadfs_fsynth_program_rate(struct st_clk_quadfs_fsynth *fs)
  404. {
  405. unsigned long flags = 0;
  406. /*
  407. * Ensure the md/pe parameters are ignored while we are
  408. * reprogramming them so we can get a glitchless change
  409. * when fine tuning the speed of a running clock.
  410. */
  411. CLKGEN_WRITE(fs, en[fs->chan], 0);
  412. CLKGEN_WRITE(fs, mdiv[fs->chan], fs->md);
  413. CLKGEN_WRITE(fs, pe[fs->chan], fs->pe);
  414. CLKGEN_WRITE(fs, sdiv[fs->chan], fs->sdiv);
  415. if (fs->lock)
  416. spin_lock_irqsave(fs->lock, flags);
  417. if (fs->data->nsdiv_present)
  418. CLKGEN_WRITE(fs, nsdiv[fs->chan], fs->nsdiv);
  419. if (fs->lock)
  420. spin_unlock_irqrestore(fs->lock, flags);
  421. }
  422. static int quadfs_fsynth_enable(struct clk_hw *hw)
  423. {
  424. struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
  425. unsigned long flags = 0;
  426. pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
  427. quadfs_fsynth_program_rate(fs);
  428. if (fs->lock)
  429. spin_lock_irqsave(fs->lock, flags);
  430. CLKGEN_WRITE(fs, nsb[fs->chan], !fs->data->standby_polarity);
  431. if (fs->data->nrst_present)
  432. CLKGEN_WRITE(fs, nrst[fs->chan], 0);
  433. if (fs->lock)
  434. spin_unlock_irqrestore(fs->lock, flags);
  435. quadfs_fsynth_program_enable(fs);
  436. return 0;
  437. }
  438. static void quadfs_fsynth_disable(struct clk_hw *hw)
  439. {
  440. struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
  441. unsigned long flags = 0;
  442. pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
  443. if (fs->lock)
  444. spin_lock_irqsave(fs->lock, flags);
  445. CLKGEN_WRITE(fs, nsb[fs->chan], fs->data->standby_polarity);
  446. if (fs->lock)
  447. spin_unlock_irqrestore(fs->lock, flags);
  448. }
  449. static int quadfs_fsynth_is_enabled(struct clk_hw *hw)
  450. {
  451. struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
  452. u32 nsb = CLKGEN_READ(fs, nsb[fs->chan]);
  453. pr_debug("%s: %s enable bit = 0x%x\n",
  454. __func__, clk_hw_get_name(hw), nsb);
  455. return fs->data->standby_polarity ? !nsb : !!nsb;
  456. }
  457. #define P20 (uint64_t)(1 << 20)
  458. static int clk_fs660c32_dig_get_rate(unsigned long input,
  459. const struct stm_fs *fs, unsigned long *rate)
  460. {
  461. unsigned long s = (1 << fs->sdiv);
  462. unsigned long ns;
  463. uint64_t res;
  464. /*
  465. * 'nsdiv' is a register value ('BIN') which is translated
  466. * to a decimal value according to following rules.
  467. *
  468. * nsdiv ns.dec
  469. * 0 3
  470. * 1 1
  471. */
  472. ns = (fs->nsdiv == 1) ? 1 : 3;
  473. res = (P20 * (32 + fs->mdiv) + 32 * fs->pe) * s * ns;
  474. *rate = (unsigned long)div64_u64(input * P20 * 32, res);
  475. return 0;
  476. }
  477. static int clk_fs660c32_get_pe(int m, int si, unsigned long *deviation,
  478. signed long input, unsigned long output, uint64_t *p,
  479. struct stm_fs *fs)
  480. {
  481. unsigned long new_freq, new_deviation;
  482. struct stm_fs fs_tmp;
  483. uint64_t val;
  484. val = (uint64_t)output << si;
  485. *p = (uint64_t)input * P20 - (32LL + (uint64_t)m) * val * (P20 / 32LL);
  486. *p = div64_u64(*p, val);
  487. if (*p > 32767LL)
  488. return 1;
  489. fs_tmp.mdiv = (unsigned long) m;
  490. fs_tmp.pe = (unsigned long)*p;
  491. fs_tmp.sdiv = si;
  492. fs_tmp.nsdiv = 1;
  493. clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq);
  494. new_deviation = abs(output - new_freq);
  495. if (new_deviation < *deviation) {
  496. fs->mdiv = m;
  497. fs->pe = (unsigned long)*p;
  498. fs->sdiv = si;
  499. fs->nsdiv = 1;
  500. *deviation = new_deviation;
  501. }
  502. return 0;
  503. }
  504. static int clk_fs660c32_dig_get_params(unsigned long input,
  505. unsigned long output, struct stm_fs *fs)
  506. {
  507. int si; /* sdiv_reg (8 downto 0) */
  508. int m; /* md value */
  509. unsigned long new_freq, new_deviation;
  510. /* initial condition to say: "infinite deviation" */
  511. unsigned long deviation = ~0;
  512. uint64_t p, p1, p2; /* pe value */
  513. int r1, r2;
  514. struct stm_fs fs_tmp;
  515. for (si = 0; (si <= 8) && deviation; si++) {
  516. /* Boundary test to avoid useless iteration */
  517. r1 = clk_fs660c32_get_pe(0, si, &deviation,
  518. input, output, &p1, fs);
  519. r2 = clk_fs660c32_get_pe(31, si, &deviation,
  520. input, output, &p2, fs);
  521. /* No solution */
  522. if (r1 && r2 && (p1 > p2))
  523. continue;
  524. /* Try to find best deviation */
  525. for (m = 1; (m < 31) && deviation; m++)
  526. clk_fs660c32_get_pe(m, si, &deviation,
  527. input, output, &p, fs);
  528. }
  529. if (deviation == ~0) /* No solution found */
  530. return -1;
  531. /* pe fine tuning if deviation not 0: +/- 2 around computed pe value */
  532. if (deviation) {
  533. fs_tmp.mdiv = fs->mdiv;
  534. fs_tmp.sdiv = fs->sdiv;
  535. fs_tmp.nsdiv = fs->nsdiv;
  536. if (fs->pe > 2)
  537. p2 = fs->pe - 2;
  538. else
  539. p2 = 0;
  540. for (; p2 < 32768ll && (p2 <= (fs->pe + 2)); p2++) {
  541. fs_tmp.pe = (unsigned long)p2;
  542. clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq);
  543. new_deviation = abs(output - new_freq);
  544. /* Check if this is a better solution */
  545. if (new_deviation < deviation) {
  546. fs->pe = (unsigned long)p2;
  547. deviation = new_deviation;
  548. }
  549. }
  550. }
  551. return 0;
  552. }
  553. static int quadfs_fsynt_get_hw_value_for_recalc(struct st_clk_quadfs_fsynth *fs,
  554. struct stm_fs *params)
  555. {
  556. /*
  557. * Get the initial hardware values for recalc_rate
  558. */
  559. params->mdiv = CLKGEN_READ(fs, mdiv[fs->chan]);
  560. params->pe = CLKGEN_READ(fs, pe[fs->chan]);
  561. params->sdiv = CLKGEN_READ(fs, sdiv[fs->chan]);
  562. if (fs->data->nsdiv_present)
  563. params->nsdiv = CLKGEN_READ(fs, nsdiv[fs->chan]);
  564. else
  565. params->nsdiv = 1;
  566. /*
  567. * If All are NULL then assume no clock rate is programmed.
  568. */
  569. if (!params->mdiv && !params->pe && !params->sdiv)
  570. return 1;
  571. fs->md = params->mdiv;
  572. fs->pe = params->pe;
  573. fs->sdiv = params->sdiv;
  574. fs->nsdiv = params->nsdiv;
  575. return 0;
  576. }
  577. static long quadfs_find_best_rate(struct clk_hw *hw, unsigned long drate,
  578. unsigned long prate, struct stm_fs *params)
  579. {
  580. struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
  581. int (*clk_fs_get_rate)(unsigned long ,
  582. const struct stm_fs *, unsigned long *);
  583. int (*clk_fs_get_params)(unsigned long, unsigned long, struct stm_fs *);
  584. unsigned long rate = 0;
  585. clk_fs_get_rate = fs->data->get_rate;
  586. clk_fs_get_params = fs->data->get_params;
  587. if (!clk_fs_get_params(prate, drate, params))
  588. clk_fs_get_rate(prate, params, &rate);
  589. return rate;
  590. }
  591. static unsigned long quadfs_recalc_rate(struct clk_hw *hw,
  592. unsigned long parent_rate)
  593. {
  594. struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
  595. unsigned long rate = 0;
  596. struct stm_fs params;
  597. int (*clk_fs_get_rate)(unsigned long ,
  598. const struct stm_fs *, unsigned long *);
  599. clk_fs_get_rate = fs->data->get_rate;
  600. if (quadfs_fsynt_get_hw_value_for_recalc(fs, &params))
  601. return 0;
  602. if (clk_fs_get_rate(parent_rate, &params, &rate)) {
  603. pr_err("%s:%s error calculating rate\n",
  604. clk_hw_get_name(hw), __func__);
  605. }
  606. pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
  607. return rate;
  608. }
  609. static long quadfs_round_rate(struct clk_hw *hw, unsigned long rate,
  610. unsigned long *prate)
  611. {
  612. struct stm_fs params;
  613. rate = quadfs_find_best_rate(hw, rate, *prate, &params);
  614. pr_debug("%s: %s new rate %ld [sdiv=0x%x,md=0x%x,pe=0x%x,nsdiv3=%u]\n",
  615. __func__, clk_hw_get_name(hw),
  616. rate, (unsigned int)params.sdiv, (unsigned int)params.mdiv,
  617. (unsigned int)params.pe, (unsigned int)params.nsdiv);
  618. return rate;
  619. }
  620. static void quadfs_program_and_enable(struct st_clk_quadfs_fsynth *fs,
  621. struct stm_fs *params)
  622. {
  623. fs->md = params->mdiv;
  624. fs->pe = params->pe;
  625. fs->sdiv = params->sdiv;
  626. fs->nsdiv = params->nsdiv;
  627. /*
  628. * In some integrations you can only change the fsynth programming when
  629. * the parent entity containing it is enabled.
  630. */
  631. quadfs_fsynth_program_rate(fs);
  632. quadfs_fsynth_program_enable(fs);
  633. }
  634. static int quadfs_set_rate(struct clk_hw *hw, unsigned long rate,
  635. unsigned long parent_rate)
  636. {
  637. struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
  638. struct stm_fs params;
  639. long hwrate;
  640. if (!rate || !parent_rate)
  641. return -EINVAL;
  642. memset(&params, 0, sizeof(struct stm_fs));
  643. hwrate = quadfs_find_best_rate(hw, rate, parent_rate, &params);
  644. if (!hwrate)
  645. return -EINVAL;
  646. quadfs_program_and_enable(fs, &params);
  647. return 0;
  648. }
  649. static const struct clk_ops st_quadfs_ops = {
  650. .enable = quadfs_fsynth_enable,
  651. .disable = quadfs_fsynth_disable,
  652. .is_enabled = quadfs_fsynth_is_enabled,
  653. .round_rate = quadfs_round_rate,
  654. .set_rate = quadfs_set_rate,
  655. .recalc_rate = quadfs_recalc_rate,
  656. };
  657. static struct clk * __init st_clk_register_quadfs_fsynth(
  658. const char *name, const char *parent_name,
  659. struct clkgen_quadfs_data *quadfs, void __iomem *reg, u32 chan,
  660. unsigned long flags, spinlock_t *lock)
  661. {
  662. struct st_clk_quadfs_fsynth *fs;
  663. struct clk *clk;
  664. struct clk_init_data init;
  665. /*
  666. * Sanity check required pointers, note that nsdiv3 is optional.
  667. */
  668. if (WARN_ON(!name || !parent_name))
  669. return ERR_PTR(-EINVAL);
  670. fs = kzalloc(sizeof(*fs), GFP_KERNEL);
  671. if (!fs)
  672. return ERR_PTR(-ENOMEM);
  673. init.name = name;
  674. init.ops = &st_quadfs_ops;
  675. init.flags = flags | CLK_GET_RATE_NOCACHE;
  676. init.parent_names = &parent_name;
  677. init.num_parents = 1;
  678. fs->data = quadfs;
  679. fs->regs_base = reg;
  680. fs->chan = chan;
  681. fs->lock = lock;
  682. fs->hw.init = &init;
  683. clk = clk_register(NULL, &fs->hw);
  684. if (IS_ERR(clk))
  685. kfree(fs);
  686. return clk;
  687. }
  688. static void __init st_of_create_quadfs_fsynths(
  689. struct device_node *np, const char *pll_name,
  690. struct clkgen_quadfs_data *quadfs, void __iomem *reg,
  691. spinlock_t *lock)
  692. {
  693. struct clk_onecell_data *clk_data;
  694. int fschan;
  695. clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
  696. if (!clk_data)
  697. return;
  698. clk_data->clk_num = QUADFS_MAX_CHAN;
  699. clk_data->clks = kcalloc(QUADFS_MAX_CHAN, sizeof(struct clk *),
  700. GFP_KERNEL);
  701. if (!clk_data->clks) {
  702. kfree(clk_data);
  703. return;
  704. }
  705. for (fschan = 0; fschan < QUADFS_MAX_CHAN; fschan++) {
  706. struct clk *clk;
  707. const char *clk_name;
  708. unsigned long flags = 0;
  709. if (of_property_read_string_index(np, "clock-output-names",
  710. fschan, &clk_name)) {
  711. break;
  712. }
  713. /*
  714. * If we read an empty clock name then the channel is unused
  715. */
  716. if (*clk_name == '\0')
  717. continue;
  718. of_clk_detect_critical(np, fschan, &flags);
  719. clk = st_clk_register_quadfs_fsynth(clk_name, pll_name,
  720. quadfs, reg, fschan,
  721. flags, lock);
  722. /*
  723. * If there was an error registering this clock output, clean
  724. * up and move on to the next one.
  725. */
  726. if (!IS_ERR(clk)) {
  727. clk_data->clks[fschan] = clk;
  728. pr_debug("%s: parent %s rate %u\n",
  729. __clk_get_name(clk),
  730. __clk_get_name(clk_get_parent(clk)),
  731. (unsigned int)clk_get_rate(clk));
  732. }
  733. }
  734. of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
  735. }
  736. static void __init st_of_quadfs_setup(struct device_node *np,
  737. struct clkgen_quadfs_data *data)
  738. {
  739. struct clk *clk;
  740. const char *pll_name, *clk_parent_name;
  741. void __iomem *reg;
  742. spinlock_t *lock;
  743. reg = of_iomap(np, 0);
  744. if (!reg)
  745. return;
  746. clk_parent_name = of_clk_get_parent_name(np, 0);
  747. if (!clk_parent_name)
  748. return;
  749. pll_name = kasprintf(GFP_KERNEL, "%pOFn.pll", np);
  750. if (!pll_name)
  751. return;
  752. lock = kzalloc(sizeof(*lock), GFP_KERNEL);
  753. if (!lock)
  754. goto err_exit;
  755. spin_lock_init(lock);
  756. clk = st_clk_register_quadfs_pll(pll_name, clk_parent_name, data,
  757. reg, lock);
  758. if (IS_ERR(clk))
  759. goto err_exit;
  760. else
  761. pr_debug("%s: parent %s rate %u\n",
  762. __clk_get_name(clk),
  763. __clk_get_name(clk_get_parent(clk)),
  764. (unsigned int)clk_get_rate(clk));
  765. st_of_create_quadfs_fsynths(np, pll_name, data, reg, lock);
  766. err_exit:
  767. kfree(pll_name); /* No longer need local copy of the PLL name */
  768. }
  769. static void __init st_of_quadfs660C_setup(struct device_node *np)
  770. {
  771. st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_C);
  772. }
  773. CLK_OF_DECLARE(quadfs660C, "st,quadfs-pll", st_of_quadfs660C_setup);
  774. static void __init st_of_quadfs660D_setup(struct device_node *np)
  775. {
  776. st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_D);
  777. }
  778. CLK_OF_DECLARE(quadfs660D, "st,quadfs", st_of_quadfs660D_setup);