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 }