sys/dev/qcom_clk/qcom_clk_rcg2.c

   1 /*-
   2  * Copyright (c) 2021 Adrian Chadd <adrian@FreeBSD.org>.
   3  *
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions
   6  * are met:
   7  * 1. Redistributions of source code must retain the above copyright
   8  *    notice, this list of conditions and the following disclaimer.
   9  * 2. Redistributions in binary form must reproduce the above copyright
  10  *    notice, this list of conditions and the following disclaimer in the
  11  *    documentation and/or other materials provided with the distribution.
  12  *
  13  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  14  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  15  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  16  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  17  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  18  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  19  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  20  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  21  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  22  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  23  * SUCH DAMAGE.
  24  */
  25
  26 #include <sys/cdefs.h>
  27 __FBSDID("$FreeBSD$");
  28
  29 #include <sys/param.h>
  30 #include <sys/systm.h>
  31 #include <sys/bus.h>
  32 #include <sys/lock.h>
  33 #include <sys/mutex.h>
  34 #include <sys/rman.h>
  35 #include <machine/bus.h>
  36
  37 #include <dev/extres/clk/clk.h>
  38 #include <dev/extres/clk/clk_div.h>
  39 #include <dev/extres/clk/clk_fixed.h>
  40 #include <dev/extres/clk/clk_mux.h>
  41
  42 #include "qcom_clk_freqtbl.h"
  43 #include "qcom_clk_rcg2.h"
  44 #include "qcom_clk_rcg2_reg.h"
  45
  46 #include "clkdev_if.h"
  47
  48 #if 0
  49 #define DPRINTF(dev, msg...) device_printf(dev, msg);
  50 #else
  51 #define DPRINTF(dev, msg...)
  52 #endif
  53
  54 #define QCOM_CLK_RCG2_CFG_OFFSET(sc)    \
  55             ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_CFG_REG)
  56 #define QCOM_CLK_RCG2_CMD_REGISTER(sc)  \
  57             ((sc)->cmd_rcgr + QCOM_CLK_RCG2_CMD_REG)
  58 #define QCOM_CLK_RCG2_M_OFFSET(sc)      \
  59             ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_M_REG)
  60 #define QCOM_CLK_RCG2_N_OFFSET(sc)      \
  61             ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_N_REG)
  62 #define QCOM_CLK_RCG2_D_OFFSET(sc)      \
  63             ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_D_REG)
  64
  65 struct qcom_clk_rcg2_sc {
  66         struct clknode *clknode;
  67         uint32_t cmd_rcgr;
  68         uint32_t hid_width;
  69         uint32_t mnd_width;
  70         int32_t safe_src_idx;
  71         uint32_t cfg_offset;
  72         int safe_pre_parent_idx;
  73         uint32_t flags;
  74         const struct qcom_clk_freq_tbl *freq_tbl;
  75 };
  76
  77
  78 /*
  79  * Finish a clock update.
  80  *
  81  * This instructs the configuration to take effect.
  82  */
  83 static bool
  84 qcom_clk_rcg2_update_config_locked(struct qcom_clk_rcg2_sc *sc)
  85 {
  86         uint32_t reg, count;
  87
  88         /*
  89          * Send "update" to the controller.
  90          */
  91         CLKDEV_READ_4(clknode_get_device(sc->clknode),
  92             QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
  93         reg |= QCOM_CLK_RCG2_CMD_UPDATE;
  94         CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
  95             QCOM_CLK_RCG2_CMD_REGISTER(sc), reg);
  96         wmb();
  97
  98         /*
  99          * Poll for completion of update.
 100          */
 101         for (count = 0; count < 1000; count++) {
 102                 CLKDEV_READ_4(clknode_get_device(sc->clknode),
 103                     QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
 104                 if ((reg & QCOM_CLK_RCG2_CMD_UPDATE) == 0) {
 105                         return (true);
 106                 }
 107                 DELAY(10);
 108                 rmb();
 109         }
 110
 111         CLKDEV_READ_4(clknode_get_device(sc->clknode),
 112             QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
 113         DPRINTF(clknode_get_device(sc->clknode), "%s: failed; reg=0x%08x\n",
 114             __func__, reg);
 115         return (false);
 116 }
 117
 118 /*
 119  * Calculate the output frequency given an input frequency and the m/n:d
 120  * configuration.
 121  */
 122 static uint64_t
 123 qcom_clk_rcg2_calc_rate(uint64_t rate, uint32_t mode, uint32_t m, uint32_t n,
 124     uint32_t hid_div)
 125 {
 126         if (hid_div != 0) {
 127                 rate = rate * 2;
 128                 rate = rate / (hid_div + 1);
 129         }
 130
 131         /* Note: assume n is not 0 here; bad things happen if it is */
 132
 133         if (mode != 0) {
 134                 rate = (rate * m) / n;
 135         }
 136
 137         return (rate);
 138 }
 139
 140 /*
 141  * The inverse of calc_rate() - calculate the required input frequency
 142  * given the desired output freqency and m/n:d configuration.
 143  */
 144 static uint64_t
 145 qcom_clk_rcg2_calc_input_freq(uint64_t freq, uint32_t m, uint32_t n,
 146     uint32_t hid_div)
 147 {
 148         if (hid_div != 0) {
 149                 freq = freq / 2;
 150                 freq = freq * (hid_div + 1);
 151         }
 152
 153         if (n != 0) {
 154                 freq = (freq * n) / m;
 155         }
 156
 157         return (freq);
 158 }
 159
 160 static int
 161 qcom_clk_rcg2_recalc(struct clknode *clk, uint64_t *freq)
 162 {
 163         struct qcom_clk_rcg2_sc *sc;
 164         uint32_t cfg, m = 0, n = 0, hid_div = 0;
 165         uint32_t mode = 0, mask;
 166
 167         sc = clknode_get_softc(clk);
 168
 169         /* Read the MODE, CFG, M and N parameters */
 170         CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 171         CLKDEV_READ_4(clknode_get_device(sc->clknode),
 172             QCOM_CLK_RCG2_CFG_OFFSET(sc),
 173             &cfg);
 174         if (sc->mnd_width != 0) {
 175                 mask = (1U << sc->mnd_width) - 1;
 176                 CLKDEV_READ_4(clknode_get_device(sc->clknode),
 177                     QCOM_CLK_RCG2_M_OFFSET(sc), &m);
 178                 CLKDEV_READ_4(clknode_get_device(sc->clknode),
 179                     QCOM_CLK_RCG2_N_OFFSET(sc), &n);
 180                 m = m & mask;
 181                 n = ~ n;
 182                 n = n & mask;
 183                 n = n + m;
 184                 mode = (cfg & QCOM_CLK_RCG2_CFG_MODE_MASK)
 185                     >> QCOM_CLK_RCG2_CFG_MODE_SHIFT;
 186         }
 187         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 188
 189         /* Fetch the divisor */
 190         mask = (1U << sc->hid_width) - 1;
 191         hid_div = (cfg >> QCOM_CLK_RCG2_CFG_SRC_DIV_SHIFT) & mask;
 192
 193         /* Calculate the rate based on the parent rate and config */
 194         *freq = qcom_clk_rcg2_calc_rate(*freq, mode, m, n, hid_div);
 195
 196         return (0);
 197 }
 198
 199 /*
 200  * configure the mn:d divisor, pre-divisor, and parent.
 201  */
 202 static void
 203 qcom_clk_rcg2_set_config_locked(struct qcom_clk_rcg2_sc *sc,
 204     const struct qcom_clk_freq_tbl *f, int parent_idx)
 205 {
 206         uint32_t mask, reg;
 207
 208         /* If we have MN:D, then update it */
 209         if (sc->mnd_width != 0 && f->n != 0) {
 210                 mask = (1U << sc->mnd_width) - 1;
 211
 212                 CLKDEV_READ_4(clknode_get_device(sc->clknode),
 213                     QCOM_CLK_RCG2_M_OFFSET(sc), &reg);
 214                 reg &= ~mask;
 215                 reg |= (f->m & mask);
 216                 CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
 217                     QCOM_CLK_RCG2_M_OFFSET(sc), reg);
 218
 219                 CLKDEV_READ_4(clknode_get_device(sc->clknode),
 220                     QCOM_CLK_RCG2_N_OFFSET(sc), &reg);
 221                 reg &= ~mask;
 222                 reg |= ((~(f->n - f->m)) & mask);
 223                 CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
 224                     QCOM_CLK_RCG2_N_OFFSET(sc), reg);
 225
 226                 CLKDEV_READ_4(clknode_get_device(sc->clknode),
 227                     QCOM_CLK_RCG2_D_OFFSET(sc), &reg);
 228                 reg &= ~mask;
 229                 reg |= ((~f->n) & mask);
 230                 CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
 231                     QCOM_CLK_RCG2_D_OFFSET(sc), reg);
 232         }
 233
 234         mask = (1U << sc->hid_width) - 1;
 235         /*
 236          * Mask out register fields we're going to modify along with
 237          * the pre-divisor.
 238          */
 239         mask |= QCOM_CLK_RCG2_CFG_SRC_SEL_MASK
 240             | QCOM_CLK_RCG2_CFG_MODE_MASK
 241             | QCOM_CLK_RCG2_CFG_HW_CLK_CTRL_MASK;
 242
 243         CLKDEV_READ_4(clknode_get_device(sc->clknode),
 244             QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
 245         reg &= ~mask;
 246
 247         /* Configure pre-divisor */
 248         reg = reg | ((f->pre_div) << QCOM_CLK_RCG2_CFG_SRC_DIV_SHIFT);
 249
 250         /* Configure parent clock */
 251         reg = reg | (((parent_idx << QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT)
 252             & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK));
 253
 254         /* Configure dual-edge if needed */
 255         if (sc->mnd_width != 0 && f->n != 0 && (f->m != f->n))
 256                 reg |= QCOM_CLK_RCG2_CFG_MODE_DUAL_EDGE;
 257
 258         CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
 259             QCOM_CLK_RCG2_CFG_OFFSET(sc), reg);
 260 }
 261
 262 static int
 263 qcom_clk_rcg2_init(struct clknode *clk, device_t dev)
 264 {
 265         struct qcom_clk_rcg2_sc *sc;
 266         uint32_t reg;
 267         uint32_t idx;
 268         bool enabled __unused;
 269
 270         sc = clknode_get_softc(clk);
 271
 272         /*
 273          * Read the mux setting to set the right parent.
 274          * Whilst here, read the config to get whether we're enabled
 275          * or not.
 276          */
 277         CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 278         /* check if rcg2 root clock is enabled */
 279         CLKDEV_READ_4(clknode_get_device(sc->clknode),
 280             QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
 281         if (reg & QCOM_CLK_RCG2_CMD_ROOT_OFF)
 282                 enabled = false;
 283         else
 284                 enabled = true;
 285
 286         /* mux settings */
 287         CLKDEV_READ_4(clknode_get_device(sc->clknode),
 288             QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
 289         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 290
 291         idx = (reg & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK)
 292             >> QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT;
 293         DPRINTF(clknode_get_device(sc->clknode),
 294             "%s: mux index %u, enabled=%d\n",
 295             __func__, idx, enabled);
 296         clknode_init_parent_idx(clk, idx);
 297
 298         /*
 299          * If we could be sure our parent clocks existed here in the tree,
 300          * we could calculate our current frequency by fetching the parent
 301          * frequency and then do our divider math.  Unfortunately that
 302          * currently isn't the case.
 303          */
 304
 305         return(0);
 306 }
 307
 308 static int
 309 qcom_clk_rcg2_set_gate(struct clknode *clk, bool enable)
 310 {
 311
 312         /*
 313          * For now this isn't supported; there's some support for
 314          * "shared" rcg2 nodes in the Qualcomm/upstream Linux trees but
 315          * it's not currently needed for the supported platforms.
 316          */
 317         return (0);
 318 }
 319
 320 /*
 321  * Program the parent index.
 322  *
 323  * This doesn't do the update.  It also must be called with the device
 324  * lock held.
 325  */
 326 static void
 327 qcom_clk_rcg2_set_parent_index_locked(struct qcom_clk_rcg2_sc *sc,
 328     uint32_t index)
 329 {
 330         uint32_t reg;
 331
 332         CLKDEV_READ_4(clknode_get_device(sc->clknode),
 333             QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
 334         reg = reg & ~QCOM_CLK_RCG2_CFG_SRC_SEL_MASK;
 335         reg = reg | (((index << QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT)
 336             & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK));
 337         CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
 338             QCOM_CLK_RCG2_CFG_OFFSET(sc),
 339             reg);
 340 }
 341
 342 /*
 343  * Set frequency
 344  *
 345  * fin - the parent frequency, if exists
 346  * fout - starts as the requested frequency, ends with the configured
 347  *        or dry-run frequency
 348  * Flags - CLK_SET_DRYRUN, CLK_SET_ROUND_UP, CLK_SET_ROUND_DOWN
 349  * retval - 0, ERANGE
 350  */
 351 static int
 352 qcom_clk_rcg2_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
 353     int flags, int *stop)
 354 {
 355         struct qcom_clk_rcg2_sc *sc;
 356         const struct qcom_clk_freq_tbl *f;
 357         const char **parent_names;
 358         uint64_t p_freq, p_clk_freq;
 359         int parent_cnt;
 360         struct clknode *p_clk;
 361         int i;
 362
 363         sc = clknode_get_softc(clk);
 364
 365         /*
 366          * Find a suitable frequency in the frequency table.
 367          *
 368          * TODO: should pay attention to ROUND_UP / ROUND_DOWN and add
 369          * a freqtbl method to handle both accordingly.
 370          */
 371         f = qcom_clk_freq_tbl_lookup(sc->freq_tbl, *fout);
 372         if (f == NULL) {
 373                 device_printf(clknode_get_device(sc->clknode),
 374                     "%s: no suitable freqtbl entry found for freq %llu\n",
 375                     __func__,
 376                     *fout);
 377                 return (ERANGE);
 378         }
 379
 380         /*
 381          * Find the parent index for the given parent clock.
 382          * Abort if we can't actually find it.
 383          *
 384          * XXX TODO: this should be a clk API call!
 385          */
 386         parent_cnt = clknode_get_parents_num(clk);
 387         parent_names = clknode_get_parent_names(clk);
 388         for (i = 0; i < parent_cnt; i++) {
 389                 if (parent_names[i] == NULL)
 390                         continue;
 391                 if (strcmp(parent_names[i], f->parent) == 0)
 392                         break;
 393         }
 394         if (i >= parent_cnt) {
 395                 device_printf(clknode_get_device(sc->clknode),
 396                     "%s: couldn't find suitable parent?\n",
 397                     __func__);
 398                 return (ENXIO);
 399         }
 400
 401         /*
 402          * If we aren't setting the parent clock, then we need
 403          * to just program the new parent clock in and update.
 404          * (or for DRYRUN just skip that and return the new
 405          * frequency.)
 406          */
 407         if ((sc->flags & QCOM_CLK_RCG2_FLAGS_SET_RATE_PARENT) == 0) {
 408                 if (flags & CLK_SET_DRYRUN) {
 409                         *fout = f->freq;
 410                         return (0);
 411                 }
 412
 413                 if (sc->safe_pre_parent_idx > -1) {
 414                         DPRINTF(clknode_get_device(sc->clknode),
 415                             "%s: setting to safe parent idx %d\n",
 416                             __func__,
 417                             sc->safe_pre_parent_idx);
 418                         CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 419                         qcom_clk_rcg2_set_parent_index_locked(sc,
 420                             sc->safe_pre_parent_idx);
 421                         DPRINTF(clknode_get_device(sc->clknode),
 422                             "%s: safe parent: updating config\n", __func__);
 423                         if (! qcom_clk_rcg2_update_config_locked(sc)) {
 424                                 CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 425                                 DPRINTF(clknode_get_device(sc->clknode),
 426                                     "%s: error updating config\n",
 427                                     __func__);
 428                                 return (ENXIO);
 429                         }
 430                         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 431                         DPRINTF(clknode_get_device(sc->clknode),
 432                             "%s: safe parent: done\n", __func__);
 433                         clknode_set_parent_by_idx(sc->clknode,
 434                             sc->safe_pre_parent_idx);
 435                 }
 436                 /* Program parent index, then schedule update */
 437                 CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 438                 qcom_clk_rcg2_set_parent_index_locked(sc, i);
 439                 if (! qcom_clk_rcg2_update_config_locked(sc)) {
 440                         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 441                         device_printf(clknode_get_device(sc->clknode),
 442                             "%s: couldn't program in parent idx %u!\n",
 443                             __func__, i);
 444                         return (ENXIO);
 445                 }
 446                 CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 447                 clknode_set_parent_by_idx(sc->clknode, i);
 448                 *fout = f->freq;
 449                 return (0);
 450         }
 451
 452         /*
 453          * If we /are/ setting the parent clock, then we need
 454          * to determine what frequency we need the parent to
 455          * be, and then reconfigure the parent to the new
 456          * frequency, and then change our parent.
 457          *
 458          * (Again, if we're doing DRYRUN, just skip that
 459          * and return the new frequency.)
 460          */
 461         p_clk = clknode_find_by_name(f->parent);
 462         if (p_clk == NULL) {
 463                 device_printf(clknode_get_device(sc->clknode),
 464                     "%s: couldn't find parent clk (%s)\n",
 465                     __func__, f->parent);
 466                 return (ENXIO);
 467         }
 468
 469         /*
 470          * Calculate required frequency from said parent clock to
 471          * meet the needs of our target clock.
 472          */
 473         p_freq = qcom_clk_rcg2_calc_input_freq(f->freq, f->m, f->n,
 474             f->pre_div);
 475         DPRINTF(clknode_get_device(sc->clknode),
 476             "%s: request %llu, parent %s freq %llu, parent freq %llu\n",
 477             __func__,
 478             *fout,
 479             f->parent,
 480             f->freq,
 481             p_freq);
 482
 483         /*
 484          * To ensure glitch-free operation on some clocks, set it to
 485          * a safe parent before programming our divisor and the parent
 486          * clock configuration.  Then once it's done, flip the parent
 487          * to the new parent.
 488          *
 489          * If we're doing a dry-run then we don't need to re-parent the
 490          * clock just yet!
 491          */
 492         if (((flags & CLK_SET_DRYRUN) == 0) &&
 493             (sc->safe_pre_parent_idx > -1)) {
 494                 DPRINTF(clknode_get_device(sc->clknode),
 495                     "%s: setting to safe parent idx %d\n",
 496                     __func__,
 497                     sc->safe_pre_parent_idx);
 498                 CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 499                 qcom_clk_rcg2_set_parent_index_locked(sc,
 500                     sc->safe_pre_parent_idx);
 501                 DPRINTF(clknode_get_device(sc->clknode),
 502                     "%s: safe parent: updating config\n", __func__);
 503                 if (! qcom_clk_rcg2_update_config_locked(sc)) {
 504                         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 505                         DPRINTF(clknode_get_device(sc->clknode),
 506                             "%s: error updating config\n",
 507                             __func__);
 508                         return (ENXIO);
 509                 }
 510                 CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 511                 DPRINTF(clknode_get_device(sc->clknode),
 512                     "%s: safe parent: done\n", __func__);
 513                 clknode_set_parent_by_idx(sc->clknode,
 514                     sc->safe_pre_parent_idx);
 515         }
 516
 517         /*
 518          * Set the parent frequency before we change our mux and divisor
 519          * configuration.
 520          */
 521         if (clknode_get_freq(p_clk, &p_clk_freq) != 0) {
 522                 device_printf(clknode_get_device(sc->clknode),
 523                     "%s: couldn't get freq for parent clock %s\n",
 524                     __func__,
 525                     f->parent);
 526                 return (ENXIO);
 527         }
 528         if (p_clk_freq != p_freq) {
 529                 uint64_t n_freq;
 530                 int rv;
 531
 532                 /*
 533                  * If we're doing a dryrun then call test_freq() not set_freq().
 534                  * That way we get the frequency back that we would be set to.
 535                  *
 536                  * If we're not doing a dry run then set the frequency, then
 537                  * call get_freq to get what it was set to.
 538                  */
 539                 if (flags & CLK_SET_DRYRUN) {
 540                         n_freq = p_freq;
 541                         rv = clknode_test_freq(p_clk, n_freq, flags, 0,
 542                             &p_freq);
 543                 } else {
 544                         rv = clknode_set_freq(p_clk, p_freq, flags, 0);
 545                 }
 546
 547                 if (rv != 0) {
 548                         device_printf(clknode_get_device(sc->clknode),
 549                             "%s: couldn't set parent clock %s frequency to "
 550                             "%llu\n",
 551                             __func__,
 552                             f->parent,
 553                             p_freq);
 554                         return (ENXIO);
 555                 }
 556
 557                 /* Frequency was set, fetch what it was set to */
 558                 if ((flags & CLK_SET_DRYRUN) == 0) {
 559                         rv = clknode_get_freq(p_clk, &p_freq);
 560                         if (rv != 0) {
 561                                 device_printf(clknode_get_device(sc->clknode),
 562                                     "%s: couldn't get parent frequency",
 563                                     __func__);
 564                                 return (ENXIO);
 565                         }
 566                 }
 567         }
 568
 569         DPRINTF(clknode_get_device(sc->clknode),
 570             "%s: requsted freq=%llu, target freq=%llu,"
 571             " parent choice=%s, parent_freq=%llu\n",
 572             __func__,
 573             *fout,
 574             f->freq,
 575             f->parent,
 576             p_freq);
 577
 578         /*
 579          * Set the parent node, the parent programming and the divisor
 580          * config.  Because they're done together, we don't go via
 581          * a mux method on this node.
 582          */
 583
 584         /*
 585          * Program the divisor and parent.
 586          */
 587         if ((flags & CLK_SET_DRYRUN) == 0) {
 588                 CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 589                 qcom_clk_rcg2_set_config_locked(sc, f, i);
 590                 if (! qcom_clk_rcg2_update_config_locked(sc)) {
 591                         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 592                         device_printf(clknode_get_device(sc->clknode),
 593                             "%s: couldn't program in divisor, help!\n",
 594                             __func__);
 595                         return (ENXIO);
 596                 }
 597                 CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 598                 clknode_set_parent_by_idx(sc->clknode, i);
 599         }
 600
 601         /*
 602          * p_freq is now the frequency that the parent /is/ set to.
 603          * (Or would be set to for a dry run.)
 604          *
 605          * Calculate what the eventual frequency would be, we'll want
 606          * this to return when we're done - and again, if it's a dryrun,
 607          * don't set anything up.  This doesn't rely on the register
 608          * contents.
 609          */
 610         *fout = qcom_clk_rcg2_calc_rate(p_freq, (f->n == 0 ? 0 : 1),
 611             f->m, f->n, f->pre_div);
 612
 613         return (0);
 614 }
 615
 616 static clknode_method_t qcom_clk_rcg2_methods[] = {
 617         /* Device interface */
 618         CLKNODEMETHOD(clknode_init,             qcom_clk_rcg2_init),
 619         CLKNODEMETHOD(clknode_recalc_freq,      qcom_clk_rcg2_recalc),
 620         CLKNODEMETHOD(clknode_set_gate,         qcom_clk_rcg2_set_gate),
 621         CLKNODEMETHOD(clknode_set_freq,         qcom_clk_rcg2_set_freq),
 622         CLKNODEMETHOD_END
 623 };
 624
 625 DEFINE_CLASS_1(qcom_clk_fepll, qcom_clk_rcg2_class, qcom_clk_rcg2_methods,
 626    sizeof(struct qcom_clk_rcg2_sc), clknode_class);
 627
 628 int
 629 qcom_clk_rcg2_register(struct clkdom *clkdom,
 630     struct qcom_clk_rcg2_def *clkdef)
 631 {
 632         struct clknode *clk;
 633         struct qcom_clk_rcg2_sc *sc;
 634
 635         /*
 636          * Right now the rcg2 code isn't supporting turning off the clock
 637          * or limiting it to the lowest parent clock.  But, do set the
 638          * flags appropriately.
 639          */
 640         if (clkdef->flags & QCOM_CLK_RCG2_FLAGS_CRITICAL)
 641                 clkdef->clkdef.flags |= CLK_NODE_CANNOT_STOP;
 642
 643         clk = clknode_create(clkdom, &qcom_clk_rcg2_class, &clkdef->clkdef);
 644         if (clk == NULL)
 645                 return (1);
 646
 647         sc = clknode_get_softc(clk);
 648         sc->clknode = clk;
 649
 650         sc->cmd_rcgr = clkdef->cmd_rcgr;
 651         sc->hid_width = clkdef->hid_width;
 652         sc->mnd_width = clkdef->mnd_width;
 653         sc->safe_src_idx = clkdef->safe_src_idx;
 654         sc->safe_pre_parent_idx = clkdef->safe_pre_parent_idx;
 655         sc->cfg_offset = clkdef->cfg_offset;
 656         sc->flags = clkdef->flags;
 657         sc->freq_tbl = clkdef->freq_tbl;
 658
 659         clknode_register(clkdom, clk);
 660
 661         return (0);
 662 }