1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (c) 2012 - 2018 Microchip Technology Inc., and its subsidiaries. 4 * All rights reserved. 5 */ 6 7 #include "cfg80211.h" 8 9 struct wilc_wfi_radiotap_hdr { 10 struct ieee80211_radiotap_header hdr; 11 u8 rate; 12 } __packed; 13 14 struct wilc_wfi_radiotap_cb_hdr { 15 struct ieee80211_radiotap_header hdr; 16 u8 rate; 17 u8 dump; 18 u16 tx_flags; 19 } __packed; 20 21 #define TX_RADIOTAP_PRESENT ((1 << IEEE80211_RADIOTAP_RATE) | \ 22 (1 << IEEE80211_RADIOTAP_TX_FLAGS)) 23 24 void wilc_wfi_monitor_rx(struct net_device *mon_dev, u8 *buff, u32 size) 25 { 26 u32 header, pkt_offset; 27 struct sk_buff *skb = NULL; 28 struct wilc_wfi_radiotap_hdr *hdr; 29 struct wilc_wfi_radiotap_cb_hdr *cb_hdr; 30 31 if (!mon_dev) 32 return; 33 34 if (!netif_running(mon_dev)) 35 return; 36 37 /* Get WILC header */ 38 header = get_unaligned_le32(buff - HOST_HDR_OFFSET); 39 /* 40 * The packet offset field contain info about what type of management 41 * the frame we are dealing with and ack status 42 */ 43 pkt_offset = FIELD_GET(WILC_PKT_HDR_OFFSET_FIELD, header); 44 45 if (pkt_offset & IS_MANAGMEMENT_CALLBACK) { 46 /* hostapd callback mgmt frame */ 47 48 skb = dev_alloc_skb(size + sizeof(*cb_hdr)); 49 if (!skb) 50 return; 51 52 skb_put_data(skb, buff, size); 53 54 cb_hdr = skb_push(skb, sizeof(*cb_hdr)); 55 memset(cb_hdr, 0, sizeof(*cb_hdr)); 56 57 cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ 58 59 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr)); 60 61 cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT); 62 63 cb_hdr->rate = 5; 64 65 if (pkt_offset & IS_MGMT_STATUS_SUCCES) { 66 /* success */ 67 cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS; 68 } else { 69 cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL; 70 } 71 72 } else { 73 skb = dev_alloc_skb(size + sizeof(*hdr)); 74 75 if (!skb) 76 return; 77 78 skb_put_data(skb, buff, size); 79 hdr = skb_push(skb, sizeof(*hdr)); 80 memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr)); 81 hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ 82 hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr)); 83 hdr->hdr.it_present = cpu_to_le32 84 (1 << IEEE80211_RADIOTAP_RATE); 85 hdr->rate = 5; 86 } 87 88 skb->dev = mon_dev; 89 skb_reset_mac_header(skb); 90 skb->ip_summed = CHECKSUM_UNNECESSARY; 91 skb->pkt_type = PACKET_OTHERHOST; 92 skb->protocol = htons(ETH_P_802_2); 93 memset(skb->cb, 0, sizeof(skb->cb)); 94 95 netif_rx(skb); 96 } 97 98 struct tx_complete_mon_data { 99 int size; 100 void *buff; 101 }; 102 103 static void mgmt_tx_complete(void *priv, int status) 104 { 105 struct tx_complete_mon_data *pv_data = priv; 106 /* 107 * in case of fully hosting mode, the freeing will be done 108 * in response to the cfg packet 109 */ 110 kfree(pv_data->buff); 111 112 kfree(pv_data); 113 } 114 115 static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len) 116 { 117 struct tx_complete_mon_data *mgmt_tx = NULL; 118 119 if (!dev) 120 return -EFAULT; 121 122 netif_stop_queue(dev); 123 mgmt_tx = kmalloc(sizeof(*mgmt_tx), GFP_ATOMIC); 124 if (!mgmt_tx) 125 return -ENOMEM; 126 127 mgmt_tx->buff = kmemdup(buf, len, GFP_ATOMIC); 128 if (!mgmt_tx->buff) { 129 kfree(mgmt_tx); 130 return -ENOMEM; 131 } 132 133 mgmt_tx->size = len; 134 135 wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size, 136 mgmt_tx_complete); 137 138 netif_wake_queue(dev); 139 return 0; 140 } 141 142 static netdev_tx_t wilc_wfi_mon_xmit(struct sk_buff *skb, 143 struct net_device *dev) 144 { 145 u32 rtap_len, ret = 0; 146 struct wilc_wfi_mon_priv *mon_priv; 147 struct sk_buff *skb2; 148 struct wilc_wfi_radiotap_cb_hdr *cb_hdr; 149 u8 srcadd[ETH_ALEN]; 150 u8 bssid[ETH_ALEN]; 151 152 mon_priv = netdev_priv(dev); 153 if (!mon_priv) 154 return -EFAULT; 155 156 rtap_len = ieee80211_get_radiotap_len(skb->data); 157 if (skb->len < rtap_len) 158 return -1; 159 160 skb_pull(skb, rtap_len); 161 162 if (skb->data[0] == 0xc0 && is_broadcast_ether_addr(&skb->data[4])) { 163 skb2 = dev_alloc_skb(skb->len + sizeof(*cb_hdr)); 164 if (!skb2) 165 return -ENOMEM; 166 167 skb_put_data(skb2, skb->data, skb->len); 168 169 cb_hdr = skb_push(skb2, sizeof(*cb_hdr)); 170 memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr)); 171 172 cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ 173 174 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr)); 175 176 cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT); 177 178 cb_hdr->rate = 5; 179 cb_hdr->tx_flags = 0x0004; 180 181 skb2->dev = dev; 182 skb_reset_mac_header(skb2); 183 skb2->ip_summed = CHECKSUM_UNNECESSARY; 184 skb2->pkt_type = PACKET_OTHERHOST; 185 skb2->protocol = htons(ETH_P_802_2); 186 memset(skb2->cb, 0, sizeof(skb2->cb)); 187 188 netif_rx(skb2); 189 190 return 0; 191 } 192 skb->dev = mon_priv->real_ndev; 193 194 ether_addr_copy(srcadd, &skb->data[10]); 195 ether_addr_copy(bssid, &skb->data[16]); 196 /* 197 * Identify if data or mgmt packet, if source address and bssid 198 * fields are equal send it to mgmt frames handler 199 */ 200 if (!(memcmp(srcadd, bssid, 6))) { 201 ret = mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len); 202 if (ret) 203 netdev_err(dev, "fail to mgmt tx\n"); 204 dev_kfree_skb(skb); 205 } else { 206 ret = wilc_mac_xmit(skb, mon_priv->real_ndev); 207 } 208 209 return ret; 210 } 211 212 static const struct net_device_ops wilc_wfi_netdev_ops = { 213 .ndo_start_xmit = wilc_wfi_mon_xmit, 214 215 }; 216 217 struct net_device *wilc_wfi_init_mon_interface(struct wilc *wl, 218 const char *name, 219 struct net_device *real_dev) 220 { 221 struct wilc_wfi_mon_priv *priv; 222 223 /* If monitor interface is already initialized, return it */ 224 if (wl->monitor_dev) 225 return wl->monitor_dev; 226 227 wl->monitor_dev = alloc_etherdev(sizeof(struct wilc_wfi_mon_priv)); 228 if (!wl->monitor_dev) 229 return NULL; 230 231 wl->monitor_dev->type = ARPHRD_IEEE80211_RADIOTAP; 232 strlcpy(wl->monitor_dev->name, name, IFNAMSIZ); 233 wl->monitor_dev->netdev_ops = &wilc_wfi_netdev_ops; 234 wl->monitor_dev->needs_free_netdev = true; 235 236 if (register_netdevice(wl->monitor_dev)) { 237 netdev_err(real_dev, "register_netdevice failed\n"); 238 free_netdev(wl->monitor_dev); 239 return NULL; 240 } 241 priv = netdev_priv(wl->monitor_dev); 242 243 priv->real_ndev = real_dev; 244 245 return wl->monitor_dev; 246 } 247 248 void wilc_wfi_deinit_mon_interface(struct wilc *wl, bool rtnl_locked) 249 { 250 if (!wl->monitor_dev) 251 return; 252 253 if (rtnl_locked) 254 unregister_netdevice(wl->monitor_dev); 255 else 256 unregister_netdev(wl->monitor_dev); 257 wl->monitor_dev = NULL; 258 } 259