drivers/clocksource/arm_arch_timer.c

   1 /*
   2  *  linux/drivers/clocksource/arm_arch_timer.c
   3  *
   4  *  Copyright (C) 2011 ARM Ltd.
   5  *  All Rights Reserved
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License version 2 as
   9  * published by the Free Software Foundation.
  10  */
  11 #include <linux/init.h>
  12 #include <linux/kernel.h>
  13 #include <linux/device.h>
  14 #include <linux/smp.h>
  15 #include <linux/cpu.h>
  16 #include <linux/cpu_pm.h>
  17 #include <linux/clockchips.h>
  18 #include <linux/interrupt.h>
  19 #include <linux/of_irq.h>
  20 #include <linux/of_address.h>
  21 #include <linux/io.h>
  22 #include <linux/slab.h>
  23
  24 #include <asm/arch_timer.h>
  25 #include <asm/virt.h>
  26
  27 #include <clocksource/arm_arch_timer.h>
  28
  29 #define CNTTIDR         0x08
  30 #define CNTTIDR_VIRT(n) (BIT(1) << ((n) * 4))
  31
  32 #define CNTVCT_LO       0x08
  33 #define CNTVCT_HI       0x0c
  34 #define CNTFRQ          0x10
  35 #define CNTP_TVAL       0x28
  36 #define CNTP_CTL        0x2c
  37 #define CNTV_TVAL       0x38
  38 #define CNTV_CTL        0x3c
  39
  40 #define ARCH_CP15_TIMER BIT(0)
  41 #define ARCH_MEM_TIMER  BIT(1)
  42 static unsigned arch_timers_present __initdata;
  43
  44 static void __iomem *arch_counter_base;
  45
  46 struct arch_timer {
  47         void __iomem *base;
  48         struct clock_event_device evt;
  49 };
  50
  51 #define to_arch_timer(e) container_of(e, struct arch_timer, evt)
  52
  53 static u32 arch_timer_rate;
  54
  55 enum ppi_nr {
  56         PHYS_SECURE_PPI,
  57         PHYS_NONSECURE_PPI,
  58         VIRT_PPI,
  59         HYP_PPI,
  60         MAX_TIMER_PPI
  61 };
  62
  63 static int arch_timer_ppi[MAX_TIMER_PPI];
  64
  65 static struct clock_event_device __percpu *arch_timer_evt;
  66
  67 static bool arch_timer_use_virtual = true;
  68 static bool arch_timer_mem_use_virtual;
  69
  70 /*
  71  * Architected system timer support.
  72  */
  73
  74 static __always_inline
  75 void arch_timer_reg_write(int access, enum arch_timer_reg reg, u32 val,
  76                 struct clock_event_device *clk)
  77 {
  78         if (access == ARCH_TIMER_MEM_PHYS_ACCESS) {
  79                 struct arch_timer *timer = to_arch_timer(clk);
  80                 switch (reg) {
  81                 case ARCH_TIMER_REG_CTRL:
  82                         writel_relaxed(val, timer->base + CNTP_CTL);
  83                         break;
  84                 case ARCH_TIMER_REG_TVAL:
  85                         writel_relaxed(val, timer->base + CNTP_TVAL);
  86                         break;
  87                 }
  88         } else if (access == ARCH_TIMER_MEM_VIRT_ACCESS) {
  89                 struct arch_timer *timer = to_arch_timer(clk);
  90                 switch (reg) {
  91                 case ARCH_TIMER_REG_CTRL:
  92                         writel_relaxed(val, timer->base + CNTV_CTL);
  93                         break;
  94                 case ARCH_TIMER_REG_TVAL:
  95                         writel_relaxed(val, timer->base + CNTV_TVAL);
  96                         break;
  97                 }
  98         } else {
  99                 arch_timer_reg_write_cp15(access, reg, val);
 100         }
 101 }
 102
 103 static __always_inline
 104 u32 arch_timer_reg_read(int access, enum arch_timer_reg reg,
 105                 struct clock_event_device *clk)
 106 {
 107         u32 val;
 108
 109         if (access == ARCH_TIMER_MEM_PHYS_ACCESS) {
 110                 struct arch_timer *timer = to_arch_timer(clk);
 111                 switch (reg) {
 112                 case ARCH_TIMER_REG_CTRL:
 113                         val = readl_relaxed(timer->base + CNTP_CTL);
 114                         break;
 115                 case ARCH_TIMER_REG_TVAL:
 116                         val = readl_relaxed(timer->base + CNTP_TVAL);
 117                         break;
 118                 }
 119         } else if (access == ARCH_TIMER_MEM_VIRT_ACCESS) {
 120                 struct arch_timer *timer = to_arch_timer(clk);
 121                 switch (reg) {
 122                 case ARCH_TIMER_REG_CTRL:
 123                         val = readl_relaxed(timer->base + CNTV_CTL);
 124                         break;
 125                 case ARCH_TIMER_REG_TVAL:
 126                         val = readl_relaxed(timer->base + CNTV_TVAL);
 127                         break;
 128                 }
 129         } else {
 130                 val = arch_timer_reg_read_cp15(access, reg);
 131         }
 132
 133         return val;
 134 }
 135
 136 static __always_inline irqreturn_t timer_handler(const int access,
 137                                         struct clock_event_device *evt)
 138 {
 139         unsigned long ctrl;
 140         ctrl = arch_timer_reg_read(access, ARCH_TIMER_REG_CTRL, evt);
 141         if (ctrl & ARCH_TIMER_CTRL_IT_STAT) {
 142                 ctrl |= ARCH_TIMER_CTRL_IT_MASK;
 143                 arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, evt);
 144                 evt->event_handler(evt);
 145                 return IRQ_HANDLED;
 146         }
 147
 148         return IRQ_NONE;
 149 }
 150
 151 static irqreturn_t arch_timer_handler_virt(int irq, void *dev_id)
 152 {
 153         struct clock_event_device *evt = dev_id;
 154
 155         return timer_handler(ARCH_TIMER_VIRT_ACCESS, evt);
 156 }
 157
 158 static irqreturn_t arch_timer_handler_phys(int irq, void *dev_id)
 159 {
 160         struct clock_event_device *evt = dev_id;
 161
 162         return timer_handler(ARCH_TIMER_PHYS_ACCESS, evt);
 163 }
 164
 165 static irqreturn_t arch_timer_handler_phys_mem(int irq, void *dev_id)
 166 {
 167         struct clock_event_device *evt = dev_id;
 168
 169         return timer_handler(ARCH_TIMER_MEM_PHYS_ACCESS, evt);
 170 }
 171
 172 static irqreturn_t arch_timer_handler_virt_mem(int irq, void *dev_id)
 173 {
 174         struct clock_event_device *evt = dev_id;
 175
 176         return timer_handler(ARCH_TIMER_MEM_VIRT_ACCESS, evt);
 177 }
 178
 179 static __always_inline void timer_set_mode(const int access, int mode,
 180                                   struct clock_event_device *clk)
 181 {
 182         unsigned long ctrl;
 183         switch (mode) {
 184         case CLOCK_EVT_MODE_UNUSED:
 185         case CLOCK_EVT_MODE_SHUTDOWN:
 186                 ctrl = arch_timer_reg_read(access, ARCH_TIMER_REG_CTRL, clk);
 187                 ctrl &= ~ARCH_TIMER_CTRL_ENABLE;
 188                 arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
 189                 break;
 190         default:
 191                 break;
 192         }
 193 }
 194
 195 static void arch_timer_set_mode_virt(enum clock_event_mode mode,
 196                                      struct clock_event_device *clk)
 197 {
 198         timer_set_mode(ARCH_TIMER_VIRT_ACCESS, mode, clk);
 199 }
 200
 201 static void arch_timer_set_mode_phys(enum clock_event_mode mode,
 202                                      struct clock_event_device *clk)
 203 {
 204         timer_set_mode(ARCH_TIMER_PHYS_ACCESS, mode, clk);
 205 }
 206
 207 static void arch_timer_set_mode_virt_mem(enum clock_event_mode mode,
 208                                          struct clock_event_device *clk)
 209 {
 210         timer_set_mode(ARCH_TIMER_MEM_VIRT_ACCESS, mode, clk);
 211 }
 212
 213 static void arch_timer_set_mode_phys_mem(enum clock_event_mode mode,
 214                                          struct clock_event_device *clk)
 215 {
 216         timer_set_mode(ARCH_TIMER_MEM_PHYS_ACCESS, mode, clk);
 217 }
 218
 219 static __always_inline void set_next_event(const int access, unsigned long evt,
 220                                   struct clock_event_device *clk)
 221 {
 222         unsigned long ctrl;
 223         ctrl = arch_timer_reg_read(access, ARCH_TIMER_REG_CTRL, clk);
 224         ctrl |= ARCH_TIMER_CTRL_ENABLE;
 225         ctrl &= ~ARCH_TIMER_CTRL_IT_MASK;
 226         arch_timer_reg_write(access, ARCH_TIMER_REG_TVAL, evt, clk);
 227         arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
 228 }
 229
 230 static int arch_timer_set_next_event_virt(unsigned long evt,
 231                                           struct clock_event_device *clk)
 232 {
 233         set_next_event(ARCH_TIMER_VIRT_ACCESS, evt, clk);
 234         return 0;
 235 }
 236
 237 static int arch_timer_set_next_event_phys(unsigned long evt,
 238                                           struct clock_event_device *clk)
 239 {
 240         set_next_event(ARCH_TIMER_PHYS_ACCESS, evt, clk);
 241         return 0;
 242 }
 243
 244 static void arch_timer_configure_evtstream(void)
 245 {
 246         int evt_stream_div, pos;
 247
 248         /* Find the closest power of two to the divisor */
 249         evt_stream_div = arch_timer_rate / ARCH_TIMER_EVT_STREAM_FREQ;
 250         pos = fls(evt_stream_div);
 251         if (pos > 1 && !(evt_stream_div & (1 << (pos - 2))))
 252                 pos--;
 253         /* enable event stream */
 254         arch_timer_evtstrm_enable(min(pos, 15));
 255 }
 256
 257 static int arch_timer_set_next_event_virt_mem(unsigned long evt,
 258                                               struct clock_event_device *clk)
 259 {
 260         set_next_event(ARCH_TIMER_MEM_VIRT_ACCESS, evt, clk);
 261         return 0;
 262 }
 263
 264 static int arch_timer_set_next_event_phys_mem(unsigned long evt,
 265                                               struct clock_event_device *clk)
 266 {
 267         set_next_event(ARCH_TIMER_MEM_PHYS_ACCESS, evt, clk);
 268         return 0;
 269 }
 270
 271 static void __cpuinit __arch_timer_setup(unsigned type,
 272                                        struct clock_event_device *clk)
 273 {
 274         clk->features = CLOCK_EVT_FEAT_ONESHOT;
 275
 276         if (type == ARCH_CP15_TIMER) {
 277                 clk->features |= CLOCK_EVT_FEAT_C3STOP;
 278                 clk->name = "arch_sys_timer";
 279                 clk->rating = 450;
 280                 clk->cpumask = cpumask_of(smp_processor_id());
 281                 if (arch_timer_use_virtual) {
 282                         clk->irq = arch_timer_ppi[VIRT_PPI];
 283                         clk->set_mode = arch_timer_set_mode_virt;
 284                         clk->set_next_event = arch_timer_set_next_event_virt;
 285                 } else {
 286                         clk->irq = arch_timer_ppi[PHYS_SECURE_PPI];
 287                         clk->set_mode = arch_timer_set_mode_phys;
 288                         clk->set_next_event = arch_timer_set_next_event_phys;
 289                 }
 290         } else {
 291                 clk->name = "arch_mem_timer";
 292                 clk->rating = 400;
 293                 clk->cpumask = cpu_all_mask;
 294                 if (arch_timer_mem_use_virtual) {
 295                         clk->set_mode = arch_timer_set_mode_virt_mem;
 296                         clk->set_next_event =
 297                                 arch_timer_set_next_event_virt_mem;
 298                 } else {
 299                         clk->set_mode = arch_timer_set_mode_phys_mem;
 300                         clk->set_next_event =
 301                                 arch_timer_set_next_event_phys_mem;
 302                 }
 303         }
 304
 305         clk->set_mode(CLOCK_EVT_MODE_SHUTDOWN, clk);
 306
 307         clockevents_config_and_register(clk, arch_timer_rate, 0xf, 0x7fffffff);
 308 }
 309
 310 static int __cpuinit arch_timer_setup(struct clock_event_device *clk)
 311 {
 312         __arch_timer_setup(ARCH_CP15_TIMER, clk);
 313
 314         if (arch_timer_use_virtual)
 315                 enable_percpu_irq(arch_timer_ppi[VIRT_PPI], 0);
 316         else {
 317                 enable_percpu_irq(arch_timer_ppi[PHYS_SECURE_PPI], 0);
 318                 if (arch_timer_ppi[PHYS_NONSECURE_PPI])
 319                         enable_percpu_irq(arch_timer_ppi[PHYS_NONSECURE_PPI], 0);
 320         }
 321
 322         arch_counter_set_user_access();
 323         if (IS_ENABLED(CONFIG_ARM_ARCH_TIMER_EVTSTREAM))
 324                 arch_timer_configure_evtstream();
 325
 326         return 0;
 327 }
 328
 329 static void
 330 arch_timer_detect_rate(void __iomem *cntbase, struct device_node *np)
 331 {
 332         /* Who has more than one independent system counter? */
 333         if (arch_timer_rate)
 334                 return;
 335
 336         /* Try to determine the frequency from the device tree or CNTFRQ */
 337         if (of_property_read_u32(np, "clock-frequency", &arch_timer_rate)) {
 338                 if (cntbase)
 339                         arch_timer_rate = readl_relaxed(cntbase + CNTFRQ);
 340                 else
 341                         arch_timer_rate = arch_timer_get_cntfrq();
 342         }
 343
 344         /* Check the timer frequency. */
 345         if (arch_timer_rate == 0)
 346                 pr_warn("Architected timer frequency not available\n");
 347 }
 348
 349 static void arch_timer_banner(unsigned type)
 350 {
 351         pr_info("Architected %s%s%s timer(s) running at %lu.%02luMHz (%s%s%s).\n",
 352                      type & ARCH_CP15_TIMER ? "cp15" : "",
 353                      type == (ARCH_CP15_TIMER | ARCH_MEM_TIMER) ?  " and " : "",
 354                      type & ARCH_MEM_TIMER ? "mmio" : "",
 355                      (unsigned long)arch_timer_rate / 1000000,
 356                      (unsigned long)(arch_timer_rate / 10000) % 100,
 357                      type & ARCH_CP15_TIMER ?
 358                         arch_timer_use_virtual ? "virt" : "phys" :
 359                         "",
 360                      type == (ARCH_CP15_TIMER | ARCH_MEM_TIMER) ?  "/" : "",
 361                      type & ARCH_MEM_TIMER ?
 362                         arch_timer_mem_use_virtual ? "virt" : "phys" :
 363                         "");
 364 }
 365
 366 u32 arch_timer_get_rate(void)
 367 {
 368         return arch_timer_rate;
 369 }
 370
 371 static u64 arch_counter_get_cntvct_mem(void)
 372 {
 373         u32 vct_lo, vct_hi, tmp_hi;
 374
 375         do {
 376                 vct_hi = readl_relaxed(arch_counter_base + CNTVCT_HI);
 377                 vct_lo = readl_relaxed(arch_counter_base + CNTVCT_LO);
 378                 tmp_hi = readl_relaxed(arch_counter_base + CNTVCT_HI);
 379         } while (vct_hi != tmp_hi);
 380
 381         return ((u64) vct_hi << 32) | vct_lo;
 382 }
 383
 384 /*
 385  * Default to cp15 based access because arm64 uses this function for
 386  * sched_clock() before DT is probed and the cp15 method is guaranteed
 387  * to exist on arm64. arm doesn't use this before DT is probed so even
 388  * if we don't have the cp15 accessors we won't have a problem.
 389  */
 390 u64 (*arch_timer_read_counter)(void) = arch_counter_get_cntvct;
 391
 392 static cycle_t arch_counter_read(struct clocksource *cs)
 393 {
 394         return arch_counter_get_cntvct();
 395 }
 396
 397 static cycle_t arch_counter_read_cc(const struct cyclecounter *cc)
 398 {
 399         return arch_counter_get_cntvct();
 400 }
 401
 402 static struct clocksource clocksource_counter = {
 403         .name   = "arch_sys_counter",
 404         .rating = 400,
 405         .read   = arch_counter_read,
 406         .mask   = CLOCKSOURCE_MASK(56),
 407         .flags  = CLOCK_SOURCE_IS_CONTINUOUS,
 408 };
 409
 410 static struct cyclecounter cyclecounter = {
 411         .read   = arch_counter_read_cc,
 412         .mask   = CLOCKSOURCE_MASK(56),
 413 };
 414
 415 static struct timecounter timecounter;
 416
 417 struct timecounter *arch_timer_get_timecounter(void)
 418 {
 419         return &timecounter;
 420 }
 421
 422 static void __init arch_counter_register(unsigned type)
 423 {
 424         u64 start_count;
 425
 426         /* Register the CP15 based counter if we have one */
 427         if (type & ARCH_CP15_TIMER)
 428                 arch_timer_read_counter = arch_counter_get_cntvct;
 429         else
 430                 arch_timer_read_counter = arch_counter_get_cntvct_mem;
 431
 432         start_count = arch_timer_read_counter();
 433         clocksource_register_hz(&clocksource_counter, arch_timer_rate);
 434         cyclecounter.mult = clocksource_counter.mult;
 435         cyclecounter.shift = clocksource_counter.shift;
 436         timecounter_init(&timecounter, &cyclecounter, start_count);
 437 }
 438
 439 static void __cpuinit arch_timer_stop(struct clock_event_device *clk)
 440 {
 441         pr_debug("arch_timer_teardown disable IRQ%d cpu #%d\n",
 442                  clk->irq, smp_processor_id());
 443
 444         if (arch_timer_use_virtual)
 445                 disable_percpu_irq(arch_timer_ppi[VIRT_PPI]);
 446         else {
 447                 disable_percpu_irq(arch_timer_ppi[PHYS_SECURE_PPI]);
 448                 if (arch_timer_ppi[PHYS_NONSECURE_PPI])
 449                         disable_percpu_irq(arch_timer_ppi[PHYS_NONSECURE_PPI]);
 450         }
 451
 452         clk->set_mode(CLOCK_EVT_MODE_UNUSED, clk);
 453 }
 454
 455 static int __cpuinit arch_timer_cpu_notify(struct notifier_block *self,
 456                                            unsigned long action, void *hcpu)
 457 {
 458         /*
 459          * Grab cpu pointer in each case to avoid spurious
 460          * preemptible warnings
 461          */
 462         switch (action & ~CPU_TASKS_FROZEN) {
 463         case CPU_STARTING:
 464                 arch_timer_setup(this_cpu_ptr(arch_timer_evt));
 465                 break;
 466         case CPU_DYING:
 467                 arch_timer_stop(this_cpu_ptr(arch_timer_evt));
 468                 break;
 469         }
 470
 471         return NOTIFY_OK;
 472 }
 473
 474 static struct notifier_block arch_timer_cpu_nb __cpuinitdata = {
 475         .notifier_call = arch_timer_cpu_notify,
 476 };
 477
 478 #ifdef CONFIG_CPU_PM
 479 static unsigned int saved_cntkctl;
 480 static int arch_timer_cpu_pm_notify(struct notifier_block *self,
 481                                     unsigned long action, void *hcpu)
 482 {
 483         if (action == CPU_PM_ENTER)
 484                 saved_cntkctl = arch_timer_get_cntkctl();
 485         else if (action == CPU_PM_ENTER_FAILED || action == CPU_PM_EXIT)
 486                 arch_timer_set_cntkctl(saved_cntkctl);
 487         return NOTIFY_OK;
 488 }
 489
 490 static struct notifier_block arch_timer_cpu_pm_notifier = {
 491         .notifier_call = arch_timer_cpu_pm_notify,
 492 };
 493
 494 static int __init arch_timer_cpu_pm_init(void)
 495 {
 496         return cpu_pm_register_notifier(&arch_timer_cpu_pm_notifier);
 497 }
 498 #else
 499 static int __init arch_timer_cpu_pm_init(void)
 500 {
 501         return 0;
 502 }
 503 #endif
 504
 505 static int __init arch_timer_register(void)
 506 {
 507         int err;
 508         int ppi;
 509
 510         arch_timer_evt = alloc_percpu(struct clock_event_device);
 511         if (!arch_timer_evt) {
 512                 err = -ENOMEM;
 513                 goto out;
 514         }
 515
 516         if (arch_timer_use_virtual) {
 517                 ppi = arch_timer_ppi[VIRT_PPI];
 518                 err = request_percpu_irq(ppi, arch_timer_handler_virt,
 519                                          "arch_timer", arch_timer_evt);
 520         } else {
 521                 ppi = arch_timer_ppi[PHYS_SECURE_PPI];
 522                 err = request_percpu_irq(ppi, arch_timer_handler_phys,
 523                                          "arch_timer", arch_timer_evt);
 524                 if (!err && arch_timer_ppi[PHYS_NONSECURE_PPI]) {
 525                         ppi = arch_timer_ppi[PHYS_NONSECURE_PPI];
 526                         err = request_percpu_irq(ppi, arch_timer_handler_phys,
 527                                                  "arch_timer", arch_timer_evt);
 528                         if (err)
 529                                 free_percpu_irq(arch_timer_ppi[PHYS_SECURE_PPI],
 530                                                 arch_timer_evt);
 531                 }
 532         }
 533
 534         if (err) {
 535                 pr_err("arch_timer: can't register interrupt %d (%d)\n",
 536                        ppi, err);
 537                 goto out_free;
 538         }
 539
 540         err = register_cpu_notifier(&arch_timer_cpu_nb);
 541         if (err)
 542                 goto out_free_irq;
 543
 544         err = arch_timer_cpu_pm_init();
 545         if (err)
 546                 goto out_unreg_notify;
 547
 548         /* Immediately configure the timer on the boot CPU */
 549         arch_timer_setup(this_cpu_ptr(arch_timer_evt));
 550
 551         return 0;
 552
 553 out_unreg_notify:
 554         unregister_cpu_notifier(&arch_timer_cpu_nb);
 555 out_free_irq:
 556         if (arch_timer_use_virtual)
 557                 free_percpu_irq(arch_timer_ppi[VIRT_PPI], arch_timer_evt);
 558         else {
 559                 free_percpu_irq(arch_timer_ppi[PHYS_SECURE_PPI],
 560                                 arch_timer_evt);
 561                 if (arch_timer_ppi[PHYS_NONSECURE_PPI])
 562                         free_percpu_irq(arch_timer_ppi[PHYS_NONSECURE_PPI],
 563                                         arch_timer_evt);
 564         }
 565
 566 out_free:
 567         free_percpu(arch_timer_evt);
 568 out:
 569         return err;
 570 }
 571
 572 static int __init arch_timer_mem_register(void __iomem *base, unsigned int irq)
 573 {
 574         int ret;
 575         irq_handler_t func;
 576         struct arch_timer *t;
 577
 578         t = kzalloc(sizeof(*t), GFP_KERNEL);
 579         if (!t)
 580                 return -ENOMEM;
 581
 582         t->base = base;
 583         t->evt.irq = irq;
 584         __arch_timer_setup(ARCH_MEM_TIMER, &t->evt);
 585
 586         if (arch_timer_mem_use_virtual)
 587                 func = arch_timer_handler_virt_mem;
 588         else
 589                 func = arch_timer_handler_phys_mem;
 590
 591         ret = request_irq(irq, func, IRQF_TIMER, "arch_mem_timer", &t->evt);
 592         if (ret) {
 593                 pr_err("arch_timer: Failed to request mem timer irq\n");
 594                 kfree(t);
 595         }
 596
 597         return ret;
 598 }
 599
 600 static const struct of_device_id arch_timer_of_match[] __initconst = {
 601         { .compatible   = "arm,armv7-timer",    },
 602         { .compatible   = "arm,armv8-timer",    },
 603         {},
 604 };
 605
 606 static const struct of_device_id arch_timer_mem_of_match[] __initconst = {
 607         { .compatible   = "arm,armv7-timer-mem", },
 608         {},
 609 };
 610
 611 static void __init arch_timer_common_init(void)
 612 {
 613         unsigned mask = ARCH_CP15_TIMER | ARCH_MEM_TIMER;
 614
 615         /* Wait until both nodes are probed if we have two timers */
 616         if ((arch_timers_present & mask) != mask) {
 617                 if (of_find_matching_node(NULL, arch_timer_mem_of_match) &&
 618                                 !(arch_timers_present & ARCH_MEM_TIMER))
 619                         return;
 620                 if (of_find_matching_node(NULL, arch_timer_of_match) &&
 621                                 !(arch_timers_present & ARCH_CP15_TIMER))
 622                         return;
 623         }
 624
 625         arch_timer_banner(arch_timers_present);
 626         arch_counter_register(arch_timers_present);
 627         arch_timer_arch_init();
 628 }
 629
 630 static void __init arch_timer_init(struct device_node *np)
 631 {
 632         int i;
 633
 634         if (arch_timers_present & ARCH_CP15_TIMER) {
 635                 pr_warn("arch_timer: multiple nodes in dt, skipping\n");
 636                 return;
 637         }
 638
 639         arch_timers_present |= ARCH_CP15_TIMER;
 640         for (i = PHYS_SECURE_PPI; i < MAX_TIMER_PPI; i++)
 641                 arch_timer_ppi[i] = irq_of_parse_and_map(np, i);
 642         arch_timer_detect_rate(NULL, np);
 643
 644         /*
 645          * If HYP mode is available, we know that the physical timer
 646          * has been configured to be accessible from PL1. Use it, so
 647          * that a guest can use the virtual timer instead.
 648          *
 649          * If no interrupt provided for virtual timer, we'll have to
 650          * stick to the physical timer. It'd better be accessible...
 651          */
 652         if (is_hyp_mode_available() || !arch_timer_ppi[VIRT_PPI]) {
 653                 arch_timer_use_virtual = false;
 654
 655                 if (!arch_timer_ppi[PHYS_SECURE_PPI] ||
 656                     !arch_timer_ppi[PHYS_NONSECURE_PPI]) {
 657                         pr_warn("arch_timer: No interrupt available, giving up\n");
 658                         return;
 659                 }
 660         }
 661
 662         arch_timer_register();
 663         arch_timer_common_init();
 664 }
 665 CLOCKSOURCE_OF_DECLARE(armv7_arch_timer, "arm,armv7-timer", arch_timer_init);
 666 CLOCKSOURCE_OF_DECLARE(armv8_arch_timer, "arm,armv8-timer", arch_timer_init);
 667
 668 static void __init arch_timer_mem_init(struct device_node *np)
 669 {
 670         struct device_node *frame, *best_frame = NULL;
 671         void __iomem *cntctlbase, *base;
 672         unsigned int irq;
 673         u32 cnttidr;
 674
 675         arch_timers_present |= ARCH_MEM_TIMER;
 676         cntctlbase = of_iomap(np, 0);
 677         if (!cntctlbase) {
 678                 pr_err("arch_timer: Can't find CNTCTLBase\n");
 679                 return;
 680         }
 681
 682         cnttidr = readl_relaxed(cntctlbase + CNTTIDR);
 683         iounmap(cntctlbase);
 684
 685         /*
 686          * Try to find a virtual capable frame. Otherwise fall back to a
 687          * physical capable frame.
 688          */
 689         for_each_available_child_of_node(np, frame) {
 690                 int n;
 691
 692                 if (of_property_read_u32(frame, "frame-number", &n)) {
 693                         pr_err("arch_timer: Missing frame-number\n");
 694                         of_node_put(best_frame);
 695                         of_node_put(frame);
 696                         return;
 697                 }
 698
 699                 if (cnttidr & CNTTIDR_VIRT(n)) {
 700                         of_node_put(best_frame);
 701                         best_frame = frame;
 702                         arch_timer_mem_use_virtual = true;
 703                         break;
 704                 }
 705                 of_node_put(best_frame);
 706                 best_frame = of_node_get(frame);
 707         }
 708
 709         base = arch_counter_base = of_iomap(best_frame, 0);
 710         if (!base) {
 711                 pr_err("arch_timer: Can't map frame's registers\n");
 712                 of_node_put(best_frame);
 713                 return;
 714         }
 715
 716         if (arch_timer_mem_use_virtual)
 717                 irq = irq_of_parse_and_map(best_frame, 1);
 718         else
 719                 irq = irq_of_parse_and_map(best_frame, 0);
 720         of_node_put(best_frame);
 721         if (!irq) {
 722                 pr_err("arch_timer: Frame missing %s irq",
 723                                 arch_timer_mem_use_virtual ? "virt" : "phys");
 724                 return;
 725         }
 726
 727         arch_timer_detect_rate(base, np);
 728         arch_timer_mem_register(base, irq);
 729         arch_timer_common_init();
 730 }
 731 CLOCKSOURCE_OF_DECLARE(armv7_arch_timer_mem, "arm,armv7-timer-mem",
 732                        arch_timer_mem_init);