marfrit/bes2600-dkms
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
d95453c98e31d7a47bc227aef5d0b426ac9e334b
bes2600-dkms/bes2600/tx_loop.c
T
test0r d95453c98e bes2600: take pending_record_lock with _bh() to fix SOFTIRQ-safe → -unsafe inversion (besser#18) 
PROVE_LOCKING reports:

  WARNING: SOFTIRQ-safe -> SOFTIRQ-unsafe lock order detected
  kworker/u16:1 is trying to acquire:
    &hw_priv->tx_loop.pending_record_lock at bes2600_queue_clear+0x80
  and this task is already holding:
    &queue->lock at bes2600_queue_clear+0x60

  which would create a new lock dependency:
    (&queue->lock){+.-.}   -> (&hw_priv->tx_loop.pending_record_lock){+.+.}

  but this new dependency connects a SOFTIRQ-irq-safe lock:
    (&queue->lock){+.-.}
  ... which became SOFTIRQ-irq-safe at:
    bes2600_tx -> ieee80211_handle_wake_tx_queue -> tasklet_action
  to a SOFTIRQ-irq-unsafe lock:
    (&hw_priv->tx_loop.pending_record_lock){+.+.}
  ... which became SOFTIRQ-irq-unsafe at:
    bes2600_queue_get_skb -> bes2600_join_work -> process_one_work

queue->lock is taken consistently with spin_lock_bh() at 22 sites;
the nested acquisition of pending_record_lock at queue.c:289 (inside
the outer queue->lock_bh held at line 285) had it implicitly BH-safe
via the outer scope. But pending_record_lock is ALSO taken from
non-BH-disabled contexts:

  bes2600_queue_get_skb  (queue.c:832)  — process context via
    bes2600_join_work (workqueue), no outer queue->lock held
  bes2600_tx_loop_item_pending_check (tx_loop.c:112)
                                     — TX-loop context, no outer
                                     queue->lock held

When CPU0 holds pending_record_lock from one of those non-BH paths
and a softirq fires that wants queue->lock, and CPU1 in softirq has
queue->lock and is about to acquire pending_record_lock — classic AB-BA
SOFTIRQ deadlock.

The fix is the conservative one: take pending_record_lock with _bh()
at every site that's not already inside a queue->lock_bh-held scope.
That makes the lock consistently SOFTIRQ-safe, eliminating the
inversion. queue.c:289/295 stays as plain spin_lock because BH is
already disabled by the outer queue->lock_bh acquired at queue.c:285.

Five sites converted:
  bes2600/queue.c:832 -- spin_lock      -> spin_lock_bh
  bes2600/queue.c:839 -- spin_unlock    -> spin_unlock_bh
  bes2600/queue.c:844 -- spin_unlock    -> spin_unlock_bh
  bes2600/tx_loop.c:112 -- spin_lock    -> spin_lock_bh
  bes2600/tx_loop.c:114 -- spin_unlock  -> spin_unlock_bh

Contract:
  - Documentation/locking/locktypes.rst spelling: spin_lock_bh() is
    the canonical way to make a non-IRQ spinlock safe against
    softirq preemption that might re-enter the same lock.
  - Same shape as queue->lock in this driver and as is_drv->lock
    in the cw1200 ancestor.

Closes: besser#18
Fixes: <bes2600 base import>
Signed-off-by: Markus Fritsche <fritsche.markus@gmail.com>
2026-05-18 16:58:49 +02:00

535 lines
20 KiB
C
Raw Blame History

// SPDX-License-Identifier: GPL-2.0-only
/*
* Test-mode TX loopback for BES2600
*
* Copyright (c) 2022, Bestechnic (Beijing) Co., Ltd.
*
*/
#include "bes2600.h"
#include "wsm.h"
#include "queue.h"
#include "bes_log.h"
struct tx_loop_table
{
u16 cmd;
void (*proc)(struct bes2600_common *hw_priv, u8 *buf, u32 len);
};
static void bes2600_tx_loop_build_lmac_tx_cfm(struct bes2600_common *hw_priv, u8 *buf, u32 len);
static void bes2600_tx_loop_build_scan_compl_ind(struct bes2600_common *hw_priv);
static void bes2600_tx_loop_build_pm_ind(struct bes2600_common *hw_priv);
void bes2600_tx_loop_init(struct bes2600_common *hw_priv)
{
hw_priv->tx_loop.enabled = false;
hw_priv->tx_loop.start_lmac_seq = 0;
hw_priv->tx_loop.start_mcu_seq = 0;
INIT_LIST_HEAD(&hw_priv->tx_loop.pending_record_list);
spin_lock_init(&hw_priv->tx_loop.pending_record_lock);
spin_lock_init(&hw_priv->tx_loop.tx_loop_lock);
skb_queue_head_init(&hw_priv->tx_loop.rx_queue);
}
struct sk_buff *bes2600_tx_loop_read(struct bes2600_common *hw_priv)
{
struct sk_buff *skb;
struct wsm_hdr *wsm;
if(hw_priv == NULL)
return NULL;
skb = skb_dequeue(&hw_priv->tx_loop.rx_queue);
if(skb != NULL) {
wsm = (struct wsm_hdr *)skb->data;
bes_devel("tx loop pipe read msg_id:0x%04x seq:%d\n",
WSM_MSG_ID_GET(wsm->id), WSM_MSG_SEQ_GET(wsm->id));
}
return skb;
}
static void bes2600_tx_loop_item_pending_item(struct bes2600_common *hw_priv, struct sk_buff *skb)
{
bes_devel("tx loop confirm pending skb.\n");
bes2600_tx_loop_build_lmac_tx_cfm(hw_priv, skb->data, skb->data_len);
}
void bes2600_tx_loop_record_wsm_cmd(struct bes2600_common *hw_priv, u8 *wsm_cmd)
{
hw_priv->tx_loop.wsm_cmd_ptr = wsm_cmd;
}
void bes2600_tx_loop_clear_wsm_cmd(struct bes2600_common *hw_priv)
{
hw_priv->tx_loop.wsm_cmd_ptr = NULL;
}
void bes2600_tx_loop_set_enable(struct bes2600_common *hw_priv, bool need_warn)
{
int i = 0;
u16 cmd_id = -1;
if(hw_priv == NULL)
return;
if(hw_priv->tx_loop.enabled)
return;
WARN_ON(need_warn);
hw_priv->tx_loop.enabled = true;
hw_priv->tx_loop.start_lmac_seq = hw_priv->wsm_rx_seq[0];
hw_priv->tx_loop.start_mcu_seq = hw_priv->wsm_rx_seq[1];
if(hw_priv->tx_loop.wsm_cmd_ptr) {
struct wsm_hdr *tx_wsm = (struct wsm_hdr *)hw_priv->tx_loop.wsm_cmd_ptr;
cmd_id = tx_wsm->id & WSM_MSG_ID_MASK;
if(cmd_id == 0x0005 || cmd_id == 0x0006){
struct wsm_mib *mib = (struct wsm_mib *)hw_priv->wsm_cmd.arg;
u16 mib_id = mib->mibId;
bes_devel("pending cmd:0x%04x seq:%d mib_id:0x%04x\n",
cmd_id, WSM_MSG_SEQ_GET(tx_wsm->id), mib_id);
} else {
bes_devel("pending cmd:0x%04x seq:%d\n",
cmd_id, WSM_MSG_SEQ_GET(tx_wsm->id));
}
bes2600_tx_loop_pipe_send(hw_priv, hw_priv->tx_loop.wsm_cmd_ptr, 8);
}
if (atomic_read(&hw_priv->scan.in_progress) && cmd_id != 0x0007) {
bes2600_tx_loop_build_scan_compl_ind(hw_priv);
} else if (atomic_read(&hw_priv->bes_power.pm_set_in_process) && cmd_id != 0x0010) {
bes2600_tx_loop_build_pm_ind(hw_priv);
}
for (i = 0; i < 4; i++) {
bes2600_queue_iterate_pending_packet(&hw_priv->tx_queue[i],
bes2600_tx_loop_item_pending_item);
}
spin_lock_bh(&hw_priv->tx_loop.pending_record_lock);
bes2600_queue_iterate_record_pending_packet(hw_priv, bes2600_tx_loop_item_pending_item);
spin_unlock_bh(&hw_priv->tx_loop.pending_record_lock);
if (atomic_read(&hw_priv->bh_rx) > 0)
wake_up(&hw_priv->bh_wq);
}
static void bes2600_tx_loop_build_lmac_generic_cfm(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
struct sk_buff *out_skb;
struct wsm_hdr *rx_wsm, *tx_wsm;
u32 msg_len = sizeof(struct wsm_hdr) + 4;
u16 msg_id = 0;
tx_wsm = (struct wsm_hdr *)buf;
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_err("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s cmd 0x%04x len:%d\n",
__func__, (tx_wsm->id & WSM_MSG_ID_MASK), msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_hdr *)out_skb->data;
msg_id = (tx_wsm->id & WSM_MSG_ID_MASK);
msg_id |= 0x400; // set confirm flag
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_lmac_seq);
rx_wsm->id = __cpu_to_le16(msg_id);
rx_wsm->len = __cpu_to_le16(msg_len);
*((u32 *)&rx_wsm[1]) = __cpu_to_le32(WSM_STATUS_SUCCESS);
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_lmac_seq = (hw_priv->tx_loop.start_lmac_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static void bes2600_tx_loop_build_lmac_tx_cfm(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
struct sk_buff *out_skb;
struct wsm_hdr *rx_wsm;
struct wsm_tx *tx_wsm;
struct wsm_tx_confirm *cfm;
u32 msg_len = sizeof(struct wsm_hdr) + sizeof(struct wsm_tx_confirm);
u16 msg_id = 0;
tx_wsm = (struct wsm_tx *)buf;
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_err("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s cmd 0x%04x len:%d\n",
__func__, (tx_wsm->hdr.id & WSM_MSG_ID_MASK), msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_hdr *)out_skb->data;
msg_id = (tx_wsm->hdr.id & WSM_MSG_ID_MASK);
msg_id |= 0x400; // set confirm flag
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_lmac_seq);
rx_wsm->id = __cpu_to_le16(msg_id);
rx_wsm->len = __cpu_to_le16(msg_len);
cfm = (struct wsm_tx_confirm *)&rx_wsm[1];
cfm->packetID = tx_wsm->packetID;
cfm->status = WSM_STATUS_SUCCESS;
cfm->txedRate = tx_wsm->maxTxRate;
cfm->ackFailures = 0;
cfm->flags = WSM_TX_STATUS_NORMAL_ACK;
cfm->txQueueDelay = 5;
cfm->mediaDelay = 3;
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_lmac_seq = (hw_priv->tx_loop.start_lmac_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static void bes2600_tx_loop_build_config_cfm(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
struct sk_buff *out_skb;
struct wsm_hdr *rx_wsm;
struct wsm_hdr *tx_wsm = (struct wsm_hdr *)buf;
u32 msg_len = sizeof(struct wsm_hdr) + sizeof(struct wsm_configuration) + 4;
u16 msg_id = 0;
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_err("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s cmd 0x%04x len:%d\n",
__func__, (tx_wsm->id & WSM_MSG_ID_MASK), msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_hdr *)out_skb->data;
msg_id = (tx_wsm->id & WSM_MSG_ID_MASK);
msg_id |= 0x400; // set confirm flag
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_lmac_seq);
rx_wsm->id = __cpu_to_le16(msg_id);
rx_wsm->len = __cpu_to_le16(msg_len);
*(u32 *)&rx_wsm[1] = WSM_STATUS_SUCCESS;
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_lmac_seq = (hw_priv->tx_loop.start_lmac_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static void bes2600_tx_loop_build_read_mib_cfm(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
struct sk_buff *out_skb;
struct wsm_hdr *rx_wsm;
struct wsm_hdr *tx_wsm = (struct wsm_hdr *)buf;
struct wsm_mib *mib = (struct wsm_mib *)hw_priv->wsm_cmd.arg;
u32 *status;
u16 *mib_id, *size;
u32 msg_len = sizeof(struct wsm_hdr) + 4 /* status */ + 2 /* mib_id */ + mib->buf_size;
u16 msg_id = 0;
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_err("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s cmd 0x%04x len:%d\n",
__func__, (tx_wsm->id & WSM_MSG_ID_MASK), msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_hdr *)out_skb->data;
msg_id = (tx_wsm->id & WSM_MSG_ID_MASK);
msg_id |= 0x400; // set confirm flag
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_lmac_seq);
rx_wsm->id = __cpu_to_le16(msg_id);
rx_wsm->len = __cpu_to_le16(msg_len);
status = (u32 *)&rx_wsm[1];
*status = WSM_STATUS_SUCCESS;
mib_id = (u16 *)&status[1];
*mib_id = mib->mibId;
size = (u16 *)&mib_id[1];
*size = mib->buf_size;
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_lmac_seq = (hw_priv->tx_loop.start_lmac_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static void bes2600_tx_loop_build_join_cfm(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
struct sk_buff *out_skb;
struct wsm_hdr *rx_wsm;
struct wsm_hdr *tx_wsm = (struct wsm_hdr *)buf;
u16 msg_id = 0;
u32 msg_len = sizeof(struct wsm_hdr) + 4 /* status */ + 8 /* power_level */;
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_err("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s cmd 0x%04x len:%d\n",
__func__, (tx_wsm->id & WSM_MSG_ID_MASK), msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_hdr *)out_skb->data;
msg_id = (tx_wsm->id & WSM_MSG_ID_MASK);
msg_id |= 0x400; // set confirm flag
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_lmac_seq);
rx_wsm->id = __cpu_to_le16(msg_id);
rx_wsm->len = __cpu_to_le16(msg_len);
*(u32 *)&rx_wsm[1] = WSM_STATUS_SUCCESS;
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_lmac_seq = (hw_priv->tx_loop.start_lmac_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static void bes2600_tx_loop_build_rfcmd_cfm(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
struct sk_buff *out_skb;
struct wsm_mcu_hdr *rx_wsm;
struct wsm_hdr *tx_wsm = (struct wsm_hdr *)buf;
u16 msg_id = 0;
u32 msg_len = sizeof(struct wsm_mcu_hdr);
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_err("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s cmd 0x%04x len:%d\n",
__func__, (tx_wsm->id & WSM_MSG_ID_MASK), msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_mcu_hdr *)out_skb->data;
msg_id = (tx_wsm->id & WSM_MSG_ID_MASK);
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_mcu_seq);
rx_wsm->hdr.id = __cpu_to_le16(msg_id);
rx_wsm->hdr.len = __cpu_to_le16(msg_len);
rx_wsm->handle_label = WSM_TO_MCU_CMD_CONFIRM_LABEL;
rx_wsm->cmd_type = (tx_wsm->id & WSM_MSG_ID_MASK);
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_mcu_seq = (hw_priv->tx_loop.start_mcu_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static void bes2600_tx_loop_build_driver_cmd_cfm(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
struct sk_buff *out_skb;
struct wsm_mcu_hdr *rx_wsm;
struct wsm_hdr *tx_wsm = (struct wsm_hdr *)buf;
u16 msg_id = 0;
u32 msg_len = sizeof(struct wsm_mcu_hdr) + 4;
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_err("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s cmd 0x%04x len:%d\n",
__func__, (tx_wsm->id & WSM_MSG_ID_MASK), msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_mcu_hdr *)out_skb->data;
msg_id = (tx_wsm->id & WSM_MSG_ID_MASK);
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_mcu_seq);
rx_wsm->hdr.id = __cpu_to_le16(msg_id);
rx_wsm->hdr.len = __cpu_to_le16(msg_len);
rx_wsm->handle_label = WSM_TO_MCU_CMD_CONFIRM_LABEL;
rx_wsm->cmd_type = (tx_wsm->id & WSM_MSG_ID_MASK);
*(uint32_t *)(rx_wsm + 1) = 0;
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_mcu_seq = (hw_priv->tx_loop.start_mcu_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static struct tx_loop_table tx_loop_tbl[] = {
{.cmd = 0x0004, .proc = bes2600_tx_loop_build_lmac_tx_cfm},
{.cmd = 0x0009, .proc = bes2600_tx_loop_build_config_cfm},
{.cmd = 0x0005, .proc = bes2600_tx_loop_build_read_mib_cfm},
{.cmd = 0x0006, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x000B, .proc = bes2600_tx_loop_build_join_cfm},
{.cmd = 0x0007, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0008, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x000A, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x000C, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x000D, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0010, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0011, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0012, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0013, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0016, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0017, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0018, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0019, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x001A, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x001B, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x001C, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0029, .proc = bes2600_tx_loop_build_lmac_generic_cfm},
{.cmd = 0x0C25, .proc = bes2600_tx_loop_build_rfcmd_cfm},
{.cmd = 0x0C27, .proc = bes2600_tx_loop_build_driver_cmd_cfm},
};
static void bes2600_tx_loop_build_scan_compl_ind(struct bes2600_common *hw_priv)
{
struct sk_buff *out_skb;
struct wsm_hdr *rx_wsm;
struct wsm_scan_complete *scan_compl;
u16 msg_id = 0;
u32 msg_len = sizeof(struct wsm_hdr) + sizeof(struct wsm_scan_complete);
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_devel("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s len:%d\n", __func__, msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_hdr *)out_skb->data;
msg_id |= 0x806; // set indication flag
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_lmac_seq);
rx_wsm->id = __cpu_to_le16(msg_id);
rx_wsm->len = __cpu_to_le16(msg_len);
scan_compl = (struct wsm_scan_complete *)&rx_wsm[1];
scan_compl->status = WSM_STATUS_SUCCESS;
scan_compl->psm = WSM_PSM_PS;
scan_compl->numChannels = 2;
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_lmac_seq = (hw_priv->tx_loop.start_lmac_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static void bes2600_tx_loop_build_pm_ind(struct bes2600_common *hw_priv)
{
struct sk_buff *out_skb;
struct wsm_hdr *rx_wsm;
struct wsm_set_pm_complete *pm_compl;
u16 msg_id = 0;
u32 msg_len = sizeof(struct wsm_hdr) + sizeof(struct wsm_set_pm_complete);
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_devel("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s len:%d\n", __func__, msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_hdr *)out_skb->data;
msg_id |= 0x809; // set indication flag
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_lmac_seq);
rx_wsm->id = __cpu_to_le16(msg_id);
rx_wsm->len = __cpu_to_le16(msg_len);
pm_compl = (struct wsm_set_pm_complete *)&rx_wsm[1];
pm_compl->status = WSM_STATUS_SUCCESS;
pm_compl->psm = WSM_PSM_PS;
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_lmac_seq = (hw_priv->tx_loop.start_lmac_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
static void bes2600_tx_loop_build_rfcmd_ind(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
struct sk_buff *out_skb;
struct wsm_mcu_hdr *rx_wsm;
struct wsm_hdr *tx_wsm = (struct wsm_hdr *)buf;
u16 msg_id = 0;
u32 msg_len = sizeof(struct wsm_mcu_hdr);
out_skb = dev_alloc_skb(msg_len);
if(IS_ERR_OR_NULL(out_skb)) {
bes_err("%s, alloc mem fail.\n", __func__);
return;
}
bes_devel("%s len:%d\n",__func__, msg_len);
skb_put(out_skb, msg_len);
rx_wsm = (struct wsm_mcu_hdr *)out_skb->data;
msg_id = (tx_wsm->id & WSM_MSG_ID_MASK);
msg_id |= WSM_TX_SEQ(hw_priv->tx_loop.start_mcu_seq);
rx_wsm->hdr.id = __cpu_to_le16(msg_id);
rx_wsm->hdr.len = __cpu_to_le16(msg_len);
rx_wsm->handle_label = WSM_TO_MCU_CMD_INDICATION_LABEL;
rx_wsm->cmd_type = (tx_wsm->id & WSM_MSG_ID_MASK);
spin_lock(&hw_priv->tx_loop.tx_loop_lock);
hw_priv->tx_loop.start_mcu_seq = (hw_priv->tx_loop.start_mcu_seq + 1) & 7;
skb_queue_tail(&hw_priv->tx_loop.rx_queue, out_skb);
spin_unlock(&hw_priv->tx_loop.tx_loop_lock);
atomic_add_return(1, &hw_priv->bh_rx);
}
void bes2600_tx_loop_pipe_send(struct bes2600_common *hw_priv, u8 *buf, u32 len)
{
int i = 0;
int tbl_size = ARRAY_SIZE(tx_loop_tbl);
struct wsm_hdr *tx_wsm = (struct wsm_hdr *)buf;
u16 cmd_id = tx_wsm->id & WSM_MSG_ID_MASK;
/* don't need to tx loop if wifi is unregistered */
if(hw_priv == NULL)
return;
bes_devel("tx loop pipe send cmd:0x%04x seq:%d\n",
cmd_id, WSM_MSG_SEQ_GET(tx_wsm->id));
/* select build confirm function based on command id */
for(i = 0; i < tbl_size; i++) {
if(cmd_id == tx_loop_tbl[i].cmd) {
tx_loop_tbl[i].proc(hw_priv, buf, len);
break;
}
}
/* build indication for special command */
if(cmd_id == 0x0007) {
bes2600_tx_loop_build_scan_compl_ind(hw_priv);
} else if(cmd_id == 0x0010) {
bes2600_tx_loop_build_pm_ind(hw_priv);
} else if(cmd_id == 0x0C25) {
bes2600_tx_loop_build_rfcmd_ind(hw_priv, buf, len);
}
}
Reference in New Issue View Git Blame Copy Permalink