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 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;
}
