int ptp_qoriq_init()

in ptp_qoriq.c [433:537]

• 85 lines of code
• 19 McCabe index (conditional complexity)

int ptp_qoriq_init(struct ptp_qoriq *ptp_qoriq, void __iomem *base,
		   const struct ptp_clock_info *caps)
{
	struct device_node *node = ptp_qoriq->dev->of_node;
	struct ptp_qoriq_registers *regs;
	struct timespec64 now;
	unsigned long flags;
	u32 tmr_ctrl;

	if (!node)
		return -ENODEV;

	ptp_qoriq->base = base;
	ptp_qoriq->caps = *caps;

	if (of_property_read_u32(node, "fsl,cksel", &ptp_qoriq->cksel))
		ptp_qoriq->cksel = DEFAULT_CKSEL;

	if (of_property_read_bool(node, "fsl,extts-fifo"))
		ptp_qoriq->extts_fifo_support = true;
	else
		ptp_qoriq->extts_fifo_support = false;

	if (of_device_is_compatible(node, "fsl,dpaa2-ptp") ||
	    of_device_is_compatible(node, "fsl,enetc-ptp"))
		ptp_qoriq->fiper3_support = true;

	if (of_property_read_u32(node,
				 "fsl,tclk-period", &ptp_qoriq->tclk_period) ||
	    of_property_read_u32(node,
				 "fsl,tmr-prsc", &ptp_qoriq->tmr_prsc) ||
	    of_property_read_u32(node,
				 "fsl,tmr-add", &ptp_qoriq->tmr_add) ||
	    of_property_read_u32(node,
				 "fsl,tmr-fiper1", &ptp_qoriq->tmr_fiper1) ||
	    of_property_read_u32(node,
				 "fsl,tmr-fiper2", &ptp_qoriq->tmr_fiper2) ||
	    of_property_read_u32(node,
				 "fsl,max-adj", &ptp_qoriq->caps.max_adj) ||
	    (ptp_qoriq->fiper3_support &&
	     of_property_read_u32(node, "fsl,tmr-fiper3",
				  &ptp_qoriq->tmr_fiper3))) {
		pr_warn("device tree node missing required elements, try automatic configuration\n");

		if (ptp_qoriq_auto_config(ptp_qoriq, node))
			return -ENODEV;
	}

	if (of_property_read_bool(node, "little-endian")) {
		ptp_qoriq->read = qoriq_read_le;
		ptp_qoriq->write = qoriq_write_le;
	} else {
		ptp_qoriq->read = qoriq_read_be;
		ptp_qoriq->write = qoriq_write_be;
	}

	/* The eTSEC uses differnt memory map with DPAA/ENETC */
	if (of_device_is_compatible(node, "fsl,etsec-ptp")) {
		ptp_qoriq->regs.ctrl_regs = base + ETSEC_CTRL_REGS_OFFSET;
		ptp_qoriq->regs.alarm_regs = base + ETSEC_ALARM_REGS_OFFSET;
		ptp_qoriq->regs.fiper_regs = base + ETSEC_FIPER_REGS_OFFSET;
		ptp_qoriq->regs.etts_regs = base + ETSEC_ETTS_REGS_OFFSET;
	} else {
		ptp_qoriq->regs.ctrl_regs = base + CTRL_REGS_OFFSET;
		ptp_qoriq->regs.alarm_regs = base + ALARM_REGS_OFFSET;
		ptp_qoriq->regs.fiper_regs = base + FIPER_REGS_OFFSET;
		ptp_qoriq->regs.etts_regs = base + ETTS_REGS_OFFSET;
	}

	spin_lock_init(&ptp_qoriq->lock);

	ktime_get_real_ts64(&now);
	ptp_qoriq_settime(&ptp_qoriq->caps, &now);

	tmr_ctrl =
	  (ptp_qoriq->tclk_period & TCLK_PERIOD_MASK) << TCLK_PERIOD_SHIFT |
	  (ptp_qoriq->cksel & CKSEL_MASK) << CKSEL_SHIFT;

	spin_lock_irqsave(&ptp_qoriq->lock, flags);

	regs = &ptp_qoriq->regs;
	ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl, tmr_ctrl);
	ptp_qoriq->write(&regs->ctrl_regs->tmr_add, ptp_qoriq->tmr_add);
	ptp_qoriq->write(&regs->ctrl_regs->tmr_prsc, ptp_qoriq->tmr_prsc);
	ptp_qoriq->write(&regs->fiper_regs->tmr_fiper1, ptp_qoriq->tmr_fiper1);
	ptp_qoriq->write(&regs->fiper_regs->tmr_fiper2, ptp_qoriq->tmr_fiper2);

	if (ptp_qoriq->fiper3_support)
		ptp_qoriq->write(&regs->fiper_regs->tmr_fiper3,
				 ptp_qoriq->tmr_fiper3);

	set_alarm(ptp_qoriq);
	ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl,
			 tmr_ctrl|FIPERST|RTPE|TE|FRD);

	spin_unlock_irqrestore(&ptp_qoriq->lock, flags);

	ptp_qoriq->clock = ptp_clock_register(&ptp_qoriq->caps, ptp_qoriq->dev);
	if (IS_ERR(ptp_qoriq->clock))
		return PTR_ERR(ptp_qoriq->clock);

	ptp_qoriq->phc_index = ptp_clock_index(ptp_qoriq->clock);
	ptp_qoriq_create_debugfs(ptp_qoriq);
	return 0;
}