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