userland support to process notifies for new NAT mappings detected in UDP encapsulation
[strongswan.git] / src / charon / 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 * $Id$
16 */
17
18 #include <sys/socket.h>
19 #include <linux/netlink.h>
20 #include <linux/rtnetlink.h>
21 #include <errno.h>
22 #include <unistd.h>
23
24 #include "kernel_netlink_shared.h"
25
26 #include <daemon.h>
27
28 typedef struct private_netlink_socket_t private_netlink_socket_t;
29
30 /**
31 * Private variables and functions of netlink_socket_t class.
32 */
33 struct private_netlink_socket_t {
34 /**
35 * public part of the netlink_socket_t object.
36 */
37 netlink_socket_t public;
38
39 /**
40 * mutex to lock access to netlink socket
41 */
42 pthread_mutex_t mutex;
43
44 /**
45 * current sequence number for netlink request
46 */
47 int seq;
48
49 /**
50 * netlink socket
51 */
52 int socket;
53 };
54
55 /**
56 * Implementation of netlink_socket_t.send
57 */
58 static status_t netlink_send(private_netlink_socket_t *this, struct nlmsghdr *in,
59 struct nlmsghdr **out, size_t *out_len)
60 {
61 int len, addr_len;
62 struct sockaddr_nl addr;
63 chunk_t result = chunk_empty, tmp;
64 struct nlmsghdr *msg, peek;
65
66 pthread_mutex_lock(&this->mutex);
67
68 in->nlmsg_seq = ++this->seq;
69 in->nlmsg_pid = getpid();
70
71 memset(&addr, 0, sizeof(addr));
72 addr.nl_family = AF_NETLINK;
73 addr.nl_pid = 0;
74 addr.nl_groups = 0;
75
76 while (TRUE)
77 {
78 len = sendto(this->socket, in, in->nlmsg_len, 0,
79 (struct sockaddr*)&addr, sizeof(addr));
80
81 if (len != in->nlmsg_len)
82 {
83 if (errno == EINTR)
84 {
85 /* interrupted, try again */
86 continue;
87 }
88 pthread_mutex_unlock(&this->mutex);
89 DBG1(DBG_KNL, "error sending to netlink socket: %s", strerror(errno));
90 return FAILED;
91 }
92 break;
93 }
94
95 while (TRUE)
96 {
97 char buf[4096];
98 tmp.len = sizeof(buf);
99 tmp.ptr = buf;
100 msg = (struct nlmsghdr*)tmp.ptr;
101
102 memset(&addr, 0, sizeof(addr));
103 addr.nl_family = AF_NETLINK;
104 addr.nl_pid = getpid();
105 addr.nl_groups = 0;
106 addr_len = sizeof(addr);
107
108 len = recvfrom(this->socket, tmp.ptr, tmp.len, 0,
109 (struct sockaddr*)&addr, &addr_len);
110
111 if (len < 0)
112 {
113 if (errno == EINTR)
114 {
115 DBG1(DBG_KNL, "got interrupted");
116 /* interrupted, try again */
117 continue;
118 }
119 DBG1(DBG_KNL, "error reading from netlink socket: %s", strerror(errno));
120 pthread_mutex_unlock(&this->mutex);
121 free(result.ptr);
122 return FAILED;
123 }
124 if (!NLMSG_OK(msg, len))
125 {
126 DBG1(DBG_KNL, "received corrupted netlink message");
127 pthread_mutex_unlock(&this->mutex);
128 free(result.ptr);
129 return FAILED;
130 }
131 if (msg->nlmsg_seq != this->seq)
132 {
133 DBG1(DBG_KNL, "received invalid netlink sequence number");
134 if (msg->nlmsg_seq < this->seq)
135 {
136 continue;
137 }
138 pthread_mutex_unlock(&this->mutex);
139 free(result.ptr);
140 return FAILED;
141 }
142
143 tmp.len = len;
144 result.ptr = realloc(result.ptr, result.len + tmp.len);
145 memcpy(result.ptr + result.len, tmp.ptr, tmp.len);
146 result.len += tmp.len;
147
148 /* NLM_F_MULTI flag does not seem to be set correctly, we use sequence
149 * numbers to detect multi header messages */
150 len = recvfrom(this->socket, &peek, sizeof(peek), MSG_PEEK | MSG_DONTWAIT,
151 (struct sockaddr*)&addr, &addr_len);
152
153 if (len == sizeof(peek) && peek.nlmsg_seq == this->seq)
154 {
155 /* seems to be multipart */
156 continue;
157 }
158 break;
159 }
160
161 *out_len = result.len;
162 *out = (struct nlmsghdr*)result.ptr;
163
164 pthread_mutex_unlock(&this->mutex);
165
166 return SUCCESS;
167 }
168
169 /**
170 * Implementation of netlink_socket_t.send_ack.
171 */
172 static status_t netlink_send_ack(private_netlink_socket_t *this, struct nlmsghdr *in)
173 {
174 struct nlmsghdr *out, *hdr;
175 size_t len;
176
177 if (netlink_send(this, in, &out, &len) != SUCCESS)
178 {
179 return FAILED;
180 }
181 hdr = out;
182 while (NLMSG_OK(hdr, len))
183 {
184 switch (hdr->nlmsg_type)
185 {
186 case NLMSG_ERROR:
187 {
188 struct nlmsgerr* err = (struct nlmsgerr*)NLMSG_DATA(hdr);
189
190 if (err->error)
191 {
192 if (-err->error == EEXIST)
193 { /* do not report existing routes */
194 free(out);
195 return ALREADY_DONE;
196 }
197 DBG1(DBG_KNL, "received netlink error: %s (%d)",
198 strerror(-err->error), -err->error);
199 free(out);
200 return FAILED;
201 }
202 free(out);
203 return SUCCESS;
204 }
205 default:
206 hdr = NLMSG_NEXT(hdr, len);
207 continue;
208 case NLMSG_DONE:
209 break;
210 }
211 break;
212 }
213 DBG1(DBG_KNL, "netlink request not acknowledged");
214 free(out);
215 return FAILED;
216 }
217
218 /**
219 * Implementation of netlink_socket_t.destroy.
220 */
221 static void destroy(private_netlink_socket_t *this)
222 {
223 close(this->socket);
224 free(this);
225 }
226
227 /**
228 * Described in header.
229 */
230 netlink_socket_t *netlink_socket_create(int protocol) {
231 private_netlink_socket_t *this = malloc_thing(private_netlink_socket_t);
232 struct sockaddr_nl addr;
233
234 /* public functions */
235 this->public.send = (status_t(*)(netlink_socket_t*,struct nlmsghdr*, struct nlmsghdr**, size_t*))netlink_send;
236 this->public.send_ack = (status_t(*)(netlink_socket_t*,struct nlmsghdr*))netlink_send_ack;
237 this->public.destroy = (void(*)(netlink_socket_t*))destroy;
238
239 /* private members */
240 this->seq = 200;
241 pthread_mutex_init(&this->mutex, NULL);
242
243 memset(&addr, 0, sizeof(addr));
244 addr.nl_family = AF_NETLINK;
245
246 this->socket = socket(AF_NETLINK, SOCK_RAW, protocol);
247 if (this->socket <= 0)
248 {
249 charon->kill(charon, "unable to create netlink socket");
250 }
251
252 addr.nl_groups = 0;
253 if (bind(this->socket, (struct sockaddr*)&addr, sizeof(addr)))
254 {
255 charon->kill(charon, "unable to bind netlink socket");
256 }
257
258 return &this->public;
259 }
260
261 /**
262 * Described in header.
263 */
264 void netlink_add_attribute(struct nlmsghdr *hdr, int rta_type, chunk_t data,
265 size_t buflen)
266 {
267 struct rtattr *rta;
268
269 if (NLMSG_ALIGN(hdr->nlmsg_len) + RTA_ALIGN(data.len) > buflen)
270 {
271 DBG1(DBG_KNL, "unable to add attribute, buffer too small");
272 return;
273 }
274
275 rta = (struct rtattr*)(((char*)hdr) + NLMSG_ALIGN(hdr->nlmsg_len));
276 rta->rta_type = rta_type;
277 rta->rta_len = RTA_LENGTH(data.len);
278 memcpy(RTA_DATA(rta), data.ptr, data.len);
279 hdr->nlmsg_len = NLMSG_ALIGN(hdr->nlmsg_len) + rta->rta_len;
280 }