drivers/ptp/ptp_qoriq.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * PTP 1588 clock for Freescale QorIQ 1588 timer
   4  *
   5  * Copyright (C) 2010 OMICRON electronics GmbH
   6  */
   7
   8 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   9
  10 #include <linux/device.h>
  11 #include <linux/hrtimer.h>
  12 #include <linux/kernel.h>
  13 #include <linux/module.h>
  14 #include <linux/of.h>
  15 #include <linux/of_platform.h>
  16 #include <linux/timex.h>
  17 #include <linux/slab.h>
  18 #include <linux/clk.h>
  19
  20 #include <linux/fsl/ptp_qoriq.h>
  21
  22 /*
  23  * Register access functions
  24  */
  25
  26 /* Caller must hold ptp_qoriq->lock. */
  27 static u64 tmr_cnt_read(struct ptp_qoriq *ptp_qoriq)
  28 {
  29         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
  30         u64 ns;
  31         u32 lo, hi;
  32
  33         lo = ptp_qoriq->read(&regs->ctrl_regs->tmr_cnt_l);
  34         hi = ptp_qoriq->read(&regs->ctrl_regs->tmr_cnt_h);
  35         ns = ((u64) hi) << 32;
  36         ns |= lo;
  37         return ns;
  38 }
  39
  40 /* Caller must hold ptp_qoriq->lock. */
  41 static void tmr_cnt_write(struct ptp_qoriq *ptp_qoriq, u64 ns)
  42 {
  43         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
  44         u32 hi = ns >> 32;
  45         u32 lo = ns & 0xffffffff;
  46
  47         ptp_qoriq->write(&regs->ctrl_regs->tmr_cnt_l, lo);
  48         ptp_qoriq->write(&regs->ctrl_regs->tmr_cnt_h, hi);
  49 }
  50
  51 /* Caller must hold ptp_qoriq->lock. */
  52 static void set_alarm(struct ptp_qoriq *ptp_qoriq)
  53 {
  54         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
  55         u64 ns;
  56         u32 lo, hi;
  57
  58         ns = tmr_cnt_read(ptp_qoriq) + 1500000000ULL;
  59         ns = div_u64(ns, 1000000000UL) * 1000000000ULL;
  60         ns -= ptp_qoriq->tclk_period;
  61         hi = ns >> 32;
  62         lo = ns & 0xffffffff;
  63         ptp_qoriq->write(&regs->alarm_regs->tmr_alarm1_l, lo);
  64         ptp_qoriq->write(&regs->alarm_regs->tmr_alarm1_h, hi);
  65 }
  66
  67 /* Caller must hold ptp_qoriq->lock. */
  68 static void set_fipers(struct ptp_qoriq *ptp_qoriq)
  69 {
  70         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
  71
  72         set_alarm(ptp_qoriq);
  73         ptp_qoriq->write(&regs->fiper_regs->tmr_fiper1, ptp_qoriq->tmr_fiper1);
  74         ptp_qoriq->write(&regs->fiper_regs->tmr_fiper2, ptp_qoriq->tmr_fiper2);
  75 }
  76
  77 static int extts_clean_up(struct ptp_qoriq *ptp_qoriq, int index,
  78                           bool update_event)
  79 {
  80         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
  81         struct ptp_clock_event event;
  82         void __iomem *reg_etts_l;
  83         void __iomem *reg_etts_h;
  84         u32 valid, stat, lo, hi;
  85
  86         switch (index) {
  87         case 0:
  88                 valid = ETS1_VLD;
  89                 reg_etts_l = &regs->etts_regs->tmr_etts1_l;
  90                 reg_etts_h = &regs->etts_regs->tmr_etts1_h;
  91                 break;
  92         case 1:
  93                 valid = ETS2_VLD;
  94                 reg_etts_l = &regs->etts_regs->tmr_etts2_l;
  95                 reg_etts_h = &regs->etts_regs->tmr_etts2_h;
  96                 break;
  97         default:
  98                 return -EINVAL;
  99         }
 100
 101         event.type = PTP_CLOCK_EXTTS;
 102         event.index = index;
 103
 104         do {
 105                 lo = ptp_qoriq->read(reg_etts_l);
 106                 hi = ptp_qoriq->read(reg_etts_h);
 107
 108                 if (update_event) {
 109                         event.timestamp = ((u64) hi) << 32;
 110                         event.timestamp |= lo;
 111                         ptp_clock_event(ptp_qoriq->clock, &event);
 112                 }
 113
 114                 stat = ptp_qoriq->read(&regs->ctrl_regs->tmr_stat);
 115         } while (ptp_qoriq->extts_fifo_support && (stat & valid));
 116
 117         return 0;
 118 }
 119
 120 /*
 121  * Interrupt service routine
 122  */
 123
 124 irqreturn_t ptp_qoriq_isr(int irq, void *priv)
 125 {
 126         struct ptp_qoriq *ptp_qoriq = priv;
 127         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
 128         struct ptp_clock_event event;
 129         u64 ns;
 130         u32 ack = 0, lo, hi, mask, val, irqs;
 131
 132         spin_lock(&ptp_qoriq->lock);
 133
 134         val = ptp_qoriq->read(&regs->ctrl_regs->tmr_tevent);
 135         mask = ptp_qoriq->read(&regs->ctrl_regs->tmr_temask);
 136
 137         spin_unlock(&ptp_qoriq->lock);
 138
 139         irqs = val & mask;
 140
 141         if (irqs & ETS1) {
 142                 ack |= ETS1;
 143                 extts_clean_up(ptp_qoriq, 0, true);
 144         }
 145
 146         if (irqs & ETS2) {
 147                 ack |= ETS2;
 148                 extts_clean_up(ptp_qoriq, 1, true);
 149         }
 150
 151         if (irqs & ALM2) {
 152                 ack |= ALM2;
 153                 if (ptp_qoriq->alarm_value) {
 154                         event.type = PTP_CLOCK_ALARM;
 155                         event.index = 0;
 156                         event.timestamp = ptp_qoriq->alarm_value;
 157                         ptp_clock_event(ptp_qoriq->clock, &event);
 158                 }
 159                 if (ptp_qoriq->alarm_interval) {
 160                         ns = ptp_qoriq->alarm_value + ptp_qoriq->alarm_interval;
 161                         hi = ns >> 32;
 162                         lo = ns & 0xffffffff;
 163                         ptp_qoriq->write(&regs->alarm_regs->tmr_alarm2_l, lo);
 164                         ptp_qoriq->write(&regs->alarm_regs->tmr_alarm2_h, hi);
 165                         ptp_qoriq->alarm_value = ns;
 166                 } else {
 167                         spin_lock(&ptp_qoriq->lock);
 168                         mask = ptp_qoriq->read(&regs->ctrl_regs->tmr_temask);
 169                         mask &= ~ALM2EN;
 170                         ptp_qoriq->write(&regs->ctrl_regs->tmr_temask, mask);
 171                         spin_unlock(&ptp_qoriq->lock);
 172                         ptp_qoriq->alarm_value = 0;
 173                         ptp_qoriq->alarm_interval = 0;
 174                 }
 175         }
 176
 177         if (irqs & PP1) {
 178                 ack |= PP1;
 179                 event.type = PTP_CLOCK_PPS;
 180                 ptp_clock_event(ptp_qoriq->clock, &event);
 181         }
 182
 183         if (ack) {
 184                 ptp_qoriq->write(&regs->ctrl_regs->tmr_tevent, ack);
 185                 return IRQ_HANDLED;
 186         } else
 187                 return IRQ_NONE;
 188 }
 189 EXPORT_SYMBOL_GPL(ptp_qoriq_isr);
 190
 191 /*
 192  * PTP clock operations
 193  */
 194
 195 int ptp_qoriq_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 196 {
 197         u64 adj, diff;
 198         u32 tmr_add;
 199         int neg_adj = 0;
 200         struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
 201         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
 202
 203         if (scaled_ppm < 0) {
 204                 neg_adj = 1;
 205                 scaled_ppm = -scaled_ppm;
 206         }
 207         tmr_add = ptp_qoriq->tmr_add;
 208         adj = tmr_add;
 209
 210         /* calculate diff as adj*(scaled_ppm/65536)/1000000
 211          * and round() to the nearest integer
 212          */
 213         adj *= scaled_ppm;
 214         diff = div_u64(adj, 8000000);
 215         diff = (diff >> 13) + ((diff >> 12) & 1);
 216
 217         tmr_add = neg_adj ? tmr_add - diff : tmr_add + diff;
 218
 219         ptp_qoriq->write(&regs->ctrl_regs->tmr_add, tmr_add);
 220
 221         return 0;
 222 }
 223 EXPORT_SYMBOL_GPL(ptp_qoriq_adjfine);
 224
 225 int ptp_qoriq_adjtime(struct ptp_clock_info *ptp, s64 delta)
 226 {
 227         s64 now;
 228         unsigned long flags;
 229         struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
 230
 231         spin_lock_irqsave(&ptp_qoriq->lock, flags);
 232
 233         now = tmr_cnt_read(ptp_qoriq);
 234         now += delta;
 235         tmr_cnt_write(ptp_qoriq, now);
 236         set_fipers(ptp_qoriq);
 237
 238         spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
 239
 240         return 0;
 241 }
 242 EXPORT_SYMBOL_GPL(ptp_qoriq_adjtime);
 243
 244 int ptp_qoriq_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 245 {
 246         u64 ns;
 247         unsigned long flags;
 248         struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
 249
 250         spin_lock_irqsave(&ptp_qoriq->lock, flags);
 251
 252         ns = tmr_cnt_read(ptp_qoriq);
 253
 254         spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
 255
 256         *ts = ns_to_timespec64(ns);
 257
 258         return 0;
 259 }
 260 EXPORT_SYMBOL_GPL(ptp_qoriq_gettime);
 261
 262 int ptp_qoriq_settime(struct ptp_clock_info *ptp,
 263                       const struct timespec64 *ts)
 264 {
 265         u64 ns;
 266         unsigned long flags;
 267         struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
 268
 269         ns = timespec64_to_ns(ts);
 270
 271         spin_lock_irqsave(&ptp_qoriq->lock, flags);
 272
 273         tmr_cnt_write(ptp_qoriq, ns);
 274         set_fipers(ptp_qoriq);
 275
 276         spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
 277
 278         return 0;
 279 }
 280 EXPORT_SYMBOL_GPL(ptp_qoriq_settime);
 281
 282 int ptp_qoriq_enable(struct ptp_clock_info *ptp,
 283                      struct ptp_clock_request *rq, int on)
 284 {
 285         struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
 286         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
 287         unsigned long flags;
 288         u32 bit, mask = 0;
 289
 290         switch (rq->type) {
 291         case PTP_CLK_REQ_EXTTS:
 292                 switch (rq->extts.index) {
 293                 case 0:
 294                         bit = ETS1EN;
 295                         break;
 296                 case 1:
 297                         bit = ETS2EN;
 298                         break;
 299                 default:
 300                         return -EINVAL;
 301                 }
 302
 303                 if (on)
 304                         extts_clean_up(ptp_qoriq, rq->extts.index, false);
 305
 306                 break;
 307         case PTP_CLK_REQ_PPS:
 308                 bit = PP1EN;
 309                 break;
 310         default:
 311                 return -EOPNOTSUPP;
 312         }
 313
 314         spin_lock_irqsave(&ptp_qoriq->lock, flags);
 315
 316         mask = ptp_qoriq->read(&regs->ctrl_regs->tmr_temask);
 317         if (on) {
 318                 mask |= bit;
 319                 ptp_qoriq->write(&regs->ctrl_regs->tmr_tevent, bit);
 320         } else {
 321                 mask &= ~bit;
 322         }
 323
 324         ptp_qoriq->write(&regs->ctrl_regs->tmr_temask, mask);
 325
 326         spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
 327         return 0;
 328 }
 329 EXPORT_SYMBOL_GPL(ptp_qoriq_enable);
 330
 331 static const struct ptp_clock_info ptp_qoriq_caps = {
 332         .owner          = THIS_MODULE,
 333         .name           = "qoriq ptp clock",
 334         .max_adj        = 512000,
 335         .n_alarm        = 0,
 336         .n_ext_ts       = N_EXT_TS,
 337         .n_per_out      = 0,
 338         .n_pins         = 0,
 339         .pps            = 1,
 340         .adjfine        = ptp_qoriq_adjfine,
 341         .adjtime        = ptp_qoriq_adjtime,
 342         .gettime64      = ptp_qoriq_gettime,
 343         .settime64      = ptp_qoriq_settime,
 344         .enable         = ptp_qoriq_enable,
 345 };
 346
 347 /**
 348  * ptp_qoriq_nominal_freq - calculate nominal frequency according to
 349  *                          reference clock frequency
 350  *
 351  * @clk_src: reference clock frequency
 352  *
 353  * The nominal frequency is the desired clock frequency.
 354  * It should be less than the reference clock frequency.
 355  * It should be a factor of 1000MHz.
 356  *
 357  * Return the nominal frequency
 358  */
 359 static u32 ptp_qoriq_nominal_freq(u32 clk_src)
 360 {
 361         u32 remainder = 0;
 362
 363         clk_src /= 1000000;
 364         remainder = clk_src % 100;
 365         if (remainder) {
 366                 clk_src -= remainder;
 367                 clk_src += 100;
 368         }
 369
 370         do {
 371                 clk_src -= 100;
 372
 373         } while (1000 % clk_src);
 374
 375         return clk_src * 1000000;
 376 }
 377
 378 /**
 379  * ptp_qoriq_auto_config - calculate a set of default configurations
 380  *
 381  * @ptp_qoriq: pointer to ptp_qoriq
 382  * @node: pointer to device_node
 383  *
 384  * If below dts properties are not provided, this function will be
 385  * called to calculate a set of default configurations for them.
 386  *   "fsl,tclk-period"
 387  *   "fsl,tmr-prsc"
 388  *   "fsl,tmr-add"
 389  *   "fsl,tmr-fiper1"
 390  *   "fsl,tmr-fiper2"
 391  *   "fsl,max-adj"
 392  *
 393  * Return 0 if success
 394  */
 395 static int ptp_qoriq_auto_config(struct ptp_qoriq *ptp_qoriq,
 396                                  struct device_node *node)
 397 {
 398         struct clk *clk;
 399         u64 freq_comp;
 400         u64 max_adj;
 401         u32 nominal_freq;
 402         u32 remainder = 0;
 403         u32 clk_src = 0;
 404
 405         ptp_qoriq->cksel = DEFAULT_CKSEL;
 406
 407         clk = of_clk_get(node, 0);
 408         if (!IS_ERR(clk)) {
 409                 clk_src = clk_get_rate(clk);
 410                 clk_put(clk);
 411         }
 412
 413         if (clk_src <= 100000000UL) {
 414                 pr_err("error reference clock value, or lower than 100MHz\n");
 415                 return -EINVAL;
 416         }
 417
 418         nominal_freq = ptp_qoriq_nominal_freq(clk_src);
 419         if (!nominal_freq)
 420                 return -EINVAL;
 421
 422         ptp_qoriq->tclk_period = 1000000000UL / nominal_freq;
 423         ptp_qoriq->tmr_prsc = DEFAULT_TMR_PRSC;
 424
 425         /* Calculate initial frequency compensation value for TMR_ADD register.
 426          * freq_comp = ceil(2^32 / freq_ratio)
 427          * freq_ratio = reference_clock_freq / nominal_freq
 428          */
 429         freq_comp = ((u64)1 << 32) * nominal_freq;
 430         freq_comp = div_u64_rem(freq_comp, clk_src, &remainder);
 431         if (remainder)
 432                 freq_comp++;
 433
 434         ptp_qoriq->tmr_add = freq_comp;
 435         ptp_qoriq->tmr_fiper1 = DEFAULT_FIPER1_PERIOD - ptp_qoriq->tclk_period;
 436         ptp_qoriq->tmr_fiper2 = DEFAULT_FIPER2_PERIOD - ptp_qoriq->tclk_period;
 437
 438         /* max_adj = 1000000000 * (freq_ratio - 1.0) - 1
 439          * freq_ratio = reference_clock_freq / nominal_freq
 440          */
 441         max_adj = 1000000000ULL * (clk_src - nominal_freq);
 442         max_adj = div_u64(max_adj, nominal_freq) - 1;
 443         ptp_qoriq->caps.max_adj = max_adj;
 444
 445         return 0;
 446 }
 447
 448 int ptp_qoriq_init(struct ptp_qoriq *ptp_qoriq, void __iomem *base,
 449                    const struct ptp_clock_info *caps)
 450 {
 451         struct device_node *node = ptp_qoriq->dev->of_node;
 452         struct ptp_qoriq_registers *regs;
 453         struct timespec64 now;
 454         unsigned long flags;
 455         u32 tmr_ctrl;
 456
 457         if (!node)
 458                 return -ENODEV;
 459
 460         ptp_qoriq->base = base;
 461         ptp_qoriq->caps = *caps;
 462
 463         if (of_property_read_u32(node, "fsl,cksel", &ptp_qoriq->cksel))
 464                 ptp_qoriq->cksel = DEFAULT_CKSEL;
 465
 466         if (of_property_read_bool(node, "fsl,extts-fifo"))
 467                 ptp_qoriq->extts_fifo_support = true;
 468         else
 469                 ptp_qoriq->extts_fifo_support = false;
 470
 471         if (of_property_read_u32(node,
 472                                  "fsl,tclk-period", &ptp_qoriq->tclk_period) ||
 473             of_property_read_u32(node,
 474                                  "fsl,tmr-prsc", &ptp_qoriq->tmr_prsc) ||
 475             of_property_read_u32(node,
 476                                  "fsl,tmr-add", &ptp_qoriq->tmr_add) ||
 477             of_property_read_u32(node,
 478                                  "fsl,tmr-fiper1", &ptp_qoriq->tmr_fiper1) ||
 479             of_property_read_u32(node,
 480                                  "fsl,tmr-fiper2", &ptp_qoriq->tmr_fiper2) ||
 481             of_property_read_u32(node,
 482                                  "fsl,max-adj", &ptp_qoriq->caps.max_adj)) {
 483                 pr_warn("device tree node missing required elements, try automatic configuration\n");
 484
 485                 if (ptp_qoriq_auto_config(ptp_qoriq, node))
 486                         return -ENODEV;
 487         }
 488
 489         if (of_property_read_bool(node, "little-endian")) {
 490                 ptp_qoriq->read = qoriq_read_le;
 491                 ptp_qoriq->write = qoriq_write_le;
 492         } else {
 493                 ptp_qoriq->read = qoriq_read_be;
 494                 ptp_qoriq->write = qoriq_write_be;
 495         }
 496
 497         /* The eTSEC uses differnt memory map with DPAA/ENETC */
 498         if (of_device_is_compatible(node, "fsl,etsec-ptp")) {
 499                 ptp_qoriq->regs.ctrl_regs = base + ETSEC_CTRL_REGS_OFFSET;
 500                 ptp_qoriq->regs.alarm_regs = base + ETSEC_ALARM_REGS_OFFSET;
 501                 ptp_qoriq->regs.fiper_regs = base + ETSEC_FIPER_REGS_OFFSET;
 502                 ptp_qoriq->regs.etts_regs = base + ETSEC_ETTS_REGS_OFFSET;
 503         } else {
 504                 ptp_qoriq->regs.ctrl_regs = base + CTRL_REGS_OFFSET;
 505                 ptp_qoriq->regs.alarm_regs = base + ALARM_REGS_OFFSET;
 506                 ptp_qoriq->regs.fiper_regs = base + FIPER_REGS_OFFSET;
 507                 ptp_qoriq->regs.etts_regs = base + ETTS_REGS_OFFSET;
 508         }
 509
 510         spin_lock_init(&ptp_qoriq->lock);
 511
 512         ktime_get_real_ts64(&now);
 513         ptp_qoriq_settime(&ptp_qoriq->caps, &now);
 514
 515         tmr_ctrl =
 516           (ptp_qoriq->tclk_period & TCLK_PERIOD_MASK) << TCLK_PERIOD_SHIFT |
 517           (ptp_qoriq->cksel & CKSEL_MASK) << CKSEL_SHIFT;
 518
 519         spin_lock_irqsave(&ptp_qoriq->lock, flags);
 520
 521         regs = &ptp_qoriq->regs;
 522         ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl, tmr_ctrl);
 523         ptp_qoriq->write(&regs->ctrl_regs->tmr_add, ptp_qoriq->tmr_add);
 524         ptp_qoriq->write(&regs->ctrl_regs->tmr_prsc, ptp_qoriq->tmr_prsc);
 525         ptp_qoriq->write(&regs->fiper_regs->tmr_fiper1, ptp_qoriq->tmr_fiper1);
 526         ptp_qoriq->write(&regs->fiper_regs->tmr_fiper2, ptp_qoriq->tmr_fiper2);
 527         set_alarm(ptp_qoriq);
 528         ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl,
 529                          tmr_ctrl|FIPERST|RTPE|TE|FRD);
 530
 531         spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
 532
 533         ptp_qoriq->clock = ptp_clock_register(&ptp_qoriq->caps, ptp_qoriq->dev);
 534         if (IS_ERR(ptp_qoriq->clock))
 535                 return PTR_ERR(ptp_qoriq->clock);
 536
 537         ptp_qoriq->phc_index = ptp_clock_index(ptp_qoriq->clock);
 538         ptp_qoriq_create_debugfs(ptp_qoriq);
 539         return 0;
 540 }
 541 EXPORT_SYMBOL_GPL(ptp_qoriq_init);
 542
 543 void ptp_qoriq_free(struct ptp_qoriq *ptp_qoriq)
 544 {
 545         struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
 546
 547         ptp_qoriq->write(&regs->ctrl_regs->tmr_temask, 0);
 548         ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl,   0);
 549
 550         ptp_qoriq_remove_debugfs(ptp_qoriq);
 551         ptp_clock_unregister(ptp_qoriq->clock);
 552         iounmap(ptp_qoriq->base);
 553         free_irq(ptp_qoriq->irq, ptp_qoriq);
 554 }
 555 EXPORT_SYMBOL_GPL(ptp_qoriq_free);
 556
 557 static int ptp_qoriq_probe(struct platform_device *dev)
 558 {
 559         struct ptp_qoriq *ptp_qoriq;
 560         int err = -ENOMEM;
 561         void __iomem *base;
 562
 563         ptp_qoriq = kzalloc(sizeof(*ptp_qoriq), GFP_KERNEL);
 564         if (!ptp_qoriq)
 565                 goto no_memory;
 566
 567         ptp_qoriq->dev = &dev->dev;
 568
 569         err = -ENODEV;
 570
 571         ptp_qoriq->irq = platform_get_irq(dev, 0);
 572         if (ptp_qoriq->irq < 0) {
 573                 pr_err("irq not in device tree\n");
 574                 goto no_node;
 575         }
 576         if (request_irq(ptp_qoriq->irq, ptp_qoriq_isr, IRQF_SHARED,
 577                         DRIVER, ptp_qoriq)) {
 578                 pr_err("request_irq failed\n");
 579                 goto no_node;
 580         }
 581
 582         ptp_qoriq->rsrc = platform_get_resource(dev, IORESOURCE_MEM, 0);
 583         if (!ptp_qoriq->rsrc) {
 584                 pr_err("no resource\n");
 585                 goto no_resource;
 586         }
 587         if (request_resource(&iomem_resource, ptp_qoriq->rsrc)) {
 588                 pr_err("resource busy\n");
 589                 goto no_resource;
 590         }
 591
 592         base = ioremap(ptp_qoriq->rsrc->start,
 593                        resource_size(ptp_qoriq->rsrc));
 594         if (!base) {
 595                 pr_err("ioremap ptp registers failed\n");
 596                 goto no_ioremap;
 597         }
 598
 599         err = ptp_qoriq_init(ptp_qoriq, base, &ptp_qoriq_caps);
 600         if (err)
 601                 goto no_clock;
 602
 603         platform_set_drvdata(dev, ptp_qoriq);
 604         return 0;
 605
 606 no_clock:
 607         iounmap(ptp_qoriq->base);
 608 no_ioremap:
 609         release_resource(ptp_qoriq->rsrc);
 610 no_resource:
 611         free_irq(ptp_qoriq->irq, ptp_qoriq);
 612 no_node:
 613         kfree(ptp_qoriq);
 614 no_memory:
 615         return err;
 616 }
 617
 618 static int ptp_qoriq_remove(struct platform_device *dev)
 619 {
 620         struct ptp_qoriq *ptp_qoriq = platform_get_drvdata(dev);
 621
 622         ptp_qoriq_free(ptp_qoriq);
 623         release_resource(ptp_qoriq->rsrc);
 624         kfree(ptp_qoriq);
 625         return 0;
 626 }
 627
 628 static const struct of_device_id match_table[] = {
 629         { .compatible = "fsl,etsec-ptp" },
 630         { .compatible = "fsl,fman-ptp-timer" },
 631         {},
 632 };
 633 MODULE_DEVICE_TABLE(of, match_table);
 634
 635 static struct platform_driver ptp_qoriq_driver = {
 636         .driver = {
 637                 .name           = "ptp_qoriq",
 638                 .of_match_table = match_table,
 639         },
 640         .probe       = ptp_qoriq_probe,
 641         .remove      = ptp_qoriq_remove,
 642 };
 643
 644 module_platform_driver(ptp_qoriq_driver);
 645
 646 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
 647 MODULE_DESCRIPTION("PTP clock for Freescale QorIQ 1588 timer");
 648 MODULE_LICENSE("GPL");