static int mpfs_mbox_send_data()

in mailbox-mpfs.c [81:127]

• 38 lines of code
• 6 McCabe index (conditional complexity)

static int mpfs_mbox_send_data(struct mbox_chan *chan, void *data)
{
	struct mpfs_mbox *mbox = (struct mpfs_mbox *)chan->con_priv;
	struct mpfs_mss_msg *msg = data;
	u32 tx_trigger;
	u16 opt_sel;
	u32 val = 0u;

	mbox->response = msg->response;
	mbox->resp_offset = msg->resp_offset;

	if (mpfs_mbox_busy(mbox))
		return -EBUSY;

	if (msg->cmd_data_size) {
		u32 index;
		u8 extra_bits = msg->cmd_data_size & 3;
		u32 *word_buf = (u32 *)msg->cmd_data;

		for (index = 0; index < (msg->cmd_data_size / 4); index++)
			writel_relaxed(word_buf[index],
				       mbox->mbox_base + MAILBOX_REG_OFFSET + index * 0x4);
		if (extra_bits) {
			u8 i;
			u8 byte_off = ALIGN_DOWN(msg->cmd_data_size, 4);
			u8 *byte_buf = msg->cmd_data + byte_off;

			val = readl_relaxed(mbox->mbox_base +
					    MAILBOX_REG_OFFSET + index * 0x4);

			for (i = 0u; i < extra_bits; i++) {
				val &= ~(0xffu << (i * 8u));
				val |= (byte_buf[i] << (i * 8u));
			}

			writel_relaxed(val,
				       mbox->mbox_base + MAILBOX_REG_OFFSET + index * 0x4);
		}
	}

	opt_sel = ((msg->mbox_offset << 7u) | (msg->cmd_opcode & 0x7fu));
	tx_trigger = (opt_sel << SCB_CTRL_POS) & SCB_CTRL_MASK;
	tx_trigger |= SCB_CTRL_REQ_MASK | SCB_STATUS_NOTIFY_MASK;
	writel_relaxed(tx_trigger, mbox->mbox_base + SERVICES_CR_OFFSET);

	return 0;
}