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_nlmsg.h"
17
18 #include <errno.h>
19 #include <pthread.h>
20
21 #include "nstackx_error.h"
22 #include "nstackx_log.h"
23 #include "nstackx_timer.h"
24 #include "nstackx_util.h"
25
26 #define TAG "nStackXCongestion"
27 #define NETLINK_REQUEST_IOV_NUM 2
28
NetlinkSocketInit()29 int32_t NetlinkSocketInit()
30 {
31 int32_t nlSockFd = socket(PF_NETLINK, SOCK_RAW | SOCK_CLOEXEC, NETLINK_ROUTE);
32 if (nlSockFd < 0) {
33 LOGE(TAG, "Open netlink socket failed");
34 return NSTACKX_EFAILED;
35 }
36
37 struct sockaddr_nl nlSockSrcAddr;
38 (void)memset_s(&nlSockSrcAddr, sizeof(nlSockSrcAddr), 0, sizeof(nlSockSrcAddr));
39 nlSockSrcAddr.nl_family = AF_NETLINK;
40 nlSockSrcAddr.nl_groups = 0;
41
42 int32_t ret = bind(nlSockFd, (struct sockaddr *)&nlSockSrcAddr, sizeof(nlSockSrcAddr));
43 if (ret < 0) {
44 LOGE(TAG, "Bind failed");
45 CloseSocketInner(nlSockFd);
46 return NSTACKX_EFAILED;
47 }
48 return nlSockFd;
49 }
50
SendNetlinkRequest(int32_t nlSockFd,int32_t ifIndex,uint16_t type)51 int32_t SendNetlinkRequest(int32_t nlSockFd, int32_t ifIndex, uint16_t type)
52 {
53 static uint32_t requestReqIndex = 0;
54 if (nlSockFd < 0 || ifIndex < 0) {
55 return NSTACKX_EFAILED;
56 }
57
58 struct tcmsg t = { .tcm_family = AF_UNSPEC };
59 t.tcm_ifindex = ifIndex;
60
61 void *req = (void *)&t;
62 int32_t len = sizeof(t);
63
64 struct nlmsghdr nlh = {
65 .nlmsg_len = NLMSG_LENGTH((uint32_t)len),
66 .nlmsg_type = type,
67 .nlmsg_flags = NLM_F_DUMP | NLM_F_REQUEST,
68 .nlmsg_seq = requestReqIndex,
69 };
70 requestReqIndex++;
71
72 struct iovec iov[NETLINK_REQUEST_IOV_NUM] = {
73 {
74 .iov_base = &nlh,
75 .iov_len = sizeof(nlh)
76 },
77 {
78 .iov_base = req,
79 .iov_len = len
80 }
81 };
82
83 struct sockaddr_nl nlSockDstAddr = { .nl_family = AF_NETLINK };
84
85 struct msghdr requestMsg = {
86 .msg_name = &nlSockDstAddr,
87 .msg_namelen = sizeof(nlSockDstAddr),
88 .msg_iov = iov,
89 .msg_iovlen = NETLINK_REQUEST_IOV_NUM,
90 };
91
92 ssize_t ret = sendmsg(nlSockFd, &requestMsg, 0);
93 if (ret <= 0) {
94 LOGE(TAG, "ret %d errno %d", ret, errno);
95 return NSTACKX_EFAILED;
96 }
97
98 return NSTACKX_EOK;
99 }
100
ParseNetlinkMsg(char * buf,int32_t recvlen,struct NlmsgCallback * nlcb)101 static int32_t ParseNetlinkMsg(char *buf, int32_t recvlen, struct NlmsgCallback *nlcb)
102 {
103 struct nlmsghdr *h = (struct nlmsghdr *)buf;
104 int32_t msglen = recvlen;
105
106 while (NLMSG_OK(h, (__u32)msglen)) {
107 if (h->nlmsg_type == NLMSG_DONE) {
108 return NLMSG_DONE;
109 }
110 if (h->nlmsg_type == NLMSG_ERROR) {
111 LOGE(TAG, "h->nlmsg_type == NLMSG_ERROR");
112 return NLMSG_ERROR;
113 }
114 nlcb->nlcb(h, nlcb->arg, nlcb->value);
115 h = NLMSG_NEXT(h, msglen);
116 }
117 return NLMSG_NOOP;
118 }
119
RecvNetlinkResponse(int32_t nlSockFd,struct NlmsgCallback * nlcb)120 int32_t RecvNetlinkResponse(int32_t nlSockFd, struct NlmsgCallback *nlcb)
121 {
122 struct sockaddr_nl nlAddr;
123 struct iovec iovRecv;
124 int32_t ret;
125 int32_t parseValue = NLMSG_DONE;
126 iovRecv.iov_base = NULL;
127 iovRecv.iov_len = 0;
128
129 struct msghdr msg = {
130 .msg_name = &nlAddr,
131 .msg_namelen = sizeof(nlAddr),
132 .msg_iov = &iovRecv,
133 .msg_iovlen = 1,
134 };
135
136 int32_t recvLen;
137 char buf[MAX_NETLINK_BUFFER_LEN] = {0};
138 iovRecv.iov_base = buf;
139 iovRecv.iov_len = MAX_NETLINK_BUFFER_LEN;
140
141 while (1) {
142 recvLen = (int32_t)recvmsg(nlSockFd, &msg, 0);
143 if (recvLen <= 0) {
144 LOGE(TAG, "2 recvlen %d netlink receive error %s (%d)", recvLen, strerror(errno), errno);
145 return NSTACKX_EFAILED;
146 }
147
148 ret = ParseNetlinkMsg(buf, recvLen, nlcb);
149 if (ret == NLMSG_DONE) {
150 break;
151 } else if (ret == NLMSG_ERROR) {
152 parseValue = NLMSG_ERROR;
153 }
154 }
155 if (parseValue == NLMSG_ERROR) {
156 return NSTACKX_EFAILED;
157 }
158
159 return NSTACKX_EOK;
160 }
161
RecvNetlinkParseAttr(struct rtattr * rta,int32_t len,struct rtattr * tb[],int32_t max)162 void RecvNetlinkParseAttr(struct rtattr *rta, int32_t len, struct rtattr *tb[], int32_t max)
163 {
164 size_t tbLength = ((size_t)sizeof(struct rtattr *) * (size_t)(max + 1));
165 (void)memset_s(tb, tbLength, 0, tbLength);
166 for (; RTA_OK(rta, len); rta = RTA_NEXT(rta, len)) {
167 if ((rta->rta_type <= max) && (!tb[rta->rta_type])) {
168 tb[rta->rta_type] = rta;
169 }
170 }
171 }
172