static int spmi_write_cmd()

in hisi-spmi-controller.c [187:261]

• 63 lines of code
• 8 McCabe index (conditional complexity)

static int spmi_write_cmd(struct spmi_controller *ctrl,
			  u8 opc, u8 slave_id, u16 slave_addr, const u8 *__buf, size_t bc)
{
	struct spmi_controller_dev *spmi_controller = dev_get_drvdata(&ctrl->dev);
	u32 chnl_ofst = SPMI_CHANNEL_OFFSET * spmi_controller->channel;
	const u8 *buf = __buf;
	unsigned long flags;
	u32 cmd, data;
	int rc;
	u8 op_code, i;

	if (bc > SPMI_CONTROLLER_MAX_TRANS_BYTES) {
		dev_err(&ctrl->dev,
			"spmi_controller supports 1..%d bytes per trans, but:%zu requested\n",
			SPMI_CONTROLLER_MAX_TRANS_BYTES, bc);
		return  -EINVAL;
	}

	switch (opc) {
	case SPMI_CMD_WRITE:
		op_code = SPMI_CMD_REG_WRITE;
		break;
	case SPMI_CMD_EXT_WRITE:
		op_code = SPMI_CMD_EXT_REG_WRITE;
		break;
	case SPMI_CMD_EXT_WRITEL:
		op_code = SPMI_CMD_EXT_REG_WRITE_L;
		break;
	default:
		dev_err(&ctrl->dev, "invalid write cmd 0x%x\n", opc);
		return -EINVAL;
	}

	cmd = SPMI_APB_SPMI_CMD_EN |
	      (op_code << SPMI_APB_SPMI_CMD_TYPE_OFFSET) |
	      ((bc - 1) << SPMI_APB_SPMI_CMD_LENGTH_OFFSET) |
	      ((slave_id & 0xf) << SPMI_APB_SPMI_CMD_SLAVEID_OFFSET) |
	      ((slave_addr & 0xffff)  << SPMI_APB_SPMI_CMD_ADDR_OFFSET);

	/* Write data to FIFOs */
	spin_lock_irqsave(&spmi_controller->lock, flags);

	for (i = 0; bc > i * SPMI_PER_DATAREG_BYTE; i++) {
		data = 0;
		if ((bc - i * SPMI_PER_DATAREG_BYTE) >> 2) {
			memcpy(&data, buf, sizeof(data));
			buf += sizeof(data);
		} else {
			memcpy(&data, buf, bc % SPMI_PER_DATAREG_BYTE);
			buf += (bc % SPMI_PER_DATAREG_BYTE);
		}

		writel((u32 __force)cpu_to_be32(data),
		       spmi_controller->base + chnl_ofst +
		       SPMI_APB_SPMI_WDATA0_BASE_ADDR +
		       SPMI_PER_DATAREG_BYTE * i);
	}

	/* Start the transaction */
	writel(cmd, spmi_controller->base + chnl_ofst + SPMI_APB_SPMI_CMD_BASE_ADDR);

	rc = spmi_controller_wait_for_done(&ctrl->dev, spmi_controller,
					   spmi_controller->base, slave_id,
					   slave_addr);
	spin_unlock_irqrestore(&spmi_controller->lock, flags);

	if (rc)
		dev_err(&ctrl->dev, "spmi write wait timeout op:0x%x slave_id:%d slave_addr:0x%x bc:%zu\n",
			opc, slave_id, slave_addr, bc);
	else
		dev_dbg(&ctrl->dev, "%s: id:%d slave_addr:0x%x, wrote value: %*ph\n",
			__func__, slave_id, slave_addr, (int)bc, __buf);

	return rc;
}