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 | return 0; | 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 device_node *node = ptp_qoriq->dev->of_node; |
| 464 | struct ptp_qoriq *ptp_qoriq; | ||
| 465 | struct ptp_qoriq_registers *regs; | 465 | struct ptp_qoriq_registers *regs; |
| 466 | struct timespec64 now; | 466 | struct timespec64 now; |
| 467 | int err = -ENOMEM; | ||
| 468 | u32 tmr_ctrl; | ||
| 469 | unsigned long flags; | 467 | unsigned long flags; |
| 470 | void __iomem *base; | 468 | u32 tmr_ctrl; |
| 471 | |||
| 472 | ptp_qoriq = kzalloc(sizeof(*ptp_qoriq), GFP_KERNEL); | ||
| 473 | if (!ptp_qoriq) | ||
| 474 | goto no_memory; | ||
| 475 | |||
| 476 | err = -EINVAL; | ||
| 477 | 469 | ||
| 478 | ptp_qoriq->dev = &dev->dev; | 470 | ptp_qoriq->base = base; |
| 479 | ptp_qoriq->caps = ptp_qoriq_caps; | 471 | ptp_qoriq->caps = caps; |
| 480 | 472 | ||
| 481 | if (of_property_read_u32(node, "fsl,cksel", &ptp_qoriq->cksel)) | 473 | if (of_property_read_u32(node, "fsl,cksel", &ptp_qoriq->cksel)) |
| 482 | ptp_qoriq->cksel = DEFAULT_CKSEL; | 474 | ptp_qoriq->cksel = DEFAULT_CKSEL; |
| @@ -501,44 +493,9 @@ static int ptp_qoriq_probe(struct platform_device *dev) · 18 unmodified lines | |||
| 501 | pr_warn("device tree node missing required elements, try automatic configuration\n"); | 493 | pr_warn("device tree node missing required elements, try automatic configuration\n"); |
| 502 | 494 | ||
| 503 | if (ptp_qoriq_auto_config(ptp_qoriq, node)) | 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 | if (of_device_is_compatible(node, "fsl,fman-ptp-timer")) { | 499 | if (of_device_is_compatible(node, "fsl,fman-ptp-timer")) { |
| 543 | ptp_qoriq->regs.ctrl_regs = base + FMAN_CTRL_REGS_OFFSET; | 500 | ptp_qoriq->regs.ctrl_regs = base + FMAN_CTRL_REGS_OFFSET; |
| 544 | ptp_qoriq->regs.alarm_regs = base + FMAN_ALARM_REGS_OFFSET; | 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 | (ptp_qoriq->tclk_period & TCLK_PERIOD_MASK) << TCLK_PERIOD_SHIFT | | 515 | (ptp_qoriq->tclk_period & TCLK_PERIOD_MASK) << TCLK_PERIOD_SHIFT | |
| 559 | (ptp_qoriq->cksel & CKSEL_MASK) << CKSEL_SHIFT; | 516 | (ptp_qoriq->cksel & CKSEL_MASK) << CKSEL_SHIFT; |
| 560 | 517 | ||
| 518 | spin_lock_init(&ptp_qoriq->lock); | ||
| 561 | spin_lock_irqsave(&ptp_qoriq->lock, flags); | 519 | spin_lock_irqsave(&ptp_qoriq->lock, flags); |
| 562 | 520 | ||
| 563 | regs = &ptp_qoriq->regs; | 521 | regs = &ptp_qoriq->regs; |
| @@ -571,16 +529,77 @@ static int ptp_qoriq_probe(struct platform_device *dev) · 7 unmodified lines | |||
| 571 | 529 | ||
| 572 | spin_unlock_irqrestore(&ptp_qoriq->lock, flags); | 530 | spin_unlock_irqrestore(&ptp_qoriq->lock, flags); |
| 573 | 531 | ||
| 574 | ptp_qoriq->clock = ptp_clock_register(&ptp_qoriq->caps, &dev->dev); | 532 | ptp_qoriq->clock = ptp_clock_register(&ptp_qoriq->caps, ptp_qoriq->dev); |
| 575 | if (IS_ERR(ptp_qoriq->clock)) { | 533 | if (IS_ERR(ptp_qoriq->clock)) |
| 576 | err = PTR_ERR(ptp_qoriq->clock); | 534 | return PTR_ERR(ptp_qoriq->clock); |
| 577 | goto no_clock; | ||
| 578 | } | ||
| 579 | ptp_qoriq->phc_index = ptp_clock_index(ptp_qoriq->clock); | ||
| 580 | 535 | ||
| 536 | ptp_qoriq->phc_index = ptp_clock_index(ptp_qoriq->clock); | ||
| 581 | ptp_qoriq_create_debugfs(ptp_qoriq); | 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 | return 0; | 603 | return 0; |
| 585 | 604 | ||
| 586 | no_clock: | 605 | no_clock: |
| @@ -589,7 +608,6 @@ no_ioremap: · 2 unmodified lines | |||
| 589 | release_resource(ptp_qoriq->rsrc); | 608 | release_resource(ptp_qoriq->rsrc); |
| 590 | no_resource: | 609 | no_resource: |
| 591 | free_irq(ptp_qoriq->irq, ptp_qoriq); | 610 | free_irq(ptp_qoriq->irq, ptp_qoriq); |
| 592 | no_config: | ||
| 593 | no_node: | 611 | no_node: |
| 594 | kfree(ptp_qoriq); | 612 | kfree(ptp_qoriq); |
| 595 | no_memory: | 613 | no_memory: |
| @@ -599,18 +617,10 @@ no_memory: · 3 unmodified lines | |||
| 599 | static int ptp_qoriq_remove(struct platform_device *dev) | 617 | static int ptp_qoriq_remove(struct platform_device *dev) |
| 600 | { | 618 | { |
| 601 | struct ptp_qoriq *ptp_qoriq = platform_get_drvdata(dev); | 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); | 621 | ptp_qoriq_free(ptp_qoriq); |
| 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); | ||
| 610 | release_resource(ptp_qoriq->rsrc); | 622 | release_resource(ptp_qoriq->rsrc); |
| 611 | free_irq(ptp_qoriq->irq, ptp_qoriq); | ||
| 612 | kfree(ptp_qoriq); | 623 | kfree(ptp_qoriq); |
| 613 | |||
| 614 | return 0; | 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 | irqreturn_t ptp_qoriq_isr(int irq, void *priv); | 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 | int ptp_qoriq_adjfine(struct ptp_clock_info *ptp, long scaled_ppm); | 179 | int ptp_qoriq_adjfine(struct ptp_clock_info *ptp, long scaled_ppm); |
| 177 | int ptp_qoriq_adjtime(struct ptp_clock_info *ptp, s64 delta); | 180 | int ptp_qoriq_adjtime(struct ptp_clock_info *ptp, s64 delta); |
| 178 | int ptp_qoriq_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts); | 181 | int ptp_qoriq_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts); |