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 <string.h>
21 #include <unistd.h>
22 #include <fcntl.h>
23 #include <sys/socket.h>
24 #include "netlink-types.h"
25
26 #define NL_BUFFER_SZ (32768U)
27
28 /* Checks message for completeness and sends it out */
nl_send_auto_complete(struct nl_sock * sk,struct nl_msg * msg)29 int nl_send_auto_complete(struct nl_sock *sk, struct nl_msg *msg)
30 {
31 struct nlmsghdr *nlh = msg->nm_nlh;
32 struct timeval tv;
33
34 if (!nlh) {
35 int errsv = errno;
36 fprintf(stderr, "Netlink message header is NULL!\n");
37 return -errsv;
38 }
39
40 /* Complete the nl_msg header */
41 if (gettimeofday(&tv, NULL))
42 nlh->nlmsg_seq = 1;
43 else
44 nlh->nlmsg_seq = (int) tv.tv_sec;
45 nlh->nlmsg_pid = sk->s_local.nl_pid;
46 nlh->nlmsg_flags |= NLM_F_REQUEST | NLM_F_ACK;
47
48 return nl_send(sk, msg);
49 }
50
51 /* Receives a netlink message, allocates a buffer in *buf and stores
52 * the message content. The peer's netlink address is stored in
53 * *nla. The caller is responsible for freeing the buffer allocated in
54 * *buf if a positive value is returned. Interrupted system calls are
55 * handled by repeating the read. The input buffer size is determined
56 * by peeking before the actual read is done */
nl_recv(struct nl_sock * sk,struct sockaddr_nl * nla,unsigned char ** buf,struct ucred ** creds)57 int nl_recv(struct nl_sock *sk, struct sockaddr_nl *nla, \
58 unsigned char **buf, struct ucred **creds)
59 {
60 int rc = -1;
61 int sk_flags;
62 int RECV_BUF_SIZE = getpagesize();
63 int errsv;
64 struct iovec recvmsg_iov;
65 struct msghdr msg;
66
67 /* Allocate buffer */
68 *buf = (unsigned char *) malloc(RECV_BUF_SIZE);
69 if (!(*buf)) {
70 rc = -ENOMEM;
71 goto fail;
72 }
73
74 /* Prepare to receive message */
75 recvmsg_iov.iov_base = *buf;
76 recvmsg_iov.iov_len = RECV_BUF_SIZE;
77
78 msg.msg_name = (void *) &sk->s_peer;
79 msg.msg_namelen = sizeof(sk->s_peer);
80 msg.msg_iov = &recvmsg_iov;
81 msg.msg_iovlen = 1;
82 msg.msg_control = NULL;
83 msg.msg_controllen = 0;
84 msg.msg_flags = 0;
85
86 /* Make non blocking and then restore previous setting */
87 sk_flags = fcntl(sk->s_fd, F_GETFL, 0);
88 fcntl(sk->s_fd, F_SETFL, O_NONBLOCK);
89 rc = recvmsg(sk->s_fd, &msg, 0);
90 errsv = errno;
91 fcntl(sk->s_fd, F_SETFL, sk_flags);
92
93 if (rc < 0) {
94 rc = -errsv;
95 free(*buf);
96 *buf = NULL;
97 }
98
99 fail:
100 return rc;
101 }
102
103 /* Receive a set of messages from a netlink socket */
104 /* NOTE: Does not currently support callback replacements!!! */
nl_recvmsgs(struct nl_sock * sk,struct nl_cb * cb)105 int nl_recvmsgs(struct nl_sock *sk, struct nl_cb *cb)
106 {
107 struct sockaddr_nl nla;
108 struct ucred *creds;
109
110 int rc, cb_rc = NL_OK, done = 0;
111
112 do {
113 unsigned char *buf;
114 int i, rem, flags;
115 struct nlmsghdr *nlh;
116 struct nlmsgerr *nlme;
117 struct nl_msg *msg;
118
119 done = 0;
120 rc = nl_recv(sk, &nla, &buf, &creds);
121 if (rc < 0)
122 break;
123
124 nlmsg_for_each_msg(nlh, (struct nlmsghdr *) buf, rc, rem) {
125
126 if (rc <= 0 || cb_rc == NL_STOP)
127 break;
128
129 /* Check for callbacks */
130
131 msg = (struct nl_msg *) malloc(sizeof(struct nl_msg));
132 memset(msg, 0, sizeof(*msg));
133 msg->nm_nlh = nlh;
134
135 /* Check netlink message type */
136
137 switch (msg->nm_nlh->nlmsg_type) {
138 case NLMSG_ERROR: /* Used for ACK too */
139 /* Certainly we should be doing some
140 * checking here to make sure this
141 * message is intended for us */
142 nlme = nlmsg_data(msg->nm_nlh);
143 if (nlme->error == 0)
144 msg->nm_nlh->nlmsg_flags |= NLM_F_ACK;
145
146 rc = nlme->error;
147 cb_rc = cb->cb_err(&nla, nlme, cb->cb_err_arg);
148 nlme = NULL;
149 break;
150
151 case NLMSG_DONE:
152 done = 1;
153
154 case NLMSG_OVERRUN:
155 case NLMSG_NOOP:
156 default:
157 break;
158 };
159
160 for (i = 0; i <= NL_CB_TYPE_MAX; i++) {
161
162 if (cb->cb_set[i]) {
163 switch (i) {
164 case NL_CB_VALID:
165 if (rc > 0)
166 cb_rc = cb->cb_set[i](msg, cb->cb_args[i]);
167 break;
168
169 case NL_CB_FINISH:
170 if ((msg->nm_nlh->nlmsg_flags & NLM_F_MULTI) &&
171 (msg->nm_nlh->nlmsg_type & NLMSG_DONE))
172 cb_rc = cb->cb_set[i](msg, cb->cb_args[i]);
173
174 break;
175
176 case NL_CB_ACK:
177 if (msg->nm_nlh->nlmsg_flags & NLM_F_ACK)
178 cb_rc = cb->cb_set[i](msg, cb->cb_args[i]);
179
180 break;
181 default:
182 break;
183 }
184 }
185 }
186
187 free(msg);
188 if (done)
189 break;
190 }
191 free(buf);
192 buf = NULL;
193
194 if (done)
195 break;
196 } while (rc > 0 && cb_rc != NL_STOP);
197
198 success:
199 fail:
200 return rc;
201 }
202
203 /* Send raw data over netlink socket */
nl_send(struct nl_sock * sk,struct nl_msg * msg)204 int nl_send(struct nl_sock *sk, struct nl_msg *msg)
205 {
206 struct nlmsghdr *nlh = nlmsg_hdr(msg);
207 struct iovec msg_iov;
208
209 /* Create IO vector with Netlink message */
210 msg_iov.iov_base = nlh;
211 msg_iov.iov_len = nlh->nlmsg_len;
212
213 return nl_send_iovec(sk, msg, &msg_iov, 1);
214 }
215
216 /* Send netlink message */
nl_send_iovec(struct nl_sock * sk,struct nl_msg * msg,struct iovec * iov,unsigned iovlen)217 int nl_send_iovec(struct nl_sock *sk, struct nl_msg *msg,
218 struct iovec *iov, unsigned iovlen)
219 {
220 int rc;
221
222 /* Socket message */
223 struct msghdr mh = {
224 .msg_name = (void *) &sk->s_peer,
225 .msg_namelen = sizeof(sk->s_peer),
226 .msg_iov = iov,
227 .msg_iovlen = iovlen,
228 .msg_control = NULL,
229 .msg_controllen = 0,
230 .msg_flags = 0
231 };
232
233 /* Send message and verify sent */
234 rc = nl_sendmsg(sk, (struct nl_msg *) &mh, 0);
235 if (rc < 0)
236 fprintf(stderr, "Error sending netlink message: %d\n", errno);
237 return rc;
238
239 }
240
241 /* Send netlink message with control over sendmsg() message header */
nl_sendmsg(struct nl_sock * sk,struct nl_msg * msg,struct msghdr * hdr)242 int nl_sendmsg(struct nl_sock *sk, struct nl_msg *msg, struct msghdr *hdr)
243 {
244 return sendmsg(sk->s_fd, (struct msghdr *) msg, (int) hdr);
245 }
246
247 /* Create and connect netlink socket */
nl_connect(struct nl_sock * sk,int protocol)248 int nl_connect(struct nl_sock *sk, int protocol)
249 {
250 struct sockaddr addr;
251 socklen_t addrlen;
252 int rc;
253
254 /* Create RX socket */
255 sk->s_fd = socket(PF_NETLINK, SOCK_RAW, protocol);
256 if (sk->s_fd < 0)
257 return -errno;
258
259 /* Set size of RX and TX buffers */
260 if (nl_socket_set_buffer_size(sk, NL_BUFFER_SZ, NL_BUFFER_SZ) < 0)
261 return -errno;
262
263 /* Bind RX socket */
264 rc = bind(sk->s_fd, (struct sockaddr *)&sk->s_local, \
265 sizeof(sk->s_local));
266 if (rc < 0)
267 return -errno;
268 addrlen = sizeof(addr);
269 getsockname(sk->s_fd, &addr, &addrlen);
270
271 return 0;
272
273 }
274