struct trace_data *trq_get_data_slot(struct trace_data_queue *trq)
{
int qsize = MAX_TRACE_SIZE;
int in;
int out;
unsigned long flags;
struct trace_data *trd;
spin_lock_irqsave(&trq->lock, flags);
in = trq->in;
out = trq->out;
if (circ_get_usage(qsize, in, out) < 1) {
spin_unlock_irqrestore(&trq->lock, flags);
return NULL;
}
/* Get a data slot and make it empty */
trd = &trq->trd[out++];
trq->out = (out == qsize) ? 0 : out;
spin_unlock_irqrestore(&trq->lock, flags);
return trd;
}
static int rx_frames_from_dev(struct mem_link_device *mld,
struct mem_ipc_device *dev)
{
struct link_device *ld = &mld->link_dev;
struct sk_buff_head *skb_rxq = dev->skb_rxq;
unsigned int qsize = get_rxq_buff_size(dev);
unsigned int in = get_rxq_head(dev);
unsigned int out = get_rxq_tail(dev);
unsigned int size = circ_get_usage(qsize, in, out);
int rcvd = 0;
if (unlikely(circ_empty(in, out)))
return 0;
while (rcvd < size) {
struct sk_buff *skb;
u8 ch;
struct io_device *iod;
skb = rxq_read(mld, dev, in);
if (!skb)
break;
ch = sipc5_get_ch(skb->data);
iod = link_get_iod_with_channel(ld, ch);
if (!iod) {
mif_err("%s: ERR! No IOD for CH.%d\n", ld->name, ch);
dev_kfree_skb_any(skb);
mem_forced_cp_crash(mld);
break;
}
/* Record the IO device and the link device into the &skb->cb */
skbpriv(skb)->iod = iod;
skbpriv(skb)->ld = ld;
skbpriv(skb)->lnk_hdr = iod->link_header;
skbpriv(skb)->sipc_ch = ch;
/* The $rcvd must be accumulated here, because $skb can be freed
in pass_skb_to_demux(). */
rcvd += skb->len;
if (likely(sipc_ps_ch(ch)))
skb_queue_tail(skb_rxq, skb);
else
pass_skb_to_demux(mld, skb);
}
if (rcvd < size) {
struct link_device *ld = &mld->link_dev;
mif_err("%s: WARN! rcvd %d < size %d\n", ld->name, rcvd, size);
}
return rcvd;
}
/**
@brief extract all IPC link frames from an SBD RB
In a while loop,\n
1) receives each IPC link frame stored in the @b @@RB.\n
2) passes it to the DEMUX layer immediately.\n
@param rb the pointer to a mem_ring_buffer instance
@retval "> 0" if valid data received
@retval "= 0" if no data received
@retval "< 0" if ANY error
*/
static int rx_ipc_frames_from_rb(struct sbd_ring_buffer *rb)
{
int rcvd = 0;
struct link_device *ld = rb->ld;
struct mem_link_device *mld = ld_to_mem_link_device(ld);
unsigned int qlen = rb->len;
unsigned int in = *rb->wp;
unsigned int out = *rb->rp;
unsigned int num_frames = circ_get_usage(qlen, in, out);
while (rcvd < num_frames) {
struct sk_buff *skb;
skb = sbd_pio_rx(rb);
if (!skb) {
#ifdef CONFIG_SEC_MODEM_DEBUG
panic("skb alloc failed.");
#else
modemctl_notify_event(MDM_CRASH_NO_MEM);
#endif
break;
}
/* The $rcvd must be accumulated here, because $skb can be freed
in pass_skb_to_demux(). */
rcvd++;
if (skbpriv(skb)->lnk_hdr) {
u8 ch = rb->ch;
u8 fch = sipc5_get_ch(skb->data);
if (fch != ch) {
mif_err("frm.ch:%d != rb.ch:%d\n", fch, ch);
dev_kfree_skb_any(skb);
modemctl_notify_event(MDM_EVENT_CP_ABNORMAL_RX);
continue;
}
}
pass_skb_to_demux(mld, skb);
}
if (rcvd < num_frames) {
struct io_device *iod = rb->iod;
struct modem_ctl *mc = ld->mc;
mif_err("%s: %s<-%s: WARN! rcvd %d < num_frames %d\n",
ld->name, iod->name, mc->name, rcvd, num_frames);
}
return rcvd;
}
static int rx_ipc_frames_from_rb(struct sbd_ring_buffer *rb)
{
int rcvd = 0;
struct link_device *ld = rb->ld;
struct mem_link_device *mld = ld_to_mem_link_device(ld);
unsigned int qlen = rb->len;
unsigned int in = *rb->wp;
unsigned int out = *rb->rp;
unsigned int num_frames = circ_get_usage(qlen, in, out);
while (rcvd < num_frames) {
struct sk_buff *skb;
skb = sbd_pio_rx(rb);
if (!skb) {
/* TODO : Replace with panic() */
mem_forced_cp_crash(mld);
break;
}
/* The $rcvd must be accumulated here, because $skb can be freed
in pass_skb_to_demux(). */
rcvd++;
if (skbpriv(skb)->lnk_hdr) {
u8 ch = rb->ch;
u8 fch = sipc5_get_ch(skb->data);
if (fch != ch) {
mif_err("frm.ch:%d != rb.ch:%d\n", fch, ch);
dev_kfree_skb_any(skb);
continue;
}
}
pass_skb_to_demux(mld, skb);
}
if (rcvd < num_frames) {
struct io_device *iod = rb->iod;
struct modem_ctl *mc = ld->mc;
mif_err("%s: %s<-%s: WARN! rcvd %d < num_frames %d\n",
ld->name, iod->name, mc->name, rcvd, num_frames);
}
return rcvd;
}
/**
@brief print a REQ_ACK
Prints a snapshot of the status of the @b @@dev circular queue when AP sends or
receives an REQ_ACK.
@param mld the pointer to a mem_link_device instance
@param mst the pointer to a mem_snapshot instance
@param dev the pointer to a mem_ipc_device instance (IPC_FMT, etc.)
@param dir the direction of communication (TX or RX)
*/
void print_req_ack(struct mem_link_device *mld, struct mem_snapshot *mst,
struct mem_ipc_device *dev, enum direction dir)
{
#ifdef DEBUG_MODEM_IF_FLOW_CTRL
struct link_device *ld = &mld->link_dev;
struct modem_ctl *mc = ld->mc;
enum dev_format id = dev->id;
unsigned int qsize = get_size(cq(dev, dir));
unsigned int in = mst->head[id][dir];
unsigned int out = mst->tail[id][dir];
unsigned int usage = circ_get_usage(qsize, in, out);
unsigned int space = circ_get_space(qsize, in, out);
mif_info("REQ_ACK: %s%s%s: %s_%s.%d "
"{in:%u out:%u usage:%u space:%u}\n",
ld->name, arrow(dir), mc->name, dev->name, q_dir(dir),
dev->req_ack_cnt[dir], in, out, usage, space);
#endif
}
/**
@brief check the free space in a circular TXQ
@param mld the pointer to a mem_link_device instance
@param dev the pointer to a mem_ipc_device instance
@param qsize the size of the buffer in @b @@dev TXQ
@param in the IN (HEAD) pointer value of the TXQ
@param out the OUT (TAIL) pointer value of the TXQ
@param count the size of the data to be transmitted
@retval "> 0" the size of free space in the @b @@dev TXQ
@retval "< 0" an error code
*/
static inline int check_txq_space(struct mem_link_device *mld,
struct mem_ipc_device *dev,
unsigned int qsize, unsigned int in,
unsigned int out, unsigned int count)
{
struct link_device *ld = &mld->link_dev;
struct modem_ctl *mc = ld->mc;
unsigned int usage;
unsigned int space;
if (!circ_valid(qsize, in, out)) {
mif_err("%s: ERR! Invalid %s_TXQ{qsize:%d in:%d out:%d}\n",
ld->name, dev->name, qsize, in, out);
return -EIO;
}
usage = circ_get_usage(qsize, in, out);
if (unlikely(usage > SHM_UL_USAGE_LIMIT) && cp_online(mc)) {
#ifdef DEBUG_MODEM_IF
mif_debug("%s: CAUTION! BUSY in %s_TXQ{qsize:%d in:%d out:%d "\
"usage:%d (count:%d)}\n", ld->name, dev->name, qsize,
in, out, usage, count);
#endif
return -EBUSY;
}
space = circ_get_space(qsize, in, out);
if (unlikely(space < count)) {
#ifdef DEBUG_MODEM_IF
if (cp_online(mc)) {
mif_err("%s: CAUTION! NOSPC in %s_TXQ{qsize:%d in:%d "\
"out:%d space:%d count:%d}\n", ld->name,
dev->name, qsize, in, out, space, count);
}
#endif
return -ENOSPC;
}
return space;
}
/**
@brief extract all IPC link frames from a circular queue
In a while loop,\n
1) Receives each IPC link frame stored in the @b @@dev RXQ.\n
2) If the frame is a PS (network) data frame, stores it to an skb_rxq and
schedules a delayed work for PS data reception.\n
3) Otherwise, passes it to the DEMUX layer immediately.\n
@param mld the pointer to a mem_link_device instance
@param dev the pointer to a mem_ipc_device instance (IPC_FMT, etc.)
@retval "> 0" if valid data received
@retval "= 0" if no data received
@retval "< 0" if ANY error
*/
static int rx_frames_from_dev(struct mem_link_device *mld,
struct mem_ipc_device *dev)
{
struct sk_buff_head *skb_rxq = dev->skb_rxq;
unsigned int qsize = get_rxq_buff_size(dev);
unsigned int in = get_rxq_head(dev);
unsigned int out = get_rxq_tail(dev);
unsigned int size = circ_get_usage(qsize, in, out);
int rcvd = 0;
if (unlikely(circ_empty(in, out)))
return 0;
while (rcvd < size) {
struct sk_buff *skb;
skb = rxq_read(mld, dev, in);
if (!skb)
break;
/* The $rcvd must be accumulated here, because $skb can be freed
in pass_skb_to_demux(). */
rcvd += skb->len;
if (likely(sipc_ps_ch(sipc5_get_ch_id(skb->data))))
skb_queue_tail(skb_rxq, skb);
else
pass_skb_to_demux(mld, skb);
}
#ifdef DEBUG_MODEM_IF
if (rcvd < size) {
struct link_device *ld = &mld->link_dev;
mif_err("%s: WARN! rcvd %d < size %d\n", ld->name, rcvd, size);
}
#endif
return rcvd;
}
/**
@brief copy each IPC link frame from a circular queue to an skb
1) Analyzes a link frame header and get the size of the current link frame.\n
2) Allocates a socket buffer (skb).\n
3) Extracts a link frame from the current @b $out (tail) pointer in the @b
@@dev RXQ up to @b @@in (head) pointer in the @b @@dev RXQ, then copies it
to the skb allocated in the step 2.\n
4) Updates the TAIL (OUT) pointer in the @b @@dev RXQ.\n
@param mld the pointer to a mem_link_device instance
@param dev the pointer to a mem_ipc_device instance (IPC_FMT, etc.)
@param in the IN (HEAD) pointer value of the @b @@dev RXQ
@retval "struct sk_buff *" if there is NO error
@retval "NULL" if there is ANY error
*/
static struct sk_buff *rxq_read(struct mem_link_device *mld,
struct mem_ipc_device *dev,
unsigned int in)
{
struct link_device *ld = &mld->link_dev;
struct sk_buff *skb;
gfp_t priority;
char *src = get_rxq_buff(dev);
unsigned int qsize = get_rxq_buff_size(dev);
unsigned int out = get_rxq_tail(dev);
unsigned int rest = circ_get_usage(qsize, in, out);
unsigned int len;
char hdr[SIPC5_MIN_HEADER_SIZE];
/* Copy the header in a frame to the header buffer */
circ_read(hdr, src, qsize, out, SIPC5_MIN_HEADER_SIZE);
/* Check the config field in the header */
if (unlikely(!sipc5_start_valid(hdr))) {
mif_err("%s: ERR! %s BAD CFG 0x%02X (in:%d out:%d rest:%d)\n",
ld->name, dev->name, hdr[SIPC5_CONFIG_OFFSET],
in, out, rest);
goto bad_msg;
}
/* Check the channel ID field in the header */
if (unlikely(!sipc5_get_ch_id(hdr))) {
mif_err("%s: ERR! %s BAD CH.ID 0x%02X (in:%d out:%d rest:%d)\n",
ld->name, dev->name, hdr[SIPC5_CH_ID_OFFSET],
in, out, rest);
goto bad_msg;
}
/* Verify the length of the frame (data + padding) */
len = sipc5_get_total_len(hdr);
if (unlikely(len > rest)) {
mif_err("%s: ERR! %s BAD LEN %d > rest %d\n",
ld->name, dev->name, len, rest);
goto bad_msg;
}
/* Allocate an skb */
priority = in_interrupt() ? GFP_ATOMIC : GFP_KERNEL;
skb = alloc_skb(len + NET_SKB_PAD, priority);
if (!skb) {
mif_err("%s: ERR! %s alloc_skb(%d,0x%x) fail\n",
ld->name, dev->name, (len + NET_SKB_PAD), priority);
goto no_mem;
}
skb_reserve(skb, NET_SKB_PAD);
/* Read the frame from the RXQ */
circ_read(skb_put(skb, len), src, qsize, out, len);
/* Update tail (out) pointer to the frame to be read in the future */
set_rxq_tail(dev, circ_new_ptr(qsize, out, len));
/* Finish reading data before incrementing tail */
smp_mb();
#ifdef DEBUG_MODEM_IF_LINK_RX
/* Record the time-stamp */
getnstimeofday(&skbpriv(skb)->ts);
#endif
return skb;
bad_msg:
#ifdef DEBUG_MODEM_IF
pr_ipc(1, "CP2AP: BAD MSG", (src + out), 4);
#endif
set_rxq_tail(dev, in); /* Reset tail (out) pointer */
mem_forced_cp_crash(mld);
no_mem:
return NULL;
}
static struct sk_buff *rxq_read(struct mem_link_device *mld,
struct mem_ipc_device *dev,
unsigned int in)
{
struct link_device *ld = &mld->link_dev;
struct sk_buff *skb;
char *src = get_rxq_buff(dev);
unsigned int qsize = get_rxq_buff_size(dev);
unsigned int out = get_rxq_tail(dev);
unsigned int rest = circ_get_usage(qsize, in, out);
unsigned int len;
char hdr[SIPC5_MIN_HEADER_SIZE];
/* Copy the header in a frame to the header buffer */
circ_read(hdr, src, qsize, out, SIPC5_MIN_HEADER_SIZE);
/* Check the config field in the header */
if (unlikely(!sipc5_start_valid(hdr))) {
mif_err("%s: ERR! %s BAD CFG 0x%02X (in:%d out:%d rest:%d)\n",
ld->name, dev->name, hdr[SIPC5_CONFIG_OFFSET],
in, out, rest);
goto bad_msg;
}
/* Verify the length of the frame (data + padding) */
len = sipc5_get_total_len(hdr);
if (unlikely(len > rest)) {
mif_err("%s: ERR! %s BAD LEN %d > rest %d\n",
ld->name, dev->name, len, rest);
goto bad_msg;
}
/* Allocate an skb */
skb = mem_alloc_skb(len);
if (!skb) {
mif_err("%s: ERR! %s mem_alloc_skb(%d) fail\n",
ld->name, dev->name, len);
goto no_mem;
}
/* Read the frame from the RXQ */
circ_read(skb_put(skb, len), src, qsize, out, len);
/* Update tail (out) pointer to the frame to be read in the future */
set_rxq_tail(dev, circ_new_ptr(qsize, out, len));
/* Finish reading data before incrementing tail */
smp_mb();
#ifdef DEBUG_MODEM_IF
/* Record the time-stamp */
getnstimeofday(&skbpriv(skb)->ts);
#endif
return skb;
bad_msg:
evt_log(0, "%s: %s%s%s: ERR! BAD MSG: %02x %02x %02x %02x\n",
FUNC, ld->name, arrow(RX), ld->mc->name,
hdr[0], hdr[1], hdr[2], hdr[3]);
set_rxq_tail(dev, in); /* Reset tail (out) pointer */
mem_forced_cp_crash(mld);
no_mem:
return NULL;
}
