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);
}
}
struct sk_buff *cdc_ncm_tx_fixup(struct if_usb_devdata *pipe_data,
struct sk_buff *skb, gfp_t flags)
{
struct sk_buff *skb_out = NULL;
struct cdc_ncm_ctx *ctx = (struct cdc_ncm_ctx *)pipe_data->sedata;
if (ctx == NULL)
goto error;
spin_lock_bh(&ctx->mtx);
cdc_ncm_post_tx_framing(skb);
skb_out = cdc_ncm_fill_tx_frame(pipe_data, skb);
if (skb_out) {
skbpriv(skb_out)->iod = pipe_data->iod;
skbpriv(skb_out)->ld = &pipe_data->usb_ld->ld;
skbpriv(skb_out)->context = pipe_data;
}
spin_unlock_bh(&ctx->mtx);
return skb_out;
error:
if (skb != NULL)
dev_kfree_skb_any(skb);
return NULL;
}
static inline void set_skb_priv(struct sbd_ring_buffer *rb, struct sk_buff *skb)
{
/* Record the IO device, the link device, etc. into &skb->cb */
skbpriv(skb)->iod = rb->iod;
skbpriv(skb)->ld = rb->ld;
skbpriv(skb)->sipc_ch = rb->ch;
}
/**
@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);
}
}
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 inline struct sk_buff *recv_data(struct sbd_ring_buffer *rb, u16 out)
{
struct sk_buff *skb;
u8 *src;
unsigned int len = rb->size_v[out];
unsigned int space = (rb->buff_size - rb->payload_offset);
if (unlikely(len > space)) {
mif_err("ERR! {id:%d ch:%d} size %d > space %d\n",
rb->id, rb->ch, len, space);
return NULL;
}
skb = dev_alloc_skb(len);
if (unlikely(!skb)) {
mif_err("ERR! {id:%d ch:%d} alloc_skb(%d) fail\n",
rb->id, rb->ch, len);
return NULL;
}
src = rb->buff[out] + rb->payload_offset;
skb_put(skb, len);
skb_copy_to_linear_data(skb, src, len);
#ifdef DEBUG_MODEM_IF
/* Record the time-stamp */
getnstimeofday(&skbpriv(skb)->ts);
#endif
return skb;
}
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);
modemctl_notify_event(MDM_CRASH_INVALID_SKBCB);
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);
modemctl_notify_event(MDM_CRASH_INVALID_SKBIOD);
return;
}
log_ipc_pkt(LNK_RX, iod->id, skb);
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);
}
}
static void hsic_tx_complete(struct urb *urb)
{
struct sk_buff *skb = urb->context;
struct io_device *iod = skbpriv(skb)->iod;
struct link_device *linkdev = get_current_link(iod);
struct usb_link_device *usb_ld = to_usb_link_device(linkdev);
switch (urb->status) {
case 0:
if (urb->actual_length != urb->transfer_buffer_length)
MIF_ERR("TX len=%d, Complete len=%d\n",
urb->transfer_buffer_length, urb->actual_length);
break;
case -ECONNRESET:
if (urb->actual_length)
MIF_ERR("ECONNRESET: TX len=%d, Complete len=%d\n",
urb->transfer_buffer_length, urb->actual_length);
case -ENOENT:
case -ESHUTDOWN:
default:
MIF_ERR("iod %d TX error (%d)\n", iod->id, urb->status);
}
if (iod->atdebug && iod->atdebugfunc)
iod->atdebugfunc(iod, skb->data, skb->len);
dev_kfree_skb_any(skb);
usb_free_urb(urb);
if (urb->dev && usb_ld->if_usb_connected)
usb_mark_last_busy(urb->dev);
}
/**
@brief check whether or not TX is possible via the link
@param mld the pointer to a mem_link_device instance
@param dev the pointer to a mem_ipc_device instance (IPC_FMT, etc.)
@param skb the pointer to an skb that will be transmitted
@retval "> 0" the size of the data in @b @@skb if there is NO ERROR
@retval "< 0" an error code (-EIO or -EBUSY)
*/
static inline int check_tx_link(struct mem_link_device *mld,
struct mem_ipc_device *dev,
struct sk_buff *skb)
{
struct link_device *ld = &mld->link_dev;
struct modem_ctl *mc = ld->mc;
struct sk_buff_head *skb_txq = dev->skb_txq;
int ret = skb->len;
if (unlikely(cp_online(mc) && !ipc_active(mld)))
return -EIO;
if (unlikely(skb_txq->qlen >= MAX_SKB_TXQ_DEPTH)) {
#ifdef DEBUG_MODEM_IF
struct io_device *iod = skbpriv(skb)->iod;
mif_debug("%s: %s->%s: ERR! %s "\
"SKB_TXQ qlen %d >= limit %d\n",
ld->name, iod->name, mc->name,
dev->name, skb_txq->qlen, MAX_SKB_TXQ_DEPTH);
#endif
return -EBUSY;
}
return ret;
}
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);
}
}
/**
* SIPC header_create() family functions
*
* Create SIPC5 header
*
* sipc5_hdr_create_skb()
* Create common SIPC5 header
*
* sipc5_hdr_create_legacy_rfs()
* Create SIPC5 header with IPC4.1 RFS header
* Because IMC modem used the 256KB rfs packet and RILD check the full
* packet with RFS header len, kernel remove the SIPC5 header and add the
* lagacy RFS header.
*
* sipc5_hdr_create_multifmt()
* TBD
*
* sipc5_hdr_create_skb_handover()
* Remove the ethernet frame When use `handover' with Network Bridge,
* user -> bridge device(rmnet0) -> real rmnet(xxxx_rmnet0) -> here.
* bridge device is ethernet device unlike xxxx_rmnet(net device).
* We remove the an ethernet header of skb before using skb->len,
* because bridge device added an ethernet header to skb.
*
* RETURN
* Returns the socket buffer that added SIPC5 header.
*
**/
static struct sk_buff *sipc5_hdr_create(struct io_device *iod,
struct sipc_hdr *hdr, struct sk_buff *skb)
{
struct sipc5_link_hdr *sipc5h;
struct link_device *ld = get_current_link(iod);
if (skb->len > 0xFFFF - SIPC5_HDR_LEN_MAX) {
sipc5h = (struct sipc5_link_hdr *)
skb_push(skb, SIPC5_HDR_LEN_EXT);
sipc5h->cfg = SIPC5_HDR_CFG_START | SIPC5_HDR_EXT;
*((u32 *)&sipc5h->len) = (u32)(skb->len);
} else {
sipc5h = (struct sipc5_link_hdr *)
skb_push(skb, SIPC5_HDR_LEN);
sipc5h->cfg = SIPC5_HDR_CFG_START;
sipc5h->len = (u16)(skb->len);
}
sipc5h->ch = iod->id;
/* Should check the alignment for dynamic switch link dev*/
if (ld->aligned) {
sipc5h->cfg |= SIPC5_HDR_PAD;
skb_set_tail_pointer(skb, SIPC_ALIGN(skb->len));
skb->len = SIPC_ALIGN(skb->len);
}
skbpriv(skb)->sipch = (void *)sipc5h;
return skb;
}
/* TODO: not verified */
static struct sk_buff *sipc5_hdr_create_multifmt(struct io_device *iod,
struct sipc_hdr *hdr, struct sk_buff *skb)
{
struct sipc5_link_hdr *sipc5h;
struct link_device *ld = get_current_link(iod);
if (hdr->multifmt) {
sipc5h = (struct sipc5_link_hdr *)
skb_push(skb, SIPC5_HDR_LEN_CTRL);
sipc5h->cfg = SIPC5_HDR_CFG_START | SIPC5_HDR_CONTROL;
sipc5h->len = skb->len;
sipc5h->ext.ctl = hdr->multifmt;
sipc5h->ch = iod->id;
/* Should check the alignment for dynamic switch link dev*/
if (ld->aligned) {
sipc5h->cfg |= SIPC5_HDR_PAD;
skb_set_tail_pointer(skb, SIPC_ALIGN(skb->len));
skb->len = SIPC_ALIGN(skb->len);
}
skbpriv(skb)->sipch = (void *)sipc5h;
return skb;
}
return sipc5_hdr_create(iod, hdr, skb);
}
static int start_ipc(struct link_device *ld, struct io_device *iod)
{
struct sk_buff *skb;
char data[1] = {'a'};
int err;
struct usb_link_device *usb_ld = to_usb_link_device(ld);
struct if_usb_devdata *pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
mif_err("\n");
if (usb_ld->link_pm_data->hub_handshake_done) {
mif_err("Aleady send start ipc, skip start ipc\n");
err = 0;
goto exit;
}
if (!usb_ld->if_usb_connected) {
mif_err("HSIC/USB not connected, skip start ipc\n");
err = -ENODEV;
goto exit;
}
if (usb_ld->if_usb_initstates == INIT_IPC_START_DONE) {
mif_debug("aleady IPC started\n");
err = 0;
goto exit;
}
mif_info("send 'a'\n");
skb = alloc_skb(16, GFP_ATOMIC);
if (unlikely(!skb))
return -ENOMEM;
memcpy(skb_put(skb, 1), data, 1);
skbpriv(skb)->iod = iod;
skbpriv(skb)->ld = &usb_ld->ld;
err = usb_tx_urb_with_skb(usb_ld, skb, pipe_data);
if (err < 0) {
mif_err("usb_tx_urb fail\n");
goto exit;
}
usb_ld->link_pm_data->hub_handshake_done = true;
usb_ld->if_usb_initstates = INIT_IPC_START_DONE;
exit:
return err;
}
static int start_ipc(struct link_device *ld, struct io_device *iod)
{
struct sk_buff *skb;
char data[1] = {'a'};
int err;
struct usb_link_device *usb_ld = to_usb_link_device(ld);
struct if_usb_devdata *pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
if (!usb_ld->if_usb_connected) {
mif_err("HSIC not connected, skip start ipc\n");
err = -ENODEV;
goto exit;
}
if (ld->mc->phone_state != STATE_ONLINE) {
mif_err("[MODEM_IF] MODEM is not online, skip start ipc\n");
err = -ENODEV;
goto exit;
}
mif_err("send 'a'\n");
skb = alloc_skb(16, GFP_ATOMIC);
if (unlikely(!skb))
return -ENOMEM;
memcpy(skb_put(skb, 1), data, 1);
skbpriv(skb)->iod = iod;
skbpriv(skb)->ld = ld;
if (!usb_ld->if_usb_connected || !usb_ld->usbdev)
return -ENODEV;
usb_mark_last_busy(usb_ld->usbdev);
err = usb_tx_urb_with_skb(usb_ld->usbdev, skb, pipe_data);
if (err < 0) {
mif_err("usb_tx_urb fail\n");
dev_kfree_skb_any(skb);
goto exit;
}
exit:
return err;
}
static int _usb_tx_work(struct sk_buff *skb)
{
struct sk_buff_head *txq;
struct io_device *iod = skbpriv(skb)->iod;
struct link_device *ld = skbpriv(skb)->ld;
struct usb_link_device *usb_ld = to_usb_link_device(ld);
struct if_usb_devdata *pipe_data;
switch (iod->format) {
case IPC_BOOT:
case IPC_FMT:
/* boot device uses same intf with fmt*/
pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
txq = &ld->sk_fmt_tx_q;
break;
case IPC_RAW:
pipe_data = &usb_ld->devdata[IF_USB_RAW_EP];
txq = &ld->sk_raw_tx_q;
break;
case IPC_RFS:
pipe_data = &usb_ld->devdata[IF_USB_RFS_EP];
txq = &ld->sk_fmt_tx_q;
break;
default:
/* wrong packet, drop it */
pipe_data = NULL;
txq = NULL;
break;
}
if (!pipe_data)
return -ENOENT;
#if 0
if (iod->format == IPC_FMT && usb_ld->if_usb_is_main)
pr_skb("IPC-TX", skb);
#endif
if (iod->format == IPC_RAW)
mif_debug("TX[RAW]\n");
return usb_tx_urb_with_skb(usb_ld->usbdev, skb, pipe_data);
}
void xmm6262_start_loopback(struct io_device *iod, struct modem_shared *msd)
{
struct link_device *ld = get_current_link(iod);
struct sk_buff *skb = alloc_skb(16, GFP_ATOMIC);
int ret;
if (unlikely(!skb))
return;
memcpy(skb_put(skb, 1), (msd->loopback_ipaddr) ? "s" : "x", 1);
skbpriv(skb)->iod = iod;
skbpriv(skb)->ld = ld;
ret = ld->send(ld, iod, skb);
if (ret < 0) {
mif_err("usb_tx_urb fail\n");
dev_kfree_skb_any(skb);
}
mif_info("Send loopback key '%s'\n",
(msd->loopback_ipaddr) ? "s" : "x");
}
/**
@brief transmit an IPC message packet
@param mld the pointer to a mem_link_device instance
@param ch the channel ID
@param skb the pointer to an skb that will be transmitted
@retval "> 0" the size of the data in @b @@skb
@retval "< 0" an error code (-ENODEV, -EBUSY)
*/
static int xmit_ipc_to_rb(struct mem_link_device *mld, enum sipc_ch_id ch,
struct sk_buff *skb)
{
int ret;
struct link_device *ld = &mld->link_dev;
struct io_device *iod = skbpriv(skb)->iod;
struct modem_ctl *mc = ld->mc;
struct sbd_ring_buffer *rb = sbd_ch2rb(&mld->sbd_link_dev, ch, TX);
struct sk_buff_head *skb_txq;
unsigned long flags;
if (!rb) {
mif_err("%s: %s->%s: ERR! NO SBD RB {ch:%d}\n",
ld->name, iod->name, mc->name, ch);
return -ENODEV;
}
skb_txq = &rb->skb_q;
#ifdef CONFIG_LINK_POWER_MANAGEMENT
if (cp_online(mc) && mld->forbid_cp_sleep)
mld->forbid_cp_sleep(mld);
#endif
spin_lock_irqsave(&rb->lock, flags);
if (unlikely(skb_txq->qlen >= MAX_SKB_TXQ_DEPTH)) {
mif_err_limited("%s: %s->%s: ERR! {ch:%d} "
"skb_txq.len %d >= limit %d\n",
ld->name, iod->name, mc->name, ch,
skb_txq->qlen, MAX_SKB_TXQ_DEPTH);
ret = -EBUSY;
} else {
skb->len = min_t(int, skb->len, rb->buff_size);
ret = skb->len;
skb_queue_tail(skb_txq, skb);
start_tx_timer(mld, &mld->sbd_tx_timer);
#ifdef DEBUG_MODEM_IF
trace_mif_event(skb, skb->len, FUNC);
#endif
}
spin_unlock_irqrestore(&rb->lock, flags);
#ifdef CONFIG_LINK_POWER_MANAGEMENT
if (cp_online(mc) && mld->permit_cp_sleep)
mld->permit_cp_sleep(mld);
#endif
return ret;
}
/**
@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 tx_frames_to_dev(struct mem_link_device *mld,
struct mem_ipc_device *dev)
{
struct sk_buff_head *skb_txq = dev->skb_txq;
int tx_bytes = 0;
int ret = 0;
while (1) {
struct sk_buff *skb;
#ifdef DEBUG_MODEM_IF_LINK_TX
u8 *hdr;
u8 ch;
#endif
skb = skb_dequeue(skb_txq);
if (unlikely(!skb))
break;
ret = txq_write(mld, dev, skb);
if (unlikely(ret < 0)) {
/* Take the skb back to the skb_txq */
skb_queue_head(skb_txq, skb);
break;
}
tx_bytes += ret;
#ifdef DEBUG_MODEM_IF_LINK_TX
hdr = skbpriv(skb)->lnk_hdr ? skb->data : NULL;
ch = skbpriv(skb)->sipc_ch;
log_ipc_pkt(ch, LINK, TX, skb, hdr);
#endif
dev_kfree_skb_any(skb);
}
return (ret < 0) ? ret : tx_bytes;
}
static void usb_tx_complete(struct urb *urb)
{
struct sk_buff *skb = urb->context;
struct io_device *iod = skbpriv(skb)->iod;
struct link_device *ld = skbpriv(skb)->ld;
struct usb_link_device *usb_ld = to_usb_link_device(ld);
switch (urb->status) {
case 0:
break;
case -ENOENT:
case -ECONNRESET:
case -ESHUTDOWN:
default:
if (iod->format != IPC_BOOT)
mif_info("TX error (%d)\n", urb->status);
}
dev_kfree_skb_any(skb);
if (urb->dev && usb_ld->if_usb_connected)
usb_mark_last_busy(urb->dev);
usb_free_urb(urb);
}
/**
@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 = skbpriv(skb)->iod;
int ret;
u8 ch = skbpriv(skb)->sipc_ch;
if (unlikely(!iod)) {
mif_err("%s: ERR! No IOD for CH.%d\n", ld->name, ch);
dev_kfree_skb_any(skb);
modemctl_notify_event(MDM_CRASH_INVALID_IOD);
return;
}
log_ipc_pkt(LNK_RX, ch, skb);
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);
}
}
static int xmit_ipc_to_dev(struct mem_link_device *mld, enum sipc_ch_id ch,
struct sk_buff *skb)
{
int ret;
struct link_device *ld = &mld->link_dev;
struct io_device *iod = skbpriv(skb)->iod;
struct modem_ctl *mc = ld->mc;
struct mem_ipc_device *dev = mld->dev[dev_id(ch)];
struct sk_buff_head *skb_txq;
unsigned long flags;
if (!dev) {
mif_err("%s: %s->%s: ERR! NO IPC DEV {ch:%d}\n",
ld->name, iod->name, mc->name, ch);
return -ENODEV;
}
skb_txq = dev->skb_txq;
#ifdef CONFIG_LINK_POWER_MANAGEMENT
if (cp_online(mc) && mld->forbid_cp_sleep)
mld->forbid_cp_sleep(mld);
#endif
spin_lock_irqsave(dev->tx_lock, flags);
if (unlikely(skb_txq->qlen >= MAX_SKB_TXQ_DEPTH)) {
mif_err_limited("%s: %s->%s: ERR! %s TXQ.qlen %d >= limit %d\n",
ld->name, iod->name, mc->name, dev->name,
skb_txq->qlen, MAX_SKB_TXQ_DEPTH);
ret = -EBUSY;
} else {
ret = skb->len;
skb_queue_tail(dev->skb_txq, skb);
start_tx_timer(mld, &mld->tx_timer);
}
spin_unlock_irqrestore(dev->tx_lock, flags);
#ifdef CONFIG_LINK_POWER_MANAGEMENT
if (cp_online(mc) && mld->permit_cp_sleep)
mld->permit_cp_sleep(mld);
#endif
return ret;
}
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 int tx_frames_to_rb(struct sbd_ring_buffer *rb)
{
struct sk_buff_head *skb_txq = &rb->skb_q;
int tx_bytes = 0;
int ret = 0;
while (1) {
struct sk_buff *skb;
#ifdef DEBUG_MODEM_IF
u8 *hdr;
#endif
skb = skb_dequeue(skb_txq);
if (unlikely(!skb))
break;
ret = sbd_pio_tx(rb, skb);
if (unlikely(ret < 0)) {
/* Take the skb back to the skb_txq */
skb_queue_head(skb_txq, skb);
break;
}
tx_bytes += ret;
#ifdef DEBUG_MODEM_IF
hdr = skbpriv(skb)->lnk_hdr ? skb->data : NULL;
#ifdef DEBUG_MODEM_IF_IP_DATA
if (sipc_ps_ch(rb->ch)) {
u8 *ip_pkt = skb->data;
if (hdr)
ip_pkt += sipc5_get_hdr_len(hdr);
print_ipv4_packet(ip_pkt, TX);
}
#endif
#ifdef DEBUG_MODEM_IF_LINK_TX
log_ipc_pkt(rb->ch, LINK, TX, skb, hdr);
#endif
#endif
dev_kfree_skb_any(skb);
}
return (ret < 0) ? ret : tx_bytes;
}
static struct sk_buff *sipc5_hdr_create_ipcloopback(struct io_device *iod,
struct sipc_hdr *hdr, struct sk_buff *skb)
{
struct sipc5_link_hdr *sipc5h;
struct link_device *ld = get_current_link(iod);
sipc5h = (struct sipc5_link_hdr *)
skb_push(skb, SIPC5_HDR_LEN);
sipc5h->cfg = SIPC5_HDR_CFG_START;
sipc5h->len = (u16)(skb->len);
mif_info("send ipcloopback data: %d\n", skb->len);
sipc5h->ch = SIPC5_CH_ID_FMT_0;
/* Should check the alignment for dynamic switch link dev*/
if (ld->aligned) {
sipc5h->cfg |= SIPC5_HDR_PAD;
skb_set_tail_pointer(skb, SIPC_ALIGN(skb->len));
skb->len = SIPC_ALIGN(skb->len);
}
skbpriv(skb)->sipch = (void *)sipc5h;
return skb;
}
/**
@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;
}
static void usb_tx_work(struct work_struct *work)
{
int ret = 0;
struct link_device *ld =
container_of(work, struct link_device, tx_delayed_work.work);
struct usb_link_device *usb_ld = to_usb_link_device(ld);
struct io_device *iod;
struct sk_buff *skb;
struct if_usb_devdata *pipe_data;
struct link_pm_data *pm_data = usb_ld->link_pm_data;
/*TODO: check the PHONE ACTIVE STATES */
/* because tx data wait until hub on with wait_for_complettion, it
should queue to single_threaded work queue */
if (!link_pm_set_active(usb_ld))
return;
while (ld->sk_fmt_tx_q.qlen || ld->sk_raw_tx_q.qlen) {
/* send skb from fmt_txq and raw_txq,
* one by one for fair flow control */
skb = skb_dequeue(&ld->sk_fmt_tx_q);
if (skb) {
iod = skbpriv(skb)->iod;
switch (iod->format) {
case IPC_BOOT:
case IPC_RAMDUMP:
case IPC_FMT:
/* boot device uses same intf with fmt*/
pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
break;
case IPC_RFS:
pipe_data = &usb_ld->devdata[IF_USB_RFS_EP];
break;
default:
/* wrong packet for fmt tx q , drop it */
dev_kfree_skb_any(skb);
continue;
}
ret = usb_tx_urb_with_skb(usb_ld, skb, pipe_data);
if (ret < 0) {
mif_err("usb_tx_urb_with_skb, ret(%d)\n",
ret);
skb_queue_head(&ld->sk_fmt_tx_q, skb);
return;
}
}
skb = skb_dequeue(&ld->sk_raw_tx_q);
if (skb) {
pipe_data = &usb_ld->devdata[IF_USB_RAW_EP];
ret = usb_tx_urb_with_skb(usb_ld, skb, pipe_data);
if (ret < 0) {
mif_err("usb_tx_urb_with_skb "
"for raw, ret(%d)\n",
ret);
skb_queue_head(&ld->sk_raw_tx_q, skb);
return;
}
}
}
}
static int start_ipc(struct link_device *ld, struct io_device *iod)
{
struct sk_buff *skb;
char data[1] = {'a'};
int err;
struct usb_link_device *usb_ld = to_usb_link_device(ld);
struct link_pm_data *pm_data = usb_ld->link_pm_data;
struct device *dev = &usb_ld->usbdev->dev;
struct if_usb_devdata *pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
if (!usb_ld->if_usb_connected) {
mif_err("HSIC not connected, skip start ipc\n");
err = -ENODEV;
goto exit;
}
retry:
if (ld->mc->phone_state != STATE_ONLINE) {
mif_err("MODEM is not online, skip start ipc\n");
err = -ENODEV;
goto exit;
}
/* check usb runtime pm first */
if (dev->power.runtime_status != RPM_ACTIVE) {
if (!pm_data->resume_requested) {
mif_debug("QW PM\n");
INIT_COMPLETION(pm_data->active_done);
queue_delayed_work(pm_data->wq,
&pm_data->link_pm_work, 0);
}
mif_debug("Wait pm\n");
err = wait_for_completion_timeout(&pm_data->active_done,
msecs_to_jiffies(500));
/* timeout or -ERESTARTSYS */
if (err <= 0)
goto retry;
}
pm_runtime_get_sync(dev);
mif_err("send 'a'\n");
skb = alloc_skb(16, GFP_ATOMIC);
if (unlikely(!skb)) {
pm_runtime_put(dev);
return -ENOMEM;
}
memcpy(skb_put(skb, 1), data, 1);
skbpriv(skb)->iod = iod;
skbpriv(skb)->ld = ld;
if (!usb_ld->if_usb_connected || !usb_ld->usbdev)
return -ENODEV;
usb_mark_last_busy(usb_ld->usbdev);
err = usb_tx_urb_with_skb(usb_ld->usbdev, skb, pipe_data);
if (err < 0) {
mif_err("usb_tx_urb fail\n");
dev_kfree_skb_any(skb);
}
pm_runtime_put(dev);
exit:
return err;
}
/**
@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 int _usb_tx_work(struct sk_buff *skb)
{
struct sk_buff_head *txq;
struct io_device *iod = skbpriv(skb)->iod;
struct link_device *ld = skbpriv(skb)->ld;
struct usb_link_device *usb_ld = to_usb_link_device(ld);
struct if_usb_devdata *pipe_data;
int ret;
switch (iod->format) {
case IPC_BOOT:
case IPC_FMT:
/* boot device uses same intf with fmt*/
pipe_data = &usb_ld->devdata[IF_USB_FMT_EP];
txq = &ld->sk_fmt_tx_q;
break;
case IPC_RAW:
pipe_data = &usb_ld->devdata[IF_USB_RAW_EP];
txq = &ld->sk_raw_tx_q;
break;
case IPC_RFS:
pipe_data = &usb_ld->devdata[IF_USB_RFS_EP];
txq = &ld->sk_fmt_tx_q;
break;
default:
/* wrong packet, drop it */
pipe_data = NULL;
break;
}
if (!pipe_data) {
dev_kfree_skb_any(skb);
return -ENOENT;
}
if (iod->format == IPC_FMT && usb_ld->if_usb_is_main)
pr_skb("IPC-TX", skb);
if (iod->format == IPC_RAW)
mif_debug("TX[RAW]\n");
if (iod->format == IPC_RFS)
pr_skb("RFS-TX", skb);
if (!usb_ld->if_usb_connected || !usb_ld->usbdev)
return -ENODEV;
usb_mark_last_busy(usb_ld->usbdev);
ret = usb_tx_urb_with_skb(usb_ld->usbdev,
skb,
pipe_data);
if (ret < 0) {
if (ret == -ENODEV || ret == -ENOENT) {
mif_err("link broken while in runtime active ..."
" purge!\n");
return ret;
}
mif_err("usb_tx_urb_with_skb for iod(%d), ret=%d\n",
iod->format, ret);
skb_queue_head(txq, skb);
queue_delayed_work(ld->tx_wq,
&ld->tx_delayed_work,
msecs_to_jiffies(20));
return ret;
}
return 0;
}
