Add a Netlink utility function to add a RTA header and reserve space for data
[strongswan.git] / src / libhydra / plugins / kernel_netlink / kernel_netlink_shared.c
1 /*
2 * Copyright (C) 2008 Tobias Brunner
3 * Hochschule fuer Technik Rapperswil
4 *
5 * This program is free software; you can redistribute it and/or modify it
6 * under the terms of the GNU General Public License as published by the
7 * Free Software Foundation; either version 2 of the License, or (at your
8 * option) any later version. See <http://www.fsf.org/copyleft/gpl.txt>.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
12 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
13 * for more details.
14 */
15
16 #include <sys/socket.h>
17 #include <linux/netlink.h>
18 #include <linux/rtnetlink.h>
19 #include <errno.h>
20 #include <unistd.h>
21
22 #include "kernel_netlink_shared.h"
23
24 #include <utils/debug.h>
25 #include <threading/mutex.h>
26
27 typedef struct private_netlink_socket_t private_netlink_socket_t;
28
29 /**
30 * Private variables and functions of netlink_socket_t class.
31 */
32 struct private_netlink_socket_t {
33 /**
34 * public part of the netlink_socket_t object.
35 */
36 netlink_socket_t public;
37
38 /**
39 * mutex to lock access to netlink socket
40 */
41 mutex_t *mutex;
42
43 /**
44 * current sequence number for netlink request
45 */
46 int seq;
47
48 /**
49 * netlink socket protocol
50 */
51 int protocol;
52
53 /**
54 * netlink socket
55 */
56 int socket;
57 };
58
59 /**
60 * Imported from kernel_netlink_ipsec.c
61 */
62 extern enum_name_t *xfrm_msg_names;
63
64 METHOD(netlink_socket_t, netlink_send, status_t,
65 private_netlink_socket_t *this, struct nlmsghdr *in, struct nlmsghdr **out,
66 size_t *out_len)
67 {
68 int len, addr_len;
69 struct sockaddr_nl addr;
70 chunk_t result = chunk_empty, tmp;
71 struct nlmsghdr *msg, peek;
72
73 this->mutex->lock(this->mutex);
74
75 in->nlmsg_seq = ++this->seq;
76 in->nlmsg_pid = getpid();
77
78 memset(&addr, 0, sizeof(addr));
79 addr.nl_family = AF_NETLINK;
80 addr.nl_pid = 0;
81 addr.nl_groups = 0;
82
83 if (this->protocol == NETLINK_XFRM)
84 {
85 chunk_t in_chunk = { (u_char*)in, in->nlmsg_len };
86
87 DBG3(DBG_KNL, "sending %N: %B", xfrm_msg_names, in->nlmsg_type, &in_chunk);
88 }
89
90 while (TRUE)
91 {
92 len = sendto(this->socket, in, in->nlmsg_len, 0,
93 (struct sockaddr*)&addr, sizeof(addr));
94
95 if (len != in->nlmsg_len)
96 {
97 if (errno == EINTR)
98 {
99 /* interrupted, try again */
100 continue;
101 }
102 this->mutex->unlock(this->mutex);
103 DBG1(DBG_KNL, "error sending to netlink socket: %s", strerror(errno));
104 return FAILED;
105 }
106 break;
107 }
108
109 while (TRUE)
110 {
111 char buf[4096];
112 tmp.len = sizeof(buf);
113 tmp.ptr = buf;
114 msg = (struct nlmsghdr*)tmp.ptr;
115
116 memset(&addr, 0, sizeof(addr));
117 addr.nl_family = AF_NETLINK;
118 addr.nl_pid = getpid();
119 addr.nl_groups = 0;
120 addr_len = sizeof(addr);
121
122 len = recvfrom(this->socket, tmp.ptr, tmp.len, 0,
123 (struct sockaddr*)&addr, &addr_len);
124
125 if (len < 0)
126 {
127 if (errno == EINTR)
128 {
129 DBG1(DBG_KNL, "got interrupted");
130 /* interrupted, try again */
131 continue;
132 }
133 DBG1(DBG_KNL, "error reading from netlink socket: %s", strerror(errno));
134 this->mutex->unlock(this->mutex);
135 free(result.ptr);
136 return FAILED;
137 }
138 if (!NLMSG_OK(msg, len))
139 {
140 DBG1(DBG_KNL, "received corrupted netlink message");
141 this->mutex->unlock(this->mutex);
142 free(result.ptr);
143 return FAILED;
144 }
145 if (msg->nlmsg_seq != this->seq)
146 {
147 DBG1(DBG_KNL, "received invalid netlink sequence number");
148 if (msg->nlmsg_seq < this->seq)
149 {
150 continue;
151 }
152 this->mutex->unlock(this->mutex);
153 free(result.ptr);
154 return FAILED;
155 }
156
157 tmp.len = len;
158 result.ptr = realloc(result.ptr, result.len + tmp.len);
159 memcpy(result.ptr + result.len, tmp.ptr, tmp.len);
160 result.len += tmp.len;
161
162 /* NLM_F_MULTI flag does not seem to be set correctly, we use sequence
163 * numbers to detect multi header messages */
164 len = recvfrom(this->socket, &peek, sizeof(peek), MSG_PEEK | MSG_DONTWAIT,
165 (struct sockaddr*)&addr, &addr_len);
166
167 if (len == sizeof(peek) && peek.nlmsg_seq == this->seq)
168 {
169 /* seems to be multipart */
170 continue;
171 }
172 break;
173 }
174
175 *out_len = result.len;
176 *out = (struct nlmsghdr*)result.ptr;
177
178 this->mutex->unlock(this->mutex);
179
180 return SUCCESS;
181 }
182
183 METHOD(netlink_socket_t, netlink_send_ack, status_t,
184 private_netlink_socket_t *this, struct nlmsghdr *in)
185 {
186 struct nlmsghdr *out, *hdr;
187 size_t len;
188
189 if (netlink_send(this, in, &out, &len) != SUCCESS)
190 {
191 return FAILED;
192 }
193 hdr = out;
194 while (NLMSG_OK(hdr, len))
195 {
196 switch (hdr->nlmsg_type)
197 {
198 case NLMSG_ERROR:
199 {
200 struct nlmsgerr* err = (struct nlmsgerr*)NLMSG_DATA(hdr);
201
202 if (err->error)
203 {
204 if (-err->error == EEXIST)
205 { /* do not report existing routes */
206 free(out);
207 return ALREADY_DONE;
208 }
209 if (-err->error == ESRCH)
210 { /* do not report missing entries */
211 free(out);
212 return NOT_FOUND;
213 }
214 DBG1(DBG_KNL, "received netlink error: %s (%d)",
215 strerror(-err->error), -err->error);
216 free(out);
217 return FAILED;
218 }
219 free(out);
220 return SUCCESS;
221 }
222 default:
223 hdr = NLMSG_NEXT(hdr, len);
224 continue;
225 case NLMSG_DONE:
226 break;
227 }
228 break;
229 }
230 DBG1(DBG_KNL, "netlink request not acknowledged");
231 free(out);
232 return FAILED;
233 }
234
235 METHOD(netlink_socket_t, destroy, void,
236 private_netlink_socket_t *this)
237 {
238 if (this->socket > 0)
239 {
240 close(this->socket);
241 }
242 this->mutex->destroy(this->mutex);
243 free(this);
244 }
245
246 /**
247 * Described in header.
248 */
249 netlink_socket_t *netlink_socket_create(int protocol)
250 {
251 private_netlink_socket_t *this;
252 struct sockaddr_nl addr;
253
254 INIT(this,
255 .public = {
256 .send = _netlink_send,
257 .send_ack = _netlink_send_ack,
258 .destroy = _destroy,
259 },
260 .seq = 200,
261 .mutex = mutex_create(MUTEX_TYPE_DEFAULT),
262 .protocol = protocol,
263 );
264
265 memset(&addr, 0, sizeof(addr));
266 addr.nl_family = AF_NETLINK;
267
268 this->socket = socket(AF_NETLINK, SOCK_RAW, protocol);
269 if (this->socket < 0)
270 {
271 DBG1(DBG_KNL, "unable to create netlink socket");
272 destroy(this);
273 return NULL;
274 }
275
276 addr.nl_groups = 0;
277 if (bind(this->socket, (struct sockaddr*)&addr, sizeof(addr)))
278 {
279 DBG1(DBG_KNL, "unable to bind netlink socket");
280 destroy(this);
281 return NULL;
282 }
283
284 return &this->public;
285 }
286
287 /**
288 * Described in header.
289 */
290 void netlink_add_attribute(struct nlmsghdr *hdr, int rta_type, chunk_t data,
291 size_t buflen)
292 {
293 struct rtattr *rta;
294
295 if (NLMSG_ALIGN(hdr->nlmsg_len) + RTA_LENGTH(data.len) > buflen)
296 {
297 DBG1(DBG_KNL, "unable to add attribute, buffer too small");
298 return;
299 }
300
301 rta = (struct rtattr*)(((char*)hdr) + NLMSG_ALIGN(hdr->nlmsg_len));
302 rta->rta_type = rta_type;
303 rta->rta_len = RTA_LENGTH(data.len);
304 memcpy(RTA_DATA(rta), data.ptr, data.len);
305 hdr->nlmsg_len = NLMSG_ALIGN(hdr->nlmsg_len) + rta->rta_len;
306 }
307
308 /**
309 * Described in header.
310 */
311 void* netlink_reserve(struct nlmsghdr *hdr, int buflen, int type, int len)
312 {
313 struct rtattr *rta;
314
315 if (NLMSG_ALIGN(hdr->nlmsg_len) + RTA_LENGTH(len) > buflen)
316 {
317 DBG1(DBG_KNL, "unable to add attribute, buffer too small");
318 return NULL;
319 }
320
321 rta = ((void*)hdr) + NLMSG_ALIGN(hdr->nlmsg_len);
322 rta->rta_type = type;
323 rta->rta_len = RTA_LENGTH(len);
324 hdr->nlmsg_len = NLMSG_ALIGN(hdr->nlmsg_len) + rta->rta_len;
325
326 return RTA_DATA(rta);
327 }