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