Brad Bishop | 286d45c | 2018-10-02 15:21:57 -0400 | [diff] [blame] | 1 | From 217e3b6f4393926b8dcad841381527ef3fc808c2 Mon Sep 17 00:00:00 2001 |
| 2 | From: Jason Wu <jason.wu.misc@gmail.com> |
| 3 | Date: Sun, 10 Apr 2016 13:16:06 +1000 |
| 4 | Subject: [PATCH 2/3] clk: Add driver for axi_dynclk IP Core |
| 5 | |
| 6 | Add support for the axi_dynclk IP Core available from Digilent. This IP |
| 7 | core dynamically configures the clock resources inside a Xilinx FPGA to |
| 8 | generate a clock with a software programmable frequency. |
| 9 | |
| 10 | Upstream-Status: Pending |
| 11 | |
| 12 | Signed-off-by: Sam Bobrowicz <sbobrowicz@digilentinc.com> |
| 13 | Signed-off-by: Jason Wu <jason.wu.misc@gmail.com> |
| 14 | --- |
| 15 | drivers/clk/Kconfig | 8 + |
| 16 | drivers/clk/Makefile | 1 + |
| 17 | drivers/clk/clk-dglnt-dynclk.c | 547 +++++++++++++++++++++++++++++++++++++++++ |
| 18 | 3 files changed, 556 insertions(+) |
| 19 | create mode 100644 drivers/clk/clk-dglnt-dynclk.c |
| 20 | |
| 21 | diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig |
| 22 | index dccb111100..7fe65a702b 100644 |
| 23 | --- a/drivers/clk/Kconfig |
| 24 | +++ b/drivers/clk/Kconfig |
| 25 | @@ -148,6 +148,14 @@ config CLK_QORIQ |
| 26 | This adds the clock driver support for Freescale QorIQ platforms |
| 27 | using common clock framework. |
| 28 | |
| 29 | +config COMMON_CLK_DGLNT_DYNCLK |
| 30 | + tristate "Digilent axi_dynclk Driver" |
| 31 | + depends on ARCH_ZYNQ || MICROBLAZE |
| 32 | + help |
| 33 | + ---help--- |
| 34 | + Support for the Digilent AXI Dynamic Clock core for Xilinx |
| 35 | + FPGAs. |
| 36 | + |
| 37 | config COMMON_CLK_XGENE |
| 38 | bool "Clock driver for APM XGene SoC" |
| 39 | default y |
| 40 | diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile |
| 41 | index 0760449dde..45ce97d053 100644 |
| 42 | --- a/drivers/clk/Makefile |
| 43 | +++ b/drivers/clk/Makefile |
| 44 | @@ -24,6 +24,7 @@ obj-$(CONFIG_COMMON_CLK_CDCE706) += clk-cdce706.o |
| 45 | obj-$(CONFIG_COMMON_CLK_CDCE925) += clk-cdce925.o |
| 46 | obj-$(CONFIG_ARCH_CLPS711X) += clk-clps711x.o |
| 47 | obj-$(CONFIG_COMMON_CLK_CS2000_CP) += clk-cs2000-cp.o |
| 48 | +obj-$(CONFIG_COMMON_CLK_DGLNT_DYNCLK) += clk-dglnt-dynclk.o |
| 49 | obj-$(CONFIG_ARCH_EFM32) += clk-efm32gg.o |
| 50 | obj-$(CONFIG_ARCH_HIGHBANK) += clk-highbank.o |
| 51 | obj-$(CONFIG_COMMON_CLK_MAX77686) += clk-max77686.o |
| 52 | diff --git a/drivers/clk/clk-dglnt-dynclk.c b/drivers/clk/clk-dglnt-dynclk.c |
| 53 | new file mode 100644 |
| 54 | index 0000000000..496ad5fc90 |
| 55 | --- /dev/null |
| 56 | +++ b/drivers/clk/clk-dglnt-dynclk.c |
| 57 | @@ -0,0 +1,547 @@ |
| 58 | +/* |
| 59 | + * clk-dglnt-dynclk.c - Digilent AXI Dynamic Clock (axi_dynclk) Driver |
| 60 | + * |
| 61 | + * Copyright (C) 2015 Digilent |
| 62 | + * Author: Sam Bobrowicz <sbobrowicz@digilentinc.com> |
| 63 | + * |
| 64 | + * Reused code from clk-axi-clkgen.c, Copyright (C) 2012-2013 Analog Devices Inc. |
| 65 | + * |
| 66 | + * This software is licensed under the terms of the GNU General Public |
| 67 | + * License version 2, as published by the Free Software Foundation, and |
| 68 | + * may be copied, distributed, and modified under those terms. |
| 69 | + * |
| 70 | + * This program is distributed in the hope that it will be useful, |
| 71 | + * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 72 | + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 73 | + * GNU General Public License for more details. |
| 74 | + */ |
| 75 | + |
| 76 | +#include <linux/platform_device.h> |
| 77 | +#include <linux/clk-provider.h> |
| 78 | +#include <linux/clk.h> |
| 79 | +#include <linux/slab.h> |
| 80 | +#include <linux/io.h> |
| 81 | +#include <linux/of.h> |
| 82 | +#include <linux/module.h> |
| 83 | +#include <linux/err.h> |
| 84 | +#include <linux/kernel.h> |
| 85 | + |
| 86 | +#define CLK_BIT_WEDGE 13 |
| 87 | +#define CLK_BIT_NOCOUNT 12 |
| 88 | + |
| 89 | +/* This value is used to signal an error */ |
| 90 | +#define ERR_CLKCOUNTCALC 0xFFFFFFFF |
| 91 | +#define ERR_CLKDIVIDER (1 << CLK_BIT_WEDGE | 1 << CLK_BIT_NOCOUNT) |
| 92 | + |
| 93 | +#define DYNCLK_DIV_1_REGMASK 0x1041 |
| 94 | +/* 25 MHz (125 KHz / 5) */ |
| 95 | +#define DYNCLK_DEFAULT_FREQ 125000 |
| 96 | + |
| 97 | +#define MMCM_FREQ_VCOMIN 600000 |
| 98 | +#define MMCM_FREQ_VCOMAX 1200000 |
| 99 | +#define MMCM_FREQ_PFDMIN 10000 |
| 100 | +#define MMCM_FREQ_PFDMAX 450000 |
| 101 | +#define MMCM_FREQ_OUTMIN 4000 |
| 102 | +#define MMCM_FREQ_OUTMAX 800000 |
| 103 | +#define MMCM_DIV_MAX 106 |
| 104 | +#define MMCM_FB_MIN 2 |
| 105 | +#define MMCM_FB_MAX 64 |
| 106 | +#define MMCM_CLKDIV_MAX 128 |
| 107 | +#define MMCM_CLKDIV_MIN 1 |
| 108 | + |
| 109 | +#define OFST_DISPLAY_CTRL 0x0 |
| 110 | +#define OFST_DISPLAY_STATUS 0x4 |
| 111 | +#define OFST_DISPLAY_CLK_L 0x8 |
| 112 | +#define OFST_DISPLAY_FB_L 0x0C |
| 113 | +#define OFST_DISPLAY_FB_H_CLK_H 0x10 |
| 114 | +#define OFST_DISPLAY_DIV 0x14 |
| 115 | +#define OFST_DISPLAY_LOCK_L 0x18 |
| 116 | +#define OFST_DISPLAY_FLTR_LOCK_H 0x1C |
| 117 | + |
| 118 | +static const u64 lock_lookup[64] = { |
| 119 | + 0b0011000110111110100011111010010000000001, |
| 120 | + 0b0011000110111110100011111010010000000001, |
| 121 | + 0b0100001000111110100011111010010000000001, |
| 122 | + 0b0101101011111110100011111010010000000001, |
| 123 | + 0b0111001110111110100011111010010000000001, |
| 124 | + 0b1000110001111110100011111010010000000001, |
| 125 | + 0b1001110011111110100011111010010000000001, |
| 126 | + 0b1011010110111110100011111010010000000001, |
| 127 | + 0b1100111001111110100011111010010000000001, |
| 128 | + 0b1110011100111110100011111010010000000001, |
| 129 | + 0b1111111111111000010011111010010000000001, |
| 130 | + 0b1111111111110011100111111010010000000001, |
| 131 | + 0b1111111111101110111011111010010000000001, |
| 132 | + 0b1111111111101011110011111010010000000001, |
| 133 | + 0b1111111111101000101011111010010000000001, |
| 134 | + 0b1111111111100111000111111010010000000001, |
| 135 | + 0b1111111111100011111111111010010000000001, |
| 136 | + 0b1111111111100010011011111010010000000001, |
| 137 | + 0b1111111111100000110111111010010000000001, |
| 138 | + 0b1111111111011111010011111010010000000001, |
| 139 | + 0b1111111111011101101111111010010000000001, |
| 140 | + 0b1111111111011100001011111010010000000001, |
| 141 | + 0b1111111111011010100111111010010000000001, |
| 142 | + 0b1111111111011001000011111010010000000001, |
| 143 | + 0b1111111111011001000011111010010000000001, |
| 144 | + 0b1111111111010111011111111010010000000001, |
| 145 | + 0b1111111111010101111011111010010000000001, |
| 146 | + 0b1111111111010101111011111010010000000001, |
| 147 | + 0b1111111111010100010111111010010000000001, |
| 148 | + 0b1111111111010100010111111010010000000001, |
| 149 | + 0b1111111111010010110011111010010000000001, |
| 150 | + 0b1111111111010010110011111010010000000001, |
| 151 | + 0b1111111111010010110011111010010000000001, |
| 152 | + 0b1111111111010001001111111010010000000001, |
| 153 | + 0b1111111111010001001111111010010000000001, |
| 154 | + 0b1111111111010001001111111010010000000001, |
| 155 | + 0b1111111111001111101011111010010000000001, |
| 156 | + 0b1111111111001111101011111010010000000001, |
| 157 | + 0b1111111111001111101011111010010000000001, |
| 158 | + 0b1111111111001111101011111010010000000001, |
| 159 | + 0b1111111111001111101011111010010000000001, |
| 160 | + 0b1111111111001111101011111010010000000001, |
| 161 | + 0b1111111111001111101011111010010000000001, |
| 162 | + 0b1111111111001111101011111010010000000001, |
| 163 | + 0b1111111111001111101011111010010000000001, |
| 164 | + 0b1111111111001111101011111010010000000001, |
| 165 | + 0b1111111111001111101011111010010000000001, |
| 166 | + 0b1111111111001111101011111010010000000001, |
| 167 | + 0b1111111111001111101011111010010000000001, |
| 168 | + 0b1111111111001111101011111010010000000001, |
| 169 | + 0b1111111111001111101011111010010000000001, |
| 170 | + 0b1111111111001111101011111010010000000001, |
| 171 | + 0b1111111111001111101011111010010000000001, |
| 172 | + 0b1111111111001111101011111010010000000001, |
| 173 | + 0b1111111111001111101011111010010000000001, |
| 174 | + 0b1111111111001111101011111010010000000001, |
| 175 | + 0b1111111111001111101011111010010000000001, |
| 176 | + 0b1111111111001111101011111010010000000001, |
| 177 | + 0b1111111111001111101011111010010000000001, |
| 178 | + 0b1111111111001111101011111010010000000001, |
| 179 | + 0b1111111111001111101011111010010000000001, |
| 180 | + 0b1111111111001111101011111010010000000001, |
| 181 | + 0b1111111111001111101011111010010000000001, |
| 182 | + 0b1111111111001111101011111010010000000001 |
| 183 | +}; |
| 184 | + |
| 185 | +static const u32 filter_lookup_low[64] = { |
| 186 | + 0b0001011111, |
| 187 | + 0b0001010111, |
| 188 | + 0b0001111011, |
| 189 | + 0b0001011011, |
| 190 | + 0b0001101011, |
| 191 | + 0b0001110011, |
| 192 | + 0b0001110011, |
| 193 | + 0b0001110011, |
| 194 | + 0b0001110011, |
| 195 | + 0b0001001011, |
| 196 | + 0b0001001011, |
| 197 | + 0b0001001011, |
| 198 | + 0b0010110011, |
| 199 | + 0b0001010011, |
| 200 | + 0b0001010011, |
| 201 | + 0b0001010011, |
| 202 | + 0b0001010011, |
| 203 | + 0b0001010011, |
| 204 | + 0b0001010011, |
| 205 | + 0b0001010011, |
| 206 | + 0b0001010011, |
| 207 | + 0b0001010011, |
| 208 | + 0b0001010011, |
| 209 | + 0b0001100011, |
| 210 | + 0b0001100011, |
| 211 | + 0b0001100011, |
| 212 | + 0b0001100011, |
| 213 | + 0b0001100011, |
| 214 | + 0b0001100011, |
| 215 | + 0b0001100011, |
| 216 | + 0b0001100011, |
| 217 | + 0b0001100011, |
| 218 | + 0b0001100011, |
| 219 | + 0b0001100011, |
| 220 | + 0b0001100011, |
| 221 | + 0b0001100011, |
| 222 | + 0b0001100011, |
| 223 | + 0b0010010011, |
| 224 | + 0b0010010011, |
| 225 | + 0b0010010011, |
| 226 | + 0b0010010011, |
| 227 | + 0b0010010011, |
| 228 | + 0b0010010011, |
| 229 | + 0b0010010011, |
| 230 | + 0b0010010011, |
| 231 | + 0b0010010011, |
| 232 | + 0b0010010011, |
| 233 | + 0b0010100011, |
| 234 | + 0b0010100011, |
| 235 | + 0b0010100011, |
| 236 | + 0b0010100011, |
| 237 | + 0b0010100011, |
| 238 | + 0b0010100011, |
| 239 | + 0b0010100011, |
| 240 | + 0b0010100011, |
| 241 | + 0b0010100011, |
| 242 | + 0b0010100011, |
| 243 | + 0b0010100011, |
| 244 | + 0b0010100011, |
| 245 | + 0b0010100011, |
| 246 | + 0b0010100011, |
| 247 | + 0b0010100011, |
| 248 | + 0b0010100011, |
| 249 | + 0b0010100011 |
| 250 | +}; |
| 251 | + |
| 252 | +struct dglnt_dynclk_reg; |
| 253 | +struct dglnt_dynclk_mode; |
| 254 | +struct dglnt_dynclk; |
| 255 | + |
| 256 | +struct dglnt_dynclk_reg { |
| 257 | + u32 clk0L; |
| 258 | + u32 clkFBL; |
| 259 | + u32 clkFBH_clk0H; |
| 260 | + u32 divclk; |
| 261 | + u32 lockL; |
| 262 | + u32 fltr_lockH; |
| 263 | +}; |
| 264 | + |
| 265 | +struct dglnt_dynclk_mode { |
| 266 | + u32 freq; |
| 267 | + u32 fbmult; |
| 268 | + u32 clkdiv; |
| 269 | + u32 maindiv; |
| 270 | +}; |
| 271 | + |
| 272 | +struct dglnt_dynclk { |
| 273 | + void __iomem *base; |
| 274 | + struct clk_hw clk_hw; |
| 275 | + unsigned long freq; |
| 276 | +}; |
| 277 | + |
| 278 | +u32 dglnt_dynclk_divider(u32 divide) |
| 279 | +{ |
| 280 | + u32 output = 0; |
| 281 | + u32 highTime = 0; |
| 282 | + u32 lowTime = 0; |
| 283 | + |
| 284 | + if ((divide < 1) || (divide > 128)) |
| 285 | + return ERR_CLKDIVIDER; |
| 286 | + |
| 287 | + if (divide == 1) |
| 288 | + return DYNCLK_DIV_1_REGMASK; |
| 289 | + |
| 290 | + highTime = divide / 2; |
| 291 | + /* if divide is odd */ |
| 292 | + if (divide & 0x1) { |
| 293 | + lowTime = highTime + 1; |
| 294 | + output = 1 << CLK_BIT_WEDGE; |
| 295 | + } else { |
| 296 | + lowTime = highTime; |
| 297 | + } |
| 298 | + |
| 299 | + output |= 0x03F & lowTime; |
| 300 | + output |= 0xFC0 & (highTime << 6); |
| 301 | + return output; |
| 302 | +} |
| 303 | + |
| 304 | +u32 dglnt_dynclk_count_calc(u32 divide) |
| 305 | +{ |
| 306 | + u32 output = 0; |
| 307 | + u32 divCalc = 0; |
| 308 | + |
| 309 | + divCalc = dglnt_dynclk_divider(divide); |
| 310 | + if (divCalc == ERR_CLKDIVIDER) |
| 311 | + output = ERR_CLKCOUNTCALC; |
| 312 | + else |
| 313 | + output = (0xFFF & divCalc) | ((divCalc << 10) & 0x00C00000); |
| 314 | + return output; |
| 315 | +} |
| 316 | + |
| 317 | + |
| 318 | +int dglnt_dynclk_find_reg(struct dglnt_dynclk_reg *regValues, |
| 319 | + struct dglnt_dynclk_mode *clkParams) |
| 320 | +{ |
| 321 | + if ((clkParams->fbmult < 2) || clkParams->fbmult > 64) |
| 322 | + return -EINVAL; |
| 323 | + |
| 324 | + regValues->clk0L = dglnt_dynclk_count_calc(clkParams->clkdiv); |
| 325 | + if (regValues->clk0L == ERR_CLKCOUNTCALC) |
| 326 | + return -EINVAL; |
| 327 | + |
| 328 | + regValues->clkFBL = dglnt_dynclk_count_calc(clkParams->fbmult); |
| 329 | + if (regValues->clkFBL == ERR_CLKCOUNTCALC) |
| 330 | + return -EINVAL; |
| 331 | + |
| 332 | + regValues->clkFBH_clk0H = 0; |
| 333 | + |
| 334 | + regValues->divclk = dglnt_dynclk_divider(clkParams->maindiv); |
| 335 | + if (regValues->divclk == ERR_CLKDIVIDER) |
| 336 | + return -EINVAL; |
| 337 | + |
| 338 | + regValues->lockL = (u32)(lock_lookup[clkParams->fbmult - 1] & |
| 339 | + 0xFFFFFFFF); |
| 340 | + |
| 341 | + regValues->fltr_lockH = (u32)((lock_lookup[clkParams->fbmult - 1] >> |
| 342 | + 32) & 0x000000FF); |
| 343 | + regValues->fltr_lockH |= ((filter_lookup_low[clkParams->fbmult - 1] << |
| 344 | + 16) & 0x03FF0000); |
| 345 | + |
| 346 | + return 0; |
| 347 | +} |
| 348 | + |
| 349 | +void dglnt_dynclk_write_reg(struct dglnt_dynclk_reg *regValues, |
| 350 | + void __iomem *baseaddr) |
| 351 | +{ |
| 352 | + writel(regValues->clk0L, baseaddr + OFST_DISPLAY_CLK_L); |
| 353 | + writel(regValues->clkFBL, baseaddr + OFST_DISPLAY_FB_L); |
| 354 | + writel(regValues->clkFBH_clk0H, baseaddr + OFST_DISPLAY_FB_H_CLK_H); |
| 355 | + writel(regValues->divclk, baseaddr + OFST_DISPLAY_DIV); |
| 356 | + writel(regValues->lockL, baseaddr + OFST_DISPLAY_LOCK_L); |
| 357 | + writel(regValues->fltr_lockH, baseaddr + OFST_DISPLAY_FLTR_LOCK_H); |
| 358 | +} |
| 359 | + |
| 360 | +u32 dglnt_dynclk_find_mode(u32 freq, u32 parentFreq, |
| 361 | + struct dglnt_dynclk_mode *bestPick) |
| 362 | +{ |
| 363 | + u32 bestError = MMCM_FREQ_OUTMAX; |
| 364 | + u32 curError; |
| 365 | + u32 curClkMult; |
| 366 | + u32 curFreq; |
| 367 | + u32 divVal; |
| 368 | + u32 curFb, curClkDiv; |
| 369 | + u32 minFb = 0; |
| 370 | + u32 maxFb = 0; |
| 371 | + u32 curDiv = 1; |
| 372 | + u32 maxDiv; |
| 373 | + bool freq_found = false; |
| 374 | + |
| 375 | + bestPick->freq = 0; |
| 376 | + if (parentFreq == 0) |
| 377 | + return 0; |
| 378 | + |
| 379 | + /* minimum frequency is actually dictated by VCOmin */ |
| 380 | + if (freq < MMCM_FREQ_OUTMIN) |
| 381 | + freq = MMCM_FREQ_OUTMIN; |
| 382 | + if (freq > MMCM_FREQ_OUTMAX) |
| 383 | + freq = MMCM_FREQ_OUTMAX; |
| 384 | + |
| 385 | + if (parentFreq > MMCM_FREQ_PFDMAX) |
| 386 | + curDiv = 2; |
| 387 | + maxDiv = parentFreq / MMCM_FREQ_PFDMIN; |
| 388 | + if (maxDiv > MMCM_DIV_MAX) |
| 389 | + maxDiv = MMCM_DIV_MAX; |
| 390 | + |
| 391 | + while (curDiv <= maxDiv && !freq_found) { |
| 392 | + minFb = curDiv * DIV_ROUND_UP(MMCM_FREQ_VCOMIN, parentFreq); |
| 393 | + maxFb = curDiv * (MMCM_FREQ_VCOMAX / parentFreq); |
| 394 | + if (maxFb > MMCM_FB_MAX) |
| 395 | + maxFb = MMCM_FB_MAX; |
| 396 | + if (minFb < MMCM_FB_MIN) |
| 397 | + minFb = MMCM_FB_MIN; |
| 398 | + |
| 399 | + divVal = curDiv * freq; |
| 400 | + /* |
| 401 | + * This multiplier is used to find the best clkDiv value for |
| 402 | + * each FB value |
| 403 | + */ |
| 404 | + curClkMult = ((parentFreq * 1000) + (divVal / 2)) / divVal; |
| 405 | + |
| 406 | + curFb = minFb; |
| 407 | + while (curFb <= maxFb && !freq_found) { |
| 408 | + curClkDiv = ((curClkMult * curFb) + 500) / 1000; |
| 409 | + if (curClkDiv > MMCM_CLKDIV_MAX) |
| 410 | + curClkDiv = MMCM_CLKDIV_MAX; |
| 411 | + if (curClkDiv < MMCM_CLKDIV_MIN) |
| 412 | + curClkDiv = MMCM_CLKDIV_MIN; |
| 413 | + curFreq = (((parentFreq * curFb) / curDiv) / curClkDiv); |
| 414 | + if (curFreq >= freq) |
| 415 | + curError = curFreq - freq; |
| 416 | + else |
| 417 | + curError = freq - curFreq; |
| 418 | + if (curError < bestError) { |
| 419 | + bestError = curError; |
| 420 | + bestPick->clkdiv = curClkDiv; |
| 421 | + bestPick->fbmult = curFb; |
| 422 | + bestPick->maindiv = curDiv; |
| 423 | + bestPick->freq = curFreq; |
| 424 | + } |
| 425 | + if (!curError) |
| 426 | + freq_found = true; |
| 427 | + curFb++; |
| 428 | + } |
| 429 | + curDiv++; |
| 430 | + } |
| 431 | + return bestPick->freq; |
| 432 | +} |
| 433 | + |
| 434 | +static struct dglnt_dynclk *clk_hw_to_dglnt_dynclk(struct clk_hw *clk_hw) |
| 435 | +{ |
| 436 | + return container_of(clk_hw, struct dglnt_dynclk, clk_hw); |
| 437 | +} |
| 438 | + |
| 439 | + |
| 440 | +static int dglnt_dynclk_enable(struct clk_hw *clk_hw) |
| 441 | +{ |
| 442 | + struct dglnt_dynclk *dglnt_dynclk = clk_hw_to_dglnt_dynclk(clk_hw); |
| 443 | + unsigned int clock_state; |
| 444 | + |
| 445 | + if (dglnt_dynclk->freq) { |
| 446 | + writel(1, dglnt_dynclk->base + OFST_DISPLAY_CTRL); |
| 447 | + do { |
| 448 | + clock_state = readl(dglnt_dynclk->base + |
| 449 | + OFST_DISPLAY_STATUS); |
| 450 | + } while (!clock_state); |
| 451 | + } |
| 452 | + return 0; |
| 453 | +} |
| 454 | + |
| 455 | +static void dglnt_dynclk_disable(struct clk_hw *clk_hw) |
| 456 | +{ |
| 457 | + struct dglnt_dynclk *dglnt_dynclk = clk_hw_to_dglnt_dynclk(clk_hw); |
| 458 | + |
| 459 | + writel(0, dglnt_dynclk->base + OFST_DISPLAY_CTRL); |
| 460 | +} |
| 461 | + |
| 462 | +static int dglnt_dynclk_set_rate(struct clk_hw *clk_hw, |
| 463 | + unsigned long rate, unsigned long parent_rate) |
| 464 | +{ |
| 465 | + struct dglnt_dynclk *dglnt_dynclk = clk_hw_to_dglnt_dynclk(clk_hw); |
| 466 | + struct dglnt_dynclk_reg clkReg; |
| 467 | + struct dglnt_dynclk_mode clkMode; |
| 468 | + |
| 469 | + if (parent_rate == 0 || rate == 0) |
| 470 | + return -EINVAL; |
| 471 | + if (rate == dglnt_dynclk->freq) |
| 472 | + return 0; |
| 473 | + |
| 474 | + /* |
| 475 | + * Convert from Hz to KHz, then multiply by five to account for |
| 476 | + * BUFR division |
| 477 | + */ |
| 478 | + rate = (rate + 100) / 200; |
| 479 | + /* convert from Hz to KHz */ |
| 480 | + parent_rate = (parent_rate + 500) / 1000; |
| 481 | + if (!dglnt_dynclk_find_mode(rate, parent_rate, &clkMode)) |
| 482 | + return -EINVAL; |
| 483 | + |
| 484 | + /* |
| 485 | + * Write to the PLL dynamic configuration registers to configure it |
| 486 | + * with the calculated parameters. |
| 487 | + */ |
| 488 | + dglnt_dynclk_find_reg(&clkReg, &clkMode); |
| 489 | + dglnt_dynclk_write_reg(&clkReg, dglnt_dynclk->base); |
| 490 | + dglnt_dynclk->freq = clkMode.freq * 200; |
| 491 | + dglnt_dynclk_disable(clk_hw); |
| 492 | + dglnt_dynclk_enable(clk_hw); |
| 493 | + |
| 494 | + return 0; |
| 495 | +} |
| 496 | + |
| 497 | +static long dglnt_dynclk_round_rate(struct clk_hw *hw, unsigned long rate, |
| 498 | + unsigned long *parent_rate) |
| 499 | +{ |
| 500 | + struct dglnt_dynclk_mode clkMode; |
| 501 | + |
| 502 | + dglnt_dynclk_find_mode(((rate + 100) / 200), |
| 503 | + ((*parent_rate) + 500) / 1000, &clkMode); |
| 504 | + |
| 505 | + return (clkMode.freq * 200); |
| 506 | +} |
| 507 | + |
| 508 | +static unsigned long dglnt_dynclk_recalc_rate(struct clk_hw *clk_hw, |
| 509 | + unsigned long parent_rate) |
| 510 | +{ |
| 511 | + struct dglnt_dynclk *dglnt_dynclk = clk_hw_to_dglnt_dynclk(clk_hw); |
| 512 | + |
| 513 | + return dglnt_dynclk->freq; |
| 514 | +} |
| 515 | + |
| 516 | + |
| 517 | +static const struct clk_ops dglnt_dynclk_ops = { |
| 518 | + .recalc_rate = dglnt_dynclk_recalc_rate, |
| 519 | + .round_rate = dglnt_dynclk_round_rate, |
| 520 | + .set_rate = dglnt_dynclk_set_rate, |
| 521 | + .enable = dglnt_dynclk_enable, |
| 522 | + .disable = dglnt_dynclk_disable, |
| 523 | +}; |
| 524 | + |
| 525 | +static const struct of_device_id dglnt_dynclk_ids[] = { |
| 526 | + { .compatible = "digilent,axi-dynclk", }, |
| 527 | + { }, |
| 528 | +}; |
| 529 | +MODULE_DEVICE_TABLE(of, dglnt_dynclk_ids); |
| 530 | + |
| 531 | +static int dglnt_dynclk_probe(struct platform_device *pdev) |
| 532 | +{ |
| 533 | + const struct of_device_id *id; |
| 534 | + struct dglnt_dynclk *dglnt_dynclk; |
| 535 | + struct clk_init_data init; |
| 536 | + const char *parent_name; |
| 537 | + const char *clk_name; |
| 538 | + struct resource *mem; |
| 539 | + struct clk *clk; |
| 540 | + |
| 541 | + if (!pdev->dev.of_node) |
| 542 | + return -ENODEV; |
| 543 | + |
| 544 | + id = of_match_node(dglnt_dynclk_ids, pdev->dev.of_node); |
| 545 | + if (!id) |
| 546 | + return -ENODEV; |
| 547 | + |
| 548 | + dglnt_dynclk = devm_kzalloc(&pdev->dev, sizeof(*dglnt_dynclk), |
| 549 | + GFP_KERNEL); |
| 550 | + if (!dglnt_dynclk) |
| 551 | + return -ENOMEM; |
| 552 | + |
| 553 | + mem = platform_get_resource(pdev, IORESOURCE_MEM, 0); |
| 554 | + dglnt_dynclk->base = devm_ioremap_resource(&pdev->dev, mem); |
| 555 | + if (IS_ERR(dglnt_dynclk->base)) |
| 556 | + return PTR_ERR(dglnt_dynclk->base); |
| 557 | + |
| 558 | + parent_name = of_clk_get_parent_name(pdev->dev.of_node, 0); |
| 559 | + if (!parent_name) |
| 560 | + return -EINVAL; |
| 561 | + |
| 562 | + clk_name = pdev->dev.of_node->name; |
| 563 | + of_property_read_string(pdev->dev.of_node, "clock-output-names", |
| 564 | + &clk_name); |
| 565 | + |
| 566 | + init.name = clk_name; |
| 567 | + init.ops = &dglnt_dynclk_ops; |
| 568 | + init.flags = 0; |
| 569 | + init.parent_names = &parent_name; |
| 570 | + init.num_parents = 1; |
| 571 | + |
| 572 | + dglnt_dynclk->freq = 0; |
| 573 | + dglnt_dynclk_disable(&dglnt_dynclk->clk_hw); |
| 574 | + |
| 575 | + dglnt_dynclk->clk_hw.init = &init; |
| 576 | + clk = devm_clk_register(&pdev->dev, &dglnt_dynclk->clk_hw); |
| 577 | + if (IS_ERR(clk)) |
| 578 | + return PTR_ERR(clk); |
| 579 | + |
| 580 | + return of_clk_add_provider(pdev->dev.of_node, of_clk_src_simple_get, |
| 581 | + clk); |
| 582 | +} |
| 583 | + |
| 584 | +static int dglnt_dynclk_remove(struct platform_device *pdev) |
| 585 | +{ |
| 586 | + of_clk_del_provider(pdev->dev.of_node); |
| 587 | + |
| 588 | + return 0; |
| 589 | +} |
| 590 | + |
| 591 | +static struct platform_driver dglnt_dynclk_driver = { |
| 592 | + .driver = { |
| 593 | + .name = "dglnt-dynclk", |
| 594 | + .owner = THIS_MODULE, |
| 595 | + .of_match_table = dglnt_dynclk_ids, |
| 596 | + }, |
| 597 | + .probe = dglnt_dynclk_probe, |
| 598 | + .remove = dglnt_dynclk_remove, |
| 599 | +}; |
| 600 | +module_platform_driver(dglnt_dynclk_driver); |
| 601 | + |
| 602 | +MODULE_LICENSE("GPL v2"); |
| 603 | +MODULE_AUTHOR("Sam Bobrowicz <sbobrowicz@digilentinc.com>"); |
| 604 | +MODULE_DESCRIPTION("CCF Driver for Digilent axi_dynclk IP Core"); |
| 605 | -- |
| 606 | 2.14.2 |
| 607 | |