static int _idtcm_set_dpll_scsr_tod()

in ptp_clockmatrix.c [752:811]

• 46 lines of code
• 10 McCabe index (conditional complexity)

static int _idtcm_set_dpll_scsr_tod(struct idtcm_channel *channel,
				    struct timespec64 const *ts,
				    enum scsr_tod_write_trig_sel wr_trig,
				    enum scsr_tod_write_type_sel wr_type)
{
	struct idtcm *idtcm = channel->idtcm;
	unsigned char buf[TOD_BYTE_COUNT], cmd;
	struct timespec64 local_ts = *ts;
	int err, count = 0;

	timespec64_add_ns(&local_ts, SETTIME_CORRECTION);

	err = timespec_to_char_array(&local_ts, buf, sizeof(buf));
	if (err)
		return err;

	err = idtcm_write(idtcm, channel->tod_write, TOD_WRITE,
			  buf, sizeof(buf));
	if (err)
		return err;

	/* Trigger the write operation. */
	err = idtcm_read(idtcm, channel->tod_write, TOD_WRITE_CMD,
			 &cmd, sizeof(cmd));
	if (err)
		return err;

	cmd &= ~(TOD_WRITE_SELECTION_MASK << TOD_WRITE_SELECTION_SHIFT);
	cmd &= ~(TOD_WRITE_TYPE_MASK << TOD_WRITE_TYPE_SHIFT);
	cmd |= (wr_trig << TOD_WRITE_SELECTION_SHIFT);
	cmd |= (wr_type << TOD_WRITE_TYPE_SHIFT);

	err = idtcm_write(idtcm, channel->tod_write, TOD_WRITE_CMD,
			   &cmd, sizeof(cmd));
	if (err)
		return err;

	/* Wait for the operation to complete. */
	while (1) {
		/* pps trigger takes up to 1 sec to complete */
		if (wr_trig == SCSR_TOD_WR_TRIG_SEL_TODPPS)
			msleep(50);

		err = idtcm_read(idtcm, channel->tod_write, TOD_WRITE_CMD,
				 &cmd, sizeof(cmd));
		if (err)
			return err;

		if ((cmd & TOD_WRITE_SELECTION_MASK) == 0)
			break;

		if (++count > 20) {
			dev_err(idtcm->dev,
				"Timed out waiting for the write counter");
			return -EIO;
		}
	}

	return 0;
}