/**
@brief copy UDL data in an skb to a circular TXQ
Enqueues a frame in @b @@skb to the @b @@dev TXQ if there is enough space in the
TXQ, then releases @b @@skb.
@param mld the pointer to a mem_link_device instance
@param dev the pointer to a mem_ipc_device instance
@param skb the pointer to an sk_buff instance
@retval "> 0" the size of the frame written in the TXQ
@retval "< 0" an error code (-EBUSY, -ENOSPC, or -EIO)
*/
static inline int udl_write(struct mem_link_device *mld,
struct mem_ipc_device *dev, struct sk_buff *skb)
{
unsigned int count = skb->len;
char *src = skb->data;
char *dst = get_txq_buff(dev);
unsigned int qsize = get_txq_buff_size(dev);
unsigned int in = get_txq_head(dev);
unsigned int out = get_txq_tail(dev);
int space;
space = check_udl_space(mld, dev, qsize, in, out, count);
if (unlikely(space < 0))
return space;
barrier();
circ_write(dst, src, qsize, in, count);
barrier();
set_txq_head(dev, circ_new_ptr(qsize, in, count));
/* Commit the item before incrementing the head */
smp_mb();
return count;
}
struct sk_buff *sbd_pio_rx(struct sbd_ring_buffer *rb)
{
struct sk_buff *skb;
unsigned int qlen = rb->len;
unsigned int out = *rb->rp;
skb = recv_data(rb, out);
if (unlikely(!skb))
return NULL;
*rb->rp = circ_new_ptr(qlen, out, 1);
set_lnk_hdr(rb, skb);
set_skb_priv(rb, skb);
check_more(rb, skb);
return skb;
}
int sbd_pio_tx(struct sbd_ring_buffer *rb, struct sk_buff *skb)
{
int ret;
unsigned int qlen = rb->len;
unsigned int in = *rb->wp;
unsigned int out = *rb->rp;
unsigned int count = skb->len;
unsigned int space = (rb->buff_size - rb->payload_offset);
u8 *dst;
pktlog_tx_bottom_skb(rb->sl, skb);
ret = check_rb_space(rb, qlen, in, out);
if (unlikely(ret < 0))
return ret;
if (unlikely(count > space)) {
mif_err("ERR! {id:%d ch:%d} count %d > space %d\n",
rb->id, rb->ch, count, space);
return -ENOSPC;
}
barrier();
dst = rb->buff[in] + rb->payload_offset;
barrier();
skb_copy_from_linear_data(skb, dst, count);
rb->size_v[in] = skb->len;
barrier();
*rb->wp = circ_new_ptr(qlen, in, 1);
/* Commit the item before incrementing the head */
smp_mb();
return count;
}
/**
@brief copy data in an skb to a circular TXQ
Enqueues a frame in @b @@skb to the @b @@dev TXQ if there is enough space in the
TXQ, then releases @b @@skb.
@param mld the pointer to a mem_link_device instance
@param dev the pointer to a mem_ipc_device instance
@param skb the pointer to an sk_buff instance
@retval "> 0" the size of the frame written in the TXQ
@retval "< 0" an error code (-EBUSY, -ENOSPC, or -EIO)
*/
static int txq_write(struct mem_link_device *mld, struct mem_ipc_device *dev,
struct sk_buff *skb)
{
char *src = skb->data;
unsigned int count = skb->len;
char *dst = get_txq_buff(dev);
unsigned int qsize = get_txq_buff_size(dev);
unsigned int in = get_txq_head(dev);
unsigned int out = get_txq_tail(dev);
int space;
space = check_txq_space(mld, dev, qsize, in, out, count);
if (unlikely(space < 0))
return space;
#ifdef DEBUG_MODEM_IF_LINK_TX
/* Record the time-stamp */
getnstimeofday(&skbpriv(skb)->ts);
#endif
circ_write(dst, src, qsize, in, count);
set_txq_head(dev, circ_new_ptr(qsize, in, count));
/* Commit the item before incrementing the head */
smp_mb();
#ifdef DEBUG_MODEM_IF_LINK_TX
if (likely(src))
log_ipc_pkt(sipc5_get_ch_id(src), LINK, TX, skb, true, true);
#endif
dev_kfree_skb_any(skb);
return count;
}
/**
@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;
}
