drivers/net/wireless/wl12xx/testmode.c

   1 /*
   2  * This file is part of wl1271
   3  *
   4  * Copyright (C) 2010 Nokia Corporation
   5  *
   6  * Contact: Luciano Coelho <luciano.coelho@nokia.com>
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU General Public License
  10  * version 2 as published by the Free Software Foundation.
  11  *
  12  * This program is distributed in the hope that it will be useful, but
  13  * WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15  * General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, write to the Free Software
  19  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
  20  * 02110-1301 USA
  21  *
  22  */
  23 #include "testmode.h"
  24
  25 #include <linux/slab.h>
  26 #include <net/genetlink.h>
  27
  28 #include "wl12xx.h"
  29 #include "acx.h"
  30 #include "reg.h"
  31
  32 #define WL1271_TM_MAX_DATA_LENGTH 1024
  33
  34 enum wl1271_tm_commands {
  35         WL1271_TM_CMD_UNSPEC,
  36         WL1271_TM_CMD_TEST,
  37         WL1271_TM_CMD_INTERROGATE,
  38         WL1271_TM_CMD_CONFIGURE,
  39         WL1271_TM_CMD_SET_PLT_MODE,
  40         WL1271_TM_CMD_RECOVER,
  41
  42         __WL1271_TM_CMD_AFTER_LAST
  43 };
  44 #define WL1271_TM_CMD_MAX (__WL1271_TM_CMD_AFTER_LAST - 1)
  45
  46 enum wl1271_tm_attrs {
  47         WL1271_TM_ATTR_UNSPEC,
  48         WL1271_TM_ATTR_CMD_ID,
  49         WL1271_TM_ATTR_ANSWER,
  50         WL1271_TM_ATTR_DATA,
  51         WL1271_TM_ATTR_IE_ID,
  52         WL1271_TM_ATTR_PLT_MODE,
  53
  54         __WL1271_TM_ATTR_AFTER_LAST
  55 };
  56 #define WL1271_TM_ATTR_MAX (__WL1271_TM_ATTR_AFTER_LAST - 1)
  57
  58 static struct nla_policy wl1271_tm_policy[WL1271_TM_ATTR_MAX + 1] = {
  59         [WL1271_TM_ATTR_CMD_ID] =       { .type = NLA_U32 },
  60         [WL1271_TM_ATTR_ANSWER] =       { .type = NLA_U8 },
  61         [WL1271_TM_ATTR_DATA] =         { .type = NLA_BINARY,
  62                                           .len = WL1271_TM_MAX_DATA_LENGTH },
  63         [WL1271_TM_ATTR_IE_ID] =        { .type = NLA_U32 },
  64         [WL1271_TM_ATTR_PLT_MODE] =     { .type = NLA_U32 },
  65 };
  66
  67
  68 static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
  69 {
  70         int buf_len, ret, len;
  71         struct sk_buff *skb;
  72         void *buf;
  73         u8 answer = 0;
  74
  75         wl1271_debug(DEBUG_TESTMODE, "testmode cmd test");
  76
  77         if (!tb[WL1271_TM_ATTR_DATA])
  78                 return -EINVAL;
  79
  80         buf = nla_data(tb[WL1271_TM_ATTR_DATA]);
  81         buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]);
  82
  83         if (tb[WL1271_TM_ATTR_ANSWER])
  84                 answer = nla_get_u8(tb[WL1271_TM_ATTR_ANSWER]);
  85
  86         if (buf_len > sizeof(struct wl1271_command))
  87                 return -EMSGSIZE;
  88
  89         mutex_lock(&wl->mutex);
  90         ret = wl1271_cmd_test(wl, buf, buf_len, answer);
  91         mutex_unlock(&wl->mutex);
  92
  93         if (ret < 0) {
  94                 wl1271_warning("testmode cmd test failed: %d", ret);
  95                 return ret;
  96         }
  97
  98         if (answer) {
  99                 len = nla_total_size(buf_len);
 100                 skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
 101                 if (!skb)
 102                         return -ENOMEM;
 103
 104                 NLA_PUT(skb, WL1271_TM_ATTR_DATA, buf_len, buf);
 105                 ret = cfg80211_testmode_reply(skb);
 106                 if (ret < 0)
 107                         return ret;
 108         }
 109
 110         return 0;
 111
 112 nla_put_failure:
 113         kfree_skb(skb);
 114         return -EMSGSIZE;
 115 }
 116
 117 static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
 118 {
 119         int ret;
 120         struct wl1271_command *cmd;
 121         struct sk_buff *skb;
 122         u8 ie_id;
 123
 124         wl1271_debug(DEBUG_TESTMODE, "testmode cmd interrogate");
 125
 126         if (!tb[WL1271_TM_ATTR_IE_ID])
 127                 return -EINVAL;
 128
 129         ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
 130
 131         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 132         if (!cmd)
 133                 return -ENOMEM;
 134
 135         mutex_lock(&wl->mutex);
 136         ret = wl1271_cmd_interrogate(wl, ie_id, cmd, sizeof(*cmd));
 137         mutex_unlock(&wl->mutex);
 138
 139         if (ret < 0) {
 140                 wl1271_warning("testmode cmd interrogate failed: %d", ret);
 141                 kfree(cmd);
 142                 return ret;
 143         }
 144
 145         skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, sizeof(*cmd));
 146         if (!skb) {
 147                 kfree(cmd);
 148                 return -ENOMEM;
 149         }
 150
 151         NLA_PUT(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd);
 152
 153         return 0;
 154
 155 nla_put_failure:
 156         kfree_skb(skb);
 157         return -EMSGSIZE;
 158 }
 159
 160 static int wl1271_tm_cmd_configure(struct wl1271 *wl, struct nlattr *tb[])
 161 {
 162         int buf_len, ret;
 163         void *buf;
 164         u8 ie_id;
 165
 166         wl1271_debug(DEBUG_TESTMODE, "testmode cmd configure");
 167
 168         if (!tb[WL1271_TM_ATTR_DATA])
 169                 return -EINVAL;
 170         if (!tb[WL1271_TM_ATTR_IE_ID])
 171                 return -EINVAL;
 172
 173         ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
 174         buf = nla_data(tb[WL1271_TM_ATTR_DATA]);
 175         buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]);
 176
 177         if (buf_len > sizeof(struct wl1271_command))
 178                 return -EMSGSIZE;
 179
 180         mutex_lock(&wl->mutex);
 181         ret = wl1271_cmd_configure(wl, ie_id, buf, buf_len);
 182         mutex_unlock(&wl->mutex);
 183
 184         if (ret < 0) {
 185                 wl1271_warning("testmode cmd configure failed: %d", ret);
 186                 return ret;
 187         }
 188
 189         return 0;
 190 }
 191
 192 static int wl1271_tm_cmd_set_plt_mode(struct wl1271 *wl, struct nlattr *tb[])
 193 {
 194         u32 val;
 195         int ret;
 196
 197         wl1271_debug(DEBUG_TESTMODE, "testmode cmd set plt mode");
 198
 199         if (!tb[WL1271_TM_ATTR_PLT_MODE])
 200                 return -EINVAL;
 201
 202         val = nla_get_u32(tb[WL1271_TM_ATTR_PLT_MODE]);
 203
 204         switch (val) {
 205         case 0:
 206                 ret = wl1271_plt_stop(wl);
 207                 break;
 208         case 1:
 209                 ret = wl1271_plt_start(wl);
 210                 break;
 211         default:
 212                 ret = -EINVAL;
 213                 break;
 214         }
 215
 216         return ret;
 217 }
 218
 219 static int wl1271_tm_cmd_recover(struct wl1271 *wl, struct nlattr *tb[])
 220 {
 221         wl1271_debug(DEBUG_TESTMODE, "testmode cmd recover");
 222
 223         wl12xx_queue_recovery_work(wl);
 224
 225         return 0;
 226 }
 227
 228 int wl1271_tm_cmd(struct ieee80211_hw *hw, void *data, int len)
 229 {
 230         struct wl1271 *wl = hw->priv;
 231         struct nlattr *tb[WL1271_TM_ATTR_MAX + 1];
 232         int err;
 233
 234         err = nla_parse(tb, WL1271_TM_ATTR_MAX, data, len, wl1271_tm_policy);
 235         if (err)
 236                 return err;
 237
 238         if (!tb[WL1271_TM_ATTR_CMD_ID])
 239                 return -EINVAL;
 240
 241         switch (nla_get_u32(tb[WL1271_TM_ATTR_CMD_ID])) {
 242         case WL1271_TM_CMD_TEST:
 243                 return wl1271_tm_cmd_test(wl, tb);
 244         case WL1271_TM_CMD_INTERROGATE:
 245                 return wl1271_tm_cmd_interrogate(wl, tb);
 246         case WL1271_TM_CMD_CONFIGURE:
 247                 return wl1271_tm_cmd_configure(wl, tb);
 248         case WL1271_TM_CMD_SET_PLT_MODE:
 249                 return wl1271_tm_cmd_set_plt_mode(wl, tb);
 250         case WL1271_TM_CMD_RECOVER:
 251                 return wl1271_tm_cmd_recover(wl, tb);
 252         default:
 253                 return -EOPNOTSUPP;
 254         }
 255 }