diff -uar uml_router/port.c uml_router.vlan/port.c --- uml_router/port.c 2003-03-12 16:19:03.000000000 +0100 +++ uml_router.vlan/port.c 2004-05-17 23:42:51.000000000 +0200 @@ -7,16 +7,49 @@ #include "switch.h" #include "hash.h" #include "port.h" +#include -struct packet { - struct { - unsigned char dest[ETH_ALEN]; - unsigned char src[ETH_ALEN]; - unsigned char proto[2]; - } header; +/* + * Modified by Ferdinand O. Tempel to add VLAN support + * (802.1q) support to uml_switch. + * Modifications released under the same license as the original port.c, + * and that was what, GPL? Who knows, noone bothered to put a license or + * copyright notice on this file. + * + * Some stuff is hatched using 2.6.4 kernel code as reference. I appologize. + */ + +#define DEBUG 1 + +#define ALLOWVLANBROADCAST 0 +#define ALLOWVLANCROSSING 0 +#define VLAN_VID_MASK 0xfff +#define VLAN_PROTO 0x8100 + +struct ethhdr { + unsigned char h_dest[ETH_ALEN]; /* destination eth addr */ + unsigned char h_source[ETH_ALEN]; /* source ether addr */ + unsigned char h_proto[2]; /* packet type ID field, 0x8100 for VLAN */ +}; + +struct vlan_packet { + unsigned short h_vlan_TCI; /* Encapsulates priority and VLAN ID */ + unsigned short h_vlan_encapsulated_proto; /* packet type ID field (or len) */ unsigned char data[1500]; }; +struct eth_packet { + unsigned char data[1500]; +}; + +struct packet { + struct ethhdr header; + union { + struct vlan_packet vpacket; + struct eth_packet epacket; + } pktdata; +}; + struct port { struct port *next; struct port *prev; @@ -24,6 +57,7 @@ void *data; int data_len; unsigned char src[ETH_ALEN]; + short vid; void (*sender)(int fd, void *packet, int len, void *data); }; @@ -31,6 +65,11 @@ #define IS_BROADCAST(addr) ((addr[0] & 1) == 1) +static short get_vid(unsigned short h_vlan_TCI) { + unsigned short vlan_TCI = ntohs(h_vlan_TCI); + return (vlan_TCI & VLAN_VID_MASK); +} + static void free_port(struct port *port) { if(port->prev) port->prev->next = port->next; @@ -54,47 +93,48 @@ free_port(port); } -static void update_src(struct port *port, struct packet *p) +static void update_src(struct port *port, struct packet *packet) { struct port *last; + + if(IS_BROADCAST(packet->header.h_source)) return; - /* We don't like broadcast source addresses */ - if(IS_BROADCAST(p->header.src)) return; - - last = find_in_hash(p->header.src); + if(packet->header.h_proto[0] == 0x81 && packet->header.h_proto[1]==0x00) + port->vid = get_vid(packet->pktdata.vpacket.h_vlan_TCI); + + last = find_in_hash(packet->header.h_source); if(port != last){ /* old value differs from actual input port */ printf(" Addr: %02x:%02x:%02x:%02x:%02x:%02x New port %d", - p->header.src[0], p->header.src[1], p->header.src[2], - p->header.src[3], p->header.src[4], p->header.src[5], + packet->header.h_source[0], packet->header.h_source[1], packet->header.h_source[2], + packet->header.h_source[3], packet->header.h_source[4], packet->header.h_source[5], port->control); if(last != NULL){ printf(" old port %d", last->control); - delete_hash(p->header.src); + delete_hash(packet->header.h_source); } printf("\n"); - - memcpy(port->src, p->header.src, sizeof(port->src)); - insert_into_hash(p->header.src, port); + + memcpy(port->src, packet->header.h_source, sizeof(port->src)); + insert_into_hash(packet->header.h_source, port); } - update_entry_time(p->header.src); + update_entry_time(packet->header.h_source); } -static void send_dst(struct port *port, struct packet *packet, int len, - int hub) +static void send_dst(struct port *port, int len, int vlan, + int hub, struct packet *packet) { struct port *target, *p; - - target = find_in_hash(packet->header.dest); - if((target == NULL) || IS_BROADCAST(packet->header.dest) || hub){ - if((target == NULL) && !IS_BROADCAST(packet->header.dest)){ + target = find_in_hash(packet->header.h_dest); + if((target == NULL) || IS_BROADCAST(packet->header.h_dest) || hub){ + if((target == NULL) && !IS_BROADCAST(packet->header.h_dest)){ printf("unknown Addr: %02x:%02x:%02x:%02x:%02x:%02x from port ", - packet->header.src[0], packet->header.src[1], - packet->header.src[2], packet->header.src[3], - packet->header.src[4], packet->header.src[5]); + packet->header.h_source[0], packet->header.h_source[1], + packet->header.h_source[2], packet->header.h_source[3], + packet->header.h_source[4], packet->header.h_source[5]); if(port == NULL) printf("UNKNOWN\n"); else printf("%d\n", port->control); } @@ -103,18 +143,37 @@ for(p = head; p != NULL; p = p->next){ /* don't send it back the port it came in */ if(p == port) continue; - - (*p->sender)(p->control, packet, len, p->data); + /* check for protocol number, if it's 0x8100, we need to do some magic[2] */ + if(vlan && packet->header.h_proto[0]==0x81 && packet->header.h_proto[1]==0x00) { +#if DEBUG + printf("Traffic from %02x:%02x:%02x:%02x:%02x:%02x, VLAN %i to %02x:%02x:%02x:%02x:%02x:%02x, VLAN %i.\n", + packet->header.h_source[0], packet->header.h_source[1], + packet->header.h_source[2], packet->header.h_source[3], + packet->header.h_source[4], packet->header.h_source[5], get_vid(packet->pktdata.vpacket.h_vlan_TCI), + packet->header.h_dest[0], packet->header.h_dest[1], + packet->header.h_dest[2], packet->header.h_dest[3], + packet->header.h_dest[4], packet->header.h_dest[5], p->vid); +#endif + if(get_vid(packet->pktdata.vpacket.h_vlan_TCI)==p->vid || ALLOWVLANBROADCAST) + (*p->sender)(p->control, packet, len, p->data); + } + else if (!vlan) (*p->sender)(p->control, packet, len, p->data); } } - else (*target->sender)(target->control, packet, len, target->data); + else { + if(vlan && packet->header.h_proto[0]==0x81 && packet->header.h_proto[1]==0x00) { + if(get_vid(packet->pktdata.vpacket.h_vlan_TCI)==target->vid || ALLOWVLANCROSSING) + (*target->sender)(target->control, packet, len, target->data); + } + else if (!vlan) (*target->sender)(target->control, packet, len, target->data); + } } -static void handle_data(int fd, int hub, struct packet *packet, int len, +static void handle_data(int fd, int vlan, int hub, int len, void *data, int (*matcher)(int port_fd, int data_fd, void *port_data, int port_data_len, - void *data)) + void *data), void *packet) { struct port *p; @@ -126,7 +185,7 @@ if(p != NULL) update_src(p, packet); else printf("Unknown connection for packet, shouldn't happen.\n"); - send_dst(p, packet, len, hub); + send_dst(p, len, vlan, hub, packet); } static int match_tap(int port_fd, int data_fd, void *port_data, @@ -135,9 +194,9 @@ return(port_fd == data_fd); } -void handle_tap_data(int fd, int hub) +void handle_tap_data(int fd, int vlan, int hub) { - struct packet packet; + struct vlan_packet packet; int len; len = read(fd, &packet, sizeof(packet)); @@ -145,7 +204,7 @@ if(errno != EAGAIN) perror("Reading tap data"); return; } - handle_data(fd, hub, &packet, len, NULL, match_tap); + handle_data(fd, vlan, hub, len, NULL, match_tap, &packet); } int setup_port(int fd, void (*sender)(int fd, void *packet, int len, @@ -198,12 +257,11 @@ return(!memcmp(&port->sock, &mine->sock, sizeof(mine->sock))); } -void handle_sock_data(int fd, int hub) +void handle_sock_data(int fd, int vlan, int hub) { - struct packet packet; + struct vlan_packet packet; struct sock_data data; int len, socklen = sizeof(data.sock); - len = recvfrom(fd, &packet, sizeof(packet), 0, (struct sockaddr *) &data.sock, &socklen); if(len < 0){ @@ -211,8 +269,7 @@ return; } data.fd = fd; - - handle_data(fd, hub, &packet, len, &data, match_sock); + handle_data(fd, vlan, hub, len, &data, match_sock, &packet); } int setup_sock_port(int fd, struct sockaddr_un *name, int data_fd) diff -uar uml_router/port.h uml_router.vlan/port.h --- uml_router/port.h 2002-04-10 15:07:02.000000000 +0200 +++ uml_router.vlan/port.h 2004-05-16 18:14:05.000000000 +0200 @@ -14,7 +14,7 @@ extern int setup_port(int fd, void (*sender)(int fd, void *packet, int len, void *data), void *data, int data_len); -extern void handle_tap_data(int fd, int hub); -extern void handle_sock_data(int fd, int hub); +extern void handle_tap_data(int fd, int vlan, int hub); +extern void handle_sock_data(int fd, int vlan, int hub); #endif diff -uar uml_router/uml_switch.c uml_router.vlan/uml_switch.c --- uml_router/uml_switch.c 2003-08-15 21:03:58.000000000 +0200 +++ uml_router.vlan/uml_switch.c 2004-05-16 18:29:04.000000000 +0200 @@ -25,6 +25,7 @@ #endif static int hub = 0; +static int vlan = 0; static int compat_v0 = 0; enum request_type { REQ_NEW_CONTROL }; @@ -391,8 +392,8 @@ tap_str = "[ -tap tap-device ]"; #endif - fprintf(stderr, "Usage : %s [ -unix control-socket ] [ -hub ] %s\n" - "or : %s -compat-v0 [ -unix control-socket data-socket ] " + fprintf(stderr, "Usage : %s [ -unix control-socket ] [ -vlan] [ -hub ] %s\n" + "or : %s -compat-v0 [ -unix control-socket data-socket ] [ -vlan ]" "[ -hub ] %s\n", prog, tap_str, prog, tap_str); exit(1); @@ -442,6 +443,13 @@ argc--; argv++; } + else if(!strcmp(argv[0], "-vlan")){ + printf("%s will have VLAN support (note: this will imply switch mode, and hence disable -hub)\n", prog); + hub = 0; + vlan = 1; + argc--; + argv++; + } else if(!strcmp(argv[0], "-compat-v0")){ printf("Control protocol 0 compatibility\n"); compat_v0 = 1; @@ -551,9 +559,9 @@ } accept_connection(connect_fd); } - else if(fds[i].fd == data_fd) handle_sock_data(data_fd, hub); + else if(fds[i].fd == data_fd) handle_sock_data(data_fd, vlan, hub); #ifdef TUNTAP - else if(fds[i].fd == tap_fd) handle_tap_data(tap_fd, hub); + else if(fds[i].fd == tap_fd) handle_tap_data(tap_fd, vlan, hub); #endif else { new = handle_port(fds[i].fd);