drivers/ptp/ptp_qoriq.c

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