drivers/devfreq/tegra30-devfreq.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * A devfreq driver for NVIDIA Tegra SoCs
   4  *
   5  * Copyright (c) 2014 NVIDIA CORPORATION. All rights reserved.
   6  * Copyright (C) 2014 Google, Inc
   7  */
   8
   9 #include <linux/clk.h>
  10 #include <linux/cpufreq.h>
  11 #include <linux/devfreq.h>
  12 #include <linux/interrupt.h>
  13 #include <linux/io.h>
  14 #include <linux/irq.h>
  15 #include <linux/module.h>
  16 #include <linux/of_device.h>
  17 #include <linux/platform_device.h>
  18 #include <linux/pm_opp.h>
  19 #include <linux/reset.h>
  20 #include <linux/workqueue.h>
  21
  22 #include "governor.h"
  23
  24 #define ACTMON_GLB_STATUS                                       0x0
  25 #define ACTMON_GLB_PERIOD_CTRL                                  0x4
  26
  27 #define ACTMON_DEV_CTRL                                         0x0
  28 #define ACTMON_DEV_CTRL_K_VAL_SHIFT                             10
  29 #define ACTMON_DEV_CTRL_ENB_PERIODIC                            BIT(18)
  30 #define ACTMON_DEV_CTRL_AVG_BELOW_WMARK_EN                      BIT(20)
  31 #define ACTMON_DEV_CTRL_AVG_ABOVE_WMARK_EN                      BIT(21)
  32 #define ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_NUM_SHIFT       23
  33 #define ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_NUM_SHIFT       26
  34 #define ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN              BIT(29)
  35 #define ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN              BIT(30)
  36 #define ACTMON_DEV_CTRL_ENB                                     BIT(31)
  37
  38 #define ACTMON_DEV_CTRL_STOP                                    0x00000000
  39
  40 #define ACTMON_DEV_UPPER_WMARK                                  0x4
  41 #define ACTMON_DEV_LOWER_WMARK                                  0x8
  42 #define ACTMON_DEV_INIT_AVG                                     0xc
  43 #define ACTMON_DEV_AVG_UPPER_WMARK                              0x10
  44 #define ACTMON_DEV_AVG_LOWER_WMARK                              0x14
  45 #define ACTMON_DEV_COUNT_WEIGHT                                 0x18
  46 #define ACTMON_DEV_AVG_COUNT                                    0x20
  47 #define ACTMON_DEV_INTR_STATUS                                  0x24
  48
  49 #define ACTMON_INTR_STATUS_CLEAR                                0xffffffff
  50
  51 #define ACTMON_DEV_INTR_CONSECUTIVE_UPPER                       BIT(31)
  52 #define ACTMON_DEV_INTR_CONSECUTIVE_LOWER                       BIT(30)
  53
  54 #define ACTMON_ABOVE_WMARK_WINDOW                               1
  55 #define ACTMON_BELOW_WMARK_WINDOW                               3
  56 #define ACTMON_BOOST_FREQ_STEP                                  16000
  57
  58 /*
  59  * Activity counter is incremented every 256 memory transactions, and each
  60  * transaction takes 4 EMC clocks for Tegra124; So the COUNT_WEIGHT is
  61  * 4 * 256 = 1024.
  62  */
  63 #define ACTMON_COUNT_WEIGHT                                     0x400
  64
  65 /*
  66  * ACTMON_AVERAGE_WINDOW_LOG2: default value for @DEV_CTRL_K_VAL, which
  67  * translates to 2 ^ (K_VAL + 1). ex: 2 ^ (6 + 1) = 128
  68  */
  69 #define ACTMON_AVERAGE_WINDOW_LOG2                      6
  70 #define ACTMON_SAMPLING_PERIOD                          12 /* ms */
  71 #define ACTMON_DEFAULT_AVG_BAND                         6  /* 1/10 of % */
  72
  73 #define KHZ                                                     1000
  74
  75 #define KHZ_MAX                                         (ULONG_MAX / KHZ)
  76
  77 /* Assume that the bus is saturated if the utilization is 25% */
  78 #define BUS_SATURATION_RATIO                                    25
  79
  80 /**
  81  * struct tegra_devfreq_device_config - configuration specific to an ACTMON
  82  * device
  83  *
  84  * Coefficients and thresholds are percentages unless otherwise noted
  85  */
  86 struct tegra_devfreq_device_config {
  87         u32             offset;
  88         u32             irq_mask;
  89
  90         /* Factors applied to boost_freq every consecutive watermark breach */
  91         unsigned int    boost_up_coeff;
  92         unsigned int    boost_down_coeff;
  93
  94         /* Define the watermark bounds when applied to the current avg */
  95         unsigned int    boost_up_threshold;
  96         unsigned int    boost_down_threshold;
  97
  98         /*
  99          * Threshold of activity (cycles) below which the CPU frequency isn't
 100          * to be taken into account. This is to avoid increasing the EMC
 101          * frequency when the CPU is very busy but not accessing the bus often.
 102          */
 103         u32             avg_dependency_threshold;
 104 };
 105
 106 enum tegra_actmon_device {
 107         MCALL = 0,
 108         MCCPU,
 109 };
 110
 111 static const struct tegra_devfreq_device_config actmon_device_configs[] = {
 112         {
 113                 /* MCALL: All memory accesses (including from the CPUs) */
 114                 .offset = 0x1c0,
 115                 .irq_mask = 1 << 26,
 116                 .boost_up_coeff = 200,
 117                 .boost_down_coeff = 50,
 118                 .boost_up_threshold = 60,
 119                 .boost_down_threshold = 40,
 120         },
 121         {
 122                 /* MCCPU: memory accesses from the CPUs */
 123                 .offset = 0x200,
 124                 .irq_mask = 1 << 25,
 125                 .boost_up_coeff = 800,
 126                 .boost_down_coeff = 90,
 127                 .boost_up_threshold = 27,
 128                 .boost_down_threshold = 10,
 129                 .avg_dependency_threshold = 50000,
 130         },
 131 };
 132
 133 /**
 134  * struct tegra_devfreq_device - state specific to an ACTMON device
 135  *
 136  * Frequencies are in kHz.
 137  */
 138 struct tegra_devfreq_device {
 139         const struct tegra_devfreq_device_config *config;
 140         void __iomem *regs;
 141
 142         /* Average event count sampled in the last interrupt */
 143         u32 avg_count;
 144
 145         /*
 146          * Extra frequency to increase the target by due to consecutive
 147          * watermark breaches.
 148          */
 149         unsigned long boost_freq;
 150
 151         /* Optimal frequency calculated from the stats for this device */
 152         unsigned long target_freq;
 153 };
 154
 155 struct tegra_devfreq {
 156         struct devfreq          *devfreq;
 157
 158         struct reset_control    *reset;
 159         struct clk              *clock;
 160         void __iomem            *regs;
 161
 162         struct clk              *emc_clock;
 163         unsigned long           max_freq;
 164         unsigned long           cur_freq;
 165         struct notifier_block   clk_rate_change_nb;
 166
 167         struct delayed_work     cpufreq_update_work;
 168         struct notifier_block   cpu_rate_change_nb;
 169
 170         struct tegra_devfreq_device devices[ARRAY_SIZE(actmon_device_configs)];
 171
 172         unsigned int            irq;
 173 };
 174
 175 struct tegra_actmon_emc_ratio {
 176         unsigned long cpu_freq;
 177         unsigned long emc_freq;
 178 };
 179
 180 static const struct tegra_actmon_emc_ratio actmon_emc_ratios[] = {
 181         { 1400000,    KHZ_MAX },
 182         { 1200000,    750000 },
 183         { 1100000,    600000 },
 184         { 1000000,    500000 },
 185         {  800000,    375000 },
 186         {  500000,    200000 },
 187         {  250000,    100000 },
 188 };
 189
 190 static u32 actmon_readl(struct tegra_devfreq *tegra, u32 offset)
 191 {
 192         return readl_relaxed(tegra->regs + offset);
 193 }
 194
 195 static void actmon_writel(struct tegra_devfreq *tegra, u32 val, u32 offset)
 196 {
 197         writel_relaxed(val, tegra->regs + offset);
 198 }
 199
 200 static u32 device_readl(struct tegra_devfreq_device *dev, u32 offset)
 201 {
 202         return readl_relaxed(dev->regs + offset);
 203 }
 204
 205 static void device_writel(struct tegra_devfreq_device *dev, u32 val,
 206                           u32 offset)
 207 {
 208         writel_relaxed(val, dev->regs + offset);
 209 }
 210
 211 static unsigned long do_percent(unsigned long val, unsigned int pct)
 212 {
 213         return val * pct / 100;
 214 }
 215
 216 static void tegra_devfreq_update_avg_wmark(struct tegra_devfreq *tegra,
 217                                            struct tegra_devfreq_device *dev)
 218 {
 219         u32 avg = dev->avg_count;
 220         u32 avg_band_freq = tegra->max_freq * ACTMON_DEFAULT_AVG_BAND / KHZ;
 221         u32 band = avg_band_freq * ACTMON_SAMPLING_PERIOD;
 222
 223         device_writel(dev, avg + band, ACTMON_DEV_AVG_UPPER_WMARK);
 224
 225         avg = max(dev->avg_count, band);
 226         device_writel(dev, avg - band, ACTMON_DEV_AVG_LOWER_WMARK);
 227 }
 228
 229 static void tegra_devfreq_update_wmark(struct tegra_devfreq *tegra,
 230                                        struct tegra_devfreq_device *dev)
 231 {
 232         u32 val = tegra->cur_freq * ACTMON_SAMPLING_PERIOD;
 233
 234         device_writel(dev, do_percent(val, dev->config->boost_up_threshold),
 235                       ACTMON_DEV_UPPER_WMARK);
 236
 237         device_writel(dev, do_percent(val, dev->config->boost_down_threshold),
 238                       ACTMON_DEV_LOWER_WMARK);
 239 }
 240
 241 static void actmon_isr_device(struct tegra_devfreq *tegra,
 242                               struct tegra_devfreq_device *dev)
 243 {
 244         u32 intr_status, dev_ctrl;
 245
 246         dev->avg_count = device_readl(dev, ACTMON_DEV_AVG_COUNT);
 247         tegra_devfreq_update_avg_wmark(tegra, dev);
 248
 249         intr_status = device_readl(dev, ACTMON_DEV_INTR_STATUS);
 250         dev_ctrl = device_readl(dev, ACTMON_DEV_CTRL);
 251
 252         if (intr_status & ACTMON_DEV_INTR_CONSECUTIVE_UPPER) {
 253                 /*
 254                  * new_boost = min(old_boost * up_coef + step, max_freq)
 255                  */
 256                 dev->boost_freq = do_percent(dev->boost_freq,
 257                                              dev->config->boost_up_coeff);
 258                 dev->boost_freq += ACTMON_BOOST_FREQ_STEP;
 259
 260                 dev_ctrl |= ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
 261
 262                 if (dev->boost_freq >= tegra->max_freq) {
 263                         dev_ctrl &= ~ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
 264                         dev->boost_freq = tegra->max_freq;
 265                 }
 266         } else if (intr_status & ACTMON_DEV_INTR_CONSECUTIVE_LOWER) {
 267                 /*
 268                  * new_boost = old_boost * down_coef
 269                  * or 0 if (old_boost * down_coef < step / 2)
 270                  */
 271                 dev->boost_freq = do_percent(dev->boost_freq,
 272                                              dev->config->boost_down_coeff);
 273
 274                 dev_ctrl |= ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
 275
 276                 if (dev->boost_freq < (ACTMON_BOOST_FREQ_STEP >> 1)) {
 277                         dev_ctrl &= ~ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
 278                         dev->boost_freq = 0;
 279                 }
 280         }
 281
 282         device_writel(dev, dev_ctrl, ACTMON_DEV_CTRL);
 283
 284         device_writel(dev, ACTMON_INTR_STATUS_CLEAR, ACTMON_DEV_INTR_STATUS);
 285 }
 286
 287 static unsigned long actmon_cpu_to_emc_rate(struct tegra_devfreq *tegra,
 288                                             unsigned long cpu_freq)
 289 {
 290         unsigned int i;
 291         const struct tegra_actmon_emc_ratio *ratio = actmon_emc_ratios;
 292
 293         for (i = 0; i < ARRAY_SIZE(actmon_emc_ratios); i++, ratio++) {
 294                 if (cpu_freq >= ratio->cpu_freq) {
 295                         if (ratio->emc_freq >= tegra->max_freq)
 296                                 return tegra->max_freq;
 297                         else
 298                                 return ratio->emc_freq;
 299                 }
 300         }
 301
 302         return 0;
 303 }
 304
 305 static unsigned long actmon_device_target_freq(struct tegra_devfreq *tegra,
 306                                                struct tegra_devfreq_device *dev)
 307 {
 308         unsigned int avg_sustain_coef;
 309         unsigned long target_freq;
 310
 311         target_freq = dev->avg_count / ACTMON_SAMPLING_PERIOD;
 312         avg_sustain_coef = 100 * 100 / dev->config->boost_up_threshold;
 313         target_freq = do_percent(target_freq, avg_sustain_coef);
 314         target_freq += dev->boost_freq;
 315
 316         return target_freq;
 317 }
 318
 319 static void actmon_update_target(struct tegra_devfreq *tegra,
 320                                  struct tegra_devfreq_device *dev)
 321 {
 322         unsigned long cpu_freq = 0;
 323         unsigned long static_cpu_emc_freq = 0;
 324
 325         if (dev->config->avg_dependency_threshold) {
 326                 cpu_freq = cpufreq_quick_get(0);
 327                 static_cpu_emc_freq = actmon_cpu_to_emc_rate(tegra, cpu_freq);
 328         }
 329
 330         dev->target_freq = actmon_device_target_freq(tegra, dev);
 331
 332         if (dev->avg_count >= dev->config->avg_dependency_threshold)
 333                 dev->target_freq = max(dev->target_freq, static_cpu_emc_freq);
 334 }
 335
 336 static irqreturn_t actmon_thread_isr(int irq, void *data)
 337 {
 338         struct tegra_devfreq *tegra = data;
 339         bool handled = false;
 340         unsigned int i;
 341         u32 val;
 342
 343         mutex_lock(&tegra->devfreq->lock);
 344
 345         val = actmon_readl(tegra, ACTMON_GLB_STATUS);
 346         for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
 347                 if (val & tegra->devices[i].config->irq_mask) {
 348                         actmon_isr_device(tegra, tegra->devices + i);
 349                         handled = true;
 350                 }
 351         }
 352
 353         if (handled)
 354                 update_devfreq(tegra->devfreq);
 355
 356         mutex_unlock(&tegra->devfreq->lock);
 357
 358         return handled ? IRQ_HANDLED : IRQ_NONE;
 359 }
 360
 361 static int tegra_actmon_clk_notify_cb(struct notifier_block *nb,
 362                                       unsigned long action, void *ptr)
 363 {
 364         struct clk_notifier_data *data = ptr;
 365         struct tegra_devfreq *tegra;
 366         struct tegra_devfreq_device *dev;
 367         unsigned int i;
 368
 369         if (action != POST_RATE_CHANGE)
 370                 return NOTIFY_OK;
 371
 372         tegra = container_of(nb, struct tegra_devfreq, clk_rate_change_nb);
 373
 374         tegra->cur_freq = data->new_rate / KHZ;
 375
 376         for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
 377                 dev = &tegra->devices[i];
 378
 379                 tegra_devfreq_update_wmark(tegra, dev);
 380         }
 381
 382         return NOTIFY_OK;
 383 }
 384
 385 static void tegra_actmon_delayed_update(struct work_struct *work)
 386 {
 387         struct tegra_devfreq *tegra = container_of(work, struct tegra_devfreq,
 388                                                    cpufreq_update_work.work);
 389
 390         mutex_lock(&tegra->devfreq->lock);
 391         update_devfreq(tegra->devfreq);
 392         mutex_unlock(&tegra->devfreq->lock);
 393 }
 394
 395 static unsigned long
 396 tegra_actmon_cpufreq_contribution(struct tegra_devfreq *tegra,
 397                                   unsigned int cpu_freq)
 398 {
 399         unsigned long static_cpu_emc_freq, dev_freq;
 400
 401         /* check whether CPU's freq is taken into account at all */
 402         if (tegra->devices[MCCPU].avg_count <
 403             tegra->devices[MCCPU].config->avg_dependency_threshold)
 404                 return 0;
 405
 406         static_cpu_emc_freq = actmon_cpu_to_emc_rate(tegra, cpu_freq);
 407         dev_freq = actmon_device_target_freq(tegra, &tegra->devices[MCCPU]);
 408
 409         if (dev_freq >= static_cpu_emc_freq)
 410                 return 0;
 411
 412         return static_cpu_emc_freq;
 413 }
 414
 415 static int tegra_actmon_cpu_notify_cb(struct notifier_block *nb,
 416                                       unsigned long action, void *ptr)
 417 {
 418         struct cpufreq_freqs *freqs = ptr;
 419         struct tegra_devfreq *tegra;
 420         unsigned long old, new, delay;
 421
 422         if (action != CPUFREQ_POSTCHANGE)
 423                 return NOTIFY_OK;
 424
 425         tegra = container_of(nb, struct tegra_devfreq, cpu_rate_change_nb);
 426
 427         /*
 428          * Quickly check whether CPU frequency should be taken into account
 429          * at all, without blocking CPUFreq's core.
 430          */
 431         if (mutex_trylock(&tegra->devfreq->lock)) {
 432                 old = tegra_actmon_cpufreq_contribution(tegra, freqs->old);
 433                 new = tegra_actmon_cpufreq_contribution(tegra, freqs->new);
 434                 mutex_unlock(&tegra->devfreq->lock);
 435
 436                 /*
 437                  * If CPU's frequency shouldn't be taken into account at
 438                  * the moment, then there is no need to update the devfreq's
 439                  * state because ISR will re-check CPU's frequency on the
 440                  * next interrupt.
 441                  */
 442                 if (old == new)
 443                         return NOTIFY_OK;
 444         }
 445
 446         /*
 447          * CPUFreq driver should support CPUFREQ_ASYNC_NOTIFICATION in order
 448          * to allow asynchronous notifications. This means we can't block
 449          * here for too long, otherwise CPUFreq's core will complain with a
 450          * warning splat.
 451          */
 452         delay = msecs_to_jiffies(ACTMON_SAMPLING_PERIOD);
 453         schedule_delayed_work(&tegra->cpufreq_update_work, delay);
 454
 455         return NOTIFY_OK;
 456 }
 457
 458 static void tegra_actmon_configure_device(struct tegra_devfreq *tegra,
 459                                           struct tegra_devfreq_device *dev)
 460 {
 461         u32 val = 0;
 462
 463         /* reset boosting on governor's restart */
 464         dev->boost_freq = 0;
 465
 466         dev->target_freq = tegra->cur_freq;
 467
 468         dev->avg_count = tegra->cur_freq * ACTMON_SAMPLING_PERIOD;
 469         device_writel(dev, dev->avg_count, ACTMON_DEV_INIT_AVG);
 470
 471         tegra_devfreq_update_avg_wmark(tegra, dev);
 472         tegra_devfreq_update_wmark(tegra, dev);
 473
 474         device_writel(dev, ACTMON_COUNT_WEIGHT, ACTMON_DEV_COUNT_WEIGHT);
 475         device_writel(dev, ACTMON_INTR_STATUS_CLEAR, ACTMON_DEV_INTR_STATUS);
 476
 477         val |= ACTMON_DEV_CTRL_ENB_PERIODIC;
 478         val |= (ACTMON_AVERAGE_WINDOW_LOG2 - 1)
 479                 << ACTMON_DEV_CTRL_K_VAL_SHIFT;
 480         val |= (ACTMON_BELOW_WMARK_WINDOW - 1)
 481                 << ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_NUM_SHIFT;
 482         val |= (ACTMON_ABOVE_WMARK_WINDOW - 1)
 483                 << ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_NUM_SHIFT;
 484         val |= ACTMON_DEV_CTRL_AVG_ABOVE_WMARK_EN;
 485         val |= ACTMON_DEV_CTRL_AVG_BELOW_WMARK_EN;
 486         val |= ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
 487         val |= ACTMON_DEV_CTRL_ENB;
 488
 489         device_writel(dev, val, ACTMON_DEV_CTRL);
 490 }
 491
 492 static void tegra_actmon_stop_devices(struct tegra_devfreq *tegra)
 493 {
 494         struct tegra_devfreq_device *dev = tegra->devices;
 495         unsigned int i;
 496
 497         for (i = 0; i < ARRAY_SIZE(tegra->devices); i++, dev++) {
 498                 device_writel(dev, ACTMON_DEV_CTRL_STOP, ACTMON_DEV_CTRL);
 499                 device_writel(dev, ACTMON_INTR_STATUS_CLEAR,
 500                               ACTMON_DEV_INTR_STATUS);
 501         }
 502 }
 503
 504 static int tegra_actmon_start(struct tegra_devfreq *tegra)
 505 {
 506         unsigned int i;
 507         int err;
 508
 509         actmon_writel(tegra, ACTMON_SAMPLING_PERIOD - 1,
 510                       ACTMON_GLB_PERIOD_CTRL);
 511
 512         /*
 513          * CLK notifications are needed in order to reconfigure the upper
 514          * consecutive watermark in accordance to the actual clock rate
 515          * to avoid unnecessary upper interrupts.
 516          */
 517         err = clk_notifier_register(tegra->emc_clock,
 518                                     &tegra->clk_rate_change_nb);
 519         if (err) {
 520                 dev_err(tegra->devfreq->dev.parent,
 521                         "Failed to register rate change notifier\n");
 522                 return err;
 523         }
 524
 525         tegra->cur_freq = clk_get_rate(tegra->emc_clock) / KHZ;
 526
 527         for (i = 0; i < ARRAY_SIZE(tegra->devices); i++)
 528                 tegra_actmon_configure_device(tegra, &tegra->devices[i]);
 529
 530         /*
 531          * We are estimating CPU's memory bandwidth requirement based on
 532          * amount of memory accesses and system's load, judging by CPU's
 533          * frequency. We also don't want to receive events about CPU's
 534          * frequency transaction when governor is stopped, hence notifier
 535          * is registered dynamically.
 536          */
 537         err = cpufreq_register_notifier(&tegra->cpu_rate_change_nb,
 538                                         CPUFREQ_TRANSITION_NOTIFIER);
 539         if (err) {
 540                 dev_err(tegra->devfreq->dev.parent,
 541                         "Failed to register rate change notifier: %d\n", err);
 542                 goto err_stop;
 543         }
 544
 545         enable_irq(tegra->irq);
 546
 547         return 0;
 548
 549 err_stop:
 550         tegra_actmon_stop_devices(tegra);
 551
 552         clk_notifier_unregister(tegra->emc_clock, &tegra->clk_rate_change_nb);
 553
 554         return err;
 555 }
 556
 557 static void tegra_actmon_stop(struct tegra_devfreq *tegra)
 558 {
 559         disable_irq(tegra->irq);
 560
 561         cpufreq_unregister_notifier(&tegra->cpu_rate_change_nb,
 562                                     CPUFREQ_TRANSITION_NOTIFIER);
 563
 564         cancel_delayed_work_sync(&tegra->cpufreq_update_work);
 565
 566         tegra_actmon_stop_devices(tegra);
 567
 568         clk_notifier_unregister(tegra->emc_clock, &tegra->clk_rate_change_nb);
 569 }
 570
 571 static int tegra_devfreq_target(struct device *dev, unsigned long *freq,
 572                                 u32 flags)
 573 {
 574         struct tegra_devfreq *tegra = dev_get_drvdata(dev);
 575         struct devfreq *devfreq = tegra->devfreq;
 576         struct dev_pm_opp *opp;
 577         unsigned long rate;
 578         int err;
 579
 580         opp = devfreq_recommended_opp(dev, freq, flags);
 581         if (IS_ERR(opp)) {
 582                 dev_err(dev, "Failed to find opp for %lu Hz\n", *freq);
 583                 return PTR_ERR(opp);
 584         }
 585         rate = dev_pm_opp_get_freq(opp);
 586         dev_pm_opp_put(opp);
 587
 588         err = clk_set_min_rate(tegra->emc_clock, rate * KHZ);
 589         if (err)
 590                 return err;
 591
 592         err = clk_set_rate(tegra->emc_clock, 0);
 593         if (err)
 594                 goto restore_min_rate;
 595
 596         return 0;
 597
 598 restore_min_rate:
 599         clk_set_min_rate(tegra->emc_clock, devfreq->previous_freq);
 600
 601         return err;
 602 }
 603
 604 static int tegra_devfreq_get_dev_status(struct device *dev,
 605                                         struct devfreq_dev_status *stat)
 606 {
 607         struct tegra_devfreq *tegra = dev_get_drvdata(dev);
 608         struct tegra_devfreq_device *actmon_dev;
 609         unsigned long cur_freq;
 610
 611         cur_freq = READ_ONCE(tegra->cur_freq);
 612
 613         /* To be used by the tegra governor */
 614         stat->private_data = tegra;
 615
 616         /* The below are to be used by the other governors */
 617         stat->current_frequency = cur_freq;
 618
 619         actmon_dev = &tegra->devices[MCALL];
 620
 621         /* Number of cycles spent on memory access */
 622         stat->busy_time = device_readl(actmon_dev, ACTMON_DEV_AVG_COUNT);
 623
 624         /* The bus can be considered to be saturated way before 100% */
 625         stat->busy_time *= 100 / BUS_SATURATION_RATIO;
 626
 627         /* Number of cycles in a sampling period */
 628         stat->total_time = ACTMON_SAMPLING_PERIOD * cur_freq;
 629
 630         stat->busy_time = min(stat->busy_time, stat->total_time);
 631
 632         return 0;
 633 }
 634
 635 static struct devfreq_dev_profile tegra_devfreq_profile = {
 636         .polling_ms     = 0,
 637         .target         = tegra_devfreq_target,
 638         .get_dev_status = tegra_devfreq_get_dev_status,
 639 };
 640
 641 static int tegra_governor_get_target(struct devfreq *devfreq,
 642                                      unsigned long *freq)
 643 {
 644         struct devfreq_dev_status *stat;
 645         struct tegra_devfreq *tegra;
 646         struct tegra_devfreq_device *dev;
 647         unsigned long target_freq = 0;
 648         unsigned int i;
 649         int err;
 650
 651         err = devfreq_update_stats(devfreq);
 652         if (err)
 653                 return err;
 654
 655         stat = &devfreq->last_status;
 656
 657         tegra = stat->private_data;
 658
 659         for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
 660                 dev = &tegra->devices[i];
 661
 662                 actmon_update_target(tegra, dev);
 663
 664                 target_freq = max(target_freq, dev->target_freq);
 665         }
 666
 667         *freq = target_freq;
 668
 669         return 0;
 670 }
 671
 672 static int tegra_governor_event_handler(struct devfreq *devfreq,
 673                                         unsigned int event, void *data)
 674 {
 675         struct tegra_devfreq *tegra = dev_get_drvdata(devfreq->dev.parent);
 676         int ret = 0;
 677
 678         /*
 679          * Couple devfreq-device with the governor early because it is
 680          * needed at the moment of governor's start (used by ISR).
 681          */
 682         tegra->devfreq = devfreq;
 683
 684         switch (event) {
 685         case DEVFREQ_GOV_START:
 686                 devfreq_monitor_start(devfreq);
 687                 ret = tegra_actmon_start(tegra);
 688                 break;
 689
 690         case DEVFREQ_GOV_STOP:
 691                 tegra_actmon_stop(tegra);
 692                 devfreq_monitor_stop(devfreq);
 693                 break;
 694
 695         case DEVFREQ_GOV_SUSPEND:
 696                 tegra_actmon_stop(tegra);
 697                 devfreq_monitor_suspend(devfreq);
 698                 break;
 699
 700         case DEVFREQ_GOV_RESUME:
 701                 devfreq_monitor_resume(devfreq);
 702                 ret = tegra_actmon_start(tegra);
 703                 break;
 704         }
 705
 706         return ret;
 707 }
 708
 709 static struct devfreq_governor tegra_devfreq_governor = {
 710         .name = "tegra_actmon",
 711         .get_target_freq = tegra_governor_get_target,
 712         .event_handler = tegra_governor_event_handler,
 713         .immutable = true,
 714 };
 715
 716 static int tegra_devfreq_probe(struct platform_device *pdev)
 717 {
 718         struct tegra_devfreq_device *dev;
 719         struct tegra_devfreq *tegra;
 720         struct devfreq *devfreq;
 721         unsigned int i;
 722         long rate;
 723         int err;
 724
 725         tegra = devm_kzalloc(&pdev->dev, sizeof(*tegra), GFP_KERNEL);
 726         if (!tegra)
 727                 return -ENOMEM;
 728
 729         tegra->regs = devm_platform_ioremap_resource(pdev, 0);
 730         if (IS_ERR(tegra->regs))
 731                 return PTR_ERR(tegra->regs);
 732
 733         tegra->reset = devm_reset_control_get(&pdev->dev, "actmon");
 734         if (IS_ERR(tegra->reset)) {
 735                 dev_err(&pdev->dev, "Failed to get reset\n");
 736                 return PTR_ERR(tegra->reset);
 737         }
 738
 739         tegra->clock = devm_clk_get(&pdev->dev, "actmon");
 740         if (IS_ERR(tegra->clock)) {
 741                 dev_err(&pdev->dev, "Failed to get actmon clock\n");
 742                 return PTR_ERR(tegra->clock);
 743         }
 744
 745         tegra->emc_clock = devm_clk_get(&pdev->dev, "emc");
 746         if (IS_ERR(tegra->emc_clock)) {
 747                 dev_err(&pdev->dev, "Failed to get emc clock\n");
 748                 return PTR_ERR(tegra->emc_clock);
 749         }
 750
 751         err = platform_get_irq(pdev, 0);
 752         if (err < 0) {
 753                 dev_err(&pdev->dev, "Failed to get IRQ: %d\n", err);
 754                 return err;
 755         }
 756         tegra->irq = err;
 757
 758         irq_set_status_flags(tegra->irq, IRQ_NOAUTOEN);
 759
 760         err = devm_request_threaded_irq(&pdev->dev, tegra->irq, NULL,
 761                                         actmon_thread_isr, IRQF_ONESHOT,
 762                                         "tegra-devfreq", tegra);
 763         if (err) {
 764                 dev_err(&pdev->dev, "Interrupt request failed: %d\n", err);
 765                 return err;
 766         }
 767
 768         reset_control_assert(tegra->reset);
 769
 770         err = clk_prepare_enable(tegra->clock);
 771         if (err) {
 772                 dev_err(&pdev->dev,
 773                         "Failed to prepare and enable ACTMON clock\n");
 774                 return err;
 775         }
 776
 777         reset_control_deassert(tegra->reset);
 778
 779         rate = clk_round_rate(tegra->emc_clock, ULONG_MAX);
 780         if (rate < 0) {
 781                 dev_err(&pdev->dev, "Failed to round clock rate: %ld\n", rate);
 782                 return rate;
 783         }
 784
 785         tegra->max_freq = rate / KHZ;
 786
 787         for (i = 0; i < ARRAY_SIZE(actmon_device_configs); i++) {
 788                 dev = tegra->devices + i;
 789                 dev->config = actmon_device_configs + i;
 790                 dev->regs = tegra->regs + dev->config->offset;
 791         }
 792
 793         for (rate = 0; rate <= tegra->max_freq * KHZ; rate++) {
 794                 rate = clk_round_rate(tegra->emc_clock, rate);
 795
 796                 if (rate < 0) {
 797                         dev_err(&pdev->dev,
 798                                 "Failed to round clock rate: %ld\n", rate);
 799                         err = rate;
 800                         goto remove_opps;
 801                 }
 802
 803                 err = dev_pm_opp_add(&pdev->dev, rate / KHZ, 0);
 804                 if (err) {
 805                         dev_err(&pdev->dev, "Failed to add OPP: %d\n", err);
 806                         goto remove_opps;
 807                 }
 808         }
 809
 810         platform_set_drvdata(pdev, tegra);
 811
 812         tegra->clk_rate_change_nb.notifier_call = tegra_actmon_clk_notify_cb;
 813         tegra->cpu_rate_change_nb.notifier_call = tegra_actmon_cpu_notify_cb;
 814
 815         INIT_DELAYED_WORK(&tegra->cpufreq_update_work,
 816                           tegra_actmon_delayed_update);
 817
 818         err = devfreq_add_governor(&tegra_devfreq_governor);
 819         if (err) {
 820                 dev_err(&pdev->dev, "Failed to add governor: %d\n", err);
 821                 goto remove_opps;
 822         }
 823
 824         tegra_devfreq_profile.initial_freq = clk_get_rate(tegra->emc_clock);
 825         tegra_devfreq_profile.initial_freq /= KHZ;
 826
 827         devfreq = devfreq_add_device(&pdev->dev, &tegra_devfreq_profile,
 828                                      "tegra_actmon", NULL);
 829         if (IS_ERR(devfreq)) {
 830                 err = PTR_ERR(devfreq);
 831                 goto remove_governor;
 832         }
 833
 834         return 0;
 835
 836 remove_governor:
 837         devfreq_remove_governor(&tegra_devfreq_governor);
 838
 839 remove_opps:
 840         dev_pm_opp_remove_all_dynamic(&pdev->dev);
 841
 842         reset_control_reset(tegra->reset);
 843         clk_disable_unprepare(tegra->clock);
 844
 845         return err;
 846 }
 847
 848 static int tegra_devfreq_remove(struct platform_device *pdev)
 849 {
 850         struct tegra_devfreq *tegra = platform_get_drvdata(pdev);
 851
 852         devfreq_remove_device(tegra->devfreq);
 853         devfreq_remove_governor(&tegra_devfreq_governor);
 854
 855         dev_pm_opp_remove_all_dynamic(&pdev->dev);
 856
 857         reset_control_reset(tegra->reset);
 858         clk_disable_unprepare(tegra->clock);
 859
 860         return 0;
 861 }
 862
 863 static const struct of_device_id tegra_devfreq_of_match[] = {
 864         { .compatible = "nvidia,tegra30-actmon" },
 865         { .compatible = "nvidia,tegra124-actmon" },
 866         { },
 867 };
 868
 869 MODULE_DEVICE_TABLE(of, tegra_devfreq_of_match);
 870
 871 static struct platform_driver tegra_devfreq_driver = {
 872         .probe  = tegra_devfreq_probe,
 873         .remove = tegra_devfreq_remove,
 874         .driver = {
 875                 .name = "tegra-devfreq",
 876                 .of_match_table = tegra_devfreq_of_match,
 877         },
 878 };
 879 module_platform_driver(tegra_devfreq_driver);
 880
 881 MODULE_LICENSE("GPL v2");
 882 MODULE_DESCRIPTION("Tegra devfreq driver");
 883 MODULE_AUTHOR("Tomeu Vizoso <tomeu.vizoso@collabora.com>");