1 /*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /* NOTICE: This is a clean room re-implementation of libnl */
18
19 #include <errno.h>
20 #include <unistd.h>
21 #include <stdio.h>
22 #include <sys/time.h>
23 #include <sys/socket.h>
24 #include <linux/netlink.h>
25 #include <netlink/genl/ctrl.h>
26 #include <netlink/genl/family.h>
27 #include "netlink-types.h"
28
29 /* Get head of attribute data. */
genlmsg_attrdata(const struct genlmsghdr * gnlh,int hdrlen)30 struct nlattr *genlmsg_attrdata(const struct genlmsghdr *gnlh, int hdrlen)
31 {
32 return (struct nlattr *) \
33 ((char *) gnlh + GENL_HDRLEN + NLMSG_ALIGN(hdrlen));
34
35 }
36
37 /* Get length of attribute data. */
genlmsg_attrlen(const struct genlmsghdr * gnlh,int hdrlen)38 int genlmsg_attrlen(const struct genlmsghdr *gnlh, int hdrlen)
39 {
40 struct nlattr *nla;
41 struct nlmsghdr *nlh;
42
43 nla = genlmsg_attrdata(gnlh, hdrlen);
44 nlh = (struct nlmsghdr *) ((char *) gnlh - NLMSG_HDRLEN);
45 return (char *) nlmsg_tail(nlh) - (char *) nla;
46 }
47
48 /* Add generic netlink header to netlink message. */
genlmsg_put(struct nl_msg * msg,uint32_t pid,uint32_t seq,int family,int hdrlen,int flags,uint8_t cmd,uint8_t version)49 void *genlmsg_put(struct nl_msg *msg, uint32_t pid, uint32_t seq, int family,
50 int hdrlen, int flags, uint8_t cmd, uint8_t version)
51 {
52 int new_size;
53 struct nlmsghdr *nlh;
54 struct timeval tv;
55 struct genlmsghdr *gmh;
56
57 /* Make sure nl_msg has enough space */
58 new_size = NLMSG_HDRLEN + GENL_HDRLEN + hdrlen;
59 if ((sizeof(struct nl_msg) + new_size) > msg->nm_size)
60 goto fail;
61
62 /* Fill in netlink header */
63 nlh = msg->nm_nlh;
64 nlh->nlmsg_len = new_size;
65 nlh->nlmsg_type = family;
66 nlh->nlmsg_pid = getpid();
67 nlh->nlmsg_flags = flags | NLM_F_REQUEST | NLM_F_ACK;
68
69 /* Get current time for sequence number */
70 if (gettimeofday(&tv, NULL))
71 nlh->nlmsg_seq = 1;
72 else
73 nlh->nlmsg_seq = (int) tv.tv_sec;
74
75 /* Setup genlmsghdr in new message */
76 gmh = (struct genlmsghdr *) ((char *)nlh + NLMSG_HDRLEN);
77 gmh->cmd = (__u8) cmd;
78 gmh->version = version;
79
80 return gmh;
81 fail:
82 return NULL;
83
84 }
85
86 /* Socket has already been alloced to connect it to kernel? */
genl_connect(struct nl_sock * sk)87 int genl_connect(struct nl_sock *sk)
88 {
89 return nl_connect(sk, NETLINK_GENERIC);
90
91 }
92
genl_ctrl_alloc_cache(struct nl_sock * sock,struct nl_cache ** result)93 int genl_ctrl_alloc_cache(struct nl_sock *sock, struct nl_cache **result)
94 {
95 int rc = -1;
96 int nl80211_genl_id = -1;
97 char sendbuf[sizeof(struct nlmsghdr)+sizeof(struct genlmsghdr)];
98 struct nlmsghdr nlmhdr;
99 struct genlmsghdr gmhhdr;
100 struct iovec sendmsg_iov;
101 struct msghdr msg;
102 int num_char;
103 const int RECV_BUF_SIZE = getpagesize();
104 char *recvbuf;
105 struct iovec recvmsg_iov;
106 int nl80211_flag = 0, nlm_f_multi = 0, nlmsg_done = 0;
107 struct nlmsghdr *nlh;
108
109 /* REQUEST GENERIC NETLINK FAMILY ID */
110 /* Message buffer */
111 nlmhdr.nlmsg_len = sizeof(sendbuf);
112 nlmhdr.nlmsg_type = NETLINK_GENERIC;
113 nlmhdr.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_DUMP;
114 nlmhdr.nlmsg_seq = sock->s_seq_next;
115 nlmhdr.nlmsg_pid = sock->s_local.nl_pid;
116
117 /* Generic netlink header */
118 memset(&gmhhdr, 0, sizeof(gmhhdr));
119 gmhhdr.cmd = CTRL_CMD_GETFAMILY;
120 gmhhdr.version = CTRL_ATTR_FAMILY_ID;
121
122 /* Combine netlink and generic netlink headers */
123 memcpy(&sendbuf[0], &nlmhdr, sizeof(nlmhdr));
124 memcpy(&sendbuf[0]+sizeof(nlmhdr), &gmhhdr, sizeof(gmhhdr));
125
126 /* Create IO vector with Netlink message */
127 sendmsg_iov.iov_base = &sendbuf;
128 sendmsg_iov.iov_len = sizeof(sendbuf);
129
130 /* Socket message */
131 msg.msg_name = (void *) &sock->s_peer;
132 msg.msg_namelen = sizeof(sock->s_peer);
133 msg.msg_iov = &sendmsg_iov;
134 msg.msg_iovlen = 1; /* Only sending one iov */
135 msg.msg_control = NULL;
136 msg.msg_controllen = 0;
137 msg.msg_flags = 0;
138
139 /* Send message and verify sent */
140 num_char = sendmsg(sock->s_fd, &msg, 0);
141 if (num_char == -1)
142 return -errno;
143
144 /* RECEIVE GENL CMD RESPONSE */
145
146 /* Create receive iov buffer */
147 recvbuf = (char *) malloc(RECV_BUF_SIZE);
148
149 /* Attach to iov */
150 recvmsg_iov.iov_base = recvbuf;
151 recvmsg_iov.iov_len = RECV_BUF_SIZE;
152
153 msg.msg_iov = &recvmsg_iov;
154 msg.msg_iovlen = 1;
155
156 /***************************************************************/
157 /* Receive message. If multipart message, keep receiving until */
158 /* message type is NLMSG_DONE */
159 /***************************************************************/
160
161 do {
162
163 int recvmsg_len, nlmsg_rem;
164
165 /* Receive message */
166 memset(recvbuf, 0, RECV_BUF_SIZE);
167 recvmsg_len = recvmsg(sock->s_fd, &msg, 0);
168
169 /* Make sure receive successful */
170 if (recvmsg_len < 0) {
171 rc = -errno;
172 goto error_recvbuf;
173 }
174
175 /* Parse nlmsghdr */
176 nlmsg_for_each_msg(nlh, (struct nlmsghdr *) recvbuf, \
177 recvmsg_len, nlmsg_rem) {
178 struct nlattr *nla;
179 int nla_rem;
180
181 /* Check type */
182 switch (nlh->nlmsg_type) {
183 case NLMSG_DONE:
184 goto return_genl_id;
185 break;
186 case NLMSG_ERROR:
187
188 /* Should check nlmsgerr struct received */
189 fprintf(stderr, "Receive message error\n");
190 goto error_recvbuf;
191 case NLMSG_OVERRUN:
192 fprintf(stderr, "Receive data partly lost\n");
193 goto error_recvbuf;
194 case NLMSG_MIN_TYPE:
195 case NLMSG_NOOP:
196 break;
197 default:
198 break;
199 }
200
201
202
203 /* Check flags */
204 if (nlh->nlmsg_flags & NLM_F_MULTI)
205 nlm_f_multi = 1;
206 else
207 nlm_f_multi = 0;
208
209 if (nlh->nlmsg_type & NLMSG_DONE)
210 nlmsg_done = 1;
211 else
212 nlmsg_done = 0;
213
214 /* Iteratve over attributes */
215 nla_for_each_attr(nla,
216 nlmsg_attrdata(nlh, GENL_HDRLEN),
217 nlmsg_attrlen(nlh, GENL_HDRLEN),
218 nla_rem){
219
220 /* If this family is nl80211 */
221 if (nla->nla_type == CTRL_ATTR_FAMILY_NAME &&
222 !strcmp((char *)nla_data(nla),
223 "nl80211"))
224 nl80211_flag = 1;
225
226 /* Save the family id */
227 else if (nl80211_flag &&
228 nla->nla_type == CTRL_ATTR_FAMILY_ID) {
229 nl80211_genl_id =
230 *((int *)nla_data(nla));
231 nl80211_flag = 0;
232 }
233
234 }
235
236 }
237
238 } while (nlm_f_multi && !nlmsg_done);
239
240 return_genl_id:
241 /* Return family id as cache pointer */
242 *result = (struct nl_cache *) nl80211_genl_id;
243 rc = 0;
244 error_recvbuf:
245 free(recvbuf);
246 error:
247 return rc;
248 }
249
250 /* Checks the netlink cache to find family reference by name string */
251 /* NOTE: Caller needs to call genl_family_put() when done with *
252 * returned object */
genl_ctrl_search_by_name(struct nl_cache * cache,const char * name)253 struct genl_family *genl_ctrl_search_by_name(struct nl_cache *cache, \
254 const char *name)
255 {
256 struct genl_family *gf = (struct genl_family *) \
257 malloc(sizeof(struct genl_family));
258 if (!gf)
259 goto fail;
260 memset(gf, 0, sizeof(*gf));
261
262 /* Add ref */
263 gf->ce_refcnt++;
264
265 /* Overriding cache pointer as family id for now */
266 gf->gf_id = (uint16_t) ((uint32_t) cache);
267 strncpy(gf->gf_name, name, GENL_NAMSIZ);
268
269 return gf;
270 fail:
271 return NULL;
272
273 }
274
genl_ctrl_resolve(struct nl_sock * sk,const char * name)275 int genl_ctrl_resolve(struct nl_sock *sk, const char *name)
276 {
277 struct nl_cache *cache = NULL;
278 struct genl_family *gf = NULL;
279 int id = -1;
280
281 /* Hack to support wpa_supplicant */
282 if (strcmp(name, "nlctrl") == 0)
283 return NETLINK_GENERIC;
284
285 if (strcmp(name, "nl80211") != 0) {
286 fprintf(stderr, "%s is not supported\n", name);
287 return id;
288 }
289
290 if (!genl_ctrl_alloc_cache(sk, &cache)) {
291 gf = genl_ctrl_search_by_name(cache, name);
292 if (gf)
293 id = genl_family_get_id(gf);
294 }
295
296 if (gf)
297 genl_family_put(gf);
298 if (cache)
299 nl_cache_free(cache);
300
301 return id;
302 }
303