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