C++ smd_cur_packet_size函数代码示例

static void nmea_work_func(struct work_struct *ws)
{
	int sz;

	for (;;) {
		sz = smd_cur_packet_size(nmea_devp->ch);
		if (sz == 0)
			break;
		if (sz > smd_read_avail(nmea_devp->ch))
			break;
		if (sz > MAX_BUF_SIZE) {
			smd_read(nmea_devp->ch, 0, sz);
			continue;
		}

		mutex_lock(&nmea_rx_buf_lock);
		if (smd_read(nmea_devp->ch, nmea_devp->rx_buf, sz) != sz) {
			mutex_unlock(&nmea_rx_buf_lock);
			printk(KERN_ERR "nmea: not enough data?!\n");
			continue;
		}
		nmea_devp->bytes_read = sz;
		mutex_unlock(&nmea_rx_buf_lock);
		wake_up_interruptible(&nmea_wait_queue);
	}
}

static void smd_lge_notify(void *priv, unsigned event)
{
	switch (event) {
	case SMD_EVENT_DATA: {
		int sz;

		if (psmd_device->ch == 0) {
			SMD_LGE_INFO("%s : psmd_device->ch is NULL \n",
				  __func__);
			break;
	}
		sz = smd_cur_packet_size(psmd_device->ch);
		SMD_LGE_DBG("%s : SMD_EVENT_DATA current_packet_size = %d\n",
			    __func__, sz);
		if ((sz > 0) && (sz <= smd_read_avail(psmd_device->ch)))
#if 0
			queue_work(smd_lge_wq, &psmd_device->read_work);
#else
			wake_up(&smd_lge_wait_queue);
#endif
		break;
	}
	case SMD_EVENT_OPEN:
		SMD_LGE_INFO("%s : SMD_EVENT_OPEN\n", __func__);
		break;
	case SMD_EVENT_CLOSE:
		SMD_LGE_INFO("%s : SMD_EVENT_CLOSE\n", __func__);
		break;
	}
}

static void sns_ocmem_smd_read(struct work_struct *ws)
{
	struct smd_channel *ch = sns_ctl.smd_ch;
	unsigned char *buf = NULL;
	int sz, len;

	for (;;) {
		sz = smd_cur_packet_size(ch);
		BUG_ON(sz > SMD_BUF_SIZE);
		len = smd_read_avail(ch);
		pr_debug("%s: sz=%d, len=%d\n", __func__, sz, len);
		if (len == 0 || len < sz)
			break;
		buf = kzalloc(SMD_BUF_SIZE, GFP_KERNEL);
		if (buf == NULL) {
			pr_err("%s: malloc failed", __func__);
			break;
		}

		if (smd_read(ch, buf, sz) != sz) {
			pr_err("%s: not enough data?!\n", __func__);
			kfree(buf);
			continue;
		}

		sns_ocmem_smd_process((struct sns_ocmem_hdr_s *)buf,
			(void *)((char *)buf +
			sizeof(struct sns_ocmem_hdr_s)));

		kfree(buf);

	}
}

static void qmi_read_work(struct work_struct *ws)
{
	struct qmi_ctxt *ctxt = container_of(ws, struct qmi_ctxt, read_work);
	struct smd_channel *ch = ctxt->ch;
	unsigned char buf[QMI_MAX_PACKET];
	int sz;

	for (;;) {
		sz = smd_cur_packet_size(ch);
		if (sz == 0)
			break;
		if (sz < smd_read_avail(ch))
			break;
		if (sz > QMI_MAX_PACKET) {
			smd_read(ch, 0, sz);
			continue;
		}
		if (smd_read(ch, buf, sz) != sz) {
			printk(KERN_ERR "qmi: not enough data?!\n");
			continue;
		}

		/* interface selector must be 1 */
		if (buf[0] != 0x01)
			continue;

		qmi_process_qmux(ctxt, buf + 1, sz - 1);
	}
}

static void qmi_notify(void *priv, unsigned event)
{
    //struct qmi_ctxt *ctxt = priv;

    switch (event) 
    {
    case SMD_EVENT_DATA: 
        {
            int sz;
            sz = smd_cur_packet_size(ctrl_ch);
            if ((sz > 0) && (sz <= smd_read_avail(ctrl_ch))) 
            {
                wake_lock_timeout(&wakelock, HZ / 2);
                queue_work(qmi_wq, &read_work);
            }
            break;
        }
    case SMD_EVENT_OPEN:
        printk(KERN_INFO "qmi: smd opened\n");
        queue_work(qmi_wq, &open_work);
        break;
    case SMD_EVENT_CLOSE:
        printk(KERN_INFO "qmi: smd closed\n");
        break;
    }
}

static void grmnet_ctrl_smd_read_w(struct work_struct *w)
{
	struct smd_ch_info *c = container_of(w, struct smd_ch_info, read_w);
	struct rmnet_ctrl_port *port = c->port;
	int sz;
	size_t len;
	void *buf;
	unsigned long flags;

	while (1) {
		sz = smd_cur_packet_size(c->ch);
		if (sz == 0)
			break;

		if (smd_read_avail(c->ch) < sz)
			break;

		buf = kmalloc(sz, GFP_KERNEL);
		if (!buf)
			return;

		len = smd_read(c->ch, buf, sz);

		/* send it to USB here */
		spin_lock_irqsave(&port->port_lock, flags);
		if (port->port_usb && port->port_usb->send_cpkt_response) {
			port->port_usb->send_cpkt_response(port->port_usb,
							buf, len);
			c->to_host++;
		}
		kfree(buf);
		spin_unlock_irqrestore(&port->port_lock, flags);
	}
}

static void grmnet_ctrl_smd_read_w(struct work_struct *w)
{
	struct smd_ch_info *c = container_of(w, struct smd_ch_info, read_w);
	struct rmnet_ctrl_port *port = c->port;
	int sz;
	struct rmnet_ctrl_pkt *cpkt;
	unsigned long flags;

	while (1) {
		sz = smd_cur_packet_size(c->ch);
		if (sz == 0)
			break;

		if (smd_read_avail(c->ch) < sz)
			break;

		cpkt = rmnet_alloc_ctrl_pkt(sz, GFP_KERNEL);
		if (IS_ERR(cpkt)) {
			pr_err("%s: unable to allocate rmnet control pkt\n",
					__func__);
			return;
		}
		cpkt->len = smd_read(c->ch, cpkt->buf, sz);

		/* send it to USB here */
		spin_lock_irqsave(&port->port_lock, flags);
		if (port->port_usb && port->port_usb->send_cpkt_response) {
			port->port_usb->send_cpkt_response(
							port->port_usb,
							cpkt);
			c->to_host++;
		}
		spin_unlock_irqrestore(&port->port_lock, flags);
	}
}

static void smd_vt_notify(void *priv, unsigned event)
{
    unsigned long flags;

    switch (event) {
        case SMD_EVENT_DATA: {
            int sz;
         
            if( (smd_vt_devp->open_count != 0) && (smd_vt_devp->ch != 0)){    // SMD bug fix. 0811
                sz = smd_cur_packet_size(smd_vt_devp->ch);
                if ((sz > 0) && (sz <= smd_read_avail(smd_vt_devp->ch))) {
                    /* queue_work(smd_vt_wq, &smd_vt_work); */
                    spin_lock_irqsave(&smd_vt_read_lock, flags);
                    if (smd_vt_devp->read_avail == 0) {
                        smd_vt_devp->read_avail = sz;
                        wake_up_interruptible(&smd_vt_wait_queue);
                    }
                    spin_unlock_irqrestore(&smd_vt_read_lock, flags);
                }
            }
            break;
        }
        case SMD_EVENT_OPEN:
            smd_vt_devp->open_flag =1;
            printk(KERN_INFO "smd_vt: smd opened\n");
            break;
        case SMD_EVENT_CLOSE:
            printk(KERN_INFO "smd_vt: smd closed\n");
            break;
    }
}

static void wcn36xx_msm_smd_work(struct work_struct *work)
{
	int avail;
	int msg_len;
	void *msg;
	int ret;
	struct wcn36xx_msm *wmsm_priv =
		container_of(work, struct wcn36xx_msm, smd_work);

	while (1) {
		msg_len = smd_cur_packet_size(wmsm_priv->smd_ch);
		if (0 == msg_len) {
			return;
		}
		avail = smd_read_avail(wmsm_priv->smd_ch);
		if (avail < msg_len) {
			return;
		}
		msg = kmalloc(msg_len, GFP_KERNEL);
		if (NULL == msg) {
			return;
		}

		ret = smd_read(wmsm_priv->smd_ch, msg, msg_len);
		if (ret != msg_len) {
			return;
		}
		wmsm_priv->rsp_cb(wmsm_priv->drv_priv, msg, msg_len);
		kfree(msg);
	}
}

static void apr_tal_notify(void *priv, unsigned event)
{
	struct apr_svc_ch_dev *apr_ch = priv;
	int len, r_len, sz;
	int pkt_cnt = 0;
	unsigned long flags;

	pr_debug("event = %d\n", event);
	switch (event) {
	case SMD_EVENT_DATA:
		pkt_cnt = 0;
		spin_lock_irqsave(&apr_ch->lock, flags);
check_pending:
		len = smd_read_avail(apr_ch->ch);
		if (len < 0) {
			pr_err("apr_tal: Invalid Read Event :%d\n", len);
			spin_unlock_irqrestore(&apr_ch->lock, flags);
			return;
		}
		sz = smd_cur_packet_size(apr_ch->ch);
		if (sz < 0) {
			pr_debug("pkt size is zero\n");
			spin_unlock_irqrestore(&apr_ch->lock, flags);
			return;
		}
		if (!len && !sz && !pkt_cnt)
			goto check_write_avail;
		if (!len) {
			pr_debug("len = %d pkt_cnt = %d\n", len, pkt_cnt);
			spin_unlock_irqrestore(&apr_ch->lock, flags);
			return;
		}
		r_len = smd_read_from_cb(apr_ch->ch, apr_ch->data, len);
		if (len != r_len) {
			pr_err("apr_tal: Invalid Read\n");
			spin_unlock_irqrestore(&apr_ch->lock, flags);
			return;
		}
		pkt_cnt++;
		pr_debug("%d %d %d\n", len, sz, pkt_cnt);
		if (apr_ch->func)
			apr_ch->func(apr_ch->data, r_len, apr_ch->priv);
		goto check_pending;
check_write_avail:
		if (smd_write_avail(apr_ch->ch))
			wake_up(&apr_ch->wait);
		spin_unlock_irqrestore(&apr_ch->lock, flags);
		break;
	case SMD_EVENT_OPEN:
		pr_debug("apr_tal: SMD_EVENT_OPEN\n");
		apr_ch->smd_state = 1;
		wake_up(&apr_ch->wait);
		break;
	case SMD_EVENT_CLOSE:
		pr_debug("apr_tal: SMD_EVENT_CLOSE\n");
		break;
	}
}

static void qmi_read_work(struct work_struct *ws)
{
    //struct qmi_ctxt *ctxt = container_of(ws, struct qmi_ctxt, read_work);
    //struct smd_channel *ch = ctxt->ch;
    unsigned char buf[QMI_MAX_PACKET];
    struct qmi_ctxt *ctxt;
    int sz;
    uint32_t chnum;

    for (;;) 
    {
        sz = smd_cur_packet_size(ctrl_ch);
        if (sz == 0)
            break;
        if (sz < smd_read_avail(ctrl_ch))
            break;
        if (sz > QMI_MAX_PACKET)
        {
            smd_read(ctrl_ch, NULL, sz);
            continue;
        }
        if (smd_read(ctrl_ch, buf, sz) != sz) 
        {
            printk(KERN_ERR "qmi: not enough data?!\n");
            continue;
        }

        DBG("packet: %d\n", sz);
        //        print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, buf, sz);

        if (sz <= 4)
        {
            DBG("packet size less 4\n");
            continue;
        }
        chnum = *(uint32_t*)&buf[sz - 4];
        DBG("chnum = %d\n", chnum);

        /* interface selector must be 1 */
        if (buf[0] != 0x01)
            continue;

        if (qmi_device0.ch_num == chnum)
            ctxt = &qmi_device0;
        else if (qmi_device1.ch_num == chnum)
            ctxt = &qmi_device1;
        else if (qmi_device2.ch_num == chnum)
            ctxt = &qmi_device2;
        else
        {
            DBG("bad chnum %d\n", chnum);
            continue;
        }

        qmi_process_qmux(ctxt, buf + 1, sz - 1 - 4);
    }
}

/* Called in soft-irq context */
static void smd_net_data_handler(unsigned long arg)
{
	struct net_device *dev = (struct net_device *) arg;
	struct rmnet_private *p = netdev_priv(dev);
	struct sk_buff *skb;
	void *ptr = 0;
	int sz;

	for (;;) {
		sz = smd_cur_packet_size(p->ch);
		if (sz == 0) break;
		if (smd_read_avail(p->ch) < sz) break;

		if (sz > 1514) {
			pr_err("rmnet_recv() discarding %d len\n", sz);
			ptr = 0;
		} else {
			skb = dev_alloc_skb(sz + NET_IP_ALIGN);
			if (skb == NULL) {
				pr_err("rmnet_recv() cannot allocate skb\n");
				/* out of memory, reschedule a later attempt */
				smd_net_data_tasklet.data = (unsigned long)dev;
				tasklet_schedule(&smd_net_data_tasklet);
				break;
			} else {
				skb->dev = dev;
				skb_reserve(skb, NET_IP_ALIGN);
				ptr = skb_put(skb, sz);
				wake_lock_timeout(&p->wake_lock, HZ / 2);
				if (smd_read(p->ch, ptr, sz) != sz) {
					pr_err("rmnet_recv() smd lied about avail?!");
					ptr = 0;
					dev_kfree_skb_irq(skb);
				} else {
					skb->protocol = eth_type_trans(skb, dev);
					if (count_this_packet(ptr, skb->len)) {
#ifdef CONFIG_MSM_RMNET_DEBUG
						p->wakeups_rcv +=
							rmnet_cause_wakeup(p);
#endif
						p->stats.rx_packets++;
						p->stats.rx_bytes += skb->len;
					}
					netif_rx(skb);
				}
				continue;
			}
		}
		if (smd_read(p->ch, ptr, sz) != sz)
			pr_err("rmnet_recv() smd lied about avail?!");
	}
}

static void smd_net_data_handler(unsigned long arg)
{
	struct net_device *dev = (struct net_device *) arg;
	struct smm6260net_private *p = netdev_priv(dev);
	struct sk_buff *skb;
	void *ptr = 0;
	int sz;
	int err;

	for (;;) {
		sz = smd_cur_packet_size(p->ch);
		if (sz == 0) break;
		if (smd_read_avail(p->ch) < sz) break;
		
		if (sz > SMM6260_NET_DEFAULT_MTU) 
		{
			ptr = 0;
			//pr_err("rmnet_recv() discarding %d len\n", sz);
		}else{
			skb = dev_alloc_skb(sz);
			if (skb == NULL) {
				//pr_err("smm6260net_recv() cannot allocate skb\n");
			} else {
				skb->dev = dev;
				ptr = skb_put(skb, sz);
				wake_lock_timeout(&p->wake_lock, HZ / 2);
				if (smd_read(p->ch, ptr, sz) != sz) {
					//pr_err("smm6260net_recv() smd lied about avail?!");
					ptr = 0;
					dev_kfree_skb_irq(skb);
				} else {
					skb->protocol = htons(ETH_P_IP);//eth_type_trans(skb, dev);			
					if(count_this_packet(skb))
					{
						/* update out statistics */
#ifdef CONFIG_MSM_RMNET_DEBUG
						p->wakeups_rcv += smm6260net_cause_wakeup(p);
#endif				
						p->stats.rx_packets++;
						p->stats.rx_bytes += skb->len;
					}
					skb_reset_mac_header(skb);
					netif_rx(skb);
					//pr_info("%s: smm6260net_recv() size=%d", p->chname, skb->len);
				}
				continue;
			}
		}
		if (smd_read(p->ch, ptr, sz) != sz)
			pr_err("rmnet_recv() smd lied about avail?!");
	}
}

/*
 * All SMD notifications and messages from Sensors on ADSP are
 * received by this function
 *
 */
void sns_ocmem_smd_notify_data(void *data, unsigned int event)
{
	if (event == SMD_EVENT_DATA) {
		int sz;
		pr_debug("%s: Received SMD event Data\n", __func__);
		sz = smd_cur_packet_size(sns_ctl.smd_ch);
		if ((sz > 0) && (sz <= smd_read_avail(sns_ctl.smd_ch)))
			queue_work(sns_ctl.smd_wq, &sns_ctl.smd_read_work);
	} else if (event == SMD_EVENT_OPEN) {
		pr_debug("%s: Received SMD event Open\n", __func__);
	} else if (event == SMD_EVENT_CLOSE) {
		pr_debug("%s: Received SMD event Close\n", __func__);
	}
}

static void nmea_notify(void *priv, unsigned event)
{
	switch (event) {
	case SMD_EVENT_DATA: {
		int sz;
		sz = smd_cur_packet_size(nmea_devp->ch);
		if ((sz > 0) && (sz <= smd_read_avail(nmea_devp->ch)))
			queue_work(nmea_wq, &nmea_work);
		break;
	}
	case SMD_EVENT_OPEN:
		printk(KERN_INFO "nmea: smd opened\n");
		break;
	case SMD_EVENT_CLOSE:
		printk(KERN_INFO "nmea: smd closed\n");
		break;
	}
}