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;
 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         /* mux settings */
 286         CLKDEV_READ_4(clknode_get_device(sc->clknode),
 287             QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
 288         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 289
 290         idx = (reg & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK)
 291             >> QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT;
 292         DPRINTF(clknode_get_device(sc->clknode), "%s: mux index %u\n",
 293             __func__, idx);
 294         clknode_init_parent_idx(clk, idx);
 295
 296         /*
 297          * If we could be sure our parent clocks existed here in the tree,
 298          * we could calculate our current frequency by fetching the parent
 299          * frequency and then do our divider math.  Unfortunately that
 300          * currently isn't the case.
 301          */
 302
 303         return(0);
 304 }
 305
 306 static int
 307 qcom_clk_rcg2_set_gate(struct clknode *clk, bool enable)
 308 {
 309
 310         /*
 311          * For now this isn't supported; there's some support for
 312          * "shared" rcg2 nodes in the Qualcomm/upstream Linux trees but
 313          * it's not currently needed for the supported platforms.
 314          */
 315         return (0);
 316 }
 317
 318 /*
 319  * Program the parent index.
 320  *
 321  * This doesn't do the update.  It also must be called with the device
 322  * lock held.
 323  */
 324 static void
 325 qcom_clk_rcg2_set_parent_index_locked(struct qcom_clk_rcg2_sc *sc,
 326     uint32_t index)
 327 {
 328         uint32_t reg;
 329
 330         CLKDEV_READ_4(clknode_get_device(sc->clknode),
 331             QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
 332         reg = reg & ~QCOM_CLK_RCG2_CFG_SRC_SEL_MASK;
 333         reg = reg | (((index << QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT)
 334             & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK));
 335         CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
 336             QCOM_CLK_RCG2_CFG_OFFSET(sc),
 337             reg);
 338 }
 339
 340 /*
 341  * Set frequency
 342  *
 343  * fin - the parent frequency, if exists
 344  * fout - starts as the requested frequency, ends with the configured
 345  *        or dry-run frequency
 346  * Flags - CLK_SET_DRYRUN, CLK_SET_ROUND_UP, CLK_SET_ROUND_DOWN
 347  * retval - 0, ERANGE
 348  */
 349 static int
 350 qcom_clk_rcg2_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
 351     int flags, int *stop)
 352 {
 353         struct qcom_clk_rcg2_sc *sc;
 354         const struct qcom_clk_freq_tbl *f;
 355         const char **parent_names;
 356         uint64_t p_freq, p_clk_freq;
 357         int parent_cnt;
 358         struct clknode *p_clk;
 359         int i;
 360
 361         sc = clknode_get_softc(clk);
 362
 363         /*
 364          * Find a suitable frequency in the frequency table.
 365          *
 366          * TODO: should pay attention to ROUND_UP / ROUND_DOWN and add
 367          * a freqtbl method to handle both accordingly.
 368          */
 369         f = qcom_clk_freq_tbl_lookup(sc->freq_tbl, *fout);
 370         if (f == NULL) {
 371                 device_printf(clknode_get_device(sc->clknode),
 372                     "%s: no suitable freqtbl entry found for freq %llu\n",
 373                     __func__,
 374                     *fout);
 375                 return (ERANGE);
 376         }
 377
 378         /*
 379          * Find the parent index for the given parent clock.
 380          * Abort if we can't actually find it.
 381          *
 382          * XXX TODO: this should be a clk API call!
 383          */
 384         parent_cnt = clknode_get_parents_num(clk);
 385         parent_names = clknode_get_parent_names(clk);
 386         for (i = 0; i < parent_cnt; i++) {
 387                 if (parent_names[i] == NULL)
 388                         continue;
 389                 if (strcmp(parent_names[i], f->parent) == 0)
 390                         break;
 391         }
 392         if (i >= parent_cnt) {
 393                 device_printf(clknode_get_device(sc->clknode),
 394                     "%s: couldn't find suitable parent?\n",
 395                     __func__);
 396                 return (ENXIO);
 397         }
 398
 399         /*
 400          * If we aren't setting the parent clock, then we need
 401          * to just program the new parent clock in and update.
 402          * (or for DRYRUN just skip that and return the new
 403          * frequency.)
 404          */
 405         if ((sc->flags & QCOM_CLK_RCG2_FLAGS_SET_RATE_PARENT) == 0) {
 406                 if (flags & CLK_SET_DRYRUN) {
 407                         *fout = f->freq;
 408                         return (0);
 409                 }
 410
 411                 if (sc->safe_pre_parent_idx > -1) {
 412                         DPRINTF(clknode_get_device(sc->clknode),
 413                             "%s: setting to safe parent idx %d\n",
 414                             __func__,
 415                             sc->safe_pre_parent_idx);
 416                         CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 417                         qcom_clk_rcg2_set_parent_index_locked(sc,
 418                             sc->safe_pre_parent_idx);
 419                         DPRINTF(clknode_get_device(sc->clknode),
 420                             "%s: safe parent: updating config\n", __func__);
 421                         if (! qcom_clk_rcg2_update_config_locked(sc)) {
 422                                 CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 423                                 DPRINTF(clknode_get_device(sc->clknode),
 424                                     "%s: error updating config\n",
 425                                     __func__);
 426                                 return (ENXIO);
 427                         }
 428                         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 429                         DPRINTF(clknode_get_device(sc->clknode),
 430                             "%s: safe parent: done\n", __func__);
 431                         clknode_set_parent_by_idx(sc->clknode,
 432                             sc->safe_pre_parent_idx);
 433                 }
 434                 /* Program parent index, then schedule update */
 435                 CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 436                 qcom_clk_rcg2_set_parent_index_locked(sc, i);
 437                 if (! qcom_clk_rcg2_update_config_locked(sc)) {
 438                         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 439                         device_printf(clknode_get_device(sc->clknode),
 440                             "%s: couldn't program in parent idx %u!\n",
 441                             __func__, i);
 442                         return (ENXIO);
 443                 }
 444                 CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 445                 clknode_set_parent_by_idx(sc->clknode, i);
 446                 *fout = f->freq;
 447                 return (0);
 448         }
 449
 450         /*
 451          * If we /are/ setting the parent clock, then we need
 452          * to determine what frequency we need the parent to
 453          * be, and then reconfigure the parent to the new
 454          * frequency, and then change our parent.
 455          *
 456          * (Again, if we're doing DRYRUN, just skip that
 457          * and return the new frequency.)
 458          */
 459         p_clk = clknode_find_by_name(f->parent);
 460         if (p_clk == NULL) {
 461                 device_printf(clknode_get_device(sc->clknode),
 462                     "%s: couldn't find parent clk (%s)\n",
 463                     __func__, f->parent);
 464                 return (ENXIO);
 465         }
 466
 467         /*
 468          * Calculate required frequency from said parent clock to
 469          * meet the needs of our target clock.
 470          */
 471         p_freq = qcom_clk_rcg2_calc_input_freq(f->freq, f->m, f->n,
 472             f->pre_div);
 473         DPRINTF(clknode_get_device(sc->clknode),
 474             "%s: request %llu, parent %s freq %llu, parent freq %llu\n",
 475             __func__,
 476             *fout,
 477             f->parent,
 478             f->freq,
 479             p_freq);
 480
 481         /*
 482          * To ensure glitch-free operation on some clocks, set it to
 483          * a safe parent before programming our divisor and the parent
 484          * clock configuration.  Then once it's done, flip the parent
 485          * to the new parent.
 486          *
 487          * If we're doing a dry-run then we don't need to re-parent the
 488          * clock just yet!
 489          */
 490         if (((flags & CLK_SET_DRYRUN) == 0) &&
 491             (sc->safe_pre_parent_idx > -1)) {
 492                 DPRINTF(clknode_get_device(sc->clknode),
 493                     "%s: setting to safe parent idx %d\n",
 494                     __func__,
 495                     sc->safe_pre_parent_idx);
 496                 CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 497                 qcom_clk_rcg2_set_parent_index_locked(sc,
 498                     sc->safe_pre_parent_idx);
 499                 DPRINTF(clknode_get_device(sc->clknode),
 500                     "%s: safe parent: updating config\n", __func__);
 501                 if (! qcom_clk_rcg2_update_config_locked(sc)) {
 502                         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 503                         DPRINTF(clknode_get_device(sc->clknode),
 504                             "%s: error updating config\n",
 505                             __func__);
 506                         return (ENXIO);
 507                 }
 508                 CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 509                 DPRINTF(clknode_get_device(sc->clknode),
 510                     "%s: safe parent: done\n", __func__);
 511                 clknode_set_parent_by_idx(sc->clknode,
 512                     sc->safe_pre_parent_idx);
 513         }
 514
 515         /*
 516          * Set the parent frequency before we change our mux and divisor
 517          * configuration.
 518          */
 519         if (clknode_get_freq(p_clk, &p_clk_freq) != 0) {
 520                 device_printf(clknode_get_device(sc->clknode),
 521                     "%s: couldn't get freq for parent clock %s\n",
 522                     __func__,
 523                     f->parent);
 524                 return (ENXIO);
 525         }
 526         if (p_clk_freq != p_freq) {
 527                 uint64_t n_freq;
 528                 int rv;
 529
 530                 /*
 531                  * If we're doing a dryrun then call test_freq() not set_freq().
 532                  * That way we get the frequency back that we would be set to.
 533                  *
 534                  * If we're not doing a dry run then set the frequency, then
 535                  * call get_freq to get what it was set to.
 536                  */
 537                 if (flags & CLK_SET_DRYRUN) {
 538                         n_freq = p_freq;
 539                         rv = clknode_test_freq(p_clk, n_freq, flags, 0,
 540                             &p_freq);
 541                 } else {
 542                         rv = clknode_set_freq(p_clk, p_freq, flags, 0);
 543                 }
 544
 545                 if (rv != 0) {
 546                         device_printf(clknode_get_device(sc->clknode),
 547                             "%s: couldn't set parent clock %s frequency to "
 548                             "%llu\n",
 549                             __func__,
 550                             f->parent,
 551                             p_freq);
 552                         return (ENXIO);
 553                 }
 554
 555                 /* Frequency was set, fetch what it was set to */
 556                 if ((flags & CLK_SET_DRYRUN) == 0) {
 557                         rv = clknode_get_freq(p_clk, &p_freq);
 558                         if (rv != 0) {
 559                                 device_printf(clknode_get_device(sc->clknode),
 560                                     "%s: couldn't get parent frequency",
 561                                     __func__);
 562                                 return (ENXIO);
 563                         }
 564                 }
 565         }
 566
 567         DPRINTF(clknode_get_device(sc->clknode),
 568             "%s: requsted freq=%llu, target freq=%llu,"
 569             " parent choice=%s, parent_freq=%llu\n",
 570             __func__,
 571             *fout,
 572             f->freq,
 573             f->parent,
 574             p_freq);
 575
 576         /*
 577          * Set the parent node, the parent programming and the divisor
 578          * config.  Because they're done together, we don't go via
 579          * a mux method on this node.
 580          */
 581
 582         /*
 583          * Program the divisor and parent.
 584          */
 585         if ((flags & CLK_SET_DRYRUN) == 0) {
 586                 CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
 587                 qcom_clk_rcg2_set_config_locked(sc, f, i);
 588                 if (! qcom_clk_rcg2_update_config_locked(sc)) {
 589                         CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 590                         device_printf(clknode_get_device(sc->clknode),
 591                             "%s: couldn't program in divisor, help!\n",
 592                             __func__);
 593                         return (ENXIO);
 594                 }
 595                 CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
 596                 clknode_set_parent_by_idx(sc->clknode, i);
 597         }
 598
 599         /*
 600          * p_freq is now the frequency that the parent /is/ set to.
 601          * (Or would be set to for a dry run.)
 602          *
 603          * Calculate what the eventual frequency would be, we'll want
 604          * this to return when we're done - and again, if it's a dryrun,
 605          * don't set anything up.  This doesn't rely on the register
 606          * contents.
 607          */
 608         *fout = qcom_clk_rcg2_calc_rate(p_freq, (f->n == 0 ? 0 : 1),
 609             f->m, f->n, f->pre_div);
 610
 611         return (0);
 612 }
 613
 614 static clknode_method_t qcom_clk_rcg2_methods[] = {
 615         /* Device interface */
 616         CLKNODEMETHOD(clknode_init,             qcom_clk_rcg2_init),
 617         CLKNODEMETHOD(clknode_recalc_freq,      qcom_clk_rcg2_recalc),
 618         CLKNODEMETHOD(clknode_set_gate,         qcom_clk_rcg2_set_gate),
 619         CLKNODEMETHOD(clknode_set_freq,         qcom_clk_rcg2_set_freq),
 620         CLKNODEMETHOD_END
 621 };
 622
 623 DEFINE_CLASS_1(qcom_clk_fepll, qcom_clk_rcg2_class, qcom_clk_rcg2_methods,
 624    sizeof(struct qcom_clk_rcg2_sc), clknode_class);
 625
 626 int
 627 qcom_clk_rcg2_register(struct clkdom *clkdom,
 628     struct qcom_clk_rcg2_def *clkdef)
 629 {
 630         struct clknode *clk;
 631         struct qcom_clk_rcg2_sc *sc;
 632
 633         /*
 634          * Right now the rcg2 code isn't supporting turning off the clock
 635          * or limiting it to the lowest parent clock.  But, do set the
 636          * flags appropriately.
 637          */
 638         if (clkdef->flags & QCOM_CLK_RCG2_FLAGS_CRITICAL)
 639                 clkdef->clkdef.flags |= CLK_NODE_CANNOT_STOP;
 640
 641         clk = clknode_create(clkdom, &qcom_clk_rcg2_class, &clkdef->clkdef);
 642         if (clk == NULL)
 643                 return (1);
 644
 645         sc = clknode_get_softc(clk);
 646         sc->clknode = clk;
 647
 648         sc->cmd_rcgr = clkdef->cmd_rcgr;
 649         sc->hid_width = clkdef->hid_width;
 650         sc->mnd_width = clkdef->mnd_width;
 651         sc->safe_src_idx = clkdef->safe_src_idx;
 652         sc->safe_pre_parent_idx = clkdef->safe_pre_parent_idx;
 653         sc->cfg_offset = clkdef->cfg_offset;
 654         sc->flags = clkdef->flags;
 655         sc->freq_tbl = clkdef->freq_tbl;
 656
 657         clknode_register(clkdom, clk);
 658
 659         return (0);
 660 }