static void pass_skb_to_net(struct mem_link_device *mld, struct sk_buff *skb)
{
struct link_device *ld = &mld->link_dev;
struct skbuff_private *priv;
struct io_device *iod;
int ret;
priv = skbpriv(skb);
if (unlikely(!priv)) {
mif_err("%s: ERR! No PRIV in skb@%p\n", ld->name, skb);
dev_kfree_skb_any(skb);
mem_forced_cp_crash(mld);
return;
}
iod = priv->iod;
if (unlikely(!iod)) {
mif_err("%s: ERR! No IOD in skb@%p\n", ld->name, skb);
dev_kfree_skb_any(skb);
mem_forced_cp_crash(mld);
return;
}
#if defined(DEBUG_MODEM_IF_LINK_RX) && defined(DEBUG_MODEM_IF_PS_DATA)
log_ipc_pkt(iod->id, LINK, RX, skb, priv->lnk_hdr ? skb->data : NULL);
#endif
ret = iod->recv_net_skb(iod, ld, skb);
if (unlikely(ret < 0)) {
struct modem_ctl *mc = ld->mc;
mif_err_limited("%s: %s<-%s: ERR! %s->recv_net_skb fail (%d)\n",
ld->name, iod->name, mc->name, iod->name, ret);
dev_kfree_skb_any(skb);
}
}
/**
@brief pass a socket buffer to the DEMUX layer
Invokes the recv_skb_single method in the io_device instance to perform
receiving IPC messages from each skb.
@param mld the pointer to a mem_link_device instance
@param skb the pointer to an sk_buff instance
@retval "> 0" if succeeded to pass an @b @@skb to the DEMUX layer
@retval "< 0" an error code
*/
static void pass_skb_to_demux(struct mem_link_device *mld, struct sk_buff *skb)
{
struct link_device *ld = &mld->link_dev;
struct io_device *iod;
int ch;
int ret;
ch = sipc5_get_ch_id(skb->data);
iod = link_get_iod_with_channel(ld, ch);
if (unlikely(!iod)) {
mif_err("%s: ERR! No IO device for Ch.%d\n", ld->name, ch);
dev_kfree_skb_any(skb);
mem_forced_cp_crash(mld);
return;
}
/* Record the RX IO device into the "iod" field in &skb->cb */
skbpriv(skb)->iod = iod;
/* Record the RX link device into the "ld" field in &skb->cb */
skbpriv(skb)->ld = ld;
#ifdef DEBUG_MODEM_IF_LINK_RX
log_ipc_pkt(sipc5_get_ch_id(skb->data), LINK, RX, skb, true, true);
#endif
ret = iod->recv_skb_single(iod, ld, skb);
if (unlikely(ret < 0)) {
mif_err("%s: ERR! %s->recv_skb_single fail (%d)\n",
ld->name, iod->name, ret);
dev_kfree_skb_any(skb);
}
}
/**
@brief forbid CP from going to sleep
Wakes up a CP if it can sleep and increases the "ref_cnt" counter in the
mem_link_device instance.
@param mld the pointer to a mem_link_device instance
@remark CAUTION!!! permit_cp_sleep() MUST be invoked after
forbid_cp_sleep() success to decrease the "ref_cnt" counter.
*/
static void forbid_cp_sleep(struct mem_link_device *mld)
{
struct link_device *ld = &mld->link_dev;
int ap_status = gpio_get_value(mld->gpio_ap_status);
int cp_wakeup = gpio_get_value(mld->gpio_cp_wakeup);
unsigned long flags;
spin_lock_irqsave(&mld->pm_lock, flags);
atomic_inc(&mld->ref_cnt);
gpio_set_value(mld->gpio_ap_status, 1);
gpio_set_value(mld->gpio_cp_wakeup, 1);
if (work_pending(&mld->cp_sleep_dwork.work))
cancel_delayed_work(&mld->cp_sleep_dwork);
spin_unlock_irqrestore(&mld->pm_lock, flags);
if (!ap_status || !cp_wakeup)
print_pm_status(mld);
if (check_link_status(mld) < 0) {
print_pm_status(mld);
mif_err("%s: ERR! check_link_status fail\n", ld->name);
mem_forced_cp_crash(mld);
}
}
static void pass_skb_to_demux(struct mem_link_device *mld, struct sk_buff *skb)
{
struct link_device *ld = &mld->link_dev;
struct io_device *iod = skbpriv(skb)->iod;
int ret;
u8 ch = skbpriv(skb)->sipc_ch;
#ifdef DEBUG_MODEM_IF_LINK_RX
u8 *hdr;
#endif
if (unlikely(!iod)) {
mif_err("%s: ERR! No IOD for CH.%d\n", ld->name, ch);
dev_kfree_skb_any(skb);
mem_forced_cp_crash(mld);
return;
}
#ifdef DEBUG_MODEM_IF_LINK_RX
hdr = skbpriv(skb)->lnk_hdr ? skb->data : NULL;
log_ipc_pkt(ch, LINK, RX, skb, hdr);
#endif
ret = iod->recv_skb_single(iod, ld, skb);
if (unlikely(ret < 0)) {
struct modem_ctl *mc = ld->mc;
mif_err_limited("%s: %s<-%s: ERR! %s->recv_skb fail (%d)\n",
ld->name, iod->name, mc->name, iod->name, ret);
dev_kfree_skb_any(skb);
}
}
/**
@brief function for the @b force_dump method in a link_device instance
@param ld the pointer to a link_device instance
@param iod the pointer to an io_device instance
*/
static int mem_force_dump(struct link_device *ld, struct io_device *iod)
{
struct mem_link_device *mld = to_mem_link_device(ld);
mif_err("+++\n");
mem_forced_cp_crash(mld);
mif_err("---\n");
return 0;
}
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;
}
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;
}
static void pm_fail_cb(struct modem_link_pm *pm)
{
struct link_device *ld = pm_to_link_device(pm);
struct mem_link_device *mld = ld_to_mem_link_device(ld);
mem_forced_cp_crash(mld);
}
/**
@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 enum hrtimer_restart tx_timer_func(struct hrtimer *timer)
{
struct mem_link_device *mld;
struct link_device *ld;
struct modem_ctl *mc;
int i;
bool need_schedule;
u16 mask;
mld = container_of(timer, struct mem_link_device, tx_timer);
ld = &mld->link_dev;
mc = ld->mc;
if (unlikely(cp_online(mc) && !ipc_active(mld)))
goto exit;
need_schedule = false;
mask = 0;
for (i = IPC_FMT; i < MAX_SIPC5_DEV; i++) {
struct mem_ipc_device *dev = mld->dev[i];
int ret;
if (unlikely(under_tx_flow_ctrl(mld, dev))) {
ret = check_tx_flow_ctrl(mld, dev);
if (ret < 0) {
if (ret == -EBUSY || ret == -ETIME) {
need_schedule = true;
continue;
} else {
mem_forced_cp_crash(mld);
goto exit;
}
}
}
ret = tx_frames_to_dev(mld, dev);
if (unlikely(ret < 0)) {
if (ret == -EBUSY || ret == -ENOSPC) {
need_schedule = true;
start_tx_flow_ctrl(mld, dev);
continue;
} else {
mem_forced_cp_crash(mld);
goto exit;
}
}
mask |= msg_mask(dev);
if (!skb_queue_empty(dev->skb_txq))
need_schedule = true;
}
if (mask)
send_ipc_irq(mld, mask2int(mask));
if (need_schedule) {
ktime_t ktime = ns_to_ktime(ms2ns(TX_PERIOD_MS));
hrtimer_forward_now(timer, ktime);
return HRTIMER_RESTART;
}
exit:
#ifdef CONFIG_LINK_POWER_MANAGEMENT
mld->permit_cp_sleep(mld);
#endif
return HRTIMER_NORESTART;
}
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;
}
static enum hrtimer_restart sbd_tx_timer_func(struct hrtimer *timer)
{
struct mem_link_device *mld;
struct link_device *ld;
struct modem_ctl *mc;
struct sbd_link_device *sl;
int i;
bool need_schedule;
u16 mask;
unsigned long flags = 0;
mld = container_of(timer, struct mem_link_device, sbd_tx_timer);
ld = &mld->link_dev;
mc = ld->mc;
sl = &mld->sbd_link_dev;
need_schedule = false;
mask = 0;
spin_lock_irqsave(&mc->lock, flags);
if (unlikely(!ipc_active(mld))) {
spin_unlock_irqrestore(&mc->lock, flags);
goto exit;
}
spin_unlock_irqrestore(&mc->lock, flags);
#ifdef CONFIG_LINK_POWER_MANAGEMENT_WITH_FSM
if (mld->link_active) {
if (!mld->link_active(mld)) {
need_schedule = true;
goto exit;
}
}
#endif
for (i = 0; i < sl->num_channels; i++) {
struct sbd_ring_buffer *rb = sbd_id2rb(sl, i, TX);
int ret;
ret = tx_frames_to_rb(rb);
if (unlikely(ret < 0)) {
if (ret == -EBUSY || ret == -ENOSPC) {
need_schedule = true;
mask = MASK_SEND_DATA;
continue;
} else {
mem_forced_cp_crash(mld);
need_schedule = false;
goto exit;
}
}
if (ret > 0)
mask = MASK_SEND_DATA;
if (!skb_queue_empty(&rb->skb_q))
need_schedule = true;
}
if (!need_schedule) {
for (i = 0; i < sl->num_channels; i++) {
struct sbd_ring_buffer *rb;
rb = sbd_id2rb(sl, i, TX);
if (!rb_empty(rb)) {
need_schedule = true;
break;
}
}
}
if (mask) {
spin_lock_irqsave(&mc->lock, flags);
if (unlikely(!ipc_active(mld))) {
spin_unlock_irqrestore(&mc->lock, flags);
need_schedule = false;
goto exit;
}
send_ipc_irq(mld, mask2int(mask));
spin_unlock_irqrestore(&mc->lock, flags);
}
exit:
if (need_schedule) {
ktime_t ktime = ktime_set(0, ms2ns(TX_PERIOD_MS));
hrtimer_start(timer, ktime, HRTIMER_MODE_REL);
}
return HRTIMER_NORESTART;
}
static enum hrtimer_restart tx_timer_func(struct hrtimer *timer)
{
struct mem_link_device *mld;
struct link_device *ld;
struct modem_ctl *mc;
int i;
bool need_schedule;
u16 mask;
unsigned long flags;
mld = container_of(timer, struct mem_link_device, tx_timer);
ld = &mld->link_dev;
mc = ld->mc;
need_schedule = false;
mask = 0;
spin_lock_irqsave(&mc->lock, flags);
if (unlikely(!ipc_active(mld)))
goto exit;
#ifdef CONFIG_LINK_POWER_MANAGEMENT_WITH_FSM
if (mld->link_active) {
if (!mld->link_active(mld)) {
need_schedule = true;
goto exit;
}
}
#endif
for (i = 0; i < MAX_SIPC5_DEVICES; i++) {
struct mem_ipc_device *dev = mld->dev[i];
int ret;
if (unlikely(under_tx_flow_ctrl(mld, dev))) {
ret = check_tx_flow_ctrl(mld, dev);
if (ret < 0) {
if (ret == -EBUSY || ret == -ETIME) {
need_schedule = true;
continue;
} else {
mem_forced_cp_crash(mld);
need_schedule = false;
goto exit;
}
}
}
ret = tx_frames_to_dev(mld, dev);
if (unlikely(ret < 0)) {
if (ret == -EBUSY || ret == -ENOSPC) {
need_schedule = true;
start_tx_flow_ctrl(mld, dev);
continue;
} else {
mem_forced_cp_crash(mld);
need_schedule = false;
goto exit;
}
}
if (ret > 0)
mask |= msg_mask(dev);
if (!skb_queue_empty(dev->skb_txq))
need_schedule = true;
}
if (!need_schedule) {
for (i = 0; i < MAX_SIPC5_DEVICES; i++) {
if (!txq_empty(mld->dev[i])) {
need_schedule = true;
break;
}
}
}
if (mask)
send_ipc_irq(mld, mask2int(mask));
exit:
if (need_schedule) {
ktime_t ktime = ktime_set(0, ms2ns(TX_PERIOD_MS));
hrtimer_start(timer, ktime, HRTIMER_MODE_REL);
}
spin_unlock_irqrestore(&mc->lock, flags);
return HRTIMER_NORESTART;
}
