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 "debug.h"
  30 #include "acx.h"
  31 #include "reg.h"
  32
  33 #define WL1271_TM_MAX_DATA_LENGTH 1024
  34
  35 enum wl1271_tm_commands {
  36         WL1271_TM_CMD_UNSPEC,
  37         WL1271_TM_CMD_TEST,
  38         WL1271_TM_CMD_INTERROGATE,
  39         WL1271_TM_CMD_CONFIGURE,
  40         WL1271_TM_CMD_SET_PLT_MODE,
  41         WL1271_TM_CMD_RECOVER,
  42
  43         __WL1271_TM_CMD_AFTER_LAST
  44 };
  45 #define WL1271_TM_CMD_MAX (__WL1271_TM_CMD_AFTER_LAST - 1)
  46
  47 enum wl1271_tm_attrs {
  48         WL1271_TM_ATTR_UNSPEC,
  49         WL1271_TM_ATTR_CMD_ID,
  50         WL1271_TM_ATTR_ANSWER,
  51         WL1271_TM_ATTR_DATA,
  52         WL1271_TM_ATTR_IE_ID,
  53         WL1271_TM_ATTR_PLT_MODE,
  54
  55         __WL1271_TM_ATTR_AFTER_LAST
  56 };
  57 #define WL1271_TM_ATTR_MAX (__WL1271_TM_ATTR_AFTER_LAST - 1)
  58
  59 static struct nla_policy wl1271_tm_policy[WL1271_TM_ATTR_MAX + 1] = {
  60         [WL1271_TM_ATTR_CMD_ID] =       { .type = NLA_U32 },
  61         [WL1271_TM_ATTR_ANSWER] =       { .type = NLA_U8 },
  62         [WL1271_TM_ATTR_DATA] =         { .type = NLA_BINARY,
  63                                           .len = WL1271_TM_MAX_DATA_LENGTH },
  64         [WL1271_TM_ATTR_IE_ID] =        { .type = NLA_U32 },
  65         [WL1271_TM_ATTR_PLT_MODE] =     { .type = NLA_U32 },
  66 };
  67
  68
  69 static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
  70 {
  71         int buf_len, ret, len;
  72         struct sk_buff *skb;
  73         void *buf;
  74         u8 answer = 0;
  75
  76         wl1271_debug(DEBUG_TESTMODE, "testmode cmd test");
  77
  78         if (!tb[WL1271_TM_ATTR_DATA])
  79                 return -EINVAL;
  80
  81         buf = nla_data(tb[WL1271_TM_ATTR_DATA]);
  82         buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]);
  83
  84         if (tb[WL1271_TM_ATTR_ANSWER])
  85                 answer = nla_get_u8(tb[WL1271_TM_ATTR_ANSWER]);
  86
  87         if (buf_len > sizeof(struct wl1271_command))
  88                 return -EMSGSIZE;
  89
  90         mutex_lock(&wl->mutex);
  91         ret = wl1271_cmd_test(wl, buf, buf_len, answer);
  92         mutex_unlock(&wl->mutex);
  93
  94         if (ret < 0) {
  95                 wl1271_warning("testmode cmd test failed: %d", ret);
  96                 return ret;
  97         }
  98
  99         if (answer) {
 100                 len = nla_total_size(buf_len);
 101                 skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
 102                 if (!skb)
 103                         return -ENOMEM;
 104
 105                 NLA_PUT(skb, WL1271_TM_ATTR_DATA, buf_len, buf);
 106                 ret = cfg80211_testmode_reply(skb);
 107                 if (ret < 0)
 108                         return ret;
 109         }
 110
 111         return 0;
 112
 113 nla_put_failure:
 114         kfree_skb(skb);
 115         return -EMSGSIZE;
 116 }
 117
 118 static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
 119 {
 120         int ret;
 121         struct wl1271_command *cmd;
 122         struct sk_buff *skb;
 123         u8 ie_id;
 124
 125         wl1271_debug(DEBUG_TESTMODE, "testmode cmd interrogate");
 126
 127         if (!tb[WL1271_TM_ATTR_IE_ID])
 128                 return -EINVAL;
 129
 130         ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
 131
 132         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 133         if (!cmd)
 134                 return -ENOMEM;
 135
 136         mutex_lock(&wl->mutex);
 137         ret = wl1271_cmd_interrogate(wl, ie_id, cmd, sizeof(*cmd));
 138         mutex_unlock(&wl->mutex);
 139
 140         if (ret < 0) {
 141                 wl1271_warning("testmode cmd interrogate failed: %d", ret);
 142                 kfree(cmd);
 143                 return ret;
 144         }
 145
 146         skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, sizeof(*cmd));
 147         if (!skb) {
 148                 kfree(cmd);
 149                 return -ENOMEM;
 150         }
 151
 152         NLA_PUT(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd);
 153
 154         return 0;
 155
 156 nla_put_failure:
 157         kfree_skb(skb);
 158         return -EMSGSIZE;
 159 }
 160
 161 static int wl1271_tm_cmd_configure(struct wl1271 *wl, struct nlattr *tb[])
 162 {
 163         int buf_len, ret;
 164         void *buf;
 165         u8 ie_id;
 166
 167         wl1271_debug(DEBUG_TESTMODE, "testmode cmd configure");
 168
 169         if (!tb[WL1271_TM_ATTR_DATA])
 170                 return -EINVAL;
 171         if (!tb[WL1271_TM_ATTR_IE_ID])
 172                 return -EINVAL;
 173
 174         ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
 175         buf = nla_data(tb[WL1271_TM_ATTR_DATA]);
 176         buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]);
 177
 178         if (buf_len > sizeof(struct wl1271_command))
 179                 return -EMSGSIZE;
 180
 181         mutex_lock(&wl->mutex);
 182         ret = wl1271_cmd_configure(wl, ie_id, buf, buf_len);
 183         mutex_unlock(&wl->mutex);
 184
 185         if (ret < 0) {
 186                 wl1271_warning("testmode cmd configure failed: %d", ret);
 187                 return ret;
 188         }
 189
 190         return 0;
 191 }
 192
 193 static int wl1271_tm_cmd_set_plt_mode(struct wl1271 *wl, struct nlattr *tb[])
 194 {
 195         u32 val;
 196         int ret;
 197
 198         wl1271_debug(DEBUG_TESTMODE, "testmode cmd set plt mode");
 199
 200         if (!tb[WL1271_TM_ATTR_PLT_MODE])
 201                 return -EINVAL;
 202
 203         val = nla_get_u32(tb[WL1271_TM_ATTR_PLT_MODE]);
 204
 205         switch (val) {
 206         case 0:
 207                 ret = wl1271_plt_stop(wl);
 208                 break;
 209         case 1:
 210                 ret = wl1271_plt_start(wl);
 211                 break;
 212         default:
 213                 ret = -EINVAL;
 214                 break;
 215         }
 216
 217         return ret;
 218 }
 219
 220 static int wl1271_tm_cmd_recover(struct wl1271 *wl, struct nlattr *tb[])
 221 {
 222         wl1271_debug(DEBUG_TESTMODE, "testmode cmd recover");
 223
 224         wl12xx_queue_recovery_work(wl);
 225
 226         return 0;
 227 }
 228
 229 int wl1271_tm_cmd(struct ieee80211_hw *hw, void *data, int len)
 230 {
 231         struct wl1271 *wl = hw->priv;
 232         struct nlattr *tb[WL1271_TM_ATTR_MAX + 1];
 233         int err;
 234
 235         err = nla_parse(tb, WL1271_TM_ATTR_MAX, data, len, wl1271_tm_policy);
 236         if (err)
 237                 return err;
 238
 239         if (!tb[WL1271_TM_ATTR_CMD_ID])
 240                 return -EINVAL;
 241
 242         switch (nla_get_u32(tb[WL1271_TM_ATTR_CMD_ID])) {
 243         case WL1271_TM_CMD_TEST:
 244                 return wl1271_tm_cmd_test(wl, tb);
 245         case WL1271_TM_CMD_INTERROGATE:
 246                 return wl1271_tm_cmd_interrogate(wl, tb);
 247         case WL1271_TM_CMD_CONFIGURE:
 248                 return wl1271_tm_cmd_configure(wl, tb);
 249         case WL1271_TM_CMD_SET_PLT_MODE:
 250                 return wl1271_tm_cmd_set_plt_mode(wl, tb);
 251         case WL1271_TM_CMD_RECOVER:
 252                 return wl1271_tm_cmd_recover(wl, tb);
 253         default:
 254                 return -EOPNOTSUPP;
 255         }
 256 }