int dhd_cfg80211_testmode_cmd(struct wiphy *wiphy, void *data, int len)
{
struct sk_buff *reply;
struct wl_priv *wl;
dhd_pub_t *dhd;
dhd_ioctl_t *ioc = data;
int err = 0;
WL_TRACE(("entry: cmd = %d\n", ioc->cmd));
wl = wiphy_priv(wiphy);
dhd = wl->pub;
DHD_OS_WAKE_LOCK(dhd);
if (dhd->hang_was_sent) {
WL_ERR(("%s: HANG was sent up earlier\n", __FUNCTION__));
DHD_OS_WAKE_LOCK_CTRL_TIMEOUT_ENABLE(dhd, DHD_EVENT_TIMEOUT_MS);
DHD_OS_WAKE_UNLOCK(dhd);
return OSL_ERROR(BCME_DONGLE_DOWN);
}
err = dhd_ioctl_process(dhd, 0, ioc);
if (err)
goto done;
reply = cfg80211_testmode_alloc_reply_skb(wiphy, sizeof(*ioc));
nla_put(reply, NL80211_ATTR_TESTDATA, sizeof(*ioc), ioc);
err = cfg80211_testmode_reply(reply);
done:
DHD_OS_WAKE_UNLOCK(dhd);
return err;
}
int dhd_cfg80211_testmode_cmd(struct wiphy *wiphy, void *data, int len)
{
struct sk_buff *reply;
struct wl_priv *wl;
dhd_pub_t *dhd;
dhd_ioctl_t *ioc = data;
int err = 0;
WL_TRACE(("entry: cmd = %d\n", ioc->cmd));
wl = wiphy_priv(wiphy);
dhd = wl->pub;
DHD_OS_WAKE_LOCK(dhd);
/* currently there is only one wiphy for ifidx 0 */
err = dhd_ioctl_process(dhd, 0, ioc);
if (err)
goto done;
/* response data is in ioc->buf so return ioc here */
reply = cfg80211_testmode_alloc_reply_skb(wiphy, sizeof(*ioc));
nla_put(reply, NL80211_ATTR_TESTDATA, sizeof(*ioc), ioc);
err = cfg80211_testmode_reply(reply);
done:
DHD_OS_WAKE_UNLOCK(dhd);
return err;
}
int dhd_cfg80211_testmode_cmd(struct wiphy *wiphy, void *data, int len)
{
struct sk_buff *reply;
struct wl_priv *wl;
dhd_pub_t *dhd;
dhd_ioctl_t *ioc = data;
int err = 0;
WL_TRACE(("entry: cmd = %d\n", ioc->cmd));
wl = wiphy_priv(wiphy);
dhd = wl->pub;
DHD_OS_WAKE_LOCK(dhd);
/* send to dongle only if we are not waiting for reload already */
if (dhd->hang_was_sent) {
WL_ERR(("HANG was sent up earlier\n"));
DHD_OS_WAKE_LOCK_CTRL_TIMEOUT_ENABLE(dhd, DHD_EVENT_TIMEOUT_MS);
DHD_OS_WAKE_UNLOCK(dhd);
return OSL_ERROR(BCME_DONGLE_DOWN);
}
/* currently there is only one wiphy for ifidx 0 */
err = dhd_ioctl_process(dhd, 0, ioc);
if (err)
goto done;
/* response data is in ioc->buf so return ioc here */
reply = cfg80211_testmode_alloc_reply_skb(wiphy, sizeof(*ioc));
nla_put(reply, NL80211_ATTR_TESTDATA, sizeof(*ioc), ioc);
err = cfg80211_testmode_reply(reply);
done:
DHD_OS_WAKE_UNLOCK(dhd);
return err;
}
int ath6kl_tm_cmd(struct wiphy *wiphy, void *data, int len)
{
struct ath6kl *ar = wiphy_priv(wiphy);
struct nlattr *tb[ATH6KL_TM_ATTR_MAX + 1];
int err, buf_len, reply_len;
struct sk_buff *skb;
void *buf;
err = nla_parse(tb, ATH6KL_TM_ATTR_MAX, data, len,
ath6kl_tm_policy);
if (err)
return err;
if (!tb[ATH6KL_TM_ATTR_CMD])
return -EINVAL;
switch (nla_get_u32(tb[ATH6KL_TM_ATTR_CMD])) {
case ATH6KL_TM_CMD_TCMD:
if (!tb[ATH6KL_TM_ATTR_DATA])
return -EINVAL;
buf = nla_data(tb[ATH6KL_TM_ATTR_DATA]);
buf_len = nla_len(tb[ATH6KL_TM_ATTR_DATA]);
ath6kl_wmi_test_cmd(ar->wmi, buf, buf_len);
return 0;
break;
case ATH6KL_TM_CMD_RX_REPORT:
if (!tb[ATH6KL_TM_ATTR_DATA])
return -EINVAL;
buf = nla_data(tb[ATH6KL_TM_ATTR_DATA]);
buf_len = nla_len(tb[ATH6KL_TM_ATTR_DATA]);
reply_len = nla_total_size(ATH6KL_TM_DATA_MAX_LEN);
skb = cfg80211_testmode_alloc_reply_skb(wiphy, reply_len);
if (!skb)
return -ENOMEM;
err = ath6kl_tm_rx_report(ar, buf, buf_len, skb);
if (err < 0) {
kfree_skb(skb);
return err;
}
return cfg80211_testmode_reply(skb);
default:
return -EOPNOTSUPP;
}
}
static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
{
int buf_len, ret, len;
struct sk_buff *skb;
void *buf;
u8 answer = 0;
wl1271_debug(DEBUG_TESTMODE, "testmode cmd test");
if (!tb[WL1271_TM_ATTR_DATA])
return -EINVAL;
buf = nla_data(tb[WL1271_TM_ATTR_DATA]);
buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]);
if (tb[WL1271_TM_ATTR_ANSWER])
answer = nla_get_u8(tb[WL1271_TM_ATTR_ANSWER]);
if (buf_len > sizeof(struct wl1271_command))
return -EMSGSIZE;
mutex_lock(&wl->mutex);
ret = wl1271_cmd_test(wl, buf, buf_len, answer);
mutex_unlock(&wl->mutex);
if (ret < 0) {
wl1271_warning("testmode cmd test failed: %d", ret);
return ret;
}
if (answer) {
len = nla_total_size(buf_len);
skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
if (!skb)
return -ENOMEM;
NLA_PUT(skb, WL1271_TM_ATTR_DATA, buf_len, buf);
ret = cfg80211_testmode_reply(skb);
if (ret < 0)
return ret;
}
return 0;
nla_put_failure:
kfree_skb(skb);
return -EMSGSIZE;
}
static int wl12xx_tm_cmd_get_mac(struct wl1271 *wl, struct nlattr *tb[])
{
struct sk_buff *skb;
u8 mac_addr[ETH_ALEN];
int ret = 0;
mutex_lock(&wl->mutex);
if (wl->state != WL1271_STATE_PLT) {
ret = -EINVAL;
goto out;
}
if(wl->fuse_oui_addr == 0 && wl->fuse_nic_addr == 0) {
ret = -EOPNOTSUPP;
goto out;
}
mac_addr[0] = (u8)(wl->fuse_oui_addr >> 16);
mac_addr[1] = (u8)(wl->fuse_oui_addr >> 8);
mac_addr[2] = (u8) wl->fuse_oui_addr;
mac_addr[3] = (u8)(wl->fuse_nic_addr >> 16);
mac_addr[4] = (u8)(wl->fuse_nic_addr >> 8);
mac_addr[5] = (u8) wl->fuse_nic_addr;
skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, ETH_ALEN);
if (!skb) {
ret = -ENOMEM;
goto out;
}
NLA_PUT(skb, WL1271_TM_ATTR_DATA, ETH_ALEN, mac_addr);
ret = cfg80211_testmode_reply(skb);
if (ret < 0)
goto out;
out:
mutex_unlock(&wl->mutex);
return ret;
nla_put_failure:
kfree_skb(skb);
ret = -EMSGSIZE;
goto out;
}
static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
{
int ret;
struct wl1271_command *cmd;
struct sk_buff *skb;
u8 ie_id;
wl1271_debug(DEBUG_TESTMODE, "testmode cmd interrogate");
if (!tb[WL1271_TM_ATTR_IE_ID])
return -EINVAL;
ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
if (!cmd)
return -ENOMEM;
mutex_lock(&wl->mutex);
ret = wl1271_cmd_interrogate(wl, ie_id, cmd, sizeof(*cmd));
mutex_unlock(&wl->mutex);
if (ret < 0) {
wl1271_warning("testmode cmd interrogate failed: %d", ret);
kfree(cmd);
return ret;
}
skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, sizeof(*cmd));
if (!skb) {
kfree(cmd);
return -ENOMEM;
}
NLA_PUT(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd);
return 0;
nla_put_failure:
kfree_skb(skb);
return -EMSGSIZE;
}
static int wl1271_tm_detect_fem(struct wl1271 *wl, struct nlattr *tb[])
{
/* return FEM type */
int ret, len;
struct sk_buff *skb;
ret = wl1271_plt_start(wl, PLT_FEM_DETECT);
if (ret < 0)
goto out;
mutex_lock(&wl->mutex);
len = nla_total_size(sizeof(wl->fem_manuf));
skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
if (!skb) {
ret = -ENOMEM;
goto out_mutex;
}
if (nla_put(skb, WL1271_TM_ATTR_DATA, sizeof(wl->fem_manuf),
&wl->fem_manuf)) {
kfree_skb(skb);
ret = -EMSGSIZE;
goto out_mutex;
}
ret = cfg80211_testmode_reply(skb);
out_mutex:
mutex_unlock(&wl->mutex);
/* We always stop plt after DETECT mode */
wl1271_plt_stop(wl);
out:
return ret;
}
static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
{
int buf_len, ret, len;
struct sk_buff *skb;
void *buf;
u8 answer = 0;
wl1271_debug(DEBUG_TESTMODE, "testmode cmd test");
if (!tb[WL1271_TM_ATTR_DATA])
return -EINVAL;
buf = nla_data(tb[WL1271_TM_ATTR_DATA]);
buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]);
if (tb[WL1271_TM_ATTR_ANSWER])
answer = nla_get_u8(tb[WL1271_TM_ATTR_ANSWER]);
if (buf_len > sizeof(struct wl1271_command))
return -EMSGSIZE;
mutex_lock(&wl->mutex);
if (wl->state == WL1271_STATE_OFF) {
ret = -EINVAL;
goto out;
}
ret = wl1271_ps_elp_wakeup(wl);
if (ret < 0)
goto out;
ret = wl1271_cmd_test(wl, buf, buf_len, answer);
if (ret < 0) {
wl1271_warning("testmode cmd test failed: %d", ret);
goto out_sleep;
}
if (answer) {
/* If we got bip calibration answer print radio status */
struct wl1271_cmd_cal_p2g *params =
(struct wl1271_cmd_cal_p2g *) buf;
s16 radio_status = (s16) le16_to_cpu(params->radio_status);
if (params->test.id == TEST_CMD_P2G_CAL &&
radio_status < 0)
wl1271_warning("testmode cmd: radio status=%d",
radio_status);
else
wl1271_info("testmode cmd: radio status=%d",
radio_status);
len = nla_total_size(buf_len);
skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
if (!skb) {
ret = -ENOMEM;
goto out_sleep;
}
if (nla_put(skb, WL1271_TM_ATTR_DATA, buf_len, buf)) {
kfree_skb(skb);
ret = -EMSGSIZE;
goto out_sleep;
}
ret = cfg80211_testmode_reply(skb);
if (ret < 0)
goto out_sleep;
}
out_sleep:
wl1271_ps_elp_sleep(wl);
out:
mutex_unlock(&wl->mutex);
return ret;
}
/*
* This function handles the user application commands for uCode trace
*
* It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
* handlers respectively.
*
* If it's an unknown commdn ID, -ENOSYS is replied; otherwise, the returned
* value of the actual command execution is replied to the user application.
*
* @hw: ieee80211_hw object that represents the device
* @tb: gnl message fields from the user space
*/
static int iwl_testmode_trace(struct ieee80211_hw *hw, struct nlattr **tb)
{
struct iwl_priv *priv = hw->priv;
struct sk_buff *skb;
int status = 0;
struct device *dev = bus(priv)->dev;
switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
if (priv->testmode_trace.trace_enabled)
return -EBUSY;
if (!tb[IWL_TM_ATTR_TRACE_SIZE])
priv->testmode_trace.buff_size = TRACE_BUFF_SIZE_DEF;
else
priv->testmode_trace.buff_size =
nla_get_u32(tb[IWL_TM_ATTR_TRACE_SIZE]);
if (!priv->testmode_trace.buff_size)
return -EINVAL;
if (priv->testmode_trace.buff_size < TRACE_BUFF_SIZE_MIN ||
priv->testmode_trace.buff_size > TRACE_BUFF_SIZE_MAX)
return -EINVAL;
priv->testmode_trace.total_size =
priv->testmode_trace.buff_size + TRACE_BUFF_PADD;
priv->testmode_trace.cpu_addr =
dma_alloc_coherent(dev,
priv->testmode_trace.total_size,
&priv->testmode_trace.dma_addr,
GFP_KERNEL);
if (!priv->testmode_trace.cpu_addr)
return -ENOMEM;
priv->testmode_trace.trace_enabled = true;
priv->testmode_trace.trace_addr = (u8 *)PTR_ALIGN(
priv->testmode_trace.cpu_addr, 0x100);
memset(priv->testmode_trace.trace_addr, 0x03B,
priv->testmode_trace.buff_size);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
sizeof(priv->testmode_trace.dma_addr) + 20);
if (!skb) {
IWL_DEBUG_INFO(priv,
"Error allocating memory\n");
iwl_trace_cleanup(priv);
return -ENOMEM;
}
NLA_PUT(skb, IWL_TM_ATTR_TRACE_ADDR,
sizeof(priv->testmode_trace.dma_addr),
(u64 *)&priv->testmode_trace.dma_addr);
status = cfg80211_testmode_reply(skb);
if (status < 0) {
IWL_DEBUG_INFO(priv,
"Error sending msg : %d\n",
status);
}
priv->testmode_trace.num_chunks =
DIV_ROUND_UP(priv->testmode_trace.buff_size,
DUMP_CHUNK_SIZE);
break;
case IWL_TM_CMD_APP2DEV_END_TRACE:
iwl_trace_cleanup(priv);
break;
default:
IWL_DEBUG_INFO(priv, "Unknown testmode mem command ID\n");
return -ENOSYS;
}
return status;
nla_put_failure:
kfree_skb(skb);
if (nla_get_u32(tb[IWL_TM_ATTR_COMMAND]) ==
IWL_TM_CMD_APP2DEV_BEGIN_TRACE)
iwl_trace_cleanup(priv);
return -EMSGSIZE;
}
/*
* This function handles the user application commands for driver.
*
* It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
* handlers respectively.
*
* If it's an unknown commdn ID, -ENOSYS is replied; otherwise, the returned
* value of the actual command execution is replied to the user application.
*
* If there's any message responding to the user space, IWL_TM_ATTR_SYNC_RSP
* is used for carry the message while IWL_TM_ATTR_COMMAND must set to
* IWL_TM_CMD_DEV2APP_SYNC_RSP.
*
* @hw: ieee80211_hw object that represents the device
* @tb: gnl message fields from the user space
*/
static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
{
struct iwl_priv *priv = hw->priv;
struct iwl_trans *trans = trans(priv);
struct sk_buff *skb;
unsigned char *rsp_data_ptr = NULL;
int status = 0, rsp_data_len = 0;
u32 devid;
switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
rsp_data_ptr = (unsigned char *)cfg(priv)->name;
rsp_data_len = strlen(cfg(priv)->name);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
rsp_data_len + 20);
if (!skb) {
IWL_DEBUG_INFO(priv,
"Error allocating memory\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND,
IWL_TM_CMD_DEV2APP_SYNC_RSP);
NLA_PUT(skb, IWL_TM_ATTR_SYNC_RSP,
rsp_data_len, rsp_data_ptr);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_DEBUG_INFO(priv, "Error sending msg : %d\n",
status);
break;
case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
status = iwl_load_ucode_wait_alive(trans, IWL_UCODE_INIT);
if (status)
IWL_DEBUG_INFO(priv,
"Error loading init ucode: %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
iwl_testmode_cfg_init_calib(priv);
iwl_trans_stop_device(trans);
break;
case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
status = iwl_load_ucode_wait_alive(trans, IWL_UCODE_REGULAR);
if (status) {
IWL_DEBUG_INFO(priv,
"Error loading runtime ucode: %d\n", status);
break;
}
status = iwl_alive_start(priv);
if (status)
IWL_DEBUG_INFO(priv,
"Error starting the device: %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
iwl_scan_cancel_timeout(priv, 200);
iwl_trans_stop_device(trans);
status = iwl_load_ucode_wait_alive(trans, IWL_UCODE_WOWLAN);
if (status) {
IWL_DEBUG_INFO(priv,
"Error loading WOWLAN ucode: %d\n", status);
break;
}
status = iwl_alive_start(priv);
if (status)
IWL_DEBUG_INFO(priv,
"Error starting the device: %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_GET_EEPROM:
if (priv->shrd->eeprom) {
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
cfg(priv)->base_params->eeprom_size + 20);
if (!skb) {
IWL_DEBUG_INFO(priv,
"Error allocating memory\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND,
IWL_TM_CMD_DEV2APP_EEPROM_RSP);
NLA_PUT(skb, IWL_TM_ATTR_EEPROM,
cfg(priv)->base_params->eeprom_size,
priv->shrd->eeprom);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_DEBUG_INFO(priv,
"Error sending msg : %d\n",
status);
} else
return -EFAULT;
break;
case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
if (!tb[IWL_TM_ATTR_FIXRATE]) {
IWL_DEBUG_INFO(priv,
"Error finding fixrate setting\n");
return -ENOMSG;
}
priv->tm_fixed_rate = nla_get_u32(tb[IWL_TM_ATTR_FIXRATE]);
break;
case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
IWL_INFO(priv, "uCode version raw: 0x%x\n", priv->ucode_ver);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
if (!skb) {
IWL_DEBUG_INFO(priv, "Error allocating memory\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_FW_VERSION, priv->ucode_ver);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_DEBUG_INFO(priv,
"Error sending msg : %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
devid = bus_get_hw_id(bus(priv));
IWL_INFO(priv, "hw version: 0x%x\n", devid);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
if (!skb) {
IWL_DEBUG_INFO(priv, "Error allocating memory\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_DEVICE_ID, devid);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_DEBUG_INFO(priv,
"Error sending msg : %d\n", status);
break;
default:
IWL_DEBUG_INFO(priv, "Unknown testmode driver command ID\n");
return -ENOSYS;
}
return status;
nla_put_failure:
kfree_skb(skb);
return -EMSGSIZE;
}
/*
* This function handles the user application commands for register access.
*
* It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
* handlers respectively.
*
* If it's an unknown commdn ID, -ENOSYS is returned; or -ENOMSG if the
* mandatory fields(IWL_TM_ATTR_REG_OFFSET,IWL_TM_ATTR_REG_VALUE32,
* IWL_TM_ATTR_REG_VALUE8) are missing; Otherwise 0 is replied indicating
* the success of the command execution.
*
* If IWL_TM_ATTR_COMMAND is IWL_TM_CMD_APP2DEV_REG_READ32, the register read
* value is returned with IWL_TM_ATTR_REG_VALUE32.
*
* @hw: ieee80211_hw object that represents the device
* @tb: gnl message fields from the user space
*/
static int iwl_testmode_reg(struct ieee80211_hw *hw, struct nlattr **tb)
{
struct iwl_priv *priv = hw->priv;
u32 ofs, val32;
u8 val8;
struct sk_buff *skb;
int status = 0;
if (!tb[IWL_TM_ATTR_REG_OFFSET]) {
IWL_DEBUG_INFO(priv, "Error finding register offset\n");
return -ENOMSG;
}
ofs = nla_get_u32(tb[IWL_TM_ATTR_REG_OFFSET]);
IWL_INFO(priv, "testmode register access command offset 0x%x\n", ofs);
switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
val32 = iwl_read32(bus(priv), ofs);
IWL_INFO(priv, "32bit value to read 0x%x\n", val32);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
if (!skb) {
IWL_DEBUG_INFO(priv, "Error allocating memory\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_REG_VALUE32, val32);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_DEBUG_INFO(priv,
"Error sending msg : %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
if (!tb[IWL_TM_ATTR_REG_VALUE32]) {
IWL_DEBUG_INFO(priv,
"Error finding value to write\n");
return -ENOMSG;
} else {
val32 = nla_get_u32(tb[IWL_TM_ATTR_REG_VALUE32]);
IWL_INFO(priv, "32bit value to write 0x%x\n", val32);
iwl_write32(bus(priv), ofs, val32);
}
break;
case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
if (!tb[IWL_TM_ATTR_REG_VALUE8]) {
IWL_DEBUG_INFO(priv, "Error finding value to write\n");
return -ENOMSG;
} else {
val8 = nla_get_u8(tb[IWL_TM_ATTR_REG_VALUE8]);
IWL_INFO(priv, "8bit value to write 0x%x\n", val8);
iwl_write8(bus(priv), ofs, val8);
}
break;
case IWL_TM_CMD_APP2DEV_INDIRECT_REG_READ32:
val32 = iwl_read_prph(bus(priv), ofs);
IWL_INFO(priv, "32bit value to read 0x%x\n", val32);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
if (!skb) {
IWL_DEBUG_INFO(priv, "Error allocating memory\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_REG_VALUE32, val32);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_DEBUG_INFO(priv,
"Error sending msg : %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_INDIRECT_REG_WRITE32:
if (!tb[IWL_TM_ATTR_REG_VALUE32]) {
IWL_DEBUG_INFO(priv,
"Error finding value to write\n");
return -ENOMSG;
} else {
val32 = nla_get_u32(tb[IWL_TM_ATTR_REG_VALUE32]);
IWL_INFO(priv, "32bit value to write 0x%x\n", val32);
iwl_write_prph(bus(priv), ofs, val32);
}
break;
default:
IWL_DEBUG_INFO(priv, "Unknown testmode register command ID\n");
return -ENOSYS;
}
return status;
nla_put_failure:
kfree_skb(skb);
return -EMSGSIZE;
}
static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
{
int ret;
struct wl1271_command *cmd;
struct sk_buff *skb;
u8 ie_id;
wl1271_debug(DEBUG_TESTMODE, "testmode cmd interrogate");
if (!tb[WL1271_TM_ATTR_IE_ID])
return -EINVAL;
ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
mutex_lock(&wl->mutex);
if (unlikely(wl->state != WLCORE_STATE_ON)) {
ret = -EINVAL;
goto out;
}
ret = wl1271_ps_elp_wakeup(wl);
if (ret < 0)
goto out;
cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
if (!cmd) {
ret = -ENOMEM;
goto out_sleep;
}
ret = wl1271_cmd_interrogate(wl, ie_id, cmd, sizeof(*cmd));
if (ret < 0) {
wl1271_warning("testmode cmd interrogate failed: %d", ret);
goto out_free;
}
skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, sizeof(*cmd));
if (!skb) {
ret = -ENOMEM;
goto out_free;
}
NLA_PUT(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd);
ret = cfg80211_testmode_reply(skb);
if (ret < 0)
goto out_free;
out_free:
kfree(cmd);
out_sleep:
wl1271_ps_elp_sleep(wl);
out:
mutex_unlock(&wl->mutex);
return ret;
nla_put_failure:
kfree_skb(skb);
ret = -EMSGSIZE;
goto out_free;
}
int
mtk_cfg80211_testmode_get_sta_statistics(IN struct wiphy *wiphy,
IN void *data, IN int len, IN P_GLUE_INFO_T prGlueInfo)
{
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 8, 0)
#define NLA_PUT(skb, attrtype, attrlen, data) \
do { \
if (unlikely(nla_put(skb, attrtype, attrlen, data) < 0)) \
printk("NLA PUT Error!!!!\n"); \
} while (0)
#define NLA_PUT_TYPE(skb, type, attrtype, value) \
do { \
type __tmp = value; \
NLA_PUT(skb, attrtype, sizeof(type), &__tmp); \
} while (0)
#define NLA_PUT_U8(skb, attrtype, value) \
NLA_PUT_TYPE(skb, u8, attrtype, value)
#define NLA_PUT_U16(skb, attrtype, value) \
NLA_PUT_TYPE(skb, u16, attrtype, value)
#define NLA_PUT_U32(skb, attrtype, value) \
NLA_PUT_TYPE(skb, u32, attrtype, value)
#endif
WLAN_STATUS rStatus = WLAN_STATUS_SUCCESS;
INT_32 i4Status = -EINVAL;
UINT_32 u4BufLen;
UINT_32 u4LinkScore;
UINT_32 u4TotalError;
UINT_32 u4TxExceedThresholdCount;
UINT_32 u4TxTotalCount;
P_NL80211_DRIVER_GET_STA_STATISTICS_PARAMS prParams = NULL;
PARAM_GET_STA_STA_STATISTICS rQueryStaStatistics;
struct sk_buff *skb;
ASSERT(wiphy);
ASSERT(prGlueInfo);
if (data && len) {
prParams = (P_NL80211_DRIVER_GET_STA_STATISTICS_PARAMS) data;
}
if (!prParams->aucMacAddr) {
DBGLOG(QM, TRACE, ("%s MAC Address is NULL\n", __func__));
i4Status = -EINVAL;
goto nla_put_failure;
}
skb = cfg80211_testmode_alloc_reply_skb(wiphy, sizeof(PARAM_GET_STA_STA_STATISTICS) + 1);
if (!skb) {
DBGLOG(QM, TRACE, ("%s allocate skb failed:\n", __func__));
i4Status = -ENOMEM;
goto nla_put_failure;
}
DBGLOG(QM, TRACE, ("Get [" MACSTR "] STA statistics\n", MAC2STR(prParams->aucMacAddr)));
kalMemZero(&rQueryStaStatistics, sizeof(rQueryStaStatistics));
COPY_MAC_ADDR(rQueryStaStatistics.aucMacAddr, prParams->aucMacAddr);
rStatus = kalIoctl(prGlueInfo,
wlanoidQueryStaStatistics,
&rQueryStaStatistics,
sizeof(rQueryStaStatistics), TRUE, FALSE, TRUE, TRUE, &u4BufLen);
/* Calcute Link Score */
u4TxExceedThresholdCount = rQueryStaStatistics.u4TxExceedThresholdCount;
u4TxTotalCount = rQueryStaStatistics.u4TxTotalCount;
u4TotalError = rQueryStaStatistics.u4TxFailCount + rQueryStaStatistics.u4TxLifeTimeoutCount;
/* u4LinkScore 10~100 , ExceedThreshold ratio 0~90 only */
/* u4LinkScore 0~9 , Drop packet ratio 0~9 and all packets exceed threshold */
if (u4TxTotalCount) {
if (u4TxExceedThresholdCount <= u4TxTotalCount) {
u4LinkScore = (90 - ((u4TxExceedThresholdCount * 90) / u4TxTotalCount));
} else {
u4LinkScore = 0;
}
} else {
u4LinkScore = 90;
}
u4LinkScore += 10;
if (u4LinkScore == 10) {
if (u4TotalError <= u4TxTotalCount) {
u4LinkScore = (10 - ((u4TotalError * 10) / u4TxTotalCount));
} else {
u4LinkScore = 0;
}
}
if (u4LinkScore > 100) {
u4LinkScore = 100;
}
NLA_PUT_U8(skb, NL80211_TESTMODE_STA_STATISTICS_INVALID, 0);
NLA_PUT_U8(skb, NL80211_TESTMODE_STA_STATISTICS_VERSION, NL80211_DRIVER_TESTMODE_VERSION);
NLA_PUT(skb, NL80211_TESTMODE_STA_STATISTICS_MAC, MAC_ADDR_LEN, prParams->aucMacAddr);
NLA_PUT_U8(skb, NL80211_TESTMODE_STA_STATISTICS_LINK_SCORE, u4LinkScore);
NLA_PUT_U32(skb, NL80211_TESTMODE_STA_STATISTICS_FLAG, rQueryStaStatistics.u4Flag);
/* FW part STA link status */
NLA_PUT_U8(skb, NL80211_TESTMODE_STA_STATISTICS_PER, rQueryStaStatistics.ucPer);
NLA_PUT_U8(skb, NL80211_TESTMODE_STA_STATISTICS_RSSI, rQueryStaStatistics.ucRcpi);
NLA_PUT_U32(skb, NL80211_TESTMODE_STA_STATISTICS_PHY_MODE, rQueryStaStatistics.u4PhyMode);
NLA_PUT_U16(skb, NL80211_TESTMODE_STA_STATISTICS_TX_RATE, rQueryStaStatistics.u2LinkSpeed);
NLA_PUT_U32(skb, NL80211_TESTMODE_STA_STATISTICS_FAIL_CNT,
rQueryStaStatistics.u4TxFailCount);
NLA_PUT_U32(skb, NL80211_TESTMODE_STA_STATISTICS_TIMEOUT_CNT,
rQueryStaStatistics.u4TxLifeTimeoutCount);
NLA_PUT_U32(skb, NL80211_TESTMODE_STA_STATISTICS_AVG_AIR_TIME,
rQueryStaStatistics.u4TxAverageAirTime);
/* Driver part link status */
NLA_PUT_U32(skb, NL80211_TESTMODE_STA_STATISTICS_TOTAL_CNT,
rQueryStaStatistics.u4TxTotalCount);
NLA_PUT_U32(skb, NL80211_TESTMODE_STA_STATISTICS_THRESHOLD_CNT,
rQueryStaStatistics.u4TxExceedThresholdCount);
NLA_PUT_U32(skb, NL80211_TESTMODE_STA_STATISTICS_AVG_PROCESS_TIME,
rQueryStaStatistics.u4TxAverageProcessTime);
/* Network counter */
NLA_PUT(skb,
NL80211_TESTMODE_STA_STATISTICS_TC_EMPTY_CNT_ARRAY,
sizeof(rQueryStaStatistics.au4TcResourceEmptyCount),
rQueryStaStatistics.au4TcResourceEmptyCount);
/* Sta queue length */
NLA_PUT(skb,
NL80211_TESTMODE_STA_STATISTICS_TC_QUE_LEN_ARRAY,
sizeof(rQueryStaStatistics.au4TcQueLen), rQueryStaStatistics.au4TcQueLen);
/* Global QM counter */
NLA_PUT(skb,
NL80211_TESTMODE_STA_STATISTICS_TC_AVG_QUE_LEN_ARRAY,
sizeof(rQueryStaStatistics.au4TcAverageQueLen),
rQueryStaStatistics.au4TcAverageQueLen);
NLA_PUT(skb,
NL80211_TESTMODE_STA_STATISTICS_TC_CUR_QUE_LEN_ARRAY,
sizeof(rQueryStaStatistics.au4TcCurrentQueLen),
rQueryStaStatistics.au4TcCurrentQueLen);
/* Reserved field */
NLA_PUT(skb,
NL80211_TESTMODE_STA_STATISTICS_RESERVED_ARRAY,
sizeof(rQueryStaStatistics.au4Reserved), rQueryStaStatistics.au4Reserved);
i4Status = cfg80211_testmode_reply(skb);
nla_put_failure:
return i4Status;
}
/*
* This function handles the user application commands for driver.
*
* It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
* handlers respectively.
*
* If it's an unknown commdn ID, -ENOSYS is replied; otherwise, the returned
* value of the actual command execution is replied to the user application.
*
* If there's any message responding to the user space, IWL_TM_ATTR_SYNC_RSP
* is used for carry the message while IWL_TM_ATTR_COMMAND must set to
* IWL_TM_CMD_DEV2APP_SYNC_RSP.
*
* @hw: ieee80211_hw object that represents the device
* @tb: gnl message fields from the user space
*/
static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
{
struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
struct iwl_trans *trans = trans(priv);
struct sk_buff *skb;
unsigned char *rsp_data_ptr = NULL;
int status = 0, rsp_data_len = 0;
u32 devid, inst_size = 0, data_size = 0;
const struct fw_img *img;
switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
rsp_data_ptr = (unsigned char *)cfg(priv)->name;
rsp_data_len = strlen(cfg(priv)->name);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
rsp_data_len + 20);
if (!skb) {
IWL_ERR(priv, "Memory allocation fail\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND,
IWL_TM_CMD_DEV2APP_SYNC_RSP);
NLA_PUT(skb, IWL_TM_ATTR_SYNC_RSP,
rsp_data_len, rsp_data_ptr);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_ERR(priv, "Error sending msg : %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_INIT);
if (status)
IWL_ERR(priv, "Error loading init ucode: %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
iwl_testmode_cfg_init_calib(priv);
priv->ucode_loaded = false;
iwl_trans_stop_device(trans);
break;
case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_REGULAR);
if (status) {
IWL_ERR(priv,
"Error loading runtime ucode: %d\n", status);
break;
}
status = iwl_alive_start(priv);
if (status)
IWL_ERR(priv,
"Error starting the device: %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
iwl_scan_cancel_timeout(priv, 200);
priv->ucode_loaded = false;
iwl_trans_stop_device(trans);
status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_WOWLAN);
if (status) {
IWL_ERR(priv,
"Error loading WOWLAN ucode: %d\n", status);
break;
}
status = iwl_alive_start(priv);
if (status)
IWL_ERR(priv,
"Error starting the device: %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_GET_EEPROM:
if (priv->shrd->eeprom) {
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
cfg(priv)->base_params->eeprom_size + 20);
if (!skb) {
IWL_ERR(priv, "Memory allocation fail\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND,
IWL_TM_CMD_DEV2APP_EEPROM_RSP);
NLA_PUT(skb, IWL_TM_ATTR_EEPROM,
cfg(priv)->base_params->eeprom_size,
priv->shrd->eeprom);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_ERR(priv, "Error sending msg : %d\n",
status);
} else
return -EFAULT;
break;
case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
if (!tb[IWL_TM_ATTR_FIXRATE]) {
IWL_ERR(priv, "Missing fixrate setting\n");
return -ENOMSG;
}
priv->tm_fixed_rate = nla_get_u32(tb[IWL_TM_ATTR_FIXRATE]);
break;
case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
IWL_INFO(priv, "uCode version raw: 0x%x\n",
priv->fw->ucode_ver);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
if (!skb) {
IWL_ERR(priv, "Memory allocation fail\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_FW_VERSION,
priv->fw->ucode_ver);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_ERR(priv, "Error sending msg : %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
devid = trans(priv)->hw_id;
IWL_INFO(priv, "hw version: 0x%x\n", devid);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
if (!skb) {
IWL_ERR(priv, "Memory allocation fail\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_DEVICE_ID, devid);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_ERR(priv, "Error sending msg : %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_GET_FW_INFO:
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20 + 8);
if (!skb) {
IWL_ERR(priv, "Memory allocation fail\n");
return -ENOMEM;
}
if (!priv->ucode_loaded) {
IWL_ERR(priv, "No uCode has not been loaded\n");
return -EINVAL;
} else {
img = &priv->fw->img[priv->shrd->ucode_type];
inst_size = img->sec[IWL_UCODE_SECTION_INST].len;
data_size = img->sec[IWL_UCODE_SECTION_DATA].len;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_FW_TYPE, priv->shrd->ucode_type);
NLA_PUT_U32(skb, IWL_TM_ATTR_FW_INST_SIZE, inst_size);
NLA_PUT_U32(skb, IWL_TM_ATTR_FW_DATA_SIZE, data_size);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_ERR(priv, "Error sending msg : %d\n", status);
break;
default:
IWL_ERR(priv, "Unknown testmode driver command ID\n");
return -ENOSYS;
}
return status;
nla_put_failure:
kfree_skb(skb);
return -EMSGSIZE;
}
/*
* This function handles the user application commands for register access.
*
* It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
* handlers respectively.
*
* If it's an unknown commdn ID, -ENOSYS is returned; or -ENOMSG if the
* mandatory fields(IWL_TM_ATTR_REG_OFFSET,IWL_TM_ATTR_REG_VALUE32,
* IWL_TM_ATTR_REG_VALUE8) are missing; Otherwise 0 is replied indicating
* the success of the command execution.
*
* If IWL_TM_ATTR_COMMAND is IWL_TM_CMD_APP2DEV_REG_READ32, the register read
* value is returned with IWL_TM_ATTR_REG_VALUE32.
*
* @hw: ieee80211_hw object that represents the device
* @tb: gnl message fields from the user space
*/
static int iwl_testmode_reg(struct ieee80211_hw *hw, struct nlattr **tb)
{
struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
u32 ofs, val32, cmd;
u8 val8;
struct sk_buff *skb;
int status = 0;
if (!tb[IWL_TM_ATTR_REG_OFFSET]) {
IWL_ERR(priv, "Missing register offset\n");
return -ENOMSG;
}
ofs = nla_get_u32(tb[IWL_TM_ATTR_REG_OFFSET]);
IWL_INFO(priv, "testmode register access command offset 0x%x\n", ofs);
/* Allow access only to FH/CSR/HBUS in direct mode.
Since we don't have the upper bounds for the CSR and HBUS segments,
we will use only the upper bound of FH for sanity check. */
cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
if ((cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32 ||
cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32 ||
cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8) &&
(ofs >= FH_MEM_UPPER_BOUND)) {
IWL_ERR(priv, "offset out of segment (0x0 - 0x%x)\n",
FH_MEM_UPPER_BOUND);
return -EINVAL;
}
switch (cmd) {
case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
val32 = iwl_read_direct32(trans(priv), ofs);
IWL_INFO(priv, "32bit value to read 0x%x\n", val32);
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
if (!skb) {
IWL_ERR(priv, "Memory allocation fail\n");
return -ENOMEM;
}
NLA_PUT_U32(skb, IWL_TM_ATTR_REG_VALUE32, val32);
status = cfg80211_testmode_reply(skb);
if (status < 0)
IWL_ERR(priv, "Error sending msg : %d\n", status);
break;
case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
if (!tb[IWL_TM_ATTR_REG_VALUE32]) {
IWL_ERR(priv, "Missing value to write\n");
return -ENOMSG;
} else {
val32 = nla_get_u32(tb[IWL_TM_ATTR_REG_VALUE32]);
IWL_INFO(priv, "32bit value to write 0x%x\n", val32);
iwl_write_direct32(trans(priv), ofs, val32);
}
break;
case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
if (!tb[IWL_TM_ATTR_REG_VALUE8]) {
IWL_ERR(priv, "Missing value to write\n");
return -ENOMSG;
} else {
val8 = nla_get_u8(tb[IWL_TM_ATTR_REG_VALUE8]);
IWL_INFO(priv, "8bit value to write 0x%x\n", val8);
iwl_write8(trans(priv), ofs, val8);
}
break;
default:
IWL_ERR(priv, "Unknown testmode register command ID\n");
return -ENOSYS;
}
return status;
nla_put_failure:
kfree_skb(skb);
return -EMSGSIZE;
}
/*
* This function handles the user application commands to the ucode.
*
* It retrieves the mandatory fields IWL_TM_ATTR_UCODE_CMD_ID and
* IWL_TM_ATTR_UCODE_CMD_DATA and calls to the handler to send the
* host command to the ucode.
*
* If any mandatory field is missing, -ENOMSG is replied to the user space
* application; otherwise, waits for the host command to be sent and checks
* the return code. In case or error, it is returned, otherwise a reply is
* allocated and the reply RX packet
* is returned.
*
* @hw: ieee80211_hw object that represents the device
* @tb: gnl message fields from the user space
*/
static int iwl_testmode_ucode(struct ieee80211_hw *hw, struct nlattr **tb)
{
struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
struct iwl_host_cmd cmd;
struct iwl_rx_packet *pkt;
struct sk_buff *skb;
void *reply_buf;
u32 reply_len;
int ret;
bool cmd_want_skb;
memset(&cmd, 0, sizeof(struct iwl_host_cmd));
if (!tb[IWL_TM_ATTR_UCODE_CMD_ID] ||
!tb[IWL_TM_ATTR_UCODE_CMD_DATA]) {
IWL_ERR(priv, "Missing ucode command mandatory fields\n");
return -ENOMSG;
}
cmd.flags = CMD_ON_DEMAND | CMD_SYNC;
cmd_want_skb = nla_get_flag(tb[IWL_TM_ATTR_UCODE_CMD_SKB]);
if (cmd_want_skb)
cmd.flags |= CMD_WANT_SKB;
cmd.id = nla_get_u8(tb[IWL_TM_ATTR_UCODE_CMD_ID]);
cmd.data[0] = nla_data(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
cmd.len[0] = nla_len(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
cmd.dataflags[0] = IWL_HCMD_DFL_NOCOPY;
IWL_DEBUG_INFO(priv, "testmode ucode command ID 0x%x, flags 0x%x,"
" len %d\n", cmd.id, cmd.flags, cmd.len[0]);
ret = iwl_dvm_send_cmd(priv, &cmd);
if (ret) {
IWL_ERR(priv, "Failed to send hcmd\n");
return ret;
}
if (!cmd_want_skb)
return ret;
/* Handling return of SKB to the user */
pkt = cmd.resp_pkt;
if (!pkt) {
IWL_ERR(priv, "HCMD received a null response packet\n");
return ret;
}
reply_len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, reply_len + 20);
reply_buf = kmalloc(reply_len, GFP_KERNEL);
if (!skb || !reply_buf) {
kfree_skb(skb);
kfree(reply_buf);
return -ENOMEM;
}
/* The reply is in a page, that we cannot send to user space. */
memcpy(reply_buf, &(pkt->hdr), reply_len);
iwl_free_resp(&cmd);
NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT);
NLA_PUT(skb, IWL_TM_ATTR_UCODE_RX_PKT, reply_len, reply_buf);
return cfg80211_testmode_reply(skb);
nla_put_failure:
IWL_DEBUG_INFO(priv, "Failed creating NL attributes\n");
return -ENOMSG;
}
int dhd_cfg80211_testmode_cmd(struct wiphy *wiphy, void *data, int len)
#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(3, 11, 0) */
{
struct sk_buff *reply;
struct bcm_cfg80211 *cfg;
dhd_pub_t *dhd;
struct bcm_nlmsg_hdr *nlioc = data;
dhd_ioctl_t ioc = { 0 };
int err = 0;
void *buf = NULL, *cur;
u16 buflen;
u16 maxmsglen = PAGE_SIZE - 0x100;
bool newbuf = false;
int8 index = 0;
#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 11, 0))
struct net_device *ndev = NULL;
#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(3, 11, 0) */
WL_TRACE(("entry: cmd = %d\n", nlioc->cmd));
cfg = wiphy_priv(wiphy);
dhd = cfg->pub;
DHD_OS_WAKE_LOCK(dhd);
/* send to dongle only if we are not waiting for reload already */
if (dhd->hang_was_sent) {
WL_ERR(("HANG was sent up earlier\n"));
DHD_OS_WAKE_LOCK_CTRL_TIMEOUT_ENABLE(dhd, DHD_EVENT_TIMEOUT_MS);
DHD_OS_WAKE_UNLOCK(dhd);
return OSL_ERROR(BCME_DONGLE_DOWN);
}
len -= sizeof(struct bcm_nlmsg_hdr);
if (nlioc->len > 0) {
if (nlioc->len <= len) {
buf = (void *)nlioc + nlioc->offset;
*(char *)(buf + nlioc->len) = '\0';
} else {
if (nlioc->len > DHD_IOCTL_MAXLEN)
nlioc->len = DHD_IOCTL_MAXLEN;
buf = vzalloc(nlioc->len);
if (!buf)
return -ENOMEM;
newbuf = true;
memcpy(buf, (void *)nlioc + nlioc->offset, len);
*(char *)(buf + len) = '\0';
}
}
#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 11, 0))
ndev = wdev_to_wlc_ndev(wdev, cfg);
index = dhd_net2idx(dhd->info, ndev);
if (index == DHD_BAD_IF) {
WL_ERR(("Bad ifidx from wdev:%p\n", wdev));
return BCME_ERROR;
}
#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(3, 11, 0) */
ioc.cmd = nlioc->cmd;
ioc.len = nlioc->len;
ioc.set = nlioc->set;
ioc.driver = nlioc->magic;
err = dhd_ioctl_process(dhd, index, &ioc, buf);
if (err) {
WL_TRACE(("dhd_ioctl_process return err %d\n", err));
err = OSL_ERROR(err);
goto done;
}
cur = buf;
while (nlioc->len > 0) {
buflen = nlioc->len > maxmsglen ? maxmsglen : nlioc->len;
nlioc->len -= buflen;
reply = cfg80211_testmode_alloc_reply_skb(wiphy, buflen+4);
if (!reply) {
WL_ERR(("Failed to allocate reply msg\n"));
err = -ENOMEM;
break;
}
if (nla_put(reply, BCM_NLATTR_DATA, buflen, cur) ||
nla_put_u16(reply, BCM_NLATTR_LEN, buflen)) {
kfree_skb(reply);
err = -ENOBUFS;
break;
}
do {
err = cfg80211_testmode_reply(reply);
} while (err == -EAGAIN);
if (err) {
WL_ERR(("testmode reply failed:%d\n", err));
break;
}
cur += buflen;
}
done:
if (newbuf)
vfree(buf);
DHD_OS_WAKE_UNLOCK(dhd);
return err;
}
static struct sk_buff*
iwl_testmode_alloc_reply(struct iwl_op_mode *op_mode, int len)
{
struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
return cfg80211_testmode_alloc_reply_skb(priv->hw->wiphy, len);
}
struct sk_buff *iwl_mvm_testmode_alloc_reply(struct iwl_op_mode *op_mode,
int len)
{
struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
return cfg80211_testmode_alloc_reply_skb(mvm->hw->wiphy, len);
}