static int spmi_read_cmd()

in hisi-spmi-controller.c [111:185]

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

static int spmi_read_cmd(struct spmi_controller *ctrl,
			 u8 opc, u8 slave_id, u16 slave_addr, 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;
	unsigned long flags;
	u8 *buf = __buf;
	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_READ:
		op_code = SPMI_CMD_REG_READ;
		break;
	case SPMI_CMD_EXT_READ:
		op_code = SPMI_CMD_EXT_REG_READ;
		break;
	case SPMI_CMD_EXT_READL:
		op_code = SPMI_CMD_EXT_REG_READ_L;
		break;
	default:
		dev_err(&ctrl->dev, "invalid read 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) |  /* slvid */
	     ((slave_addr & 0xffff)  << SPMI_APB_SPMI_CMD_ADDR_OFFSET); /* slave_addr */

	spin_lock_irqsave(&spmi_controller->lock, flags);

	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);
	if (rc)
		goto done;

	for (i = 0; bc > i * SPMI_PER_DATAREG_BYTE; i++) {
		data = readl(spmi_controller->base + chnl_ofst +
			     SPMI_SLAVE_OFFSET * slave_id +
			     SPMI_APB_SPMI_RDATA0_BASE_ADDR +
			     i * SPMI_PER_DATAREG_BYTE);
		data = be32_to_cpu((__be32 __force)data);
		if ((bc - i * SPMI_PER_DATAREG_BYTE) >> 2) {
			memcpy(buf, &data, sizeof(data));
			buf += sizeof(data);
		} else {
			memcpy(buf, &data, bc % SPMI_PER_DATAREG_BYTE);
			buf += (bc % SPMI_PER_DATAREG_BYTE);
		}
	}

done:
	spin_unlock_irqrestore(&spmi_controller->lock, flags);
	if (rc)
		dev_err(&ctrl->dev,
			"spmi read wait timeout op:0x%x slave_id:%d slave_addr:0x%x bc:%zu\n",
			opc, slave_id, slave_addr, bc + 1);
	else
		dev_dbg(&ctrl->dev, "%s: id:%d slave_addr:0x%x, read value: %*ph\n",
			__func__, slave_id, slave_addr, (int)bc, __buf);

	return rc;
}