drivers/aic8800d80
1
0 Fork 0
mirror of https://github.com/shenmintao/aic8800d80.git synced 2026-11-10 16:49:17 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
6e8222c10a99d8e2d5bc4b344803e7273092a5ae
aic8800d80/drivers/aic8800/aic8800_fdrv/aicwf_txrxif.c
T
xuxinchun1003 089ee0bf87 fix: NULL-guard bus teardown and clear drvdata after cleanup completes 
Review-driven rework of the teardown lifetime (shenmintao/aic8800d80#103):

- aicwf_bus_deinit(): keep the entry NULL guard (sequential double-deinit
  on probe-failure paths), but move dev_set_drvdata(dev, NULL) to the END
  of completed cleanup — after URB cancel, platform/rx/cmd teardown and the
  kthread_stop() join of bustx_thread. The previous placement (right after
  bus_stop) opened a real race: aicwf_usb_bus_txdata() re-reads drvdata and
  runs from bustx_thread, which is joined only later, and the TX-path state
  check is not a synchronization point. With the new ordering every cleanup
  callback and TX reader observes a non-NULL pointer throughout.
- aicwf_usb_bus_stop(): NULL-check bus_if before dereferencing
  bus_if->bus_priv.usb (defense in depth; historical host oops
  RIP aicwf_usb_bus_stop+0x11 on probe-failure rollback).

Verification (isolated VFIO lab, one 2357:0147 unit, sustained ping -f):
a test-only fault-injection build (not included) reaches the repeated-deinit
path and prints "already deinitialized, skip" with BUG=0; missing-firmware
init failure runs one clean deinit; physical unplug x8, sysfs unbind and
authorized cold cycles, disconnect during scanning and during connection
establishment, ifdown/up x5, rmmod/insmod x3 -> BUG=0 throughout (only the
pre-existing WARNs at rwnx_main.c:1529 and rwnx_cfg80211_disconnect:3328).
2026-10-07 07:42:31 +08:00

1382 lines
42 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/**
* aicwf_bus.c
*
* bus function declarations
*
* Copyright (C) AICSemi 2018-2020
*/
#include <linux/kthread.h>
#include <linux/netdevice.h>
#include <linux/printk.h>
#include <linux/interrupt.h>
#include <linux/sched.h>
#include <linux/completion.h>
#include <linux/semaphore.h>
#include <linux/debugfs.h>
#include <linux/atomic.h>
#include <linux/vmalloc.h>
#include "lmac_msg.h"
#include "aicwf_txrxif.h"
#include "rwnx_platform.h"
#include "rwnx_defs.h"
#include "rwnx_msg_rx.h"
#include "rwnx_rx.h"
#include "aicwf_rx_prealloc.h"
#ifdef AICWF_SDIO_SUPPORT
#include "sdio_host.h"
#endif
#ifdef CONFIG_RX_TASKLET
int aicwf_tasklet_rxframes(struct aicwf_rx_priv *rx_priv);
#endif
#ifdef CONFIG_TX_TASKLET
void aicwf_tasklet_tx_process(struct aic_usb_dev *usb_dev);
#endif
extern bool aicwf_usb_rx_aggr;
#ifdef CONFIG_PREALLOC_RX_SKB
void aicwf_rxframe_queue_init_2(struct rx_frame_queue *pq, int max_len)
{
//int prio;
memset(pq, 0, offsetof(struct rx_frame_queue, queuelist) + (sizeof(struct list_head)));
pq->qmax = (u16)max_len;
INIT_LIST_HEAD(&pq->queuelist);
#if 0
memset(pq, 0, offsetof(struct rx_frame_queue, queuelist) + (sizeof(struct list_head) * num_prio));
pq->num_prio = (u16)num_prio;
pq->qmax = (u16)max_len;
for (prio = 0; prio < num_prio; prio++) {
INIT_LIST_HEAD(&pq->queuelist[prio]);
}
#endif
}
//extern struct aic_sdio_dev *g_sdiodev;
void rxbuff_queue_flush(struct aicwf_rx_priv* rx_priv)
{
//int prio;
struct rx_frame_queue *pq = &rx_priv->rxq;
struct list_head *pos;
struct list_head *n;
struct list_head *head;
struct rx_buff *tempbuf = NULL;
head = &pq->queuelist;
list_for_each_safe(pos, n, head) {
tempbuf = list_entry(pos, struct rx_buff, queue);
list_del_init(&tempbuf->queue);
#if 0
rxbuff_free(tempbuf);
#else
aicwf_prealloc_rxbuff_free(tempbuf, &rx_priv->rxbuff_lock);
#endif
pq->qcnt--;
}
}
#endif
int aicwf_bus_init(uint bus_hdrlen, struct device *dev)
{
int ret = 0;
struct aicwf_bus *bus_if;
if (!dev) {
AICWFDBG(LOGERROR, "device not found\n");
return -1;
}
bus_if = dev_get_drvdata(dev);
#if defined CONFIG_USB_SUPPORT && defined CONFIG_USB_NO_TRANS_DMA_MAP
#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
bus_if->cmd_buf = usb_alloc_coherent(bus_if->bus_priv.usb->udev, CMD_BUF_MAX, (in_interrupt() ? GFP_ATOMIC : GFP_KERNEL), &bus_if->bus_priv.usb->cmd_dma_trans_addr);
#else
bus_if->cmd_buf = usb_buffer_alloc(bus_if->bus_priv.usb->udev, CMD_BUF_MAX, (in_interrupt() ? GFP_ATOMIC : GFP_KERNEL), &bus_if->bus_priv.usb->cmd_dma_trans_addr);
#endif
#else
bus_if->cmd_buf = kzalloc(CMD_BUF_MAX, GFP_KERNEL);
#endif
if(!bus_if->cmd_buf) {
ret = -ENOMEM;
AICWFDBG(LOGERROR, "proto_attach failed\n");
goto fail;
}
memset(bus_if->cmd_buf, '\0', CMD_BUF_MAX);
init_completion(&bus_if->bustx_trgg);
init_completion(&bus_if->busrx_trgg);
#ifdef CONFIG_USB_MSG_IN_EP
if(bus_if->bus_priv.usb->msg_in_pipe) {
init_completion(&bus_if->msg_busrx_trgg);
}
#endif
#ifdef AICWF_SDIO_SUPPORT
bus_if->bustx_thread = kthread_run(sdio_bustx_thread, (void *)bus_if, "aicwf_bustx_thread");
bus_if->busrx_thread = kthread_run(sdio_busrx_thread, (void *)bus_if->bus_priv.sdio->rx_priv, "aicwf_busrx_thread");
#endif
#ifdef AICWF_USB_SUPPORT
bus_if->busrx_thread = kthread_run(usb_busrx_thread, (void *)bus_if->bus_priv.usb->rx_priv, "aicwf_busrx_thread");
bus_if->bustx_thread = kthread_run(usb_bustx_thread, (void *)bus_if, "aicwf_bustx_thread");
#if 1
//waiting for rx/tx thread init finish
#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 14, 0)
while(bus_if->busrx_thread->__state != TASK_INTERRUPTIBLE ||
bus_if->bustx_thread->__state != TASK_INTERRUPTIBLE)
#else
while(bus_if->busrx_thread->state != TASK_INTERRUPTIBLE ||
bus_if->bustx_thread->state != TASK_INTERRUPTIBLE)
#endif
{
AICWFDBG(LOGINFO, "%s waiting for rx/tx thread init finish \r\n", __func__);
msleep(100);
}
//waiting for rx/tx thread Initialization finish
#endif
#ifdef CONFIG_USB_MSG_IN_EP
if(bus_if->bus_priv.usb->msg_in_pipe)
{
bus_if->msg_busrx_thread = kthread_run(usb_msg_busrx_thread, (void *)bus_if->bus_priv.usb->rx_priv, "aicwf_msg_busrx_thread");
}
#endif
#ifdef CONFIG_RX_TASKLET//AIDEN tasklet
AICWFDBG(LOGINFO, "%s use tasklet for rx \r\n", __func__);
tasklet_init(&((bus_if->bus_priv.usb)->recv_tasklet),
(void(*)(unsigned long))aicwf_tasklet_rxframes,
(unsigned long)(bus_if->bus_priv.usb)->rx_priv);
#endif
#ifdef CONFIG_TX_TASKLET//AIDEN tasklet
AICWFDBG(LOGINFO, "%s use tasklet for tx \r\n", __func__);
tasklet_init(&((bus_if->bus_priv.usb)->xmit_tasklet),
(void(*)(unsigned long))aicwf_tasklet_tx_process,
(unsigned long)(bus_if->bus_priv.usb));
#endif
#endif
if (IS_ERR(bus_if->bustx_thread)) {
bus_if->bustx_thread = NULL;
AICWFDBG(LOGERROR, "aicwf_bustx_thread run fail\n");
goto fail;
}
if (IS_ERR(bus_if->busrx_thread)) {
bus_if->busrx_thread = NULL;
AICWFDBG(LOGERROR, "aicwf_bustx_thread run fail\n");
goto fail;
}
#ifdef CONFIG_USB_MSG_IN_EP
if(bus_if->bus_priv.usb->msg_in_pipe){
if (IS_ERR(bus_if->msg_busrx_thread)) {
bus_if->msg_busrx_thread = NULL;
txrx_err("aicwf_msg_busrx_thread run fail\n");
goto fail;
}
}
#endif
return ret;
fail:
aicwf_bus_deinit(dev);
return ret;
}
void aicwf_usb_cancel_all_urbs(struct aic_usb_dev *usb_dev);
void aicwf_bus_deinit(struct device *dev)
{
struct aicwf_bus *bus_if;
#ifdef AICWF_USB_SUPPORT
struct aic_usb_dev *usb;
#endif
#ifdef AICWF_SDIO_SUPPORT
struct aic_sdio_dev *sdiodev;
#endif
if (!dev) {
txrx_err("device not found\n");
return;
}
AICWFDBG(LOGINFO, "%s Enter\r\n", __func__);
bus_if = dev_get_drvdata(dev);
/* 入口判空：本函数在探测失败路径上会被调用**两次**
* aicwf_bus_init() 的 `fail:` 先调一次，随后 aicwf_usb_probe() 的
* `out_free_bus:` 再调一次（用局部 bus_if 做 kfree，不依赖 drvdata）。
* 第二次进入时 drvdata 已在**上一次调用的末尾**被清空 → 必然命中本判空。
* 若不清空，第二次会对已完整拆解的对象重复执行整套 teardown。 */
if (!bus_if) {
AICWFDBG(LOGINFO, "%s: already deinitialized, skip\n", __func__);
return;
}
aicwf_bus_stop(bus_if);
#ifdef AICWF_USB_SUPPORT
usb = bus_if->bus_priv.usb;
aicwf_usb_cancel_all_urbs(usb);//AIDEN
if(g_rwnx_plat && g_rwnx_plat->enabled){
rwnx_platform_deinit(usb->rwnx_hw);
}else{
AICWFDBG(LOGINFO, "%s g_rwnx_plat->enabled is false\r\n", __func__);
}
#endif
#ifdef AICWF_SDIO_SUPPORT
sdiodev =bus_if->bus_priv.sdio;
if(g_rwnx_plat && g_rwnx_plat->enabled)
rwnx_platform_deinit(sdiodev->rwnx_hw);
#endif
if (bus_if->cmd_buf) {
#if defined CONFIG_USB_SUPPORT && defined CONFIG_USB_NO_TRANS_DMA_MAP
#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
usb_free_coherent(bus_if->bus_priv.usb->udev, CMD_BUF_MAX, bus_if->cmd_buf, bus_if->bus_priv.usb->cmd_dma_trans_addr);
#else
usb_buffer_free(bus_if->bus_priv.usb->udev, CMD_BUF_MAX, bus_if->cmd_buf, bus_if->bus_priv.usb->cmd_dma_trans_addr);
#endif
#else
kfree(bus_if->cmd_buf);
#endif
bus_if->cmd_buf = NULL;
}
if (bus_if->bustx_thread) {
AICWFDBG(LOGINFO, "%s stop to bustx_thread!! \r\n", __func__);
complete_all(&bus_if->bustx_trgg);
kthread_stop(bus_if->bustx_thread);
bus_if->bustx_thread = NULL;
}
if (usb->rx_priv)
aicwf_rx_deinit(usb->rx_priv);
rwnx_cmd_mgr_deinit(&usb->cmd_mgr);
#ifdef CONFIG_TX_TASKLET//AIDEN tasklet
tasklet_kill(&usb->xmit_tasklet);
#endif
/* 清空 drvdata：全部拆解完成之后、对象由调用方释放之前。
* （评审 shenmintao/aic8800d80#103 指出的顺序问题：此前清空点位于
* bus_stop 之后、URB/platform/rx/cmd 拆解与 TX 线程 join 之前。）
* 1) 顺序双 deinit：第二次调用靠上方入口判空在本行之后必然 skip；
* 2) 过早清空会伤害仍在运行的读者——TX 链
* usb_bustx_thread → aicwf_bus_txdata(usbdev->bus_if) →
* ops->txdata = aicwf_usb_bus_txdata()（usb.c 在该处重读
* dev_get_drvdata）仅靠 state 检查做 check-then-act、无锁，
* 且该线程直到上方 kthread_stop(bustx_thread) 才被 join——
* 必须先 join 再清；
* 3) cmd/rx/platform 拆解与 rwnx_close() 等读者在本行之前全程
* 看到非空 drvdata，行为与未打补丁树一致。 */
dev_set_drvdata(dev, NULL);
AICWFDBG(LOGINFO, "%s Exit \n", __func__);
}
void aicwf_frame_tx(void *dev, struct sk_buff *skb)
{
#ifdef AICWF_SDIO_SUPPORT
struct aic_sdio_dev *sdiodev = (struct aic_sdio_dev *)dev;
aicwf_bus_txdata(sdiodev->bus_if, skb);
#else
struct aic_usb_dev *usbdev = (struct aic_usb_dev *)dev;
struct rwnx_txhdr *txhdr = NULL;
unsigned long flags;
if (!usbdev->state) {
txrx_err("down\n");
spin_lock_irqsave(&usbdev->tx_flow_lock, flags);
if(!usbdev->tbusy)
aicwf_usb_tx_flowctrl(usbdev->rwnx_hw, true);
spin_unlock_irqrestore(&usbdev->tx_flow_lock, flags);
txhdr = (struct rwnx_txhdr *)skb->data;
kmem_cache_free(usbdev->rwnx_hw->sw_txhdr_cache, txhdr->sw_hdr);
dev_kfree_skb(skb);
return;
}
aicwf_bus_txdata(usbdev->bus_if, skb);
#endif
}
struct aicwf_tx_priv* aicwf_tx_init(void *arg)
{
struct aicwf_tx_priv* tx_priv;
tx_priv = kzalloc(sizeof(struct aicwf_tx_priv), GFP_KERNEL);
if (!tx_priv)
return NULL;
#ifdef AICWF_SDIO_SUPPORT
tx_priv->sdiodev = (struct aic_sdio_dev *)arg;
#else
tx_priv->usbdev = (struct aic_usb_dev *)arg;
#endif
atomic_set(&tx_priv->aggr_count, 0);
tx_priv->aggr_buf = dev_alloc_skb(MAX_AGGR_TXPKT_LEN);
if(!tx_priv->aggr_buf) {
txrx_err("Alloc bus->txdata_buf failed!\n");
kfree(tx_priv);
return NULL;
}
tx_priv->head = tx_priv->aggr_buf->data;
tx_priv->tail = tx_priv->aggr_buf->data;
return tx_priv;
}
void aicwf_tx_deinit(struct aicwf_tx_priv* tx_priv)
{
if (tx_priv && tx_priv->aggr_buf)
dev_kfree_skb(tx_priv->aggr_buf);
kfree(tx_priv);
}
#ifdef CONFIG_PREALLOC_RX_SKB
static bool aicwf_another_ptk_1(struct rx_buff *buffer)
{
u8 *read = buffer->read;
u16 aggr_len = 0;
if(read == NULL || read >= (buffer->end - 2)) {
return false;
}
if(buffer->len < 2) {
return false;
}
aggr_len = (*read | (*(read + 1) << 8));
if(aggr_len == 0) {
return false;
}
return true;
}
#else
static bool aicwf_another_ptk(struct sk_buff *skb)
{
u8 *data;
u16 aggr_len = 0;
if (skb->data == NULL || skb->len < 2) {
return false;
}
data = skb->data;
aggr_len = (*skb->data | (*(skb->data + 1) << 8));
if (aggr_len == 0) {
return false;
}
return true;
}
#endif
#if 0//AIDEN
void rwnx_frame_parser(char* tag, char* data, unsigned long len);
#endif
#ifdef CONFIG_RX_TASKLET
int aicwf_tasklet_rxframes(struct aicwf_rx_priv *rx_priv)
{
int ret = 0;
unsigned long flags = 0;
struct sk_buff *skb = NULL; /* Packet for event or data frames */
u16 pkt_len = 0;
//struct sk_buff *skb_inblock = NULL;
u16 aggr_len = 0, adjust_len = 0;
u8 *data = NULL;
u8_l *msg = NULL;
while (1) {
//aicwf_usb_rx_submit_all_urb_(rx_priv->usbdev);
spin_lock_irqsave(&rx_priv->rxqlock, flags);
if(aicwf_is_framequeue_empty(&rx_priv->rxq)) {
usb_info("no more rxdata\n");
spin_unlock_irqrestore(&rx_priv->rxqlock,flags);
break;
}
skb = aicwf_frame_dequeue(&rx_priv->rxq);
spin_unlock_irqrestore(&rx_priv->rxqlock, flags);
if (skb == NULL) {
txrx_err("skb_error\r\n");
break;
}
data = skb->data;
pkt_len = (*skb->data | (*(skb->data + 1) << 8));
//printk("p:%d, s:%d , %x\n", pkt_len, skb->len, data[2]);
#ifndef CONFIG_USB_RX_REASSEMBLE
if (pkt_len > skb->len) {
AICWFDBG(LOGERROR, "%s pkt_len:%d skb->len:%d\r\n", __func__, pkt_len, skb->len);
dev_kfree_skb(skb);
atomic_dec(&rx_priv->rx_cnt);
continue;
}
#endif
if((skb->data[2] & USB_TYPE_CFG) != USB_TYPE_CFG) { // type : data
#if 0
aggr_len = pkt_len + RX_HWHRD_LEN;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
skb_inblock = __dev_alloc_skb(aggr_len + CCMP_OR_WEP_INFO, GFP_ATOMIC);//8 is for ccmp mic or wep icv
if(skb_inblock == NULL){
txrx_err("no more space! skip!\n");
skb_pull(skb, adjust_len);
continue;
}
skb_put(skb_inblock, aggr_len);
memcpy(skb_inblock->data, data, aggr_len);
#if 0//AIDEN
rwnx_frame_parser((char*)__func__, skb_inblock->data + 60, aggr_len - 60);
#endif
#endif
rwnx_rxdataind_aicwf(rx_priv->usbdev->rwnx_hw, skb, (void *)rx_priv);
///TODO: here need to add rx data process
//skb_pull(skb, adjust_len);
}
else { // type : config
aggr_len = pkt_len;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
msg = kmalloc(aggr_len+4, GFP_ATOMIC);
if(msg == NULL){
txrx_err("no more space for msg!\n");
aicwf_dev_skb_free(skb);
return -EBADE;
}
memcpy(msg, data, aggr_len + 4);
if((*(msg + 2) & 0x7f) == USB_TYPE_CFG_CMD_RSP)
rwnx_rx_handle_msg(rx_priv->usbdev->rwnx_hw, (struct ipc_e2a_msg *)(msg + 4));
if((*(msg + 2) & 0x7f) == USB_TYPE_CFG_DATA_CFM)
aicwf_usb_host_tx_cfm_handler(&(rx_priv->usbdev->rwnx_hw->usb_env), (u32 *)(msg + 4));
skb_pull(skb, adjust_len+4);
kfree(msg);
dev_kfree_skb(skb);
}
//dev_kfree_skb(skb);
atomic_dec(&rx_priv->rx_cnt);
}
return ret;
}
#endif
extern bool rx_urb_sched;
int aicwf_process_rxframes(struct aicwf_rx_priv *rx_priv)
{
#ifdef AICWF_SDIO_SUPPORT
int ret = 0;
unsigned long flags = 0;
struct sk_buff *skb = NULL;
u16 pkt_len = 0;
struct sk_buff *skb_inblock = NULL;
u16 aggr_len = 0, adjust_len = 0;
u8 *data = NULL;
u8_l *msg = NULL;
while (1) {
spin_lock_irqsave(&rx_priv->rxqlock, flags);
if(aicwf_is_framequeue_empty(&rx_priv->rxq)) {
spin_unlock_irqrestore(&rx_priv->rxqlock,flags);
break;
}
skb = aicwf_frame_dequeue(&rx_priv->rxq);
spin_unlock_irqrestore(&rx_priv->rxqlock,flags);
if (skb == NULL) {
txrx_err("skb_error\r\n");
break;
}
while(aicwf_another_ptk(skb)) {
data = skb->data;
pkt_len = (*skb->data | (*(skb->data + 1) << 8));
if((skb->data[2] & SDIO_TYPE_CFG) != SDIO_TYPE_CFG) { // type : data
aggr_len = pkt_len + RX_HWHRD_LEN;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
skb_inblock = __dev_alloc_skb(aggr_len + CCMP_OR_WEP_INFO, GFP_KERNEL);
if(skb_inblock == NULL){
txrx_err("no more space! skip\n");
skb_pull(skb, adjust_len);
continue;
}
skb_put(skb_inblock, aggr_len);
memcpy(skb_inblock->data, data, aggr_len);
rwnx_rxdataind_aicwf(rx_priv->sdiodev->rwnx_hw, skb_inblock, (void *)rx_priv);
skb_pull(skb, adjust_len);
}
else { // type : config
aggr_len = pkt_len;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
msg = kmalloc(aggr_len+4, GFP_KERNEL);
if(msg == NULL){
txrx_err("no more space for msg!\n");
aicwf_dev_skb_free(skb);
return -EBADE;
}
memcpy(msg, data, aggr_len + 4);
if((*(msg + 2) & 0x7f) == SDIO_TYPE_CFG_CMD_RSP)
rwnx_rx_handle_msg(rx_priv->sdiodev->rwnx_hw, (struct ipc_e2a_msg *)(msg + 4));
if((*(msg + 2) & 0x7f) == SDIO_TYPE_CFG_DATA_CFM)
aicwf_sdio_host_tx_cfm_handler(&(rx_priv->sdiodev->rwnx_hw->sdio_env), (u32 *)(msg + 4));
skb_pull(skb, adjust_len+4);
kfree(msg);
}
}
dev_kfree_skb(skb);
atomic_dec(&rx_priv->rx_cnt);
}
aicwf_sdio_pwr_stctl(rx_priv->sdiodev, SDIO_ACTIVE_ST);
return ret;
#else //AICWF_USB_SUPPORT
int ret = 0;
unsigned long flags = 0;
#ifndef CONFIG_PREALLOC_RX_SKB
struct sk_buff *skb = NULL;/* Packet for event or data frames */
#endif
struct sk_buff *skb_inblock = NULL;
u16 pkt_len = 0;
u16 aggr_len = 0, adjust_len = 0;
u8 *data = NULL;
u8_l *msg = NULL;
//#ifdef CONFIG_USB_RX_AGGR
// struct sk_buff *skb_inblock = NULL;
u8 cnt = 0 ;
//#endif
#ifdef CONFIG_PREALLOC_RX_SKB
struct rx_buff *buffer = NULL;
if(aicwf_usb_rx_aggr){
while (1) {
spin_lock_irqsave(&rx_priv->rxqlock, flags);
if (!rx_priv->rxq.qcnt) {
usb_info("no more rxdata\n");
spin_unlock_irqrestore(&rx_priv->rxqlock,flags);
break;
}
buffer = rxbuff_dequeue(&rx_priv->rxq);
spin_unlock_irqrestore(&rx_priv->rxqlock, flags);
if (buffer == NULL) {
txrx_err("skb_error\r\n");
ASSERT_ERR(1);
break;
}
while(aicwf_another_ptk_1(buffer)) {
cnt++;
data = buffer->read;
pkt_len = (*data | (*(data + 1) << 8));
//printk("%s cnt:%d pkt_len:%d \r\n", __func__, cnt, pkt_len);
#ifndef CONFIG_USB_RX_REASSEMBLE
if (pkt_len > buffer->len) {
AICWFDBG(LOGERROR, "%s pkt_len:%d buffer->len:%d\r\n", __func__, pkt_len, buffer->len);
aicwf_prealloc_rxbuff_free(buffer, &rx_priv->rxbuff_lock);
atomic_dec(&rx_priv->rx_cnt);
return -EBADE;
}
#endif
if((data[2] & USB_TYPE_CFG) != USB_TYPE_CFG) { // type : data
aggr_len = pkt_len + RX_HWHRD_LEN;
skb_inblock = __dev_alloc_skb(aggr_len + CCMP_OR_WEP_INFO, GFP_KERNEL);
if (skb_inblock == NULL) {
txrx_err("no more space! skip\n");
aicwf_prealloc_rxbuff_free(buffer, &rx_priv->rxbuff_lock);
atomic_dec(&rx_priv->rx_cnt);
return -EBADE;
}
skb_put(skb_inblock, aggr_len);
memcpy(skb_inblock->data, data, aggr_len);
rwnx_rxdataind_aicwf(rx_priv->usbdev->rwnx_hw, skb_inblock, (void *)rx_priv);
buffer->read = buffer->read + aggr_len;
buffer->len -= aggr_len;
}
else { // type : config
aggr_len = pkt_len;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
msg = kmalloc(aggr_len+4, GFP_KERNEL);
if(msg == NULL){
txrx_err("no more space for msg!\n");
aicwf_prealloc_rxbuff_free(buffer, &rx_priv->rxbuff_lock);
return -EBADE;
}
memcpy(msg, data, aggr_len + 4);
if(((*(msg + 2) & 0x7f) == USB_TYPE_CFG_CMD_RSP) && (rx_priv->usbdev->bus_if->state != (int)USB_DOWN_ST))
rwnx_rx_handle_msg(rx_priv->usbdev->rwnx_hw, (struct ipc_e2a_msg *)(msg + 4));
if((*(msg + 2) & 0x7f) == USB_TYPE_CFG_DATA_CFM)
aicwf_usb_host_tx_cfm_handler(&(rx_priv->usbdev->rwnx_hw->usb_env), (u32 *)(msg + 4));
if ((*(msg + 2) & 0x7f) == USB_TYPE_CFG_PRINT)
rwnx_rx_handle_print(rx_priv->usbdev->rwnx_hw, msg + 4, aggr_len);
buffer->read = buffer->read + (adjust_len + 4);
buffer->len -= (adjust_len + 4);
kfree(msg);
}
}
aicwf_prealloc_rxbuff_free(buffer, &rx_priv->rxbuff_lock);
if (rx_urb_sched) {
schedule_work(&rx_priv->usbdev->rx_urb_work);
rx_urb_sched = false;
}
atomic_dec(&rx_priv->rx_cnt);
}
}else{
while (1) {
spin_lock_irqsave(&rx_priv->rxqlock, flags);
if (!rx_priv->rxq.qcnt) {
usb_info("no more rxdata\n");
spin_unlock_irqrestore(&rx_priv->rxqlock,flags);
break;
}
buffer = rxbuff_dequeue(&rx_priv->rxq);
spin_unlock_irqrestore(&rx_priv->rxqlock, flags);
if (buffer == NULL) {
txrx_err("skb_error\r\n");
ASSERT_ERR(1);
break;
}
data = buffer->data;
pkt_len = (*data | (*(data + 1) << 8));
//printk("p:%d, s:%d , %x\n", pkt_len, skb->len, data[2]);
#ifndef CONFIG_USB_RX_REASSEMBLE
if (pkt_len > buffer->len) {
AICWFDBG(LOGERROR, "%s pkt_len:%d buffer->len:%d\r\n", __func__, pkt_len, buffer->len);
aicwf_prealloc_rxbuff_free(buffer, &rx_priv->rxbuff_lock);
atomic_dec(&rx_priv->rx_cnt);
continue;
}
#endif
if((data[2] & USB_TYPE_CFG) != USB_TYPE_CFG) { // type : data
skb_inblock = __dev_alloc_skb(pkt_len + RX_HWHRD_LEN + CCMP_OR_WEP_INFO, GFP_KERNEL);
if (skb_inblock == NULL) {
txrx_err("no more space! skip\n");
aicwf_prealloc_rxbuff_free(buffer, &rx_priv->rxbuff_lock);
atomic_dec(&rx_priv->rx_cnt);
continue;
}
skb_put(skb_inblock, pkt_len + RX_HWHRD_LEN);
memcpy(skb_inblock->data, data, pkt_len + RX_HWHRD_LEN);
rwnx_rxdataind_aicwf(rx_priv->usbdev->rwnx_hw, skb_inblock, (void *)rx_priv);
}
else { // type : config
aggr_len = pkt_len;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
msg = kmalloc(aggr_len+4, GFP_KERNEL);
if(msg == NULL){
txrx_err("no more space for msg!\n");
aicwf_prealloc_rxbuff_free(buffer, &rx_priv->rxbuff_lock);
return -EBADE;
}
memcpy(msg, data, aggr_len + 4);
if(((*(msg + 2) & 0x7f) == USB_TYPE_CFG_CMD_RSP) && (rx_priv->usbdev->bus_if->state != (int)USB_DOWN_ST))
rwnx_rx_handle_msg(rx_priv->usbdev->rwnx_hw, (struct ipc_e2a_msg *)(msg + 4));
if((*(msg + 2) & 0x7f) == USB_TYPE_CFG_DATA_CFM)
aicwf_usb_host_tx_cfm_handler(&(rx_priv->usbdev->rwnx_hw->usb_env), (u32 *)(msg + 4));
if ((*(msg + 2) & 0x7f) == USB_TYPE_CFG_PRINT)
rwnx_rx_handle_print(rx_priv->usbdev->rwnx_hw, msg + 4, aggr_len);
kfree(msg);
}
aicwf_prealloc_rxbuff_free(buffer, &rx_priv->rxbuff_lock);
if (rx_urb_sched) {
schedule_work(&rx_priv->usbdev->rx_urb_work);
rx_urb_sched = false;
}
atomic_dec(&rx_priv->rx_cnt);
}
}
#else
if(aicwf_usb_rx_aggr){
while (1) {
spin_lock_irqsave(&rx_priv->rxqlock, flags);
if(aicwf_is_framequeue_empty(&rx_priv->rxq)) {
usb_info("no more rxdata\n");
spin_unlock_irqrestore(&rx_priv->rxqlock,flags);
break;
}
skb = aicwf_frame_dequeue(&rx_priv->rxq);
spin_unlock_irqrestore(&rx_priv->rxqlock, flags);
if (skb == NULL) {
txrx_err("skb_error\r\n");
ASSERT_ERR(1);
break;
}
while(aicwf_another_ptk(skb)) {
cnt++;
if (cnt > 30) {
//printk("%s err, break %d\n", __func__, cnt);
//break;
}
data = skb->data;
pkt_len = (*skb->data | (*(skb->data + 1) << 8));
//printk("p:%d, s:%d , %x\n", pkt_len, skb->len, data[2]);
#ifndef CONFIG_USB_RX_REASSEMBLE
if (pkt_len > skb->len) {
AICWFDBG(LOGERROR, "%s pkt_len:%d skb->len:%d\r\n", __func__, pkt_len, skb->len);
//dev_kfree_skb(skb);
//atomic_dec(&rx_priv->rx_cnt);
//continue;
break;
}
#endif
if((skb->data[2] & USB_TYPE_CFG) != USB_TYPE_CFG) { // type : data
aggr_len = pkt_len + RX_HWHRD_LEN;
adjust_len = aggr_len;
skb_inblock = __dev_alloc_skb(aggr_len + CCMP_OR_WEP_INFO, GFP_KERNEL);//8 is for ccmp mic or wep icv
if(skb_inblock == NULL){
txrx_err("no more space! skip!\n");
skb_pull(skb, adjust_len);
continue;
}
skb_put(skb_inblock, aggr_len);
memcpy(skb_inblock->data, data, aggr_len);
rwnx_rxdataind_aicwf(rx_priv->usbdev->rwnx_hw, skb_inblock, (void *)rx_priv);
///TODO: here need to add rx data process
skb_pull(skb, adjust_len);
}
else { // type : config
aggr_len = pkt_len;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
msg = kmalloc(aggr_len+4, GFP_KERNEL);//hdr(4)+config(aggrlen)
if(msg == NULL){
txrx_err("no more space for msg!\n");
aicwf_dev_skb_free(skb);
return -EBADE;
}
memcpy(msg, data, aggr_len + 4);
if(((*(msg + 2) & 0x7f) == USB_TYPE_CFG_CMD_RSP) && (rx_priv->usbdev->bus_if->state != (int)USB_DOWN_ST))
rwnx_rx_handle_msg(rx_priv->usbdev->rwnx_hw, (struct ipc_e2a_msg *)(msg + 4));
if((*(msg + 2) & 0x7f) == USB_TYPE_CFG_DATA_CFM)
aicwf_usb_host_tx_cfm_handler(&(rx_priv->usbdev->rwnx_hw->usb_env), (u32 *)(msg + 4));
if ((*(msg + 2) & 0x7f) == USB_TYPE_CFG_PRINT)
rwnx_rx_handle_print(rx_priv->usbdev->rwnx_hw, msg + 4, aggr_len);
skb_pull(skb, adjust_len+4);
kfree(msg);
}
}
dev_kfree_skb(skb);
atomic_dec(&rx_priv->rx_cnt);
}
}else{
while (1) {
spin_lock_irqsave(&rx_priv->rxqlock, flags);
if(aicwf_is_framequeue_empty(&rx_priv->rxq)) {
usb_info("no more rxdata\n");
spin_unlock_irqrestore(&rx_priv->rxqlock,flags);
break;
}
skb = aicwf_frame_dequeue(&rx_priv->rxq);
spin_unlock_irqrestore(&rx_priv->rxqlock, flags);
if (skb == NULL) {
txrx_err("skb_error\r\n");
ASSERT_ERR(1);
break;
}
data = skb->data;
pkt_len = (*skb->data | (*(skb->data + 1) << 8));
//printk("p:%d, s:%d , %x\n", pkt_len, skb->len, data[2]);
#ifndef CONFIG_USB_RX_REASSEMBLE
if (pkt_len > skb->len) {
AICWFDBG(LOGERROR, "%s pkt_len:%d skb->len:%d\r\n", __func__, pkt_len, skb->len);
dev_kfree_skb(skb);
atomic_dec(&rx_priv->rx_cnt);
continue;
}
#endif
if((skb->data[2] & USB_TYPE_CFG) != USB_TYPE_CFG) { // type : data
rwnx_rxdataind_aicwf(rx_priv->usbdev->rwnx_hw, skb, (void *)rx_priv);
}
else { // type : config
aggr_len = pkt_len;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
msg = kmalloc(aggr_len+4, GFP_KERNEL);
if(msg == NULL){
txrx_err("no more space for msg!\n");
aicwf_dev_skb_free(skb);
return -EBADE;
}
memcpy(msg, data, aggr_len + 4);
if(((*(msg + 2) & 0x7f) == USB_TYPE_CFG_CMD_RSP) && (rx_priv->usbdev->bus_if->state != (int)USB_DOWN_ST))
rwnx_rx_handle_msg(rx_priv->usbdev->rwnx_hw, (struct ipc_e2a_msg *)(msg + 4));
if((*(msg + 2) & 0x7f) == USB_TYPE_CFG_DATA_CFM)
aicwf_usb_host_tx_cfm_handler(&(rx_priv->usbdev->rwnx_hw->usb_env), (u32 *)(msg + 4));
if ((*(msg + 2) & 0x7f) == USB_TYPE_CFG_PRINT)
rwnx_rx_handle_print(rx_priv->usbdev->rwnx_hw, msg + 4, aggr_len);
skb_pull(skb, adjust_len+4);
kfree(msg);
dev_kfree_skb(skb);
}
atomic_dec(&rx_priv->rx_cnt);
}
}
#endif
return ret;
#endif //AICWF_SDIO_SUPPORT
}
#ifdef CONFIG_USB_MSG_IN_EP
int aicwf_process_msg_rxframes(struct aicwf_rx_priv *rx_priv)
{
int ret = 0;
unsigned long flags = 0;
struct sk_buff *skb = NULL; /* Packet for event or data frames */
u16 pkt_len = 0;
struct sk_buff *skb_inblock = NULL;
u16 aggr_len = 0, adjust_len = 0;
u8 *data = NULL;
u8_l *msg = NULL;
while (1) {
spin_lock_irqsave(&rx_priv->msg_rxqlock, flags);
if(aicwf_is_framequeue_empty(&rx_priv->msg_rxq)) {
usb_info("no more rxmsg\n");
spin_unlock_irqrestore(&rx_priv->msg_rxqlock,flags);
break;
}
skb = aicwf_frame_dequeue(&rx_priv->msg_rxq);
spin_unlock_irqrestore(&rx_priv->msg_rxqlock, flags);
if (skb == NULL) {
txrx_err("skb_error\r\n");
break;
}
data = skb->data;
pkt_len = (*skb->data | (*(skb->data + 1) << 8));
//printk("p:%d, s:%d , %x\n", pkt_len, skb->len, data[2]);
#if 0 //amsdu > 1600
if (pkt_len > 1600) {
dev_kfree_skb(skb);
atomic_dec(&rx_priv->rx_cnt);
continue;
}
#endif
if((skb->data[2] & USB_TYPE_CFG) != USB_TYPE_CFG) { // type : data
aggr_len = pkt_len + RX_HWHRD_LEN;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
skb_inblock = __dev_alloc_skb(aggr_len + CCMP_OR_WEP_INFO, GFP_KERNEL);//8 is for ccmp mic or wep icv
if(skb_inblock == NULL){
txrx_err("no more space! skip!\n");
skb_pull(skb, adjust_len);
continue;
}
skb_put(skb_inblock, aggr_len);
memcpy(skb_inblock->data, data, aggr_len);
#if 0//AIDEN
rwnx_frame_parser((char*)__func__, skb_inblock->data + 60, aggr_len - 60);
#endif
rwnx_rxdataind_aicwf(rx_priv->usbdev->rwnx_hw, skb_inblock, (void *)rx_priv);
///TODO: here need to add rx data process
skb_pull(skb, adjust_len);
}
else { // type : config
aggr_len = pkt_len;
if (aggr_len & (RX_ALIGNMENT - 1))
adjust_len = roundup(aggr_len, RX_ALIGNMENT);
else
adjust_len = aggr_len;
msg = kmalloc(aggr_len+4, GFP_KERNEL);
if(msg == NULL){
txrx_err("no more space for msg!\n");
aicwf_dev_skb_free(skb);
return -EBADE;
}
memcpy(msg, data, aggr_len + 4);
if(((*(msg + 2) & 0x7f) == USB_TYPE_CFG_CMD_RSP) && (rx_priv->usbdev->bus_if->state != (int)USB_DOWN_ST))
rwnx_rx_handle_msg(rx_priv->usbdev->rwnx_hw, (struct ipc_e2a_msg *)(msg + 4));
if((*(msg + 2) & 0x7f) == USB_TYPE_CFG_DATA_CFM)
aicwf_usb_host_tx_cfm_handler(&(rx_priv->usbdev->rwnx_hw->usb_env), (u32 *)(msg + 4));
if ((*(msg + 2) & 0x7f) == USB_TYPE_CFG_PRINT)
rwnx_rx_handle_print(rx_priv->usbdev->rwnx_hw, msg + 4, aggr_len);
skb_pull(skb, adjust_len+4);
kfree(msg);
}
dev_kfree_skb(skb);
atomic_dec(&rx_priv->msg_rx_cnt);
}
return ret;
}
#endif
#ifdef AICWF_RX_REORDER
static struct recv_msdu *aicwf_rxframe_queue_init(struct list_head *q, int qsize)
{
int i;
struct recv_msdu *req, *reqs;
reqs = vmalloc(qsize*sizeof(struct recv_msdu));
if (reqs == NULL)
return NULL;
req = reqs;
for (i = 0; i < qsize; i++) {
INIT_LIST_HEAD(&req->rxframe_list);
list_add(&req->rxframe_list, q);
//req->len = 0;
req++;
}
return reqs;
}
#endif
struct aicwf_rx_priv *aicwf_rx_init(void *arg)
{
struct aicwf_rx_priv* rx_priv;
rx_priv = kzalloc(sizeof(struct aicwf_rx_priv), GFP_KERNEL);
if (!rx_priv)
return NULL;
#ifdef AICWF_SDIO_SUPPORT
rx_priv->sdiodev = (struct aic_sdio_dev *)arg;
#else
rx_priv->usbdev = (struct aic_usb_dev *)arg;
#endif
#ifdef CONFIG_PREALLOC_RX_SKB
aicwf_rxframe_queue_init_2(&rx_priv->rxq, MAX_RXQLEN);
#else
aicwf_frame_queue_init(&rx_priv->rxq, 1, MAX_RXQLEN);
#endif
spin_lock_init(&rx_priv->rxqlock);
#ifdef CONFIG_PREALLOC_RX_SKB
spin_lock_init(&rx_priv->rxbuff_lock);
//aicwf_prealloc_init();
#endif
atomic_set(&rx_priv->rx_cnt, 0);
#ifdef CONFIG_USB_MSG_IN_EP
if(rx_priv->usbdev->msg_in_pipe){
aicwf_frame_queue_init(&rx_priv->msg_rxq, 1, MAX_RXQLEN);
spin_lock_init(&rx_priv->msg_rxqlock);
atomic_set(&rx_priv->msg_rx_cnt, 0);
}
#endif
#ifdef AICWF_RX_REORDER
INIT_LIST_HEAD(&rx_priv->rxframes_freequeue);
spin_lock_init(&rx_priv->freeq_lock);
rx_priv->recv_frames = aicwf_rxframe_queue_init(&rx_priv->rxframes_freequeue, MAX_REORD_RXFRAME);
if (!rx_priv->recv_frames) {
txrx_err("no enough buffer for free recv frame queue!\n");
kfree(rx_priv);
return NULL;
}
spin_lock_init(&rx_priv->stas_reord_lock);
INIT_LIST_HEAD(&rx_priv->stas_reord_list);
#endif
return rx_priv;
}
#ifdef AICWF_RX_REORDER
static void aicwf_recvframe_queue_deinit(struct list_head *q)
{
struct recv_msdu *req, *next;
list_for_each_entry_safe(req, next, q, rxframe_list) {
list_del_init(&req->rxframe_list);
}
}
#endif
void aicwf_rx_deinit(struct aicwf_rx_priv* rx_priv)
{
#ifdef AICWF_RX_REORDER
struct reord_ctrl_info *reord_info, *tmp;
AICWFDBG(LOGINFO, "%s Enter\n", __func__);
list_for_each_entry_safe(reord_info, tmp,
&rx_priv->stas_reord_list, list) {
reord_deinit_sta(rx_priv, reord_info);
}
#endif
AICWFDBG(LOGINFO, "stio rx thread\n");
#ifdef AICWF_SDIO_SUPPORT
if (rx_priv->sdiodev->bus_if->busrx_thread) {
complete_all(&rx_priv->sdiodev->bus_if->busrx_trgg);
kthread_stop(rx_priv->sdiodev->bus_if->busrx_thread);
rx_priv->sdiodev->bus_if->busrx_thread = NULL;
}
#endif
#ifdef AICWF_USB_SUPPORT
#ifdef CONFIG_RX_TASKLET//AIDEN tasklet
tasklet_kill(&rx_priv->usbdev->recv_tasklet);
#endif
if (rx_priv->usbdev->bus_if->busrx_thread) {
complete_all(&rx_priv->usbdev->bus_if->busrx_trgg);
kthread_stop(rx_priv->usbdev->bus_if->busrx_thread);
rx_priv->usbdev->bus_if->busrx_thread = NULL;
}
#ifdef CONFIG_USB_MSG_IN_EP
if(rx_priv->usbdev->msg_in_pipe){
if (rx_priv->usbdev->bus_if->msg_busrx_thread) {
complete_all(&rx_priv->usbdev->bus_if->msg_busrx_trgg);
kthread_stop(rx_priv->usbdev->bus_if->msg_busrx_thread);
rx_priv->usbdev->bus_if->msg_busrx_thread = NULL;
}
}
#endif
#ifdef CONFIG_USB_RX_REASSEMBLE
if (rx_priv->rx_reassemble_skb) {
aicwf_dev_skb_free(rx_priv->rx_reassemble_skb);
}
if (rx_priv->rx_msg_reassemble_skb) {
aicwf_dev_skb_free(rx_priv->rx_msg_reassemble_skb);
}
#endif
#endif
#ifdef CONFIG_USB_MSG_IN_EP
if(rx_priv->usbdev->msg_in_pipe){
aicwf_frame_queue_flush(&rx_priv->msg_rxq);
}
#endif
#ifdef CONFIG_PREALLOC_RX_SKB
rxbuff_queue_flush(rx_priv);
#else
aicwf_frame_queue_flush(&rx_priv->rxq);
#endif
#ifdef AICWF_RX_REORDER
aicwf_recvframe_queue_deinit(&rx_priv->rxframes_freequeue);
if (rx_priv->recv_frames)
vfree(rx_priv->recv_frames);
#endif
#ifdef CONFIG_PREALLOC_RX_SKB
//aicwf_prealloc_exit();
#endif
kfree(rx_priv);
AICWFDBG(LOGINFO, "%s Exit\n", __func__);
}
bool aicwf_rxframe_enqueue(struct device *dev, struct frame_queue *q, struct sk_buff *pkt)
{
return aicwf_frame_enq(dev, q, pkt, 0);
}
void aicwf_dev_skb_free(struct sk_buff *skb)
{
if (!skb)
return;
dev_kfree_skb_any(skb);
}
static struct sk_buff *aicwf_frame_queue_penq(struct frame_queue *pq, int prio, struct sk_buff *p)
{
struct sk_buff_head *q;
if (pq->queuelist[prio].qlen >= pq->qmax)
return NULL;
q = &pq->queuelist[prio];
__skb_queue_tail(q, p);
pq->qcnt++;
if (pq->hi_prio < prio)
pq->hi_prio = (u16)prio;
return p;
}
void aicwf_frame_queue_flush(struct frame_queue *pq)
{
int prio;
struct sk_buff_head *q;
struct sk_buff *p, *next;
for (prio = 0; prio < pq->num_prio; prio++)
{
q = &pq->queuelist[prio];
skb_queue_walk_safe(q, p, next) {
skb_unlink(p, q);
aicwf_dev_skb_free(p);
pq->qcnt--;
}
}
}
void aicwf_frame_queue_init(struct frame_queue *pq, int num_prio, int max_len)
{
int prio;
memset(pq, 0, offsetof(struct frame_queue, queuelist) + (sizeof(struct sk_buff_head) * num_prio));
pq->num_prio = (u16)num_prio;
pq->qmax = (u16)max_len;
for (prio = 0; prio < num_prio; prio++) {
skb_queue_head_init(&pq->queuelist[prio]);
}
}
struct sk_buff *aicwf_frame_queue_peek_tail(struct frame_queue *pq, int *prio_out)
{
int prio;
if (pq->qcnt == 0)
return NULL;
for (prio = 0; prio < pq->hi_prio; prio++)
if (!skb_queue_empty(&pq->queuelist[prio]))
break;
if (prio_out)
*prio_out = prio;
return skb_peek_tail(&pq->queuelist[prio]);
}
bool aicwf_is_framequeue_empty(struct frame_queue *pq)
{
int prio, len = 0;
for (prio = 0; prio <= pq->hi_prio; prio++)
len += pq->queuelist[prio].qlen;
if(len > 0)
return false;
else
return true;
}
struct sk_buff *aicwf_frame_dequeue(struct frame_queue *pq)
{
struct sk_buff_head *q;
struct sk_buff *p;
int prio;
if (pq->qcnt == 0)
return NULL;
while ((prio = pq->hi_prio) > 0 && skb_queue_empty(&pq->queuelist[prio]))
pq->hi_prio--;
q = &pq->queuelist[prio];
p = __skb_dequeue(q);
if (p == NULL)
return NULL;
pq->qcnt--;
return p;
}
static struct sk_buff *aicwf_skb_dequeue_tail(struct frame_queue *pq, int prio)
{
struct sk_buff_head *q = &pq->queuelist[prio];
struct sk_buff *p = skb_dequeue_tail(q);
if (!p)
return NULL;
pq->qcnt--;
return p;
}
bool aicwf_frame_enq(struct device *dev, struct frame_queue *q, struct sk_buff *pkt, int prio)
{
struct sk_buff *p = NULL;
int prio_modified = -1;
if (q->queuelist[prio].qlen < q->qmax && q->qcnt < q->qmax) {
aicwf_frame_queue_penq(q, prio, pkt);
return true;
}
if (q->queuelist[prio].qlen >= q->qmax) {
prio_modified = prio;
} else if (q->qcnt >= q->qmax) {
p = aicwf_frame_queue_peek_tail(q, &prio_modified);
if (prio_modified > prio)
return false;
}
if (prio_modified >= 0) {
if (prio_modified == prio)
return false;
p = aicwf_skb_dequeue_tail(q, prio_modified);
aicwf_dev_skb_free(p);
p = aicwf_frame_queue_penq(q, prio_modified, pkt);
if (p == NULL)
txrx_err("failed\n");
}
return p != NULL;
}
#ifdef CONFIG_PREALLOC_RX_SKB
void rxbuff_free(struct rx_buff *rxbuff)
{
kfree(rxbuff->data);
kfree(rxbuff);
}
struct rx_buff *rxbuff_queue_penq(struct rx_frame_queue *pq, struct rx_buff *p)
{
struct list_head *q;
if (pq->qcnt >= pq->qmax)
return NULL;
q = &pq->queuelist;
list_add_tail(&p->queue,q);
pq->qcnt++;
return p;
}
struct rx_buff *rxbuff_dequeue(struct rx_frame_queue *pq)
{
struct rx_buff *p = NULL;
if (pq->qcnt == 0) {
printk("%s %d, rxq is empty\n", __func__, __LINE__);
return NULL;
}
if(list_empty(&pq->queuelist)) {
printk("%s %d, rxq is empty\n", __func__, __LINE__);
return NULL;
} else {
p = list_first_entry(&pq->queuelist, struct rx_buff, queue);
list_del_init(&p->queue);
pq->qcnt--;
}
return p;
}
bool aicwf_rxbuff_enqueue(struct device *dev, struct rx_frame_queue *rxq, struct rx_buff *pkt)
{
// struct rx_buff *p = NULL;
if ((rxq == NULL) || (pkt == NULL)) {
printk("%s %d, rxq or pkt is NULL\n", __func__, __LINE__);
return false;
}
if (rxq->qcnt < rxq->qmax) {
if (rxbuff_queue_penq(rxq, pkt)) {
return true;
} else {
printk("%s %d, rxbuff enqueue fail\n", __func__, __LINE__);
return false;
}
} else {
printk("%s %d, rxq or pkt is full\n", __func__, __LINE__);
return false;
}
}
#endif
Reference in new issue View Git Blame Copy Permalink