static ssize_t notrace stm_generic_packet()

in coresight/coresight-stm.c [405:461]

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

static ssize_t notrace stm_generic_packet(struct stm_data *stm_data,
				  unsigned int master,
				  unsigned int channel,
				  unsigned int packet,
				  unsigned int flags,
				  unsigned int size,
				  const unsigned char *payload)
{
	void __iomem *ch_addr;
	struct stm_drvdata *drvdata = container_of(stm_data,
						   struct stm_drvdata, stm);
	unsigned int stm_flags;

	if (!(drvdata && local_read(&drvdata->mode)))
		return -EACCES;

	if (channel >= drvdata->numsp)
		return -EINVAL;

	ch_addr = stm_channel_addr(drvdata, channel);

	stm_flags = (flags & STP_PACKET_TIMESTAMPED) ?
			STM_FLAG_TIMESTAMPED : 0;
	stm_flags |= test_bit(channel, drvdata->chs.guaranteed) ?
			   STM_FLAG_GUARANTEED : 0;

	if (size > drvdata->write_bytes)
		size = drvdata->write_bytes;
	else
		size = rounddown_pow_of_two(size);

	switch (packet) {
	case STP_PACKET_FLAG:
		ch_addr += stm_channel_off(STM_PKT_TYPE_FLAG, stm_flags);

		/*
		 * The generic STM core sets a size of '0' on flag packets.
		 * As such send a flag packet of size '1' and tell the
		 * core we did so.
		 */
		stm_send(ch_addr, payload, 1, drvdata->write_bytes);
		size = 1;
		break;

	case STP_PACKET_DATA:
		stm_flags |= (flags & STP_PACKET_MARKED) ? STM_FLAG_MARKED : 0;
		ch_addr += stm_channel_off(STM_PKT_TYPE_DATA, stm_flags);
		stm_send(ch_addr, payload, size,
				drvdata->write_bytes);
		break;

	default:
		return -ENOTSUPP;
	}

	return size;
}