예제 #1
0
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);

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

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

	ch_addr = stm_channel_addr(drvdata, channel);

	flags = (flags == STP_PACKET_TIMESTAMPED) ? STM_FLAG_TIMESTAMPED : 0;
	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, 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:
		ch_addr += stm_channel_off(STM_PKT_TYPE_DATA, flags);
		stm_send(ch_addr, payload, size,
				drvdata->write_bytes);
		break;

	default:
		return -ENOTSUPP;
	}

	return size;
}
static int stm_trace_ost_header_64bit(unsigned long ch_addr, uint32_t options,
				      uint8_t entity_id, uint8_t proto_id,
				      const void *payload_data,
				      uint32_t payload_size)
{
	void *addr;
	uint8_t prepad_size;
	uint64_t header;
	char *hdr;

	hdr = (char *)&header;

	hdr[0] = OST_TOKEN_STARTBASE;
	hdr[1] = OST_VERSION_PROP;
	hdr[2] = entity_id;
	hdr[3] = proto_id;
	prepad_size = (unsigned long)payload_data & 0x7;
	*(uint32_t *)(hdr + 4) = (prepad_size << 24) | payload_size;

	/*                                                            */
	options |= STM_OPTION_MARKED;
	options &= ~STM_OPTION_TIMESTAMPED;
	addr =  (void *)(ch_addr | stm_channel_off(STM_PKT_TYPE_DATA, options));

	return stm_send_64bit(addr, &header, sizeof(header));
}
static int stm_trace_ost_tail(unsigned long ch_addr, uint32_t options)
{
	void *addr;
	uint32_t tail = 0x0;

	addr = (void *)(ch_addr | stm_channel_off(STM_PKT_TYPE_FLAG, options));

	return stm_send(addr, &tail, sizeof(tail));
}
static int stm_trace_data(unsigned long ch_addr, uint32_t options,
			  const void *data, uint32_t size)
{
	void *addr;

	options &= ~STM_OPTION_TIMESTAMPED;
	addr = (void *)(ch_addr | stm_channel_off(STM_PKT_TYPE_DATA, options));

	return stm_send(addr, data, size);
}
static int stm_trace_ost_header(unsigned long ch_addr, uint32_t options,
				uint8_t entity_id, uint8_t proto_id,
				const void *payload_data, uint32_t payload_size)
{
	void *addr;
	uint32_t header;
	char *hdr;

	hdr = (char *)&header;

	hdr[0] = OST_TOKEN_STARTSIMPLE;
	hdr[1] = OST_VERSION_MIPI1;
	hdr[2] = entity_id;
	hdr[3] = proto_id;

	/*                                    */
	options |= STM_OPTION_MARKED;
	options &= ~STM_OPTION_TIMESTAMPED;
	addr =  (void *)(ch_addr | stm_channel_off(STM_PKT_TYPE_DATA, options));

	return stm_send(addr, &header, sizeof(header));
}