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(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_START_TOKEN;
hdr[1] = OST_VERSION;
hdr[2] = entity_id;
hdr[3] = proto_id;
prepad_size = (unsigned long)payload_data & 0x7;
*(uint32_t *)(hdr + 4) = (prepad_size << 24) | payload_size;
/* For 64bit writes, header is expected to be of the D32M, D32M */
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));
}
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));
}
