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