/drivers/net/wireless/bcmdhd/wl_linux_mon.c

https://bitbucket.org/cyanogenmod/android_kernel_asus_tf300t · C · 409 lines · 294 code · 56 blank · 59 comment · 59 complexity · a5540ce6c55d41040702dad3ea2facf1 MD5 · raw file

  1. /*
  2. * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
  3. *
  4. * Copyright (C) 1999-2011, Broadcom Corporation
  5. *
  6. * Unless you and Broadcom execute a separate written software license
  7. * agreement governing use of this software, this software is licensed to you
  8. * under the terms of the GNU General Public License version 2 (the "GPL"),
  9. * available at http://www.broadcom.com/licenses/GPLv2.php, with the
  10. * following added to such license:
  11. *
  12. * As a special exception, the copyright holders of this software give you
  13. * permission to link this software with independent modules, and to copy and
  14. * distribute the resulting executable under terms of your choice, provided that
  15. * you also meet, for each linked independent module, the terms and conditions of
  16. * the license of that module. An independent module is a module which is not
  17. * derived from this software. The special exception does not apply to any
  18. * modifications of the software.
  19. *
  20. * Notwithstanding the above, under no circumstances may you combine this
  21. * software in any way with any other Broadcom software provided under a license
  22. * other than the GPL, without Broadcom's express prior written consent.
  23. *
  24. * $Id: wl_linux_mon.c 303266 2011-12-16 00:15:23Z $
  25. */
  26. #include <linux/string.h>
  27. #include <linux/module.h>
  28. #include <linux/netdevice.h>
  29. #include <linux/etherdevice.h>
  30. #include <linux/if_arp.h>
  31. #include <linux/ieee80211.h>
  32. #include <linux/rtnetlink.h>
  33. #include <net/ieee80211_radiotap.h>
  34. #include <wlioctl.h>
  35. #include <bcmutils.h>
  36. #include <linux_osl.h>
  37. #include <dhd_dbg.h>
  38. #include <dngl_stats.h>
  39. #include <dhd.h>
  40. typedef enum monitor_states
  41. {
  42. MONITOR_STATE_DEINIT = 0x0,
  43. MONITOR_STATE_INIT = 0x1,
  44. MONITOR_STATE_INTERFACE_ADDED = 0x2,
  45. MONITOR_STATE_INTERFACE_DELETED = 0x4
  46. } monitor_states_t;
  47. extern int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
  48. /**
  49. * Local declarations and defintions (not exposed)
  50. */
  51. #define MON_PRINT(format, ...) printf("DHD-MON: %s " format, __func__, ##__VA_ARGS__)
  52. #define MON_TRACE MON_PRINT
  53. typedef struct monitor_interface {
  54. int radiotap_enabled;
  55. struct net_device* real_ndev; /* The real interface that the monitor is on */
  56. struct net_device* mon_ndev;
  57. } monitor_interface;
  58. typedef struct dhd_linux_monitor {
  59. void *dhd_pub;
  60. monitor_states_t monitor_state;
  61. monitor_interface mon_if[DHD_MAX_IFS];
  62. struct mutex lock; /* lock to protect mon_if */
  63. } dhd_linux_monitor_t;
  64. static dhd_linux_monitor_t g_monitor;
  65. static struct net_device* lookup_real_netdev(char *name);
  66. static monitor_interface* ndev_to_monif(struct net_device *ndev);
  67. static int dhd_mon_if_open(struct net_device *ndev);
  68. static int dhd_mon_if_stop(struct net_device *ndev);
  69. static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
  70. static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
  71. static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
  72. static const struct net_device_ops dhd_mon_if_ops = {
  73. .ndo_open = dhd_mon_if_open,
  74. .ndo_stop = dhd_mon_if_stop,
  75. .ndo_start_xmit = dhd_mon_if_subif_start_xmit,
  76. .ndo_set_multicast_list = dhd_mon_if_set_multicast_list,
  77. .ndo_set_mac_address = dhd_mon_if_change_mac,
  78. };
  79. /**
  80. * Local static function defintions
  81. */
  82. /* Look up dhd's net device table to find a match (e.g. interface "eth0" is a match for "mon.eth0"
  83. * "p2p-eth0-0" is a match for "mon.p2p-eth0-0")
  84. */
  85. static struct net_device* lookup_real_netdev(char *name)
  86. {
  87. int i;
  88. int len = 0;
  89. int last_name_len = 0;
  90. struct net_device *ndev;
  91. struct net_device *ndev_found = NULL;
  92. /* We need to find interface "p2p-p2p-0" corresponding to monitor interface "mon-p2p-0",
  93. * Once mon iface name reaches IFNAMSIZ, it is reset to p2p0-0 and corresponding mon
  94. * iface would be mon-p2p0-0.
  95. */
  96. for (i = 0; i < DHD_MAX_IFS; i++) {
  97. ndev = dhd_idx2net(g_monitor.dhd_pub, i);
  98. /* Skip "p2p" and look for "-p2p0-x" in monitor interface name. If it
  99. * it matches, then this netdev is the corresponding real_netdev.
  100. */
  101. if (ndev && strstr(ndev->name, "p2p-p2p0")) {
  102. len = strlen("p2p");
  103. } else {
  104. /* if p2p- is not present, then the IFNAMSIZ have reached and name
  105. * would have got reset. In this casse,look for p2p0-x in mon-p2p0-x
  106. */
  107. len = 0;
  108. }
  109. if (ndev && strstr(name, (ndev->name + len))) {
  110. if (strlen(ndev->name) > last_name_len) {
  111. ndev_found = ndev;
  112. last_name_len = strlen(ndev->name);
  113. }
  114. }
  115. }
  116. return ndev_found;
  117. }
  118. static monitor_interface* ndev_to_monif(struct net_device *ndev)
  119. {
  120. int i;
  121. for (i = 0; i < DHD_MAX_IFS; i++) {
  122. if (g_monitor.mon_if[i].mon_ndev == ndev)
  123. return &g_monitor.mon_if[i];
  124. }
  125. return NULL;
  126. }
  127. static int dhd_mon_if_open(struct net_device *ndev)
  128. {
  129. int ret = 0;
  130. MON_PRINT("enter\n");
  131. return ret;
  132. }
  133. static int dhd_mon_if_stop(struct net_device *ndev)
  134. {
  135. int ret = 0;
  136. MON_PRINT("enter\n");
  137. return ret;
  138. }
  139. static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
  140. {
  141. int ret = 0;
  142. int rtap_len;
  143. int qos_len = 0;
  144. int dot11_hdr_len = 24;
  145. int snap_len = 6;
  146. unsigned char *pdata;
  147. unsigned short frame_ctl;
  148. unsigned char src_mac_addr[6];
  149. unsigned char dst_mac_addr[6];
  150. struct ieee80211_hdr *dot11_hdr;
  151. struct ieee80211_radiotap_header *rtap_hdr;
  152. monitor_interface* mon_if;
  153. MON_PRINT("enter\n");
  154. mon_if = ndev_to_monif(ndev);
  155. if (mon_if == NULL || mon_if->real_ndev == NULL) {
  156. MON_PRINT(" cannot find matched net dev, skip the packet\n");
  157. goto fail;
  158. }
  159. if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
  160. goto fail;
  161. rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
  162. if (unlikely(rtap_hdr->it_version))
  163. goto fail;
  164. rtap_len = ieee80211_get_radiotap_len(skb->data);
  165. if (unlikely(skb->len < rtap_len))
  166. goto fail;
  167. MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
  168. /* Skip the ratio tap header */
  169. skb_pull(skb, rtap_len);
  170. dot11_hdr = (struct ieee80211_hdr *)skb->data;
  171. frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
  172. /* Check if the QoS bit is set */
  173. if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
  174. /* Check if this ia a Wireless Distribution System (WDS) frame
  175. * which has 4 MAC addresses
  176. */
  177. if (dot11_hdr->frame_control & 0x0080)
  178. qos_len = 2;
  179. if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
  180. dot11_hdr_len += 6;
  181. memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
  182. memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
  183. /* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
  184. * for two MAC addresses
  185. */
  186. skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
  187. pdata = (unsigned char*)skb->data;
  188. memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
  189. memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
  190. MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
  191. /* Use the real net device to transmit the packet */
  192. ret = dhd_start_xmit(skb, mon_if->real_ndev);
  193. return ret;
  194. }
  195. fail:
  196. dev_kfree_skb(skb);
  197. return 0;
  198. }
  199. static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
  200. {
  201. monitor_interface* mon_if;
  202. mon_if = ndev_to_monif(ndev);
  203. if (mon_if == NULL || mon_if->real_ndev == NULL) {
  204. MON_PRINT(" cannot find matched net dev, skip the packet\n");
  205. } else {
  206. MON_PRINT("enter, if name: %s, matched if name %s\n",
  207. ndev->name, mon_if->real_ndev->name);
  208. }
  209. }
  210. static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
  211. {
  212. int ret = 0;
  213. monitor_interface* mon_if;
  214. mon_if = ndev_to_monif(ndev);
  215. if (mon_if == NULL || mon_if->real_ndev == NULL) {
  216. MON_PRINT(" cannot find matched net dev, skip the packet\n");
  217. } else {
  218. MON_PRINT("enter, if name: %s, matched if name %s\n",
  219. ndev->name, mon_if->real_ndev->name);
  220. }
  221. return ret;
  222. }
  223. /**
  224. * Global function definitions (declared in dhd_linux_mon.h)
  225. */
  226. int dhd_add_monitor(char *name, struct net_device **new_ndev)
  227. {
  228. int i;
  229. int idx = -1;
  230. int ret = 0;
  231. struct net_device* ndev = NULL;
  232. dhd_linux_monitor_t **dhd_mon;
  233. mutex_lock(&g_monitor.lock);
  234. MON_TRACE("enter, if name: %s\n", name);
  235. if (!name || !new_ndev) {
  236. MON_PRINT("invalid parameters\n");
  237. ret = -EINVAL;
  238. goto out;
  239. }
  240. /*
  241. * Find a vacancy
  242. */
  243. for (i = 0; i < DHD_MAX_IFS; i++)
  244. if (g_monitor.mon_if[i].mon_ndev == NULL) {
  245. idx = i;
  246. break;
  247. }
  248. if (idx == -1) {
  249. MON_PRINT("exceeds maximum interfaces\n");
  250. ret = -EFAULT;
  251. goto out;
  252. }
  253. ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
  254. if (!ndev) {
  255. MON_PRINT("failed to allocate memory\n");
  256. ret = -ENOMEM;
  257. goto out;
  258. }
  259. ndev->type = ARPHRD_IEEE80211_RADIOTAP;
  260. strncpy(ndev->name, name, IFNAMSIZ);
  261. ndev->name[IFNAMSIZ - 1] = 0;
  262. ndev->netdev_ops = &dhd_mon_if_ops;
  263. ret = register_netdevice(ndev);
  264. if (ret) {
  265. MON_PRINT(" register_netdevice failed (%d)\n", ret);
  266. goto out;
  267. }
  268. *new_ndev = ndev;
  269. g_monitor.mon_if[idx].radiotap_enabled = TRUE;
  270. g_monitor.mon_if[idx].mon_ndev = ndev;
  271. g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
  272. dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
  273. *dhd_mon = &g_monitor;
  274. g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
  275. MON_PRINT("net device returned: 0x%p\n", ndev);
  276. MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
  277. out:
  278. if (ret && ndev)
  279. free_netdev(ndev);
  280. mutex_unlock(&g_monitor.lock);
  281. return ret;
  282. }
  283. int dhd_del_monitor(struct net_device *ndev)
  284. {
  285. int i;
  286. bool rollback_lock = false;
  287. if (!ndev)
  288. return -EINVAL;
  289. mutex_lock(&g_monitor.lock);
  290. for (i = 0; i < DHD_MAX_IFS; i++) {
  291. if (g_monitor.mon_if[i].mon_ndev == ndev ||
  292. g_monitor.mon_if[i].real_ndev == ndev) {
  293. g_monitor.mon_if[i].real_ndev = NULL;
  294. if (rtnl_is_locked()) {
  295. rtnl_unlock();
  296. rollback_lock = true;
  297. }
  298. unregister_netdev(g_monitor.mon_if[i].mon_ndev);
  299. free_netdev(g_monitor.mon_if[i].mon_ndev);
  300. g_monitor.mon_if[i].mon_ndev = NULL;
  301. g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
  302. break;
  303. }
  304. }
  305. if (rollback_lock) {
  306. rtnl_lock();
  307. rollback_lock = false;
  308. }
  309. if (g_monitor.monitor_state !=
  310. MONITOR_STATE_INTERFACE_DELETED)
  311. MON_PRINT("interface not found in monitor IF array, is this a monitor IF? 0x%p\n",
  312. ndev);
  313. mutex_unlock(&g_monitor.lock);
  314. return 0;
  315. }
  316. int dhd_monitor_init(void *dhd_pub)
  317. {
  318. if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
  319. g_monitor.dhd_pub = dhd_pub;
  320. mutex_init(&g_monitor.lock);
  321. g_monitor.monitor_state = MONITOR_STATE_INIT;
  322. }
  323. return 0;
  324. }
  325. int dhd_monitor_uninit(void)
  326. {
  327. int i;
  328. struct net_device *ndev;
  329. bool rollback_lock = false;
  330. mutex_lock(&g_monitor.lock);
  331. if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
  332. for (i = 0; i < DHD_MAX_IFS; i++) {
  333. ndev = g_monitor.mon_if[i].mon_ndev;
  334. if (ndev) {
  335. if (rtnl_is_locked()) {
  336. rtnl_unlock();
  337. rollback_lock = true;
  338. }
  339. unregister_netdev(ndev);
  340. free_netdev(ndev);
  341. g_monitor.mon_if[i].real_ndev = NULL;
  342. g_monitor.mon_if[i].mon_ndev = NULL;
  343. if (rollback_lock) {
  344. rtnl_lock();
  345. rollback_lock = false;
  346. }
  347. }
  348. }
  349. g_monitor.monitor_state = MONITOR_STATE_DEINIT;
  350. }
  351. mutex_unlock(&g_monitor.lock);
  352. return 0;
  353. }