workbench/network/WirelessManager/src/ap/vlan_init.c

   1 /*
   2  * hostapd / VLAN initialization
   3  * Copyright 2003, Instant802 Networks, Inc.
   4  * Copyright 2005-2006, Devicescape Software, Inc.
   5  * Copyright (c) 2009, Jouni Malinen <j@w1.fi>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License version 2 as
   9  * published by the Free Software Foundation.
  10  *
  11  * Alternatively, this software may be distributed under the terms of BSD
  12  * license.
  13  *
  14  * See README and COPYING for more details.
  15  */
  16
  17 #include "utils/includes.h"
  18
  19 #include "utils/common.h"
  20 #include "hostapd.h"
  21 #include "ap_config.h"
  22 #include "vlan_init.h"
  23
  24
  25 #ifdef CONFIG_FULL_DYNAMIC_VLAN
  26
  27 #include <net/if.h>
  28 #include <sys/ioctl.h>
  29 #include <linux/sockios.h>
  30 #include <linux/if_vlan.h>
  31 #include <linux/if_bridge.h>
  32
  33 #include "drivers/priv_netlink.h"
  34 #include "utils/eloop.h"
  35
  36
  37 struct full_dynamic_vlan {
  38         int s; /* socket on which to listen for new/removed interfaces. */
  39 };
  40
  41
  42 static int ifconfig_helper(const char *if_name, int up)
  43 {
  44         int fd;
  45         struct ifreq ifr;
  46
  47         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
  48                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
  49                            "failed: %s", __func__, strerror(errno));
  50                 return -1;
  51         }
  52
  53         os_memset(&ifr, 0, sizeof(ifr));
  54         os_strlcpy(ifr.ifr_name, if_name, IFNAMSIZ);
  55
  56         if (ioctl(fd, SIOCGIFFLAGS, &ifr) != 0) {
  57                 wpa_printf(MSG_ERROR, "VLAN: %s: ioctl(SIOCGIFFLAGS) failed "
  58                            "for interface %s: %s",
  59                            __func__, if_name, strerror(errno));
  60                 close(fd);
  61                 return -1;
  62         }
  63
  64         if (up)
  65                 ifr.ifr_flags |= IFF_UP;
  66         else
  67                 ifr.ifr_flags &= ~IFF_UP;
  68
  69         if (ioctl(fd, SIOCSIFFLAGS, &ifr) != 0) {
  70                 wpa_printf(MSG_ERROR, "VLAN: %s: ioctl(SIOCSIFFLAGS) failed "
  71                            "for interface %s (up=%d): %s",
  72                            __func__, if_name, up, strerror(errno));
  73                 close(fd);
  74                 return -1;
  75         }
  76
  77         close(fd);
  78         return 0;
  79 }
  80
  81
  82 static int ifconfig_up(const char *if_name)
  83 {
  84         wpa_printf(MSG_DEBUG, "VLAN: Set interface %s up", if_name);
  85         return ifconfig_helper(if_name, 1);
  86 }
  87
  88
  89 static int ifconfig_down(const char *if_name)
  90 {
  91         wpa_printf(MSG_DEBUG, "VLAN: Set interface %s down", if_name);
  92         return ifconfig_helper(if_name, 0);
  93 }
  94
  95
  96 /*
  97  * These are only available in recent linux headers (without the leading
  98  * underscore).
  99  */
 100 #define _GET_VLAN_REALDEV_NAME_CMD      8
 101 #define _GET_VLAN_VID_CMD               9
 102
 103 /* This value should be 256 ONLY. If it is something else, then hostapd
 104  * might crash!, as this value has been hard-coded in 2.4.x kernel
 105  * bridging code.
 106  */
 107 #define MAX_BR_PORTS                    256
 108
 109 static int br_delif(const char *br_name, const char *if_name)
 110 {
 111         int fd;
 112         struct ifreq ifr;
 113         unsigned long args[2];
 114         int if_index;
 115
 116         wpa_printf(MSG_DEBUG, "VLAN: br_delif(%s, %s)", br_name, if_name);
 117         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 118                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
 119                            "failed: %s", __func__, strerror(errno));
 120                 return -1;
 121         }
 122
 123         if_index = if_nametoindex(if_name);
 124
 125         if (if_index == 0) {
 126                 wpa_printf(MSG_ERROR, "VLAN: %s: Failure determining "
 127                            "interface index for '%s'",
 128                            __func__, if_name);
 129                 close(fd);
 130                 return -1;
 131         }
 132
 133         args[0] = BRCTL_DEL_IF;
 134         args[1] = if_index;
 135
 136         os_strlcpy(ifr.ifr_name, br_name, sizeof(ifr.ifr_name));
 137         ifr.ifr_data = (__caddr_t) args;
 138
 139         if (ioctl(fd, SIOCDEVPRIVATE, &ifr) < 0 && errno != EINVAL) {
 140                 /* No error if interface already removed. */
 141                 wpa_printf(MSG_ERROR, "VLAN: %s: ioctl[SIOCDEVPRIVATE,"
 142                            "BRCTL_DEL_IF] failed for br_name=%s if_name=%s: "
 143                            "%s", __func__, br_name, if_name, strerror(errno));
 144                 close(fd);
 145                 return -1;
 146         }
 147
 148         close(fd);
 149         return 0;
 150 }
 151
 152
 153 /*
 154         Add interface 'if_name' to the bridge 'br_name'
 155
 156         returns -1 on error
 157         returns 1 if the interface is already part of the bridge
 158         returns 0 otherwise
 159 */
 160 static int br_addif(const char *br_name, const char *if_name)
 161 {
 162         int fd;
 163         struct ifreq ifr;
 164         unsigned long args[2];
 165         int if_index;
 166
 167         wpa_printf(MSG_DEBUG, "VLAN: br_addif(%s, %s)", br_name, if_name);
 168         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 169                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
 170                            "failed: %s", __func__, strerror(errno));
 171                 return -1;
 172         }
 173
 174         if_index = if_nametoindex(if_name);
 175
 176         if (if_index == 0) {
 177                 wpa_printf(MSG_ERROR, "VLAN: %s: Failure determining "
 178                            "interface index for '%s'",
 179                            __func__, if_name);
 180                 close(fd);
 181                 return -1;
 182         }
 183
 184         args[0] = BRCTL_ADD_IF;
 185         args[1] = if_index;
 186
 187         os_strlcpy(ifr.ifr_name, br_name, sizeof(ifr.ifr_name));
 188         ifr.ifr_data = (__caddr_t) args;
 189
 190         if (ioctl(fd, SIOCDEVPRIVATE, &ifr) < 0) {
 191                 if (errno == EBUSY) {
 192                         /* The interface is already added. */
 193                         close(fd);
 194                         return 1;
 195                 }
 196
 197                 wpa_printf(MSG_ERROR, "VLAN: %s: ioctl[SIOCDEVPRIVATE,"
 198                            "BRCTL_ADD_IF] failed for br_name=%s if_name=%s: "
 199                            "%s", __func__, br_name, if_name, strerror(errno));
 200                 close(fd);
 201                 return -1;
 202         }
 203
 204         close(fd);
 205         return 0;
 206 }
 207
 208
 209 static int br_delbr(const char *br_name)
 210 {
 211         int fd;
 212         unsigned long arg[2];
 213
 214         wpa_printf(MSG_DEBUG, "VLAN: br_delbr(%s)", br_name);
 215         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 216                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
 217                            "failed: %s", __func__, strerror(errno));
 218                 return -1;
 219         }
 220
 221         arg[0] = BRCTL_DEL_BRIDGE;
 222         arg[1] = (unsigned long) br_name;
 223
 224         if (ioctl(fd, SIOCGIFBR, arg) < 0 && errno != ENXIO) {
 225                 /* No error if bridge already removed. */
 226                 wpa_printf(MSG_ERROR, "VLAN: %s: BRCTL_DEL_BRIDGE failed for "
 227                            "%s: %s", __func__, br_name, strerror(errno));
 228                 close(fd);
 229                 return -1;
 230         }
 231
 232         close(fd);
 233         return 0;
 234 }
 235
 236
 237 /*
 238         Add a bridge with the name 'br_name'.
 239
 240         returns -1 on error
 241         returns 1 if the bridge already exists
 242         returns 0 otherwise
 243 */
 244 static int br_addbr(const char *br_name)
 245 {
 246         int fd;
 247         unsigned long arg[4];
 248         struct ifreq ifr;
 249
 250         wpa_printf(MSG_DEBUG, "VLAN: br_addbr(%s)", br_name);
 251         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 252                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
 253                            "failed: %s", __func__, strerror(errno));
 254                 return -1;
 255         }
 256
 257         arg[0] = BRCTL_ADD_BRIDGE;
 258         arg[1] = (unsigned long) br_name;
 259
 260         if (ioctl(fd, SIOCGIFBR, arg) < 0) {
 261                 if (errno == EEXIST) {
 262                         /* The bridge is already added. */
 263                         close(fd);
 264                         return 1;
 265                 } else {
 266                         wpa_printf(MSG_ERROR, "VLAN: %s: BRCTL_ADD_BRIDGE "
 267                                    "failed for %s: %s",
 268                                    __func__, br_name, strerror(errno));
 269                         close(fd);
 270                         return -1;
 271                 }
 272         }
 273
 274         /* Decrease forwarding delay to avoid EAPOL timeouts. */
 275         os_memset(&ifr, 0, sizeof(ifr));
 276         os_strlcpy(ifr.ifr_name, br_name, IFNAMSIZ);
 277         arg[0] = BRCTL_SET_BRIDGE_FORWARD_DELAY;
 278         arg[1] = 1;
 279         arg[2] = 0;
 280         arg[3] = 0;
 281         ifr.ifr_data = (char *) &arg;
 282         if (ioctl(fd, SIOCDEVPRIVATE, &ifr) < 0) {
 283                 wpa_printf(MSG_ERROR, "VLAN: %s: "
 284                            "BRCTL_SET_BRIDGE_FORWARD_DELAY (1 sec) failed for "
 285                            "%s: %s", __func__, br_name, strerror(errno));
 286                 /* Continue anyway */
 287         }
 288
 289         close(fd);
 290         return 0;
 291 }
 292
 293
 294 static int br_getnumports(const char *br_name)
 295 {
 296         int fd;
 297         int i;
 298         int port_cnt = 0;
 299         unsigned long arg[4];
 300         int ifindices[MAX_BR_PORTS];
 301         struct ifreq ifr;
 302
 303         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 304                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
 305                            "failed: %s", __func__, strerror(errno));
 306                 return -1;
 307         }
 308
 309         arg[0] = BRCTL_GET_PORT_LIST;
 310         arg[1] = (unsigned long) ifindices;
 311         arg[2] = MAX_BR_PORTS;
 312         arg[3] = 0;
 313
 314         os_memset(ifindices, 0, sizeof(ifindices));
 315         os_strlcpy(ifr.ifr_name, br_name, sizeof(ifr.ifr_name));
 316         ifr.ifr_data = (__caddr_t) arg;
 317
 318         if (ioctl(fd, SIOCDEVPRIVATE, &ifr) < 0) {
 319                 wpa_printf(MSG_ERROR, "VLAN: %s: BRCTL_GET_PORT_LIST "
 320                            "failed for %s: %s",
 321                            __func__, br_name, strerror(errno));
 322                 close(fd);
 323                 return -1;
 324         }
 325
 326         for (i = 1; i < MAX_BR_PORTS; i++) {
 327                 if (ifindices[i] > 0) {
 328                         port_cnt++;
 329                 }
 330         }
 331
 332         close(fd);
 333         return port_cnt;
 334 }
 335
 336
 337 static int vlan_rem(const char *if_name)
 338 {
 339         int fd;
 340         struct vlan_ioctl_args if_request;
 341
 342         wpa_printf(MSG_DEBUG, "VLAN: vlan_rem(%s)", if_name);
 343         if ((os_strlen(if_name) + 1) > sizeof(if_request.device1)) {
 344                 wpa_printf(MSG_ERROR, "VLAN: Interface name too long: '%s'",
 345                            if_name);
 346                 return -1;
 347         }
 348
 349         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 350                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
 351                            "failed: %s", __func__, strerror(errno));
 352                 return -1;
 353         }
 354
 355         os_memset(&if_request, 0, sizeof(if_request));
 356
 357         os_strlcpy(if_request.device1, if_name, sizeof(if_request.device1));
 358         if_request.cmd = DEL_VLAN_CMD;
 359
 360         if (ioctl(fd, SIOCSIFVLAN, &if_request) < 0) {
 361                 wpa_printf(MSG_ERROR, "VLAN: %s: DEL_VLAN_CMD failed for %s: "
 362                            "%s", __func__, if_name, strerror(errno));
 363                 close(fd);
 364                 return -1;
 365         }
 366
 367         close(fd);
 368         return 0;
 369 }
 370
 371
 372 /*
 373         Add a vlan interface with VLAN ID 'vid' and tagged interface
 374         'if_name'.
 375
 376         returns -1 on error
 377         returns 1 if the interface already exists
 378         returns 0 otherwise
 379 */
 380 static int vlan_add(const char *if_name, int vid)
 381 {
 382         int fd;
 383         struct vlan_ioctl_args if_request;
 384
 385         wpa_printf(MSG_DEBUG, "VLAN: vlan_add(if_name=%s, vid=%d)",
 386                    if_name, vid);
 387         ifconfig_up(if_name);
 388
 389         if ((os_strlen(if_name) + 1) > sizeof(if_request.device1)) {
 390                 wpa_printf(MSG_ERROR, "VLAN: Interface name too long: '%s'",
 391                            if_name);
 392                 return -1;
 393         }
 394
 395         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 396                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
 397                            "failed: %s", __func__, strerror(errno));
 398                 return -1;
 399         }
 400
 401         os_memset(&if_request, 0, sizeof(if_request));
 402
 403         /* Determine if a suitable vlan device already exists. */
 404
 405         os_snprintf(if_request.device1, sizeof(if_request.device1), "vlan%d",
 406                     vid);
 407
 408         if_request.cmd = _GET_VLAN_VID_CMD;
 409
 410         if (ioctl(fd, SIOCSIFVLAN, &if_request) == 0) {
 411
 412                 if (if_request.u.VID == vid) {
 413                         if_request.cmd = _GET_VLAN_REALDEV_NAME_CMD;
 414
 415                         if (ioctl(fd, SIOCSIFVLAN, &if_request) == 0 &&
 416                             os_strncmp(if_request.u.device2, if_name,
 417                                        sizeof(if_request.u.device2)) == 0) {
 418                                 close(fd);
 419                                 wpa_printf(MSG_DEBUG, "VLAN: vlan_add: "
 420                                            "if_name %s exists already",
 421                                            if_request.device1);
 422                                 return 1;
 423                         }
 424                 }
 425         }
 426
 427         /* A suitable vlan device does not already exist, add one. */
 428
 429         os_memset(&if_request, 0, sizeof(if_request));
 430         os_strlcpy(if_request.device1, if_name, sizeof(if_request.device1));
 431         if_request.u.VID = vid;
 432         if_request.cmd = ADD_VLAN_CMD;
 433
 434         if (ioctl(fd, SIOCSIFVLAN, &if_request) < 0) {
 435                 wpa_printf(MSG_ERROR, "VLAN: %s: ADD_VLAN_CMD failed for %s: "
 436                            "%s",
 437                            __func__, if_request.device1, strerror(errno));
 438                 close(fd);
 439                 return -1;
 440         }
 441
 442         close(fd);
 443         return 0;
 444 }
 445
 446
 447 static int vlan_set_name_type(unsigned int name_type)
 448 {
 449         int fd;
 450         struct vlan_ioctl_args if_request;
 451
 452         wpa_printf(MSG_DEBUG, "VLAN: vlan_set_name_type(name_type=%u)",
 453                    name_type);
 454         if ((fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
 455                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(AF_INET,SOCK_STREAM) "
 456                            "failed: %s", __func__, strerror(errno));
 457                 return -1;
 458         }
 459
 460         os_memset(&if_request, 0, sizeof(if_request));
 461
 462         if_request.u.name_type = name_type;
 463         if_request.cmd = SET_VLAN_NAME_TYPE_CMD;
 464         if (ioctl(fd, SIOCSIFVLAN, &if_request) < 0) {
 465                 wpa_printf(MSG_ERROR, "VLAN: %s: SET_VLAN_NAME_TYPE_CMD "
 466                            "name_type=%u failed: %s",
 467                            __func__, name_type, strerror(errno));
 468                 close(fd);
 469                 return -1;
 470         }
 471
 472         close(fd);
 473         return 0;
 474 }
 475
 476
 477 static void vlan_newlink(char *ifname, struct hostapd_data *hapd)
 478 {
 479         char vlan_ifname[IFNAMSIZ];
 480         char br_name[IFNAMSIZ];
 481         struct hostapd_vlan *vlan = hapd->conf->vlan;
 482         char *tagged_interface = hapd->conf->ssid.vlan_tagged_interface;
 483
 484         wpa_printf(MSG_DEBUG, "VLAN: vlan_newlink(%s)", ifname);
 485
 486         while (vlan) {
 487                 if (os_strcmp(ifname, vlan->ifname) == 0) {
 488
 489                         os_snprintf(br_name, sizeof(br_name), "brvlan%d",
 490                                     vlan->vlan_id);
 491
 492                         if (!br_addbr(br_name))
 493                                 vlan->clean |= DVLAN_CLEAN_BR;
 494
 495                         ifconfig_up(br_name);
 496
 497                         if (tagged_interface) {
 498
 499                                 if (!vlan_add(tagged_interface, vlan->vlan_id))
 500                                         vlan->clean |= DVLAN_CLEAN_VLAN;
 501
 502                                 os_snprintf(vlan_ifname, sizeof(vlan_ifname),
 503                                             "vlan%d", vlan->vlan_id);
 504
 505                                 if (!br_addif(br_name, vlan_ifname))
 506                                         vlan->clean |= DVLAN_CLEAN_VLAN_PORT;
 507
 508                                 ifconfig_up(vlan_ifname);
 509                         }
 510
 511                         if (!br_addif(br_name, ifname))
 512                                 vlan->clean |= DVLAN_CLEAN_WLAN_PORT;
 513
 514                         ifconfig_up(ifname);
 515
 516                         break;
 517                 }
 518                 vlan = vlan->next;
 519         }
 520 }
 521
 522
 523 static void vlan_dellink(char *ifname, struct hostapd_data *hapd)
 524 {
 525         char vlan_ifname[IFNAMSIZ];
 526         char br_name[IFNAMSIZ];
 527         struct hostapd_vlan *first, *prev, *vlan = hapd->conf->vlan;
 528         char *tagged_interface = hapd->conf->ssid.vlan_tagged_interface;
 529
 530         wpa_printf(MSG_DEBUG, "VLAN: vlan_dellink(%s)", ifname);
 531
 532         first = prev = vlan;
 533
 534         while (vlan) {
 535                 if (os_strcmp(ifname, vlan->ifname) == 0) {
 536                         os_snprintf(br_name, sizeof(br_name), "brvlan%d",
 537                                     vlan->vlan_id);
 538
 539                         if (vlan->clean & DVLAN_CLEAN_WLAN_PORT)
 540                                 br_delif(br_name, vlan->ifname);
 541
 542                         if (tagged_interface) {
 543                                 os_snprintf(vlan_ifname, sizeof(vlan_ifname),
 544                                             "vlan%d", vlan->vlan_id);
 545                                 if (vlan->clean & DVLAN_CLEAN_VLAN_PORT)
 546                                         br_delif(br_name, vlan_ifname);
 547                                 ifconfig_down(vlan_ifname);
 548
 549                                 if (vlan->clean & DVLAN_CLEAN_VLAN)
 550                                         vlan_rem(vlan_ifname);
 551                         }
 552
 553                         if ((vlan->clean & DVLAN_CLEAN_BR) &&
 554                             br_getnumports(br_name) == 0) {
 555                                 ifconfig_down(br_name);
 556                                 br_delbr(br_name);
 557                         }
 558
 559                         if (vlan == first) {
 560                                 hapd->conf->vlan = vlan->next;
 561                         } else {
 562                                 prev->next = vlan->next;
 563                         }
 564                         os_free(vlan);
 565
 566                         break;
 567                 }
 568                 prev = vlan;
 569                 vlan = vlan->next;
 570         }
 571 }
 572
 573
 574 static void
 575 vlan_read_ifnames(struct nlmsghdr *h, size_t len, int del,
 576                   struct hostapd_data *hapd)
 577 {
 578         struct ifinfomsg *ifi;
 579         int attrlen, nlmsg_len, rta_len;
 580         struct rtattr *attr;
 581
 582         if (len < sizeof(*ifi))
 583                 return;
 584
 585         ifi = NLMSG_DATA(h);
 586
 587         nlmsg_len = NLMSG_ALIGN(sizeof(struct ifinfomsg));
 588
 589         attrlen = h->nlmsg_len - nlmsg_len;
 590         if (attrlen < 0)
 591                 return;
 592
 593         attr = (struct rtattr *) (((char *) ifi) + nlmsg_len);
 594
 595         rta_len = RTA_ALIGN(sizeof(struct rtattr));
 596         while (RTA_OK(attr, attrlen)) {
 597                 char ifname[IFNAMSIZ + 1];
 598
 599                 if (attr->rta_type == IFLA_IFNAME) {
 600                         int n = attr->rta_len - rta_len;
 601                         if (n < 0)
 602                                 break;
 603
 604                         os_memset(ifname, 0, sizeof(ifname));
 605
 606                         if ((size_t) n > sizeof(ifname))
 607                                 n = sizeof(ifname);
 608                         os_memcpy(ifname, ((char *) attr) + rta_len, n);
 609
 610                         if (del)
 611                                 vlan_dellink(ifname, hapd);
 612                         else
 613                                 vlan_newlink(ifname, hapd);
 614                 }
 615
 616                 attr = RTA_NEXT(attr, attrlen);
 617         }
 618 }
 619
 620
 621 static void vlan_event_receive(int sock, void *eloop_ctx, void *sock_ctx)
 622 {
 623         char buf[8192];
 624         int left;
 625         struct sockaddr_nl from;
 626         socklen_t fromlen;
 627         struct nlmsghdr *h;
 628         struct hostapd_data *hapd = eloop_ctx;
 629
 630         fromlen = sizeof(from);
 631         left = recvfrom(sock, buf, sizeof(buf), MSG_DONTWAIT,
 632                         (struct sockaddr *) &from, &fromlen);
 633         if (left < 0) {
 634                 if (errno != EINTR && errno != EAGAIN)
 635                         wpa_printf(MSG_ERROR, "VLAN: %s: recvfrom failed: %s",
 636                                    __func__, strerror(errno));
 637                 return;
 638         }
 639
 640         h = (struct nlmsghdr *) buf;
 641         while (left >= (int) sizeof(*h)) {
 642                 int len, plen;
 643
 644                 len = h->nlmsg_len;
 645                 plen = len - sizeof(*h);
 646                 if (len > left || plen < 0) {
 647                         wpa_printf(MSG_DEBUG, "VLAN: Malformed netlink "
 648                                    "message: len=%d left=%d plen=%d",
 649                                    len, left, plen);
 650                         break;
 651                 }
 652
 653                 switch (h->nlmsg_type) {
 654                 case RTM_NEWLINK:
 655                         vlan_read_ifnames(h, plen, 0, hapd);
 656                         break;
 657                 case RTM_DELLINK:
 658                         vlan_read_ifnames(h, plen, 1, hapd);
 659                         break;
 660                 }
 661
 662                 len = NLMSG_ALIGN(len);
 663                 left -= len;
 664                 h = (struct nlmsghdr *) ((char *) h + len);
 665         }
 666
 667         if (left > 0) {
 668                 wpa_printf(MSG_DEBUG, "VLAN: %s: %d extra bytes in the end of "
 669                            "netlink message", __func__, left);
 670         }
 671 }
 672
 673
 674 static struct full_dynamic_vlan *
 675 full_dynamic_vlan_init(struct hostapd_data *hapd)
 676 {
 677         struct sockaddr_nl local;
 678         struct full_dynamic_vlan *priv;
 679
 680         priv = os_zalloc(sizeof(*priv));
 681         if (priv == NULL)
 682                 return NULL;
 683
 684         vlan_set_name_type(VLAN_NAME_TYPE_PLUS_VID_NO_PAD);
 685
 686         priv->s = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
 687         if (priv->s < 0) {
 688                 wpa_printf(MSG_ERROR, "VLAN: %s: socket(PF_NETLINK,SOCK_RAW,"
 689                            "NETLINK_ROUTE) failed: %s",
 690                            __func__, strerror(errno));
 691                 os_free(priv);
 692                 return NULL;
 693         }
 694
 695         os_memset(&local, 0, sizeof(local));
 696         local.nl_family = AF_NETLINK;
 697         local.nl_groups = RTMGRP_LINK;
 698         if (bind(priv->s, (struct sockaddr *) &local, sizeof(local)) < 0) {
 699                 wpa_printf(MSG_ERROR, "VLAN: %s: bind(netlink) failed: %s",
 700                            __func__, strerror(errno));
 701                 close(priv->s);
 702                 os_free(priv);
 703                 return NULL;
 704         }
 705
 706         if (eloop_register_read_sock(priv->s, vlan_event_receive, hapd, NULL))
 707         {
 708                 close(priv->s);
 709                 os_free(priv);
 710                 return NULL;
 711         }
 712
 713         return priv;
 714 }
 715
 716
 717 static void full_dynamic_vlan_deinit(struct full_dynamic_vlan *priv)
 718 {
 719         if (priv == NULL)
 720                 return;
 721         eloop_unregister_read_sock(priv->s);
 722         close(priv->s);
 723         os_free(priv);
 724 }
 725 #endif /* CONFIG_FULL_DYNAMIC_VLAN */
 726
 727
 728 int vlan_setup_encryption_dyn(struct hostapd_data *hapd,
 729                               struct hostapd_ssid *mssid, const char *dyn_vlan)
 730 {
 731         int i;
 732
 733         if (dyn_vlan == NULL)
 734                 return 0;
 735
 736         /* Static WEP keys are set here; IEEE 802.1X and WPA uses their own
 737          * functions for setting up dynamic broadcast keys. */
 738         for (i = 0; i < 4; i++) {
 739                 if (mssid->wep.key[i] &&
 740                     hapd->drv.set_key(dyn_vlan, hapd, WPA_ALG_WEP, NULL, i,
 741                                       i == mssid->wep.idx, NULL, 0,
 742                                       mssid->wep.key[i], mssid->wep.len[i])) {
 743                         wpa_printf(MSG_ERROR, "VLAN: Could not set WEP "
 744                                    "encryption for dynamic VLAN");
 745                         return -1;
 746                 }
 747         }
 748
 749         return 0;
 750 }
 751
 752
 753 static int vlan_dynamic_add(struct hostapd_data *hapd,
 754                             struct hostapd_vlan *vlan)
 755 {
 756         while (vlan) {
 757                 if (vlan->vlan_id != VLAN_ID_WILDCARD) {
 758                         if (hapd->drv.vlan_if_add(hapd, vlan->ifname)) {
 759                                 if (errno != EEXIST) {
 760                                         wpa_printf(MSG_ERROR, "VLAN: Could "
 761                                                    "not add VLAN %s: %s",
 762                                                    vlan->ifname,
 763                                                    strerror(errno));
 764                                         return -1;
 765                                 }
 766                         }
 767 #ifdef CONFIG_FULL_DYNAMIC_VLAN
 768                         ifconfig_up(vlan->ifname);
 769 #endif /* CONFIG_FULL_DYNAMIC_VLAN */
 770                 }
 771
 772                 vlan = vlan->next;
 773         }
 774
 775         return 0;
 776 }
 777
 778
 779 static void vlan_dynamic_remove(struct hostapd_data *hapd,
 780                                 struct hostapd_vlan *vlan)
 781 {
 782         struct hostapd_vlan *next;
 783
 784         while (vlan) {
 785                 next = vlan->next;
 786
 787                 if (vlan->vlan_id != VLAN_ID_WILDCARD &&
 788                     hapd->drv.vlan_if_remove(hapd, vlan->ifname)) {
 789                         wpa_printf(MSG_ERROR, "VLAN: Could not remove VLAN "
 790                                    "iface: %s: %s",
 791                                    vlan->ifname, strerror(errno));
 792                 }
 793 #ifdef CONFIG_FULL_DYNAMIC_VLAN
 794                 if (vlan->clean)
 795                         vlan_dellink(vlan->ifname, hapd);
 796 #endif /* CONFIG_FULL_DYNAMIC_VLAN */
 797
 798                 vlan = next;
 799         }
 800 }
 801
 802
 803 int vlan_init(struct hostapd_data *hapd)
 804 {
 805 #ifdef CONFIG_FULL_DYNAMIC_VLAN
 806         hapd->full_dynamic_vlan = full_dynamic_vlan_init(hapd);
 807 #endif /* CONFIG_FULL_DYNAMIC_VLAN */
 808
 809         if (vlan_dynamic_add(hapd, hapd->conf->vlan))
 810                 return -1;
 811
 812         return 0;
 813 }
 814
 815
 816 void vlan_deinit(struct hostapd_data *hapd)
 817 {
 818         vlan_dynamic_remove(hapd, hapd->conf->vlan);
 819
 820 #ifdef CONFIG_FULL_DYNAMIC_VLAN
 821         full_dynamic_vlan_deinit(hapd->full_dynamic_vlan);
 822 #endif /* CONFIG_FULL_DYNAMIC_VLAN */
 823 }
 824
 825
 826 struct hostapd_vlan * vlan_add_dynamic(struct hostapd_data *hapd,
 827                                        struct hostapd_vlan *vlan,
 828                                        int vlan_id)
 829 {
 830         struct hostapd_vlan *n;
 831         char *ifname, *pos;
 832
 833         if (vlan == NULL || vlan_id <= 0 || vlan_id > MAX_VLAN_ID ||
 834             vlan->vlan_id != VLAN_ID_WILDCARD)
 835                 return NULL;
 836
 837         wpa_printf(MSG_DEBUG, "VLAN: %s(vlan_id=%d ifname=%s)",
 838                    __func__, vlan_id, vlan->ifname);
 839         ifname = os_strdup(vlan->ifname);
 840         if (ifname == NULL)
 841                 return NULL;
 842         pos = os_strchr(ifname, '#');
 843         if (pos == NULL) {
 844                 os_free(ifname);
 845                 return NULL;
 846         }
 847         *pos++ = '\0';
 848
 849         n = os_zalloc(sizeof(*n));
 850         if (n == NULL) {
 851                 os_free(ifname);
 852                 return NULL;
 853         }
 854
 855         n->vlan_id = vlan_id;
 856         n->dynamic_vlan = 1;
 857
 858         os_snprintf(n->ifname, sizeof(n->ifname), "%s%d%s", ifname, vlan_id,
 859                     pos);
 860         os_free(ifname);
 861
 862         if (hapd->drv.vlan_if_add(hapd, n->ifname)) {
 863                 os_free(n);
 864                 return NULL;
 865         }
 866
 867         n->next = hapd->conf->vlan;
 868         hapd->conf->vlan = n;
 869
 870 #ifdef CONFIG_FULL_DYNAMIC_VLAN
 871         ifconfig_up(n->ifname);
 872 #endif /* CONFIG_FULL_DYNAMIC_VLAN */
 873
 874         return n;
 875 }
 876
 877
 878 int vlan_remove_dynamic(struct hostapd_data *hapd, int vlan_id)
 879 {
 880         struct hostapd_vlan *vlan;
 881
 882         if (vlan_id <= 0 || vlan_id > MAX_VLAN_ID)
 883                 return 1;
 884
 885         wpa_printf(MSG_DEBUG, "VLAN: %s(vlan_id=%d)", __func__, vlan_id);
 886
 887         vlan = hapd->conf->vlan;
 888         while (vlan) {
 889                 if (vlan->vlan_id == vlan_id && vlan->dynamic_vlan > 0) {
 890                         vlan->dynamic_vlan--;
 891                         break;
 892                 }
 893                 vlan = vlan->next;
 894         }
 895
 896         if (vlan == NULL)
 897                 return 1;
 898
 899         if (vlan->dynamic_vlan == 0)
 900                 hapd->drv.vlan_if_remove(hapd, vlan->ifname);
 901
 902         return 0;
 903 }