static int pmif_spmi_write_cmd()

in spmi-mtk-pmif.c [373:419]

• 35 lines of code
• 9 McCabe index (conditional complexity)

static int pmif_spmi_write_cmd(struct spmi_controller *ctrl, u8 opc, u8 sid,
			       u16 addr, const u8 *buf, size_t len)
{
	struct pmif *arb = spmi_controller_get_drvdata(ctrl);
	struct ch_reg *inf_reg;
	int ret;
	u32 data, cmd;

	if (len > 4) {
		dev_err(&ctrl->dev, "pmif supports 1..4 bytes per trans, but:%zu requested", len);

		return -EINVAL;
	}

	/* Check the opcode */
	if (opc >= 0x40 && opc <= 0x5F)
		opc = PMIF_CMD_REG;
	else if ((opc <= 0xF) || (opc >= 0x30 && opc <= 0x37))
		opc = PMIF_CMD_EXT_REG_LONG;
	else if (opc >= 0x80)
		opc = PMIF_CMD_REG_0;
	else
		return -EINVAL;

	/* Wait for Software Interface FSM state to be IDLE. */
	inf_reg = &arb->chan;
	ret = readl_poll_timeout_atomic(arb->base + arb->data->regs[inf_reg->ch_sta],
					data, GET_SWINF(data) == SWINF_IDLE,
					PMIF_DELAY_US, PMIF_TIMEOUT_US);
	if (ret < 0) {
		/* set channel ready if the data has transferred */
		if (pmif_is_fsm_vldclr(arb))
			pmif_writel(arb, 1, inf_reg->ch_rdy);
		dev_err(&ctrl->dev, "failed to wait for SWINF_IDLE\n");
		return ret;
	}

	/* Set the write data. */
	memcpy(&data, buf, len);
	pmif_writel(arb, data, inf_reg->wdata);

	/* Send the command. */
	cmd = (opc << 30) | BIT(29) | (sid << 24) | ((len - 1) << 16) | addr;
	pmif_writel(arb, cmd, inf_reg->ch_send);

	return 0;
}