// SPDX-License-Identifier: GPL-2.0
|
|
/******************************************************************************
|
*
|
* Copyright (C) 2020 SeekWave Technology Co.,Ltd.
|
*
|
* This program is free software; you can redistribute it and/or modify
|
* it under the terms of version 2 of the GNU General Public License as
|
* published by the Free Software Foundation;
|
*
|
* This program is distributed in the hope that it will be useful,
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
* GNU General Public License for more details.
|
*
|
******************************************************************************/
|
|
#include <linux/kthread.h>
|
#include <linux/ip.h>
|
#include <linux/ctype.h>
|
|
#include "skw_core.h"
|
#include "skw_tx.h"
|
#include "skw_msg.h"
|
#include "skw_iface.h"
|
#include "skw_edma.h"
|
#include "trace.h"
|
|
#define SKW_BASE_VO 16
|
#define SKW_BASE_VI 24
|
#define SKW_TX_TIMEOUT 200
|
#define SKW_TX_RUNING_TIMES 20
|
|
struct skw_tx_info {
|
int quota;
|
bool reset;
|
struct sk_buff_head *list;
|
};
|
|
struct skw_tx_lmac {
|
bool reset;
|
int cred;
|
u16 txq_map;
|
u16 nr_txq;
|
int bk_tx_limit;
|
int current_qlen;
|
int ac_reset;
|
int tx_count_limit;
|
int pending_qlen;
|
|
struct sk_buff_head tx_list;
|
struct skw_tx_info tx[SKW_NR_IFACE];
|
};
|
|
unsigned int tx_wait_time;
|
|
static int skw_tx_time_show(struct seq_file *seq, void *data)
|
{
|
seq_printf(seq, "current tx_wait_time = %dus\n", tx_wait_time);
|
return 0;
|
}
|
|
static int skw_tx_time_open(struct inode *inode, struct file *file)
|
{
|
return single_open(file, skw_tx_time_show, inode->i_private);
|
}
|
|
static ssize_t skw_tx_time_write(struct file *fp, const char __user *buf,
|
size_t len, loff_t *offset)
|
{
|
int i;
|
char cmd[32] = {0};
|
unsigned int res = 0;
|
|
for (i = 0; i < 32; i++) {
|
char c;
|
|
if (get_user(c, buf))
|
return -EFAULT;
|
|
if (c == '\n' || c == '\0')
|
break;
|
|
if (isdigit(c) != 0)
|
cmd[i] = c;
|
else {
|
skw_warn("set fail, not number\n");
|
return -EFAULT;
|
}
|
buf++;
|
}
|
|
if (kstrtouint(cmd, 10, &res))
|
return -EFAULT;
|
|
skw_info("set tx_wait_time = %dus\n", res);
|
tx_wait_time = res;
|
|
return len;
|
}
|
|
#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 6, 0)
|
static const struct proc_ops skw_tx_time_fops = {
|
.proc_open = skw_tx_time_open,
|
.proc_read = seq_read,
|
.proc_release = single_release,
|
.proc_write = skw_tx_time_write,
|
};
|
#else
|
static const struct file_operations skw_tx_time_fops = {
|
.owner = THIS_MODULE,
|
.open = skw_tx_time_open,
|
.read = seq_read,
|
.release = single_release,
|
.write = skw_tx_time_write,
|
};
|
#endif
|
|
#ifdef CONFIG_SWT6621S_SKB_RECYCLE
|
void skw_recycle_skb_free(struct skw_core *skw, struct sk_buff *skb)
|
{
|
if (!skb)
|
return;
|
|
skb->data = skb->head;
|
skb_reset_tail_pointer(skb);
|
|
skb->mac_header = (typeof(skb->mac_header))~0U;
|
skb->transport_header = (typeof(skb->transport_header))~0U;
|
skb->network_header = 0;
|
|
skb->len = 0;
|
|
skb_reserve(skb, NET_SKB_PAD);
|
|
skb_queue_tail(&skw->skb_recycle_qlist, skb);
|
}
|
|
int skw_recycle_skb_copy(struct skw_core *skw, struct sk_buff *skb_recycle, struct sk_buff *skb)
|
{
|
if (!skw || !skb_recycle || !skb)
|
return -1;
|
|
skb_recycle->dev = skb->dev;
|
|
skb_reserve(skb_recycle, skw->skb_headroom);
|
skb_put(skb_recycle, skb->len);
|
memcpy(skb_recycle->data, skb->data, skb->len);
|
|
skb_set_mac_header(skb_recycle, 0);
|
skb_set_network_header(skb_recycle, ETH_HLEN);
|
skb_set_transport_header(skb_recycle, skb->transport_header - skb->mac_header);
|
if (skb->ip_summed == CHECKSUM_PARTIAL) {
|
skb_recycle->ip_summed = skb->ip_summed;
|
skb_recycle->csum_start = skb_headroom(skb_recycle) + skb_checksum_start_offset(skb);
|
}
|
|
return 0;
|
}
|
|
struct sk_buff *skw_recycle_skb_get(struct skw_core *skw)
|
{
|
unsigned long flags;
|
struct sk_buff *skb = NULL;
|
|
spin_lock_irqsave(&skw->skb_recycle_qlist.lock, flags);
|
if (skw->skb_recycle_qlist.qlen > 0) {
|
skb = skb_peek(&skw->skb_recycle_qlist);
|
if (skb)
|
__skb_unlink(skb, &skw->skb_recycle_qlist);
|
}
|
spin_unlock_irqrestore(&skw->skb_recycle_qlist.lock, flags);
|
|
return skb;
|
}
|
#endif
|
|
void skw_skb_kfree(struct skw_core *skw, struct sk_buff *skb)
|
{
|
#ifdef CONFIG_SWT6621S_SKB_RECYCLE
|
if (1 == SKW_SKB_TXCB(skb)->recycle)
|
skw_recycle_skb_free(skw, skb);
|
else
|
dev_kfree_skb_any(skb);
|
#else
|
dev_kfree_skb_any(skb);
|
#endif
|
}
|
|
int skw_pcie_cmd_xmit(struct skw_core *skw, void *data, int data_len)
|
{
|
skw->edma.cmd_chn.hdr[0].data_len = data_len;
|
skw_edma_set_data(priv_to_wiphy(skw), &skw->edma.cmd_chn, data, data_len);
|
|
return skw_edma_tx(priv_to_wiphy(skw), &skw->edma.cmd_chn, data_len);
|
}
|
|
int skw_sdio_cmd_xmit(struct skw_core *skw, void *data, int data_len)
|
{
|
int nr = 0, total_len;
|
|
sg_init_table(skw->sgl_cmd, SKW_NR_SGL_CMD);
|
sg_set_buf(&skw->sgl_cmd[nr++], data, data_len);
|
total_len = data_len;
|
|
skw_set_extra_hdr(skw, skw->eof_blk, skw->hw.cmd_port, skw->hw.align, 0, 1);
|
sg_set_buf(&skw->sgl_cmd[nr++], skw->eof_blk, skw->hw.align);
|
total_len += skw->hw.align;
|
|
if (test_bit(SKW_CMD_FLAG_DISABLE_IRQ, &skw->cmd.flags))
|
return skw->hw.cmd_disable_irq_xmit(skw, NULL, -1,
|
skw->hw.cmd_port, skw->sgl_cmd, nr, total_len);
|
else
|
return skw->hw.cmd_xmit(skw, NULL, -1, skw->hw.cmd_port,
|
skw->sgl_cmd, nr, total_len);
|
}
|
|
int skw_usb_cmd_xmit(struct skw_core *skw, void *data, int data_len)
|
{
|
int nr = 0, total_len;
|
|
sg_init_table(skw->sgl_cmd, SKW_NR_SGL_CMD);
|
sg_set_buf(&skw->sgl_cmd[nr++], data, data_len);
|
total_len = data_len;
|
|
return skw->hw.cmd_xmit(skw, NULL, -1, skw->hw.cmd_port,
|
skw->sgl_cmd, nr, total_len);
|
}
|
|
static int skw_sync_sdma_tx(struct skw_core *skw, struct sk_buff_head *list,
|
int lmac_id, int port, struct scatterlist *sgl,
|
int nents, int tx_len)
|
{
|
int total_len;
|
int ret;
|
struct sk_buff *skb, *tmp;
|
|
if (!skw->hw_pdata || !skw->hw_pdata->hw_sdma_tx)
|
return -EINVAL;
|
|
if (!skw->sdma_buff) {
|
skw_err("invalid buff\n");
|
return -ENOMEM;
|
}
|
|
total_len = sg_copy_to_buffer(sgl, nents, skw->sdma_buff,
|
skw->hw_pdata->max_buffer_size);
|
|
ret = skw->hw_pdata->hw_sdma_tx(port, skw->sdma_buff, total_len);
|
if (ret < 0)
|
skw_err("failed, ret: %d nents:%d\n", ret, nents);
|
|
if (list) {
|
if (likely(0 == ret))
|
skw_sub_credit(skw, lmac_id, skb_queue_len(list));
|
|
skb_queue_walk_safe(list, skb, tmp) {
|
if (likely(0 == ret)) {
|
skb->dev->stats.tx_packets++;
|
skb->dev->stats.tx_bytes += SKW_SKB_TXCB(skb)->skb_native_len;
|
} else
|
skb->dev->stats.tx_errors++;
|
|
__skb_unlink(skb, list);
|
//kfree_skb(skb);
|
skw_skb_kfree(skw, skb);
|
}
|
//skw_sub_credit(skw, lmac_id, skb_queue_len(list));
|
//__skb_queue_purge(list);
|
}
|
|
return ret;
|
}
|
|
static int skw_sync_sdma_cmd_disable_irq_tx(struct skw_core *skw,
|
struct sk_buff_head *list, int lmac_id, int port,
|
struct scatterlist *sgl, int nents, int tx_len)
|
{
|
int total_len;
|
|
if (!skw->hw_pdata || !skw->hw_pdata->suspend_sdma_cmd)
|
return -EINVAL;
|
|
if (!skw->sdma_buff) {
|
skw_err("invalid buff\n");
|
return -ENOMEM;
|
}
|
|
total_len = sg_copy_to_buffer(sgl, nents, skw->sdma_buff,
|
skw->hw_pdata->max_buffer_size);
|
|
return skw->hw_pdata->suspend_sdma_cmd(port, skw->sdma_buff, total_len);
|
}
|
|
|
static int skw_async_sdma_tx(struct skw_core *skw, struct sk_buff_head *list,
|
int lmac_id, int port, struct scatterlist *sgl,
|
int nents, int tx_len)
|
{
|
void *buff;
|
int ret, total_len;
|
struct sk_buff *skb, *tmp;
|
struct skw_sg_node *node;
|
|
if (!skw->hw_pdata || !skw->hw_pdata->hw_sdma_tx_async)
|
return -EINVAL;
|
|
tx_len += sizeof(struct skw_sg_node);
|
|
buff = SKW_ZALLOC(tx_len, GFP_KERNEL);
|
if (!buff) {
|
skw_err("invalid buffer\n");
|
return -ENOMEM;
|
}
|
|
node = (struct skw_sg_node *)buff;
|
buff = (u8 *)buff + sizeof(struct skw_sg_node);
|
|
total_len = sg_copy_to_buffer(sgl, nents, buff, tx_len);
|
node->lmac_id = lmac_id;
|
node->data = (void *)skw;
|
node->nents = nents;
|
ret = skw->hw_pdata->hw_sdma_tx_async(port, buff, total_len);
|
if (ret < 0) {
|
skw_err("failed, ret: %d nents:%d\n", ret, nents);
|
SKW_KFREE(buff);
|
}
|
|
if (list) {
|
if (likely(0 == ret))
|
skw_sub_credit(skw, lmac_id, skb_queue_len(list));
|
|
skb_queue_walk_safe(list, skb, tmp) {
|
if (likely(0 == ret)) {
|
skb->dev->stats.tx_packets++;
|
skb->dev->stats.tx_bytes += SKW_SKB_TXCB(skb)->skb_native_len;
|
} else
|
skb->dev->stats.tx_errors++;
|
|
__skb_unlink(skb, list);
|
//kfree_skb(skb);
|
skw_skb_kfree(skw, skb);
|
}
|
|
//skw_sub_credit(skw, lmac_id, skb_queue_len(list));
|
//__skb_queue_purge(list);
|
}
|
|
return ret;
|
}
|
|
static int skw_sync_adma_tx(struct skw_core *skw, struct sk_buff_head *list,
|
int lmac_id, int port, struct scatterlist *sgl,
|
int nents, int tx_len)
|
{
|
struct sk_buff *skb, *tmp;
|
int ret;
|
unsigned long flags;
|
|
if (!skw->hw_pdata || !skw->hw_pdata->hw_adma_tx)
|
return -EINVAL;
|
|
ret = skw->hw_pdata->hw_adma_tx(port, sgl, nents, tx_len);
|
trace_skw_hw_adma_tx_done(nents);
|
if (ret < 0)
|
skw_err("failed, ret: %d nents:%d\n", ret, nents);
|
|
if (list) {
|
if (likely(0 == ret))
|
skw_sub_credit(skw, lmac_id, skb_queue_len(list));
|
else
|
skb_queue_walk_safe(list, skb, tmp)
|
SKW_SKB_TXCB(skb)->ret = 1;
|
|
spin_lock_irqsave(&skw->kfree_skb_qlist.lock, flags);
|
skb_queue_splice_tail_init(list, &skw->kfree_skb_qlist);
|
spin_unlock_irqrestore(&skw->kfree_skb_qlist.lock, flags);
|
schedule_work(&skw->kfree_skb_task);
|
}
|
|
return ret;
|
}
|
|
static int skw_sync_adma_cmd_disable_irq_tx(struct skw_core *skw,
|
struct sk_buff_head *list, int lmac_id, int port,
|
struct scatterlist *sgl, int nents, int tx_len)
|
{
|
if (!skw->hw_pdata || !skw->hw_pdata->suspend_adma_cmd)
|
return -EINVAL;
|
|
return skw->hw_pdata->suspend_adma_cmd(port, sgl, nents, tx_len);
|
}
|
|
static int skw_async_adma_tx(struct skw_core *skw, struct sk_buff_head *list,
|
int lmac_id, int port, struct scatterlist *sgl,
|
int nents, int tx_len)
|
{
|
int ret, idx;
|
struct sk_buff *skb;
|
struct scatterlist *sg_list, *sg;
|
unsigned long *skb_addr, *sg_addr;
|
|
if (!skw->hw_pdata || !skw->hw_pdata->hw_adma_tx_async)
|
return -EINVAL;
|
|
if (!sgl) {
|
ret = -ENOMEM;
|
skw_err("sgl is NULL\n");
|
goto out;
|
}
|
|
sg_list = kcalloc(SKW_NR_SGL_DAT, sizeof(*sg_list), GFP_KERNEL);
|
|
ret = skw->hw_pdata->hw_adma_tx_async(port, sgl, nents, tx_len);
|
if (unlikely(ret < 0)) {
|
skw_err("failed, ret: %d nents:%d\n", ret, nents);
|
|
for_each_sg(sgl, sg, nents, idx) {
|
sg_addr = (unsigned long *)sg_virt(sg);
|
|
skb_addr = sg_addr - 1;
|
skb = (struct sk_buff *)*skb_addr;
|
|
skb->dev->stats.tx_errors++;
|
//kfree_skb(skb);
|
skw_skb_kfree(skw, skb);
|
}
|
|
SKW_KFREE(sgl);
|
} else {
|
atomic_add(nents, &skw->txqlen_pending);
|
skw_sub_credit(skw, lmac_id, nents);
|
}
|
|
skw->sgl_dat = sg_list;
|
out:
|
return ret;
|
}
|
|
static int skw_async_adma_tx_free(int id, struct scatterlist *sg, int nents,
|
void *data, int status)
|
{
|
struct skw_sg_node node = {0};
|
struct skw_core *skw = data;
|
struct wiphy *wiphy = priv_to_wiphy(skw);
|
|
node.sg = sg;
|
node.nents = nents;
|
node.status = status;
|
|
skw_queue_work(wiphy, NULL, SKW_WORK_TX_FREE, &node, sizeof(node));
|
|
return 0;
|
}
|
|
static int skw_async_sdma_tx_free(int id, void *buffer, int size,
|
void *data, int status)
|
{
|
struct skw_sg_node *node;
|
|
if (unlikely(!buffer)) {
|
skw_err("buffer is invalid\n");
|
return 0;
|
}
|
|
node = (struct skw_sg_node *) ((u8 *)buffer - sizeof(struct skw_sg_node));
|
if (unlikely(status < 0)) {
|
skw_err("failed status:%d %p\n", status, node->data);
|
if (node->data == NULL)
|
skw_err("buffer content was broke is NULL\n");
|
else
|
skw_add_credit((struct skw_core *)node->data, node->lmac_id, node->nents);
|
}
|
|
buffer = (void *) node;
|
SKW_KFREE(buffer);
|
|
return 0;
|
}
|
|
int skw_sdio_xmit(struct skw_core *skw, int lmac_id, struct sk_buff_head *txq)
|
{
|
struct sk_buff *skb, *tmp;
|
struct skw_lmac *lmac = &skw->hw.lmac[lmac_id];
|
|
skb_queue_walk_safe(txq, skb, tmp) {
|
int aligned;
|
struct skw_packet_header *extra_hdr;
|
|
extra_hdr = (void *)skb_push(skb, SKW_EXTER_HDR_SIZE);
|
aligned = round_up(skb->len, skw->hw.align);
|
skw_set_extra_hdr(skw, extra_hdr, lmac->lport, aligned, 0, 0);
|
|
skw_vring_set(skw, skb->data, skb->len, lmac_id);
|
__skb_unlink(skb, txq);
|
skb->dev->stats.tx_packets++;
|
skb->dev->stats.tx_bytes += SKW_SKB_TXCB(skb)->skb_native_len;
|
dev_kfree_skb_any(skb);
|
}
|
schedule_work(&skw->hw.lmac[lmac_id].dy_work);
|
|
return 0;
|
}
|
|
#if 0
|
int skw_sdio_xmit(struct skw_core *skw, int lmac_id, struct sk_buff_head *txq)
|
{
|
struct sk_buff *skb;
|
int nents = 0, tx_bytes = 0;
|
struct skw_lmac *lmac = &skw->hw.lmac[lmac_id];
|
|
sg_init_table(skw->sgl_dat, SKW_NR_SGL_DAT);
|
|
skb_queue_walk(txq, skb) {
|
int aligned;
|
struct skw_packet_header *extra_hdr;
|
|
extra_hdr = (void *)skb_push(skb, SKW_EXTER_HDR_SIZE);
|
|
aligned = round_up(skb->len, skw->hw.align);
|
skw_set_extra_hdr(skw, extra_hdr, lmac->lport, aligned, 0, 0);
|
|
sg_set_buf(&skw->sgl_dat[nents++], skb->data, aligned);
|
|
tx_bytes += aligned;
|
}
|
|
skw_set_extra_hdr(skw, skw->eof_blk, lmac->lport, skw->hw.align, 0, 1);
|
sg_set_buf(&skw->sgl_dat[nents++], skw->eof_blk, skw->hw.align);
|
tx_bytes += skw->hw.align;
|
skw_detail("nents:%d", nents);
|
|
return skw->hw.dat_xmit(skw, txq, lmac_id, lmac->dport,
|
skw->sgl_dat, nents, tx_bytes);
|
}
|
#endif
|
|
int skw_usb_xmit(struct skw_core *skw, int lmac_id, struct sk_buff_head *txq)
|
{
|
struct sk_buff *skb, *tmp;
|
int nents = 0, tx_bytes = 0;
|
unsigned long *skb_addr;
|
struct skw_lmac *lmac = &skw->hw.lmac[lmac_id];
|
|
sg_init_table(skw->sgl_dat, SKW_NR_SGL_DAT);
|
|
skb_queue_walk_safe(txq, skb, tmp) {
|
int aligned;
|
struct skw_packet_header *extra_hdr;
|
|
extra_hdr = (void *)skb_push(skb, SKW_EXTER_HDR_SIZE);
|
|
aligned = round_up(skb->len, skw->hw.align);
|
skw_set_extra_hdr(skw, extra_hdr, lmac->lport, aligned, 0, 0);
|
|
sg_set_buf(&skw->sgl_dat[nents++], skb->data, aligned);
|
|
skb_addr = (unsigned long *)skb_push(skb, sizeof(unsigned long));
|
*skb_addr = (unsigned long)skb;
|
skb_pull(skb, sizeof(unsigned long));
|
|
tx_bytes += aligned;
|
|
if (skw->hw.dma == SKW_ASYNC_ADMA_TX)
|
__skb_unlink(skb, txq);
|
}
|
|
return skw->hw.dat_xmit(skw, txq, lmac_id, lmac->dport,
|
skw->sgl_dat, nents, tx_bytes);
|
}
|
|
int skw_pcie_xmit(struct skw_core *skw, int lmac_id, struct sk_buff_head *txq)
|
{
|
int ret, tx_bytes = 0;
|
unsigned long flags;
|
struct sk_buff *skb;
|
struct skw_lmac *lmac = &skw->hw.lmac[lmac_id];
|
struct wiphy *wiphy = priv_to_wiphy(skw);
|
unsigned long *addr;
|
|
skb_queue_walk(txq, skb) {
|
addr = (unsigned long *)skb_push(skb, sizeof(unsigned long)*2);
|
*addr = (unsigned long)skw->edma.tx_chn[lmac_id].current_node;
|
skb_pull(skb, 2 * sizeof(unsigned long));
|
|
skw_edma_set_data(wiphy, &skw->edma.tx_chn[lmac_id],
|
&SKW_SKB_TXCB(skb)->e,
|
sizeof(SKW_SKB_TXCB(skb)->e));
|
|
tx_bytes += round_up(skb->len, skw->hw.align);
|
addr = (unsigned long *)skb_push(skb, sizeof(unsigned long));
|
*addr = (unsigned long)skb;
|
}
|
|
skb = skb_peek(txq);
|
|
spin_lock_irqsave(&lmac->edma_free_list.lock, flags);
|
skb_queue_splice_tail_init(txq, &lmac->edma_free_list);
|
spin_unlock_irqrestore(&lmac->edma_free_list.lock, flags);
|
|
ret = skw_edma_tx(wiphy, &skw->edma.tx_chn[lmac_id], tx_bytes);
|
if (ret < 0) {
|
skw_err("failed, ret: %d\n", ret);
|
// TODO:
|
// release free list
|
}
|
|
return ret;
|
}
|
|
static inline int skw_bus_data_xmit(struct skw_core *skw, int mac_id,
|
struct sk_buff_head *txq_list)
|
{
|
int ret;
|
|
if (!skb_queue_len(txq_list))
|
return 0;
|
|
skw->tx_packets += skb_queue_len(txq_list);
|
|
skw->dbg.dat_idx = (skw->dbg.dat_idx + 1) % skw->dbg.nr_dat;
|
skw->dbg.dat[skw->dbg.dat_idx].qlen = skb_queue_len(txq_list);
|
skw->dbg.dat[skw->dbg.dat_idx].trigger = skw_local_clock();
|
|
ret = skw->hw.bus_dat_xmit(skw, mac_id, txq_list);
|
|
skw->dbg.dat[skw->dbg.dat_idx].done = skw_local_clock();
|
|
return ret;
|
}
|
|
static inline int skw_bus_cmd_xmit(struct skw_core *skw, void *cmd, int cmd_len)
|
{
|
int ret;
|
unsigned long flags = READ_ONCE(skw->flags);
|
|
if (test_bit(SKW_CMD_FLAG_IGNORE_BLOCK_TX, &skw->cmd.flags))
|
clear_bit(SKW_FLAG_BLOCK_TX, &flags);
|
|
if (!skw_cmd_tx_allowed(flags)) {
|
skw_warn("cmd: %s[%d] not allowed, flags: 0x%lx, cmd flags: 0x%lx\n",
|
skw->cmd.name, skw->cmd.id, skw->flags, skw->cmd.flags);
|
|
skw_abort_cmd(skw);
|
|
return 0;
|
}
|
|
skw->dbg.cmd[skw->dbg.cmd_idx].xmit = skw_local_clock();
|
skw->dbg.cmd[skw->dbg.cmd_idx].loop = atomic_read(&skw->dbg.loop);
|
|
ret = skw->hw.bus_cmd_xmit(skw, cmd, cmd_len);
|
|
skw->dbg.cmd[skw->dbg.cmd_idx].done = skw_local_clock();
|
|
return ret;
|
}
|
|
static inline bool is_skw_same_tcp_stream(struct sk_buff *skb,
|
struct sk_buff *next)
|
{
|
return ip_hdr(skb)->saddr == ip_hdr(next)->saddr &&
|
ip_hdr(skb)->daddr == ip_hdr(next)->daddr &&
|
tcp_hdr(skb)->source == tcp_hdr(next)->source &&
|
tcp_hdr(skb)->dest == tcp_hdr(next)->dest;
|
}
|
|
static void skw_merge_pure_ack(struct skw_core *skw, struct sk_buff_head *ackq,
|
struct sk_buff_head *txq)
|
{
|
int i, drop = 0;
|
struct sk_buff *skb, *tmp;
|
|
while ((skb = __skb_dequeue_tail(ackq))) {
|
for (i = 0; i < ackq->qlen; i++) {
|
tmp = __skb_dequeue(ackq);
|
if (!tmp)
|
break;
|
|
if (is_skw_same_tcp_stream(skb, tmp)) {
|
if (tcp_optlen(tmp) == 0 &&
|
tcp_flag_word(tcp_hdr(tmp)) == TCP_FLAG_ACK) {
|
skw_skb_kfree(skw, tmp);
|
drop++;
|
} else {
|
__skb_queue_tail(txq, tmp);
|
}
|
} else {
|
__skb_queue_tail(ackq, tmp);
|
}
|
}
|
|
__skb_queue_tail(txq, skb);
|
}
|
}
|
|
static bool is_skw_peer_data_valid(struct skw_core *skw, struct sk_buff *skb)
|
{
|
struct skw_ctx_entry *entry;
|
bool valid = true;
|
int peer_idx = SKW_SKB_TXCB(skb)->peer_idx;
|
int i;
|
|
rcu_read_lock();
|
for (i = 0; i < skw->hw.nr_lmac; i++) {
|
entry = rcu_dereference(skw->hw.lmac[i].peer_ctx[peer_idx].entry);
|
if (entry) {
|
if (entry->peer) {
|
entry->peer->tx.bytes += skb->len;
|
entry->peer->tx.pkts++;
|
|
if (entry->peer->flags & SKW_PEER_FLAG_DEAUTHED)
|
valid = false;
|
} else {
|
if (is_unicast_ether_addr(eth_hdr(skb)->h_dest))
|
valid = false;
|
}
|
}
|
}
|
rcu_read_unlock();
|
|
return valid;
|
}
|
|
static inline void skw_reset_ac(struct skw_tx_lmac *txlp)
|
{
|
int i;
|
|
for (i = 0; i < SKW_MAX_LMAC_SUPPORT; i++) {
|
txlp->ac_reset = 0xF;
|
txlp++;
|
}
|
}
|
|
bool skw_check_txq(struct skw_core *skw)
|
{
|
bool ret;
|
int ac, i;
|
unsigned long flags;
|
struct skw_iface *iface;
|
|
ret = false;
|
for (ac = 0; ac < SKW_WMM_AC_MAX; ac++) {
|
for (i = 0; i < SKW_NR_IFACE; i++) {
|
iface = skw->vif.iface[i];
|
if (!iface || !iface->ndev)
|
continue;
|
|
if (skb_queue_len(&iface->tx_cache[ac]) != 0) {
|
ret = true;
|
goto _exit;
|
}
|
|
spin_lock_irqsave(&iface->txq[ac].lock, flags);
|
if (skb_queue_len(&iface->txq[ac]) != 0) {
|
ret = true;
|
spin_unlock_irqrestore(&iface->txq[ac].lock, flags);
|
goto _exit;
|
|
}
|
spin_unlock_irqrestore(&iface->txq[ac].lock, flags);
|
}
|
}
|
|
_exit:
|
return ret;
|
}
|
|
bool skw_tx_exit_check(struct skw_core *skw, struct skw_tx_lmac *txlp)
|
{
|
bool ret;
|
int mac, credit = 0, pending_qlen = 0;
|
|
ret = true;
|
|
if (skw->hw.bus == SKW_BUS_USB || skw->hw.bus == SKW_BUS_USB2) {
|
for (mac = 0; mac < skw->hw.nr_lmac; mac++) {
|
if (skw_lmac_is_actived(skw, mac)) {
|
credit = skw_get_hw_credit(skw, mac);
|
if (credit > 0 && txlp[mac].pending_qlen > 0)
|
ret = false;
|
}
|
trace_skw_tx_exit_check(mac, credit, txlp[mac].pending_qlen);
|
}
|
}
|
|
if (skw->hw.bus == SKW_BUS_SDIO || skw->hw.bus == SKW_BUS_SDIO2) {
|
for (mac = 0; mac < skw->hw.nr_lmac; mac++) {
|
pending_qlen += txlp[mac].pending_qlen;
|
}
|
|
if (pending_qlen == 0) {
|
if (skw_check_txq(skw))
|
ret = false;
|
} else
|
ret = false;
|
|
if (msecs_to_jiffies(100) > (jiffies - skw->trans_start))
|
ret = true;
|
}
|
|
if (test_bit(SKW_CMD_FLAG_XMIT, &skw->cmd.flags))
|
ret = false;
|
|
return ret;
|
}
|
|
int skw_tx_running_check(struct skw_core *skw, int times)
|
{
|
int all_credit, mac, ret;
|
|
ret = SKW_TX_RUNNING_EXIT;
|
|
if (!skw_tx_allowed(READ_ONCE(skw->flags)))
|
return ret;
|
|
all_credit = 0;
|
for (mac = 0; mac < skw->hw.nr_lmac; mac++)
|
if (skw_lmac_is_actived(skw, mac))
|
all_credit += skw_get_hw_credit(skw, mac);
|
|
if (all_credit == 0) {
|
if (skw->hw.bus == SKW_BUS_SDIO || skw->hw.bus == SKW_BUS_SDIO2) {
|
if (times < tx_wait_time)
|
ret = SKW_TX_RUNNING_RESTART;
|
}
|
} else
|
ret = SKW_TX_RUNNING_GO;
|
|
return ret;
|
}
|
|
void skw_cmd_do(struct skw_core *skw)
|
{
|
if (test_and_clear_bit(SKW_CMD_FLAG_XMIT, &skw->cmd.flags)) {
|
skw_bus_cmd_xmit(skw, skw->cmd.data, skw->cmd.data_len);
|
|
if (test_bit(SKW_CMD_FLAG_NO_ACK, &skw->cmd.flags)) {
|
set_bit(SKW_CMD_FLAG_DONE, &skw->cmd.flags);
|
skw->cmd.callback(skw);
|
}
|
}
|
}
|
|
int skw_txq_prepare(struct skw_core *skw, struct sk_buff_head *pure_ack_list, struct skw_tx_lmac *txl, int ac)
|
{
|
int i, qlen, ac_qlen = 0;
|
unsigned long flags;
|
struct sk_buff *skb;
|
struct skw_iface *iface;
|
struct sk_buff_head *qlist;
|
struct netdev_queue *txq;
|
struct skw_tx_lmac *txlp;
|
|
for (i = 0; i < SKW_NR_IFACE; i++) {
|
iface = skw->vif.iface[i];
|
// if (!iface || skw_lmac_is_actived(skw, iface->lmac_id))
|
if (!iface || !iface->ndev)
|
continue;
|
|
if (ac == SKW_WMM_AC_BE && iface->txq[SKW_ACK_TXQ].qlen) {
|
qlist = &iface->txq[SKW_ACK_TXQ];
|
|
__skb_queue_head_init(pure_ack_list);
|
|
spin_lock_irqsave(&qlist->lock, flags);
|
skb_queue_splice_tail_init(&iface->txq[SKW_ACK_TXQ],
|
pure_ack_list);
|
spin_unlock_irqrestore(&qlist->lock, flags);
|
|
skw_merge_pure_ack(skw, pure_ack_list,
|
&iface->tx_cache[ac]);
|
}
|
|
qlist = &iface->txq[ac];
|
if (!skb_queue_empty(qlist)) {
|
spin_lock_irqsave(&qlist->lock, flags);
|
skb_queue_splice_tail_init(qlist,
|
&iface->tx_cache[ac]);
|
spin_unlock_irqrestore(&qlist->lock, flags);
|
}
|
|
if (READ_ONCE(iface->flags) & SKW_IFACE_FLAG_DEAUTH) {
|
while ((skb = __skb_dequeue(&iface->tx_cache[ac])) != NULL)
|
skw_skb_kfree(skw, skb);
|
}
|
|
qlen = skb_queue_len(&iface->tx_cache[ac]);
|
if (qlen < SKW_TXQ_LOW_THRESHOLD) {
|
txq = netdev_get_tx_queue(iface->ndev, ac);
|
if (netif_tx_queue_stopped(txq)) {
|
netif_tx_start_queue(txq);
|
netif_schedule_queue(txq);
|
}
|
}
|
|
if (qlen) {
|
txlp = &txl[iface->lmac_id];
|
txlp->current_qlen += qlen;
|
|
trace_skw_txq_prepare(iface->ndev->name, qlen);
|
|
txlp->txq_map |= BIT(txlp->nr_txq);
|
|
txlp->tx[txlp->nr_txq].list = &iface->tx_cache[ac];
|
txlp->tx[txlp->nr_txq].reset = true;
|
txlp->reset = true;
|
if (txlp->ac_reset & BIT(ac)) {
|
txlp->tx[txlp->nr_txq].quota = iface->wmm.factor[ac];
|
txlp->ac_reset ^= BIT(ac);
|
}
|
|
txlp->nr_txq++;
|
ac_qlen += qlen;
|
}
|
}
|
|
return ac_qlen;
|
}
|
|
static inline bool skw_vring_check(struct skw_core *skw, struct sk_buff_head *qlist, struct skw_tx_vring *tx_vring)
|
{
|
if (skw->hw.bus == SKW_BUS_SDIO || skw->hw.bus == SKW_BUS_SDIO2) {
|
if (skb_queue_len(qlist) + 1 >= skw_vring_available_count(tx_vring))
|
return true;
|
}
|
|
return false;
|
}
|
|
#ifdef CONFIG_SWT6621S_TX_WORKQUEUE
|
void skw_tx_worker(struct work_struct *work)
|
{
|
int i, ac, mac;
|
int base = 0;
|
//unsigned long flags;
|
int lmac_tx_capa;
|
//int qlen, pending_qlen = 0;
|
int pending_qlen = 0;
|
int max_tx_count_limit = 0;
|
struct sk_buff *skb;
|
//struct skw_iface *iface;
|
//struct sk_buff_head *qlist;
|
struct skw_tx_lmac txl[SKW_MAX_LMAC_SUPPORT];
|
struct skw_tx_lmac *txlp;
|
struct sk_buff_head pure_ack_list;
|
//int xmit_tx_flag;
|
struct skw_core *skw = container_of(to_delayed_work(work), struct skw_core, tx_worker);
|
int times = 0, ret;
|
|
start:
|
|
memset(txl, 0, sizeof(txl));
|
skw_reset_ac(txl);
|
|
max_tx_count_limit = skw->hw.pkt_limit;
|
|
/* reserve one for eof block */
|
if (skw->hw.bus == SKW_BUS_SDIO)
|
max_tx_count_limit--;
|
|
for (i = 0; i < skw->hw.nr_lmac; i++) {
|
__skb_queue_head_init(&txl[i].tx_list);
|
txl[i].tx_count_limit = max_tx_count_limit;
|
}
|
|
while (!atomic_read(&skw->exit)) {
|
|
// TODO:
|
/* CPU bind */
|
/* check if frame in pending queue is timeout */
|
|
atomic_inc(&skw->dbg.loop);
|
skw_cmd_do(skw);
|
|
pending_qlen = 0;
|
ret = skw_tx_running_check(skw, times++);
|
if (ret == SKW_TX_RUNNING_RESTART)
|
goto start;
|
else if (ret == SKW_TX_RUNNING_EXIT) {
|
if (skw->hw.bus == SKW_BUS_PCIE)
|
skw_wakeup_tx(skw, 0);
|
|
return;
|
}
|
|
for (ac = 0; ac < SKW_WMM_AC_MAX; ac++) {
|
int ac_qlen = 0;
|
|
ac_qlen = skw_txq_prepare(skw, &pure_ack_list, txl, ac);
|
|
if (!ac_qlen)
|
continue;
|
|
pending_qlen += ac_qlen;
|
|
lmac_tx_capa = 0;
|
|
for (mac = 0; mac < skw->hw.nr_lmac; mac++) {
|
int credit;
|
|
txlp = &txl[mac];
|
if (!txlp->txq_map)
|
goto reset;
|
|
credit = txlp->cred = skw_get_hw_credit(skw, mac);
|
trace_skw_get_credit(credit, mac);
|
if (!txlp->cred)
|
goto reset;
|
|
if (txlp->reset) {
|
switch (ac) {
|
case SKW_WMM_AC_VO:
|
base = SKW_BASE_VO;
|
break;
|
|
case SKW_WMM_AC_VI:
|
base = SKW_BASE_VI;
|
break;
|
|
case SKW_WMM_AC_BK:
|
if (txlp->bk_tx_limit) {
|
base = min(txlp->cred, txlp->bk_tx_limit);
|
txlp->bk_tx_limit = 0;
|
} else {
|
base = txlp->cred;
|
}
|
|
base = base / txlp->nr_txq;
|
break;
|
|
default:
|
base = min(txlp->cred, txlp->current_qlen);
|
//base = base / txlp->nr_txq;
|
txlp->bk_tx_limit = (txlp->cred + 1) >> 1;
|
break;
|
}
|
|
base = base ? base : 1;
|
txlp->reset = false;
|
} else
|
base = 0;
|
|
trace_skw_txlp_mac(mac, txlp->cred, txlp->current_qlen, base);
|
|
for (i = 0; txlp->txq_map != 0; i++) {
|
|
i = i % txlp->nr_txq;
|
if (!(txlp->txq_map & BIT(i)))
|
continue;
|
|
if (skw_vring_check(skw, &txlp->tx_list, skw->hw.lmac[mac].tx_vring))
|
break;
|
|
if (!txlp->cred)
|
break;
|
|
if (txlp->tx[i].reset) {
|
txlp->tx[i].quota += base;
|
|
if (txlp->tx[i].quota < 0)
|
txlp->tx[i].quota = 0;
|
|
txlp->tx[i].reset = false;
|
}
|
|
skb = skb_peek(txlp->tx[i].list);
|
if (!skb) {
|
txlp->txq_map ^= BIT(i);
|
continue;
|
}
|
|
if (!is_skw_peer_data_valid(skw, skb)) {
|
|
skw_detail("drop dest: %pM\n",
|
eth_hdr(skb)->h_dest);
|
|
__skb_unlink(skb, txlp->tx[i].list);
|
skw_skb_kfree(skw, skb);
|
|
continue;
|
}
|
|
if (!txlp->tx_count_limit--)
|
break;
|
|
if (txlp->tx[i].quota) {
|
txlp->tx[i].quota--;
|
} else {
|
txlp->txq_map ^= BIT(i);
|
continue;
|
}
|
|
__skb_unlink(skb, txlp->tx[i].list);
|
__skb_queue_tail(&txlp->tx_list, skb);
|
|
trace_skw_txlp_tx_list(skb->dev->name, ((struct skw_iface *)(netdev_priv(skb->dev)))->id);
|
|
txlp->cred--;
|
}
|
|
pending_qlen = pending_qlen - credit + txlp->cred;
|
//txlp->pending_qlen = pending_qlen;
|
txlp->pending_qlen = txlp->current_qlen - credit + txlp->cred;
|
|
trace_skw_tx_info(mac, ac, credit, credit - txlp->cred, txlp->current_qlen, pending_qlen);
|
|
if (skw_bus_data_xmit(skw, mac, &txlp->tx_list) >= 0)
|
skw->trans_start = jiffies;
|
|
txlp->tx_count_limit = max_tx_count_limit;
|
|
if (txlp->cred)
|
lmac_tx_capa |= BIT(mac);
|
|
reset:
|
txlp->nr_txq = 0;
|
txlp->txq_map = 0;
|
txlp->current_qlen = 0;
|
}
|
|
if (!lmac_tx_capa)
|
break;
|
}
|
|
if (ac == SKW_WMM_AC_MAX)
|
skw_reset_ac(txl);
|
|
if (skw_tx_exit_check(skw, txl)) {
|
skw_start_dev_queue(skw);
|
return;
|
}
|
}
|
}
|
|
static void skw_kfree_skb_worker(struct work_struct *work)
|
{
|
unsigned long flags;
|
struct sk_buff *skb, *tmp;
|
struct sk_buff_head qlist;
|
struct skw_core *skw = container_of(work, struct skw_core, kfree_skb_task);
|
|
__skb_queue_head_init(&qlist);
|
|
while (!skb_queue_empty(&skw->kfree_skb_qlist)) {
|
spin_lock_irqsave(&skw->kfree_skb_qlist.lock, flags);
|
skb_queue_splice_tail_init(&skw->kfree_skb_qlist, &qlist);
|
spin_unlock_irqrestore(&skw->kfree_skb_qlist.lock, flags);
|
|
skb_queue_walk_safe(&qlist, skb, tmp) {
|
if (likely(0 == SKW_SKB_TXCB(skb)->ret)) {
|
skb->dev->stats.tx_packets++;
|
skb->dev->stats.tx_bytes += SKW_SKB_TXCB(skb)->skb_native_len;
|
} else
|
skb->dev->stats.tx_errors++;
|
|
__skb_unlink(skb, &qlist);
|
skw_skb_kfree(skw, skb);
|
}
|
}
|
}
|
|
void skw_dump_vring(struct vring *vr)
|
{
|
int i;
|
u16 avail_idx = vr->avail->idx;
|
u16 used_idx = vr->used->idx;
|
u16 pending, available;
|
|
available = vr->num - (avail_idx - used_idx);
|
pending = (avail_idx - used_idx) & (vr->num - 1);
|
|
skw_dbg("===== VRING DUMP:=====\n");
|
skw_dbg(" Queue Size: %u\n", vr->num);
|
skw_dbg(" avail->idx: %u, used->idx: %u\n",
|
avail_idx, used_idx);
|
skw_dbg(" Available descriptors: %u\n",
|
available);
|
skw_dbg(" Descriptors pending hardware processing: %u\n", pending);
|
|
// 打印描述符表
|
skw_dbg(" Descriptor Table:\n");
|
for (i = 0; i < vr->num; i++) {
|
struct vring_desc *desc = &vr->desc[i];
|
skw_dbg(" Desc[%03u]: addr=0x%016llx, len=%u, flags=0x%04x, next=%u\n",
|
i, (unsigned long long)desc->addr, desc->len, desc->flags, desc->next);
|
skw_hex_dump("desc addr", phys_to_virt(desc->addr), desc->len, true);
|
}
|
|
// 打印可用环
|
skw_dbg(" Available Ring (avail->idx=%u):\n", avail_idx);
|
for (i = 0; i < vr->num; i++) {
|
unsigned int idx = i % vr->num;
|
skw_dbg(" Avail[%03u]: desc_idx=%u\n", i, vr->avail->ring[idx]);
|
}
|
|
// 打印已用环
|
skw_dbg(" Used Ring (used->idx=%u):\n", used_idx);
|
for (i = 0; i < vr->num; i++) {
|
unsigned int idx = i % vr->num;
|
skw_dbg(" Used[%03u]: id=%u, len=%u\n",
|
i, vr->used->ring[idx].id, vr->used->ring[idx].len);
|
}
|
}
|
|
u16 skw_vring_available_count(struct skw_tx_vring *tx_vring)
|
{
|
struct vring *vr = &tx_vring->vr;
|
u16 avail_idx;
|
u16 used_idx;
|
|
spin_lock(&tx_vring->lock);
|
avail_idx = vr->avail->idx;
|
used_idx = vr->used->idx;
|
spin_unlock(&tx_vring->lock);
|
|
// 可用描述符数量 = 总容量 - (avail_idx - used_idx)
|
// 其中 (avail_idx - used_idx) 是已分配但未被设备处理的描述符数量
|
return vr->num - ((avail_idx - used_idx) & (vr->num - 1));
|
}
|
|
u16 skw_vring_pending_count(struct skw_tx_vring *tx_vring)
|
{
|
u16 avail_idx, used_idx;
|
struct vring *vr;
|
|
vr = &tx_vring->vr;
|
|
spin_lock(&tx_vring->lock);
|
avail_idx = vr->avail->idx;
|
used_idx = vr->used->idx;
|
spin_unlock(&tx_vring->lock);
|
|
|
// 利用位运算处理回绕（要求vr->num是2的幂）
|
return (avail_idx - used_idx) & (vr->num - 1);
|
}
|
|
bool tx_vring_is_full(struct skw_tx_vring *tx_vring)
|
{
|
struct vring *vr = &tx_vring->vr;
|
u16 avail_idx, used_idx;
|
u16 pending;
|
|
//spin_lock(&tx_vring->lock);
|
avail_idx = vr->avail->idx;
|
used_idx = vr->used->idx;
|
//spin_unlock(&tx_vring->lock);
|
|
// 计算待处理的描述符数量
|
pending = (avail_idx - used_idx) & (vr->num - 1);
|
|
// 当待处理数量达到队列大小时，表示队列已满
|
return pending >= (vr->num - 1);
|
}
|
|
bool vring_has_pending(struct skw_tx_vring *tx_vring)
|
{
|
struct vring *vr;
|
u16 avail_idx, used_idx;
|
|
vr = &tx_vring->vr;
|
|
spin_lock(&tx_vring->lock);
|
|
avail_idx = vr->avail->idx;
|
used_idx = vr->used->idx;
|
|
spin_unlock(&tx_vring->lock);
|
|
// 计算待处理描述符数量（考虑回绕）
|
return ((avail_idx - used_idx) & (vr->num - 1)) != 0;
|
}
|
|
int skw_vring_set(struct skw_core *skw, void *data, u16 len, int lmac_id)
|
{
|
u16 desc_idx, aligned;
|
struct vring *vr;
|
struct skw_tx_vring *tx_vring;
|
|
tx_vring = skw->hw.lmac[lmac_id].tx_vring;
|
vr = &tx_vring->vr;
|
|
// 1. 先获取锁，再检查队列状态
|
spin_lock(&tx_vring->lock);
|
smp_rmb();
|
|
if (tx_vring_is_full(tx_vring)) {
|
spin_unlock(&tx_vring->lock);
|
//skw_dbg("tx_vring_is_full\n");
|
return -EINVAL;
|
}
|
|
// 2. 获取当前可用描述符索引（正确方式）
|
desc_idx = vr->avail->idx & (vr->num - 1);
|
|
// 3. 配置描述符指向数据
|
//skw_sdio_hdr_set(skw, skb);
|
aligned = round_up(len, skw->hw.align);
|
memcpy(phys_to_virt(vr->desc[desc_idx].addr), data, aligned);
|
vr->desc[desc_idx].len = aligned;
|
vr->desc[desc_idx].flags = VRING_DESC_F_WRITE;
|
|
// 4. 更新可用环索引
|
vr->avail->ring[desc_idx] = desc_idx;
|
|
// 6. 使用内存屏障确保描述符更新先于索引更新
|
smp_wmb();
|
vr->avail->idx += 1;
|
|
//atomic_inc(&tx_vring->set);
|
|
spin_unlock(&tx_vring->lock);
|
|
return 0;
|
}
|
|
struct skw_tx_vring *skw_tx_vring_init(void)
|
{
|
int i;
|
struct skw_tx_vring *tx_vring;
|
void *queue_mem;
|
|
tx_vring = kcalloc(1, sizeof(struct skw_tx_vring), GFP_KERNEL);
|
if (!tx_vring) {
|
skw_dbg("alloc tx_vring fail\n");
|
goto __1;
|
}
|
|
tx_vring->page = alloc_pages(GFP_KERNEL | GFP_DMA, get_order(SKW_TX_PACK_SIZE*SKW_TX_VRING_SIZE));
|
if (!tx_vring->page) {
|
skw_dbg("alloc page fail\n");
|
goto __2;
|
}
|
|
tx_vring->sgl_dat = kcalloc(SKW_TX_VRING_SIZE, sizeof(struct scatterlist), GFP_KERNEL);
|
if (!tx_vring->sgl_dat) {
|
skw_dbg("alloc sgl_dat fail\n");
|
goto __3;
|
}
|
sg_init_table(tx_vring->sgl_dat, SKW_TX_VRING_SIZE);
|
tx_vring->tx_bytes = 0;
|
spin_lock_init(&tx_vring->lock);
|
|
skw_dbg("size:%d\n", vring_size(SKW_TX_VRING_SIZE, PAGE_SIZE));
|
queue_mem = kzalloc(vring_size(SKW_TX_VRING_SIZE, PAGE_SIZE), GFP_KERNEL);
|
if (!queue_mem) {
|
skw_dbg("Failed to allocate vring memory\n");
|
goto __4;
|
}
|
skw_dbg("queue_mem:%p\n", queue_mem);
|
|
vring_init(&tx_vring->vr, SKW_TX_VRING_SIZE, queue_mem, PAGE_SIZE);
|
for (i = 0; i < SKW_TX_VRING_SIZE; i++) {
|
tx_vring->vr.avail->ring[i] = i; // 将描述符索引 0~num-1 依次放入可用环
|
tx_vring->vr.desc[i].addr = virt_to_phys(page_address(tx_vring->page)) + i * SKW_TX_PACK_SIZE;
|
}
|
tx_vring->queue_mem = queue_mem; // 记录指针以便后续释放
|
|
atomic_set(&tx_vring->set, 0);
|
atomic_set(&tx_vring->write, 0);
|
|
return tx_vring;
|
|
__4:
|
kfree(tx_vring->sgl_dat);
|
__3:
|
__free_pages(tx_vring->page, get_order(SKW_TX_PACK_SIZE*SKW_TX_VRING_SIZE));
|
__2:
|
kfree(tx_vring);
|
__1:
|
return NULL;
|
}
|
|
void skw_tx_vring_deinit(struct skw_tx_vring *tx_vring)
|
{
|
if (tx_vring)
|
return ;
|
if (tx_vring->page)
|
__free_pages(tx_vring->page, get_order(SKW_TX_PACK_SIZE*SKW_TX_VRING_SIZE));
|
if (tx_vring->sgl_dat)
|
kfree(tx_vring->sgl_dat);
|
|
if (tx_vring->queue_mem)
|
kfree(tx_vring->queue_mem);
|
|
kfree(tx_vring);
|
|
return ;
|
}
|
|
int skw_sdio_write(struct skw_core *skw, struct skw_tx_vring *tx_vring, u16 limit, int lmac_id)
|
{
|
struct vring *vr = &tx_vring->vr;
|
//u16 avail_idx;
|
//u16 used_idx; // 使用vr->used->idx而不是last_used_idx
|
u16 desc_idx, data_len, used_ring_idx, used_idx, avail_idx;
|
u16 nents;
|
u32 tx_bytes;
|
int ret;
|
|
// 检查是否有新的可用描述符
|
if (!vring_has_pending(tx_vring)) {
|
skw_dbg("no avail data vr->avail->idx:%d vr->used->idx:%d\n", vr->avail->idx, vr->used->idx);
|
return -EINVAL;
|
}
|
|
spin_lock(&tx_vring->lock);
|
avail_idx = vr->avail->idx;
|
used_idx = vr->used->idx;
|
|
// 处理所有待处理的描述符（从used_idx到avail_idx-1）
|
sg_init_table(tx_vring->sgl_dat, SKW_TX_VRING_SIZE);
|
nents = 0;
|
tx_bytes = 0;
|
while ((used_idx & (vr->num - 1)) != (avail_idx & (vr->num - 1))) {
|
if (nents == limit)
|
break;
|
// 获取当前待处理的描述符索引
|
desc_idx = vr->avail->ring[used_idx % vr->num];
|
|
// 从描述符读取数据地址和长度
|
data_len = vr->desc[desc_idx].len;
|
|
// 记录调试信息
|
//printk("Processing descriptor %u: len=%u\n", desc_idx, data_len);
|
|
// 将结果添加到已用环
|
used_ring_idx = used_idx % vr->num;
|
vr->used->ring[used_ring_idx].id = desc_idx;
|
vr->used->ring[used_ring_idx].len = data_len;
|
|
sg_set_buf(&tx_vring->sgl_dat[nents++], phys_to_virt(vr->desc[desc_idx].addr), vr->desc[desc_idx].len);
|
tx_bytes += vr->desc[desc_idx].len;
|
|
// 清理已处理描述符的状态
|
vr->desc[desc_idx].flags &= ~VRING_DESC_F_WRITE;
|
vr->desc[desc_idx].len = 0;
|
|
// 移动到下一个待处理的描述符
|
used_idx++;
|
|
//atomic_inc(&tx_vring->write);
|
}
|
spin_unlock(&tx_vring->lock);
|
|
skw_set_extra_hdr(skw, skw->eof_blk, skw->hw.lmac[lmac_id].lport, skw->hw.align, 0, 1);
|
sg_set_buf(&tx_vring->sgl_dat[nents++], skw->eof_blk, skw->hw.align);
|
tx_bytes += 512;
|
|
ret = skw->hw_pdata->hw_adma_tx(skw->hw.lmac[lmac_id].dport, tx_vring->sgl_dat, nents, tx_bytes);
|
if (ret)
|
skw_err("hw_adma_tx fail ret:%d nents:%d tx_bytes:%d\n", ret, nents, tx_bytes);
|
|
// 更新已用环索引
|
spin_lock(&tx_vring->lock);
|
smp_wmb();
|
vr->used->idx = used_idx;
|
spin_unlock(&tx_vring->lock);
|
|
return ret;
|
}
|
|
void skw_dy_worker(struct work_struct *work)
|
{
|
int credit;
|
int pending, limit;
|
//int nents;
|
struct skw_tx_vring *tx_vring;
|
//struct skw_core *skw = container_of(work, struct skw_core, dy_work);
|
struct skw_lmac *lmac = container_of(work, struct skw_lmac, dy_work);
|
struct skw_core *skw = lmac->skw;
|
|
//tx_vring = skw->tx_vring;
|
tx_vring = lmac->tx_vring;
|
while (!atomic_read(&skw->exit)) {
|
credit = skw_get_hw_credit(skw, lmac->id);
|
if (credit == 0)
|
break;
|
|
//spin_lock(&tx_vring->lock);
|
pending = skw_vring_pending_count(tx_vring);
|
//spin_unlock(&tx_vring->lock);
|
|
if (pending == 0)
|
break;
|
|
limit = min(pending, credit);
|
|
if (skw_sdio_write(skw, tx_vring, limit, lmac->id) == 0)
|
skw_sub_credit(skw, lmac->id, limit);
|
}
|
}
|
|
static int __skw_tx_init(struct skw_core *skw)
|
{
|
//struct workqueue_attrs wq_attrs;
|
int j;
|
#ifdef CONFIG_SWT6621S_SKB_RECYCLE
|
int i;
|
struct sk_buff *skb;
|
#endif
|
|
skw->tx_wq = alloc_workqueue("skw_txwq.%d",
|
WQ_UNBOUND | WQ_CPU_INTENSIVE | WQ_HIGHPRI | WQ_SYSFS,
|
0, skw->idx);
|
if (!skw->tx_wq) {
|
skw_err("alloc skwtx_workqueue failed\n");
|
return -EFAULT;
|
}
|
|
//memset(&wq_attrs, 0, sizeof(wq_attrs));
|
|
//wq_attrs.nice = MIN_NICE;
|
|
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 16, 0) && LINUX_VERSION_CODE <= KERNEL_VERSION(5, 2, 0)
|
//apply_workqueue_attrs(skw->tx_wq, &wq_attrs);
|
#endif
|
|
INIT_DELAYED_WORK(&skw->tx_worker, skw_tx_worker);
|
queue_delayed_work(skw->tx_wq, &skw->tx_worker, msecs_to_jiffies(0));
|
//queue_work_on(cpumask_last(cpu_online_mask), skw->tx_wq, &skw->tx_worker);
|
skw->trans_start = 0;
|
|
skb_queue_head_init(&skw->kfree_skb_qlist);
|
INIT_WORK(&skw->kfree_skb_task, skw_kfree_skb_worker);
|
|
skb_queue_head_init(&skw->skb_recycle_qlist);
|
#ifdef CONFIG_SWT6621S_SKB_RECYCLE
|
for (i = 0; i < SKW_SKB_RECYCLE_COUNT; i++) {
|
skb = dev_alloc_skb(SKW_2K_SIZE);
|
if (skb)
|
skb_queue_tail(&skw->skb_recycle_qlist, skb);
|
else {
|
__skb_queue_purge(&skw->skb_recycle_qlist);
|
destroy_workqueue(skw->tx_wq);
|
skw_err("alloc skb recycle failed\n");
|
return -ENOMEM;
|
}
|
}
|
#endif
|
|
for (j = 0; j < SKW_MAX_LMAC_SUPPORT; j++) {
|
INIT_WORK(&skw->hw.lmac[j].dy_work, skw_dy_worker);
|
skw->hw.lmac[j].tx_vring = skw_tx_vring_init();
|
}
|
|
return 0;
|
}
|
|
static void __skw_tx_deinit(struct skw_core *skw)
|
{
|
int j;
|
|
atomic_set(&skw->exit, 1);
|
cancel_delayed_work_sync(&skw->tx_worker);
|
cancel_work_sync(&skw->kfree_skb_task);
|
skb_queue_purge(&skw->kfree_skb_qlist);
|
destroy_workqueue(skw->tx_wq);
|
skb_queue_purge(&skw->skb_recycle_qlist);
|
|
for (j = 0; j < SKW_MAX_LMAC_SUPPORT; j++) {
|
cancel_work_sync(&skw->hw.lmac[j].dy_work);
|
skw_tx_vring_deinit(skw->hw.lmac[j].tx_vring);
|
}
|
}
|
|
#else
|
|
static int skw_tx_thread(void *data)
|
{
|
struct skw_core *skw = data;
|
int i, ac, mac;
|
int base = 0;
|
unsigned long flags;
|
int lmac_tx_capa;
|
int qlen, pending_qlen = 0;
|
int max_tx_count_limit = 0;
|
int lmac_tx_map = 0;
|
struct sk_buff *skb;
|
struct skw_iface *iface;
|
struct sk_buff_head *qlist;
|
struct skw_tx_lmac txl[SKW_MAX_LMAC_SUPPORT];
|
struct skw_tx_lmac *txlp;
|
struct sk_buff_head pure_ack_list;
|
int xmit_tx_flag;
|
int all_credit;
|
|
memset(txl, 0, sizeof(txl));
|
skw_reset_ac(txl);
|
|
max_tx_count_limit = skw->hw.pkt_limit;
|
|
/* reserve one for eof block */
|
if (skw->hw.bus == SKW_BUS_SDIO)
|
max_tx_count_limit--;
|
|
for (i = 0; i < skw->hw.nr_lmac; i++) {
|
__skb_queue_head_init(&txl[i].tx_list);
|
txl[i].tx_count_limit = max_tx_count_limit;
|
}
|
|
while (!kthread_should_stop()) {
|
// TODO:
|
/* CPU bind */
|
/* check if frame in pending queue is timeout */
|
|
atomic_inc(&skw->dbg.loop);
|
|
if (test_and_clear_bit(SKW_CMD_FLAG_XMIT, &skw->cmd.flags)) {
|
skw_bus_cmd_xmit(skw, skw->cmd.data, skw->cmd.data_len);
|
|
if (test_bit(SKW_CMD_FLAG_NO_ACK, &skw->cmd.flags)) {
|
set_bit(SKW_CMD_FLAG_DONE, &skw->cmd.flags);
|
skw->cmd.callback(skw);
|
}
|
}
|
|
if (!skw_tx_allowed(skw->flags)) {
|
set_current_state(TASK_INTERRUPTIBLE);
|
schedule_timeout(msecs_to_jiffies(200));
|
continue;
|
}
|
|
pending_qlen = 0;
|
lmac_tx_map = 0;
|
|
for (ac = 0; ac < SKW_WMM_AC_MAX; ac++) {
|
int ac_qlen = 0;
|
|
for (i = 0; i < SKW_NR_IFACE; i++) {
|
iface = skw->vif.iface[i];
|
// if (!iface || skw_lmac_is_actived(skw, iface->lmac_id))
|
if (!iface || !iface->ndev)
|
continue;
|
|
if (ac == SKW_WMM_AC_BE && iface->txq[SKW_ACK_TXQ].qlen) {
|
qlist = &iface->txq[SKW_ACK_TXQ];
|
__skb_queue_head_init(&pure_ack_list);
|
|
spin_lock_irqsave(&qlist->lock, flags);
|
skb_queue_splice_tail_init(&iface->txq[SKW_ACK_TXQ],
|
&pure_ack_list);
|
spin_unlock_irqrestore(&qlist->lock, flags);
|
|
skw_merge_pure_ack(skw, &pure_ack_list, &iface->tx_cache[ac]);
|
}
|
|
qlist = &iface->txq[ac];
|
if (!skb_queue_empty(qlist)) {
|
spin_lock_irqsave(&qlist->lock, flags);
|
skb_queue_splice_tail_init(qlist, &iface->tx_cache[ac]);
|
spin_unlock_irqrestore(&qlist->lock, flags);
|
}
|
|
qlen = skb_queue_len(&iface->tx_cache[ac]);
|
if (qlen) {
|
txlp = &txl[iface->lmac_id];
|
txlp->current_qlen += qlen;
|
txlp->txq_map |= BIT(txlp->nr_txq);
|
txlp->tx[txlp->nr_txq].list = &iface->tx_cache[ac];
|
|
if (txlp->ac_reset & BIT(ac)) {
|
txlp->tx[txlp->nr_txq].quota = iface->wmm.factor[ac];
|
txlp->tx[txlp->nr_txq].reset = true;
|
txlp->reset = true;
|
txlp->ac_reset ^= BIT(ac);
|
}
|
|
txlp->nr_txq++;
|
ac_qlen += qlen;
|
}
|
}
|
|
if (!ac_qlen)
|
continue;
|
|
pending_qlen += ac_qlen;
|
lmac_tx_capa = 0;
|
|
all_credit = 0;
|
for (mac = 0; mac < skw->hw.nr_lmac; mac++)
|
all_credit += skw_get_hw_credit(skw, mac);
|
|
if (all_credit == 0) {
|
skw_stop_dev_queue(skw);
|
wait_event_interruptible_exclusive(skw->tx_wait_q,
|
atomic_xchg(&skw->tx_wake, 0) || atomic_xchg(&skw->exit, 0));
|
skw_start_dev_queue(skw);
|
}
|
|
for (mac = 0; mac < skw->hw.nr_lmac; mac++) {
|
int credit;
|
|
txlp = &txl[mac];
|
if (!txlp->txq_map)
|
goto reset;
|
|
credit = txlp->cred = skw_get_hw_credit(skw, mac);
|
if (!txlp->cred)
|
goto reset;
|
|
if (txlp->reset) {
|
switch (ac) {
|
case SKW_WMM_AC_VO:
|
base = SKW_BASE_VO;
|
break;
|
|
case SKW_WMM_AC_VI:
|
base = SKW_BASE_VI;
|
break;
|
|
case SKW_WMM_AC_BK:
|
if (txlp->bk_tx_limit) {
|
base = min(txlp->cred, txlp->bk_tx_limit);
|
txlp->bk_tx_limit = 0;
|
} else {
|
base = txlp->cred;
|
}
|
|
base = base / txlp->nr_txq;
|
|
break;
|
default:
|
base = min(txlp->cred, txlp->current_qlen);
|
base = base / txlp->nr_txq;
|
txlp->bk_tx_limit = (txlp->cred + 1) >> 1;
|
|
break;
|
}
|
|
base = base ? base : 1;
|
txlp->reset = false;
|
}
|
|
for (i = 0; txlp->txq_map != 0; i++) {
|
i = i % txlp->nr_txq;
|
|
if (!(txlp->txq_map & BIT(i)))
|
continue;
|
|
if (!txlp->cred)
|
break;
|
|
if (txlp->tx[i].reset) {
|
txlp->tx[i].quota += base;
|
|
if (txlp->tx[i].quota < 0)
|
txlp->tx[i].quota = 0;
|
|
txlp->tx[i].reset = false;
|
}
|
|
skb = skb_peek(txlp->tx[i].list);
|
if (!skb) {
|
txlp->txq_map ^= BIT(i);
|
continue;
|
}
|
|
if (!is_skw_peer_data_valid(skw, skb)) {
|
skw_detail("drop dest: %pM\n",
|
eth_hdr(skb)->h_dest);
|
|
__skb_unlink(skb, txlp->tx[i].list);
|
kfree_skb(skb);
|
|
continue;
|
}
|
|
if (!txlp->tx_count_limit--)
|
break;
|
|
if (txlp->tx[i].quota) {
|
txlp->tx[i].quota--;
|
} else {
|
txlp->txq_map ^= BIT(i);
|
continue;
|
}
|
|
if ((long)skb->data & SKW_DATA_ALIGN_MASK)
|
skw_warn("address unaligned\n");
|
#if 0
|
if (skb->len % skw->hw_pdata->align_value)
|
skw_warn("len: %d unaligned\n", skb->len);
|
#endif
|
__skb_unlink(skb, txlp->tx[i].list);
|
__skb_queue_tail(&txlp->tx_list, skb);
|
txlp->cred--;
|
}
|
|
pending_qlen = pending_qlen - credit + txlp->cred;
|
|
trace_skw_tx_info(mac, ac, credit, credit - txlp->cred,
|
txlp->current_qlen, pending_qlen);
|
|
// skw_lmac_tx(skw, mac, &txlp->tx_list);
|
skw_bus_data_xmit(skw, mac, &txlp->tx_list);
|
txlp->tx_count_limit = max_tx_count_limit;
|
|
lmac_tx_map |= BIT(mac);
|
|
if (txlp->cred)
|
lmac_tx_capa |= BIT(mac);
|
reset:
|
txlp->nr_txq = 0;
|
txlp->txq_map = 0;
|
txlp->current_qlen = 0;
|
}
|
|
if (!lmac_tx_capa)
|
break;
|
}
|
|
if (ac == SKW_WMM_AC_MAX)
|
skw_reset_ac(txl);
|
|
if (pending_qlen == 0) {
|
xmit_tx_flag = 0;
|
|
for (ac = 0; ac < SKW_WMM_AC_MAX; ac++) {
|
for (i = 0; i < SKW_NR_IFACE; i++) {
|
iface = skw->vif.iface[i];
|
if (!iface || !iface->ndev)
|
continue;
|
|
if (skb_queue_len(&iface->tx_cache[ac]) != 0) {
|
xmit_tx_flag = 1;
|
goto need_running;
|
}
|
|
spin_lock_irqsave(&iface->txq[ac].lock, flags);
|
if (skb_queue_len(&iface->txq[ac]) != 0) {
|
xmit_tx_flag = 1;
|
spin_unlock_irqrestore(&iface->txq[ac].lock, flags);
|
goto need_running;
|
|
}
|
spin_unlock_irqrestore(&iface->txq[ac].lock, flags);
|
}
|
}
|
need_running:
|
|
if (xmit_tx_flag == 0) {
|
//skw_start_dev_queue(skw);
|
wait_event_interruptible_exclusive(skw->tx_wait_q,
|
atomic_xchg(&skw->tx_wake, 0) || atomic_xchg(&skw->exit, 0));
|
}
|
}
|
}
|
|
skw_info("exit");
|
|
return 0;
|
}
|
|
static int __skw_tx_init(struct skw_core *skw)
|
{
|
skw->tx_thread = kthread_create(skw_tx_thread, skw, "skw_tx.%d", skw->idx);
|
if (IS_ERR(skw->tx_thread)) {
|
skw_err("create tx thread failed\n");
|
return PTR_ERR(skw->tx_thread);
|
}
|
|
//kthread_bind(skw->tx_thread, cpumask_last(cpu_online_mask));
|
|
skw_set_thread_priority(skw->tx_thread, SCHED_RR, 1);
|
set_user_nice(skw->tx_thread, MIN_NICE);
|
wake_up_process(skw->tx_thread);
|
|
return 0;
|
}
|
|
static void __skw_tx_deinit(struct skw_core *skw)
|
{
|
if (skw->tx_thread) {
|
atomic_set(&skw->exit, 1);
|
kthread_stop(skw->tx_thread);
|
skw->tx_thread = NULL;
|
}
|
}
|
|
#endif
|
|
static int skw_register_tx_callback(struct skw_core *skw, void *func, void *data)
|
{
|
int i, map, ret = 0;
|
|
for (map = 0, i = 0; i < SKW_MAX_LMAC_SUPPORT; i++) {
|
if (!(skw->hw.lmac[i].flags & SKW_LMAC_FLAG_TXCB))
|
continue;
|
|
ret = skw_register_tx_cb(skw, skw->hw.lmac[i].dport, func, data);
|
if (ret < 0) {
|
skw_err("chip: %d, hw mac: %d, port: %d failed, ret: %d\n",
|
skw->idx, i, skw->hw.lmac[i].dport, ret);
|
|
break;
|
}
|
|
map |= BIT(skw->hw.lmac[i].dport);
|
}
|
|
skw_dbg("chip: %d, %s data port: 0x%x\n",
|
skw->idx, func ? "register" : "unregister", map);
|
|
return ret;
|
}
|
|
int skw_hw_xmit_init(struct skw_core *skw, int dma)
|
{
|
int ret = 0;
|
|
skw_dbg("dma: %d\n", dma);
|
|
switch (dma) {
|
case SKW_SYNC_ADMA_TX:
|
skw->hw.dat_xmit = skw_sync_adma_tx;
|
skw->hw.cmd_xmit = skw_sync_adma_tx;
|
skw->hw.cmd_disable_irq_xmit = skw_sync_adma_cmd_disable_irq_tx;
|
break;
|
|
case SKW_SYNC_SDMA_TX:
|
skw->hw.dat_xmit = skw_sync_sdma_tx;
|
skw->hw.cmd_xmit = skw_sync_sdma_tx;
|
skw->hw.cmd_disable_irq_xmit = skw_sync_sdma_cmd_disable_irq_tx;
|
|
if (skw->sdma_buff)
|
break;
|
skw->sdma_buff = SKW_ZALLOC(skw->hw_pdata->max_buffer_size, GFP_KERNEL);
|
if (!skw->sdma_buff)
|
ret = -ENOMEM;
|
|
break;
|
|
case SKW_ASYNC_ADMA_TX:
|
skw->hw.dat_xmit = skw_async_adma_tx;
|
skw->hw.cmd_xmit = skw_sync_adma_tx;
|
skw->hw.cmd_disable_irq_xmit = NULL;
|
|
ret = skw_register_tx_callback(skw, skw_async_adma_tx_free, skw);
|
break;
|
|
case SKW_ASYNC_SDMA_TX:
|
skw->hw.dat_xmit = skw_async_sdma_tx;
|
skw->hw.cmd_xmit = skw_sync_sdma_tx;
|
skw->hw.cmd_disable_irq_xmit = NULL;
|
|
ret = skw_register_tx_callback(skw, skw_async_sdma_tx_free, skw);
|
|
if (skw->sdma_buff)
|
break;
|
skw->sdma_buff = SKW_ZALLOC(skw->hw_pdata->max_buffer_size, GFP_KERNEL);
|
if (!skw->sdma_buff)
|
ret = -ENOMEM;
|
break;
|
|
case SKW_ASYNC_EDMA_TX:
|
skw->hw.dat_xmit = NULL;
|
skw->hw.cmd_xmit = skw_sync_adma_tx;
|
skw->hw.cmd_disable_irq_xmit = NULL;
|
break;
|
|
default:
|
ret = -EINVAL;
|
skw->hw.dat_xmit = NULL;
|
skw->hw.cmd_xmit = NULL;
|
break;
|
}
|
|
return ret;
|
}
|
|
int skw_tx_init(struct skw_core *skw)
|
{
|
int ret;
|
|
skw->skb_share_len = SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
|
skw->skb_headroom = sizeof(struct skw_tx_desc_hdr) +
|
sizeof(struct skw_tx_desc_conf) +
|
skw->hw.extra.hdr_len +
|
SKW_DATA_ALIGN_SIZE + sizeof(unsigned long);
|
|
if (skw->hw.bus == SKW_BUS_SDIO) {
|
skw->eof_blk = SKW_ZALLOC(skw->hw.align, GFP_KERNEL);
|
if (!skw->eof_blk)
|
return -ENOMEM;
|
} else if (skw->hw.bus == SKW_BUS_PCIE) {
|
skw->skb_headroom = sizeof(struct skw_tx_desc_conf) +
|
SKW_DATA_ALIGN_SIZE + 2 * sizeof(unsigned long);
|
}
|
|
ret = skw_hw_xmit_init(skw, skw->hw.dma);
|
if (ret < 0) {
|
SKW_KFREE(skw->eof_blk);
|
return ret;
|
}
|
|
ret = __skw_tx_init(skw);
|
if (ret < 0) {
|
SKW_KFREE(skw->eof_blk);
|
SKW_KFREE(skw->sdma_buff);
|
}
|
|
tx_wait_time = 5000;
|
skw_procfs_file(skw->pentry, "tx_wait_time", 0666, &skw_tx_time_fops, NULL);
|
|
return ret;
|
}
|
|
int skw_tx_deinit(struct skw_core *skw)
|
{
|
__skw_tx_deinit(skw);
|
|
skw_register_tx_callback(skw, NULL, NULL);
|
|
SKW_KFREE(skw->eof_blk);
|
SKW_KFREE(skw->sdma_buff);
|
|
return 0;
|
}
|
