1 /*
2  * Copyright (C) 2021 Huawei Device Co., Ltd.
3  * Licensed under the Apache License, Version 2.0 (the "License");
4  * you may not use this file except in compliance with the License.
5  * You may obtain a copy of the License at
6  *
7  *     http://www.apache.org/licenses/LICENSE-2.0
8  *
9  * Unless required by applicable law or agreed to in writing, software
10  * distributed under the License is distributed on an "AS IS" BASIS,
11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12  * See the License for the specific language governing permissions and
13  * limitations under the License.
14  */
15 
16 #include "nstackx_wifi_stat_linux.h"
17 
18 #include <ctype.h>
19 #include <errno.h>
20 
21 #include <netlink/attr.h>
22 #include <netlink/genl/ctrl.h>
23 #include <netlink/genl/family.h>
24 #include <netlink/genl/genl.h>
25 #include <netlink/socket.h>
26 
27 #include <linux/socket.h>
28 
29 #include "nstackx_congestion.h"
30 #include "nstackx_error.h"
31 #include "nstackx_log.h"
32 #include "securec.h"
33 
34 #define TAG "nStackXCongestion"
35 #define NETLINK_SOCKET_BUFFER_SIZE 8192
36 
CheckNl80211MsgBss(struct nlattr * bss[])37 static inline int32_t CheckNl80211MsgBss(struct nlattr *bss[])
38 {
39     if (!bss[NL80211_BSS_BSSID] || !bss[NL80211_BSS_STATUS]) {
40         return NSTACKX_EFAILED;
41     }
42     return NSTACKX_EOK;
43 }
44 
CheckNl80211MsgBssStatus(struct nlattr * bss[])45 static inline int32_t CheckNl80211MsgBssStatus(struct nlattr *bss[])
46 {
47     switch (nla_get_u32(bss[NL80211_BSS_STATUS])) {
48         case NL80211_BSS_STATUS_ASSOCIATED:
49         case NL80211_BSS_STATUS_IBSS_JOINED:
50             break;
51         default:
52             return NSTACKX_EFAILED;
53     }
54     return NSTACKX_EOK;
55 }
56 
GetScanInfo(struct nl_msg * msg,void * arg)57 static int32_t GetScanInfo(struct nl_msg *msg, void *arg)
58 {
59     struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
60 
61     struct nlattr *tb[NL80211_ATTR_MAX + 1] = {0};
62     struct nlattr *bss[NL80211_BSS_MAX + 1] = {0};
63     struct nla_policy bssPolicy[NL80211_BSS_MAX + 1] = {
64         [NL80211_BSS_FREQUENCY] = { .type = NLA_U32 },
65         [NL80211_BSS_BSSID] = { },
66         [NL80211_BSS_STATUS] = { .type = NLA_U32 },
67     };
68 
69     int32_t ret = nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
70     if (ret < 0) {
71         LOGE(TAG, "nla_parse failed! %d", ret);
72         return NL_SKIP;
73     }
74 
75     if (!tb[NL80211_ATTR_BSS] || nla_parse_nested(bss, NL80211_BSS_MAX, tb[NL80211_ATTR_BSS], bssPolicy) ||
76         CheckNl80211MsgBss(bss) != NSTACKX_EOK) {
77         return NL_SKIP;
78     }
79 
80     if (CheckNl80211MsgBssStatus(bss) != NSTACKX_EOK) {
81         return NL_SKIP;
82     }
83 
84     CallbackResult *cbRes = (CallbackResult *)arg;
85     if (memcpy_s(cbRes->mac, ETH_ALEN, nla_data(bss[NL80211_BSS_BSSID]), ETH_ALEN) != NSTACKX_EOK) {
86         LOGE(TAG, "memcpy_s failed");
87     }
88     return NL_SKIP;
89 }
90 
ResolveBitrateInfoInner(struct nlattr * rInfo[])91 static inline uint32_t ResolveBitrateInfoInner(struct nlattr *rInfo[])
92 {
93     uint32_t rate;
94     if (rInfo[NL80211_RATE_INFO_BITRATE32])
95         rate = nla_get_u32(rInfo[NL80211_RATE_INFO_BITRATE32]);
96     else if (rInfo[NL80211_RATE_INFO_BITRATE])
97         rate = (uint32_t)nla_get_u16(rInfo[NL80211_RATE_INFO_BITRATE]);
98     else
99         rate = 0;
100     return rate;
101 }
102 
ResolveBitrateInfo(struct nlattr * bitrateAttr,WifiRateInfo * rateInfo)103 static int32_t ResolveBitrateInfo(struct nlattr *bitrateAttr, WifiRateInfo *rateInfo)
104 {
105     struct nlattr *rInfo[NL80211_RATE_INFO_MAX + 1] = {0};
106     static struct nla_policy ratePolicy[NL80211_RATE_INFO_MAX + 1] = {
107         [NL80211_RATE_INFO_BITRATE] = { .type = NLA_U16 },
108         [NL80211_RATE_INFO_BITRATE32] = { .type = NLA_U32 },
109         [NL80211_RATE_INFO_MCS] = { .type = NLA_U8 },
110         [NL80211_RATE_INFO_40_MHZ_WIDTH] = { .type = NLA_FLAG },
111     };
112 
113     if (nla_parse_nested(rInfo, NL80211_RATE_INFO_MAX, bitrateAttr, ratePolicy)) {
114         LOGE(TAG, "nla_parse_nested failed");
115         return NSTACKX_EFAILED;
116     }
117     rateInfo->rateBitrate = ResolveBitrateInfoInner(rInfo);
118     return NSTACKX_EOK;
119 }
120 
GetStationInfoRateSignal(WifiStationInfo * wifiStationInfo,struct nlattr * sinfo[])121 static inline void GetStationInfoRateSignal(WifiStationInfo *wifiStationInfo, struct nlattr *sinfo[])
122 {
123     if (sinfo[NL80211_STA_INFO_SIGNAL]) {
124         wifiStationInfo->signal = (int32_t)((int8_t)nla_get_u8(sinfo[NL80211_STA_INFO_SIGNAL]));
125     }
126 }
127 
GetStationInfoRateRx(WifiStationInfo * wifiStationInfo,struct nlattr * sinfo[])128 static inline void GetStationInfoRateRx(WifiStationInfo *wifiStationInfo, struct nlattr *sinfo[])
129 {
130     if (sinfo[NL80211_STA_INFO_RX_BITRATE]) {
131         WifiRateInfo rxRateInfo;
132         if (ResolveBitrateInfo(sinfo[NL80211_STA_INFO_RX_BITRATE], &rxRateInfo) == NSTACKX_EOK) {
133             wifiStationInfo->rxRate = rxRateInfo.rateBitrate / WIFI_NEGO_RATE_ACCURACY;
134         }
135     }
136 }
137 
GetStationInfoRateTx(WifiStationInfo * wifiStationInfo,struct nlattr * sinfo[])138 static inline void GetStationInfoRateTx(WifiStationInfo *wifiStationInfo, struct nlattr *sinfo[])
139 {
140     if (sinfo[NL80211_STA_INFO_TX_BITRATE]) {
141         WifiRateInfo txRateInfo;
142         if (ResolveBitrateInfo(sinfo[NL80211_STA_INFO_TX_BITRATE], &txRateInfo) == NSTACKX_EOK) {
143             wifiStationInfo->txRate = txRateInfo.rateBitrate / WIFI_NEGO_RATE_ACCURACY;
144         }
145     }
146 }
147 
GetStationInfoRate(WifiStationInfo * wifiStationInfo,struct nlattr * sinfo[])148 static void GetStationInfoRate(WifiStationInfo *wifiStationInfo, struct nlattr *sinfo[])
149 {
150     GetStationInfoRateSignal(wifiStationInfo, sinfo);
151     GetStationInfoRateRx(wifiStationInfo, sinfo);
152     GetStationInfoRateTx(wifiStationInfo, sinfo);
153     return;
154 }
155 
GetStationInfo(struct nl_msg * msg,void * arg)156 static int32_t GetStationInfo(struct nl_msg *msg, void *arg)
157 {
158     struct nlattr *tb[NL80211_ATTR_MAX + 1] = {0};
159 
160     struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
161     struct nlattr *sinfo[NL80211_STA_INFO_MAX + 1] = {0};
162     static struct nla_policy statsPolicy[NL80211_STA_INFO_MAX + 1] = {
163         [NL80211_STA_INFO_SIGNAL] = { .type = NLA_U8 },
164         [NL80211_STA_INFO_RX_BITRATE] = { .type = NLA_NESTED },
165         [NL80211_STA_INFO_TX_BITRATE] = { .type = NLA_NESTED },
166     };
167 
168     int32_t ret = nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
169     if (ret < 0) {
170         LOGE(TAG, "nla_parse failed! %d", ret);
171         return NL_SKIP;
172     }
173 
174     if (!tb[NL80211_ATTR_STA_INFO] ||
175         nla_parse_nested(sinfo, NL80211_STA_INFO_MAX, tb[NL80211_ATTR_STA_INFO], statsPolicy)) {
176         LOGE(TAG, "sta stats missing! or failed to parse nested attributes!\n");
177         return NL_SKIP;
178     }
179 
180     CallbackResult *cbRes = (CallbackResult *)arg;
181     WifiStationInfo *wifiStationInfo = &cbRes->wifiStationInfo;
182     GetStationInfoRate(wifiStationInfo, sinfo);
183     return NL_SKIP;
184 }
185 
Nl80211Msg(Nl80211MsgSet * nl80211MsgSet)186 int32_t Nl80211Msg(Nl80211MsgSet *nl80211MsgSet)
187 {
188     struct nl_msg *msg = nlmsg_alloc();
189 
190     if (!msg) {
191         LOGE(TAG, "nlmsg_alloc failed.");
192         return NSTACKX_ENOMEM;
193     }
194 
195     void *fret = genlmsg_put(msg, 0, 0, nl80211MsgSet->nlDevInfo.nl80211_id, 0, nl80211MsgSet->flags,
196                              nl80211MsgSet->cmd, 0);
197     if (fret == NULL) {
198         LOGE(TAG, "genlmsg_put failed");
199         goto FAIL_EXIT;
200     }
201 
202     int32_t ret = nla_put_u32(msg, NL80211_ATTR_IFINDEX, nl80211MsgSet->nlDevInfo.if_index);
203     if (ret < 0) {
204         LOGE(TAG, "nla_put_u32 failed");
205         goto FAIL_EXIT;
206     }
207 
208     if (nl80211MsgSet->handle != NULL) {
209         nl80211MsgSet->handle(msg, &nl80211MsgSet->handleParam);
210     }
211 
212     int32_t err = nl_send_auto_complete(nl80211MsgSet->nlDevInfo.nl_sock, msg);
213     if (err < 0) {
214         LOGE(TAG, "send failed.%d", err);
215         goto FAIL_EXIT;
216     }
217 
218     struct nl_cb *cb = nl_cb_alloc(NL_CB_DEBUG);
219     if (cb == NULL) {
220         LOGE(TAG, "nl_cb alloc failed");
221         goto FAIL_EXIT;
222     }
223 
224     ret = nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, nl80211MsgSet->func, &nl80211MsgSet->cbRes);
225     if (ret < 0) {
226         LOGE(TAG, "nl_cb_set failed! %d", ret);
227         nl_cb_put(cb);
228         goto FAIL_EXIT;
229     }
230 
231     ret = nl_recvmsgs(nl80211MsgSet->nlDevInfo.nl_sock, cb);
232     if (ret < 0 || errno) {
233         LOGE(TAG, "err .%d errno %d ret is %d", err, errno, ret);
234         nl_cb_put(cb);
235         goto FAIL_EXIT;
236     }
237 
238     nl_cb_put(cb);
239     nlmsg_free(msg);
240     return NSTACKX_EOK;
241 
242 FAIL_EXIT:
243     nlmsg_free(msg);
244     return NSTACKX_EFAILED;
245 }
246 
GetStationHandler(struct nl_msg * msg,HandleParam * handleParam)247 int32_t GetStationHandler(struct nl_msg *msg, HandleParam *handleParam)
248 {
249     NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, handleParam->mac);
250     return 0;
251 }
252 
GetWifiStaInfo(const NLDevInfo nlDevInfo,WifiStationInfo * wifiStationInfo)253 int32_t GetWifiStaInfo(const NLDevInfo nlDevInfo, WifiStationInfo *wifiStationInfo)
254 {
255     Nl80211MsgSet nl80211MsgSet;
256     nl80211MsgSet.nlDevInfo.nl_sock = nlDevInfo.nl_sock;
257     nl80211MsgSet.nlDevInfo.if_index = nlDevInfo.if_index;
258     nl80211MsgSet.nlDevInfo.nl80211_id = nlDevInfo.nl80211_id;
259 
260     nl80211MsgSet.cmd = NL80211_CMD_GET_SCAN;
261     nl80211MsgSet.flags = NLM_F_DUMP;
262     nl80211MsgSet.handle = NULL;
263     nl80211MsgSet.func = GetScanInfo;
264     (void)memset_s(&nl80211MsgSet.cbRes, sizeof(nl80211MsgSet.cbRes), 0, sizeof(nl80211MsgSet.cbRes));
265     int32_t ret = Nl80211Msg(&nl80211MsgSet);
266     if (ret != NSTACKX_EOK) {
267         return ret;
268     }
269 
270     char macAddr[ETH_ALEN];
271     ret = memcpy_s(macAddr, ETH_ALEN, nl80211MsgSet.cbRes.mac, ETH_ALEN);
272     if (ret != NSTACKX_EOK) {
273         return ret;
274     }
275 
276     nl80211MsgSet.cmd = NL80211_CMD_GET_STATION;
277     nl80211MsgSet.flags = 0;
278     nl80211MsgSet.handle = GetStationHandler;
279     nl80211MsgSet.func = GetStationInfo;
280     nl80211MsgSet.handleParam.mac = macAddr;
281     ret = Nl80211Msg(&nl80211MsgSet);
282     if (ret == NSTACKX_EOK) {
283         ret = memcpy_s(wifiStationInfo, sizeof(WifiStationInfo),
284             &nl80211MsgSet.cbRes.wifiStationInfo, sizeof(WifiStationInfo));
285     }
286     return ret;
287 }
288 
GetNlDevInfo(NLDevInfo * nlDevInfo,const char * devName)289 static int32_t GetNlDevInfo(NLDevInfo *nlDevInfo, const char *devName)
290 {
291     nlDevInfo->nl_sock = nl_socket_alloc();
292     if (nlDevInfo->nl_sock == NULL) {
293         LOGE(TAG, "create netlink.error no is %d", errno);
294         return NSTACKX_EFAILED;
295     }
296     if (genl_connect(nlDevInfo->nl_sock)) {
297         LOGE(TAG, "Failed to connect to generic netlink.error no is %d", errno);
298         goto FAIL_EXIT;
299     }
300 
301     nl_socket_enable_msg_peek(nlDevInfo->nl_sock);
302     int32_t ret = nl_socket_set_buffer_size(nlDevInfo->nl_sock, NETLINK_SOCKET_BUFFER_SIZE, NETLINK_SOCKET_BUFFER_SIZE);
303     if (ret < 0) {
304         goto FAIL_EXIT;
305     }
306 
307     int32_t err = 1;
308     ret = setsockopt(nl_socket_get_fd(nlDevInfo->nl_sock), SOL_NETLINK, NETLINK_EXT_ACK, &err, sizeof(err));
309     if (ret == -1) {
310         goto FAIL_EXIT;
311     }
312 
313     nlDevInfo->nl80211_id = genl_ctrl_resolve(nlDevInfo->nl_sock, "nl80211");
314     if (nlDevInfo->nl80211_id < 0) {
315         LOGE(TAG, "nl80211 id get fail.");
316         goto FAIL_EXIT;
317     }
318 
319     nlDevInfo->if_index = if_nametoindex(devName);
320     if (nlDevInfo->if_index == 0) {
321         LOGE(TAG, "if_index is 0 dev is not exist");
322         goto FAIL_EXIT;
323     }
324     return NSTACKX_EOK;
325 
326 FAIL_EXIT:
327     nl_socket_free(nlDevInfo->nl_sock);
328     return NSTACKX_EFAILED;
329 }
330 
FreeNlDevInfo(NLDevInfo * nlDevInfo)331 static void FreeNlDevInfo(NLDevInfo *nlDevInfo)
332 {
333     if (nlDevInfo->nl_sock != NULL) {
334         nl_socket_free(nlDevInfo->nl_sock);
335         nlDevInfo->nl_sock = NULL;
336     }
337 }
338 
GetWifiInfoFromLinux(const char * devName,WifiStationInfo * wifiStationInfo)339 int32_t GetWifiInfoFromLinux(const char *devName, WifiStationInfo *wifiStationInfo)
340 {
341     NLDevInfo nlDevInfo;
342     (void)memset_s(&nlDevInfo, sizeof(nlDevInfo), 0, sizeof(nlDevInfo));
343     int32_t ret = GetNlDevInfo(&nlDevInfo, devName);
344     if (ret != NSTACKX_EOK) {
345         LOGE(TAG, "GetNlDevInfo failed.error no is %d", errno);
346         return ret;
347     }
348 
349     ret = GetWifiStaInfo(nlDevInfo, wifiStationInfo);
350     if (ret != NSTACKX_EOK || CheckWlanNegoRateValid(wifiStationInfo->txRate) != NSTACKX_EOK) {
351         LOGE(TAG, "getWifiStaInfo fail.or wifiStationInfo->txRate %u", wifiStationInfo->txRate);
352         ret = NSTACKX_EFAILED;
353         goto FAIL_EXIT;
354     }
355 
356 FAIL_EXIT:
357     FreeNlDevInfo(&nlDevInfo);
358     return ret;
359 }
360