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 */
29int 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 */
57int 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
99fail:
100	return rc;
101}
102
103/* Receive a set of messages from a netlink socket */
104/* NOTE: Does not currently support callback replacements!!! */
105int 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
198success:
199fail:
200	return rc;
201}
202
203/* Send raw data over netlink socket */
204int 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 */
217int 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 */
242int 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 */
248int 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