Moved QorIQ PTP clock initialization/free into new functions ptp_qoriq_init()/ptp_qoriq_free(). These functions could also be reused by ENETC PTP drvier which is a PCI driver for same 1588 timer IP block.
Unified · Split
| @@ -458,25 +458,17 @@ static int ptp_qoriq_auto_config(struct ptp_qoriq *ptp_qoriq, · 457 unmodified lines | ||
| 458 | 458 | return 0; |
| 459 | 459 | } |
| 460 | 460 | |
| 461 | −static int ptp_qoriq_probe(struct platform_device *dev) | |
| 461 | +int ptp_qoriq_init(struct ptp_qoriq *ptp_qoriq, void __iomem *base, | |
| 462 | + const struct ptp_clock_info caps) | |
| 462 | 463 | { |
| 463 | − struct device_node *node = dev->dev.of_node; | |
| 464 | − struct ptp_qoriq *ptp_qoriq; | |
| 464 | + struct device_node *node = ptp_qoriq->dev->of_node; | |
| 465 | 465 | struct ptp_qoriq_registers *regs; |
| 466 | 466 | struct timespec64 now; |
| 467 | − int err = -ENOMEM; | |
| 468 | − u32 tmr_ctrl; | |
| 469 | 467 | unsigned long flags; |
| 470 | − void __iomem *base; | |
| 471 | − | |
| 472 | − ptp_qoriq = kzalloc(sizeof(*ptp_qoriq), GFP_KERNEL); | |
| 473 | − if (!ptp_qoriq) | |
| 474 | − goto no_memory; | |
| 475 | − | |
| 476 | − err = -EINVAL; | |
| 468 | + u32 tmr_ctrl; | |
| 477 | 469 | |
| 478 | − ptp_qoriq->dev = &dev->dev; | |
| 479 | − ptp_qoriq->caps = ptp_qoriq_caps; | |
| 470 | + ptp_qoriq->base = base; | |
| 471 | + ptp_qoriq->caps = caps; | |
| 480 | 472 | |
| 481 | 473 | if (of_property_read_u32(node, "fsl,cksel", &ptp_qoriq->cksel)) |
| 482 | 474 | ptp_qoriq->cksel = DEFAULT_CKSEL; |
| @@ -501,44 +493,9 @@ static int ptp_qoriq_probe(struct platform_device *dev) · 18 unmodified lines | ||
| 501 | 493 | pr_warn("device tree node missing required elements, try automatic configuration\n"); |
| 502 | 494 | |
| 503 | 495 | if (ptp_qoriq_auto_config(ptp_qoriq, node)) |
| 504 | − goto no_config; | |
| 496 | + return -ENODEV; | |
| 505 | 497 | } |
| 506 | 498 | |
| 507 | − err = -ENODEV; | |
| 508 | − | |
| 509 | − ptp_qoriq->irq = platform_get_irq(dev, 0); | |
| 510 | − | |
| 511 | − if (ptp_qoriq->irq < 0) { | |
| 512 | − pr_err("irq not in device tree\n"); | |
| 513 | − goto no_node; | |
| 514 | − } | |
| 515 | − if (request_irq(ptp_qoriq->irq, ptp_qoriq_isr, IRQF_SHARED, | |
| 516 | − DRIVER, ptp_qoriq)) { | |
| 517 | − pr_err("request_irq failed\n"); | |
| 518 | − goto no_node; | |
| 519 | − } | |
| 520 | − | |
| 521 | − ptp_qoriq->rsrc = platform_get_resource(dev, IORESOURCE_MEM, 0); | |
| 522 | − if (!ptp_qoriq->rsrc) { | |
| 523 | − pr_err("no resource\n"); | |
| 524 | − goto no_resource; | |
| 525 | − } | |
| 526 | − if (request_resource(&iomem_resource, ptp_qoriq->rsrc)) { | |
| 527 | − pr_err("resource busy\n"); | |
| 528 | − goto no_resource; | |
| 529 | − } | |
| 530 | − | |
| 531 | − spin_lock_init(&ptp_qoriq->lock); | |
| 532 | − | |
| 533 | − base = ioremap(ptp_qoriq->rsrc->start, | |
| 534 | − resource_size(ptp_qoriq->rsrc)); | |
| 535 | − if (!base) { | |
| 536 | − pr_err("ioremap ptp registers failed\n"); | |
| 537 | − goto no_ioremap; | |
| 538 | − } | |
| 539 | − | |
| 540 | − ptp_qoriq->base = base; | |
| 541 | − | |
| 542 | 499 | if (of_device_is_compatible(node, "fsl,fman-ptp-timer")) { |
| 543 | 500 | ptp_qoriq->regs.ctrl_regs = base + FMAN_CTRL_REGS_OFFSET; |
| 544 | 501 | ptp_qoriq->regs.alarm_regs = base + FMAN_ALARM_REGS_OFFSET; |
| @@ -558,6 +515,7 @@ static int ptp_qoriq_probe(struct platform_device *dev) · 13 unmodified lines | ||
| 558 | 515 | (ptp_qoriq->tclk_period & TCLK_PERIOD_MASK) << TCLK_PERIOD_SHIFT | |
| 559 | 516 | (ptp_qoriq->cksel & CKSEL_MASK) << CKSEL_SHIFT; |
| 560 | 517 | |
| 518 | + spin_lock_init(&ptp_qoriq->lock); | |
| 561 | 519 | spin_lock_irqsave(&ptp_qoriq->lock, flags); |
| 562 | 520 | |
| 563 | 521 | regs = &ptp_qoriq->regs; |
| @@ -571,16 +529,77 @@ static int ptp_qoriq_probe(struct platform_device *dev) · 7 unmodified lines | ||
| 571 | 529 | |
| 572 | 530 | spin_unlock_irqrestore(&ptp_qoriq->lock, flags); |
| 573 | 531 | |
| 574 | − ptp_qoriq->clock = ptp_clock_register(&ptp_qoriq->caps, &dev->dev); | |
| 575 | − if (IS_ERR(ptp_qoriq->clock)) { | |
| 576 | − err = PTR_ERR(ptp_qoriq->clock); | |
| 577 | − goto no_clock; | |
| 578 | − } | |
| 579 | − ptp_qoriq->phc_index = ptp_clock_index(ptp_qoriq->clock); | |
| 532 | + ptp_qoriq->clock = ptp_clock_register(&ptp_qoriq->caps, ptp_qoriq->dev); | |
| 533 | + if (IS_ERR(ptp_qoriq->clock)) | |
| 534 | + return PTR_ERR(ptp_qoriq->clock); | |
| 580 | 535 | |
| 536 | + ptp_qoriq->phc_index = ptp_clock_index(ptp_qoriq->clock); | |
| 581 | 537 | ptp_qoriq_create_debugfs(ptp_qoriq); |
| 582 | − platform_set_drvdata(dev, ptp_qoriq); | |
| 538 | + return 0; | |
| 539 | +} | |
| 540 | +EXPORT_SYMBOL_GPL(ptp_qoriq_init); | |
| 541 | + | |
| 542 | +void ptp_qoriq_free(struct ptp_qoriq *ptp_qoriq) | |
| 543 | +{ | |
| 544 | + struct ptp_qoriq_registers *regs = &ptp_qoriq->regs; | |
| 545 | + | |
| 546 | + qoriq_write(®s->ctrl_regs->tmr_temask, 0); | |
| 547 | + qoriq_write(®s->ctrl_regs->tmr_ctrl, 0); | |
| 548 | + | |
| 549 | + ptp_qoriq_remove_debugfs(ptp_qoriq); | |
| 550 | + ptp_clock_unregister(ptp_qoriq->clock); | |
| 551 | + iounmap(ptp_qoriq->base); | |
| 552 | + free_irq(ptp_qoriq->irq, ptp_qoriq); | |
| 553 | +} | |
| 554 | +EXPORT_SYMBOL_GPL(ptp_qoriq_free); | |
| 555 | + | |
| 556 | +static int ptp_qoriq_probe(struct platform_device *dev) | |
| 557 | +{ | |
| 558 | + struct ptp_qoriq *ptp_qoriq; | |
| 559 | + int err = -ENOMEM; | |
| 560 | + void __iomem *base; | |
| 583 | 561 | |
| 562 | + ptp_qoriq = kzalloc(sizeof(*ptp_qoriq), GFP_KERNEL); | |
| 563 | + if (!ptp_qoriq) | |
| 564 | + goto no_memory; | |
| 565 | + | |
| 566 | + ptp_qoriq->dev = &dev->dev; | |
| 567 | + | |
| 568 | + err = -ENODEV; | |
| 569 | + | |
| 570 | + ptp_qoriq->irq = platform_get_irq(dev, 0); | |
| 571 | + if (ptp_qoriq->irq < 0) { | |
| 572 | + pr_err("irq not in device tree\n"); | |
| 573 | + goto no_node; | |
| 574 | + } | |
| 575 | + if (request_irq(ptp_qoriq->irq, ptp_qoriq_isr, IRQF_SHARED, | |
| 576 | + DRIVER, ptp_qoriq)) { | |
| 577 | + pr_err("request_irq failed\n"); | |
| 578 | + goto no_node; | |
| 579 | + } | |
| 580 | + | |
| 581 | + ptp_qoriq->rsrc = platform_get_resource(dev, IORESOURCE_MEM, 0); | |
| 582 | + if (!ptp_qoriq->rsrc) { | |
| 583 | + pr_err("no resource\n"); | |
| 584 | + goto no_resource; | |
| 585 | + } | |
| 586 | + if (request_resource(&iomem_resource, ptp_qoriq->rsrc)) { | |
| 587 | + pr_err("resource busy\n"); | |
| 588 | + goto no_resource; | |
| 589 | + } | |
| 590 | + | |
| 591 | + base = ioremap(ptp_qoriq->rsrc->start, | |
| 592 | + resource_size(ptp_qoriq->rsrc)); | |
| 593 | + if (!base) { | |
| 594 | + pr_err("ioremap ptp registers failed\n"); | |
| 595 | + goto no_ioremap; | |
| 596 | + } | |
| 597 | + | |
| 598 | + err = ptp_qoriq_init(ptp_qoriq, base, ptp_qoriq_caps); | |
| 599 | + if (err) | |
| 600 | + goto no_clock; | |
| 601 | + | |
| 602 | + platform_set_drvdata(dev, ptp_qoriq); | |
| 584 | 603 | return 0; |
| 585 | 604 | |
| 586 | 605 | no_clock: |
| @@ -589,7 +608,6 @@ no_ioremap: · 2 unmodified lines | ||
| 589 | 608 | release_resource(ptp_qoriq->rsrc); |
| 590 | 609 | no_resource: |
| 591 | 610 | free_irq(ptp_qoriq->irq, ptp_qoriq); |
| 592 | −no_config: | |
| 593 | 611 | no_node: |
| 594 | 612 | kfree(ptp_qoriq); |
| 595 | 613 | no_memory: |
| @@ -599,18 +617,10 @@ no_memory: · 3 unmodified lines | ||
| 599 | 617 | static int ptp_qoriq_remove(struct platform_device *dev) |
| 600 | 618 | { |
| 601 | 619 | struct ptp_qoriq *ptp_qoriq = platform_get_drvdata(dev); |
| 602 | − struct ptp_qoriq_registers *regs = &ptp_qoriq->regs; | |
| 603 | 620 | |
| 604 | − qoriq_write(®s->ctrl_regs->tmr_temask, 0); | |
| 605 | − qoriq_write(®s->ctrl_regs->tmr_ctrl, 0); | |
| 606 | − | |
| 607 | − ptp_qoriq_remove_debugfs(ptp_qoriq); | |
| 608 | − ptp_clock_unregister(ptp_qoriq->clock); | |
| 609 | − iounmap(ptp_qoriq->base); | |
| 621 | + ptp_qoriq_free(ptp_qoriq); | |
| 610 | 622 | release_resource(ptp_qoriq->rsrc); |
| 611 | − free_irq(ptp_qoriq->irq, ptp_qoriq); | |
| 612 | 623 | kfree(ptp_qoriq); |
| 613 | − | |
| 614 | 624 | return 0; |
| 615 | 625 | } |
| 616 | 626 | |
| @@ -173,6 +173,9 @@ static inline void qoriq_write(unsigned __iomem *addr, u32 val) · 172 unmodified lines | ||
| 173 | 173 | } |
| 174 | 174 | |
| 175 | 175 | irqreturn_t ptp_qoriq_isr(int irq, void *priv); |
| 176 | +int ptp_qoriq_init(struct ptp_qoriq *ptp_qoriq, void __iomem *base, | |
| 177 | + const struct ptp_clock_info caps); | |
| 178 | +void ptp_qoriq_free(struct ptp_qoriq *ptp_qoriq); | |
| 176 | 179 | int ptp_qoriq_adjfine(struct ptp_clock_info *ptp, long scaled_ppm); |
| 177 | 180 | int ptp_qoriq_adjtime(struct ptp_clock_info *ptp, s64 delta); |
| 178 | 181 | int ptp_qoriq_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts); |