1/* 2 * Ethernet Switch IGMP Snooper 3 * Copyright (C) 2014 ASUSTeK Inc. 4 * All Rights Reserved. 5 6 * This program is free software: you can redistribute it and/or modify 7 * it under the terms of the GNU Affero General Public License as 8 * published by the Free Software Foundation, either version 3 of the 9 * License, or (at your option) any later version. 10 11 * This program is distributed in the hope that it will be useful, 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 * GNU Affero General Public License for more details. 15 16 * You should have received a copy of the GNU Affero General Public License 17 * along with this program. If not, see <http://www.gnu.org/licenses/>. 18 */ 19 20#include <stdio.h> 21#include <stdint.h> 22#include <stdlib.h> 23#include <string.h> 24#include <netinet/ether.h> 25 26#include "snooper.h" 27#include "queue.h" 28 29#ifdef DEBUG_SWITCH 30#define log_switch(fmt, args...) log_debug("%s::" fmt, "switch", ##args) 31#else 32#define log_switch(...) do {} while (0) 33#endif 34#ifdef TEST 35#define logger(level, fmt, args...) printf(fmt, ##args) 36#endif 37 38#define PORT_CPU PORT_MAX 39extern unsigned char ifhwaddr[ETHER_ADDR_LEN]; 40 41struct entry { 42 TAILQ_ENTRY(entry) link; 43 unsigned char ea[ETHER_ADDR_LEN]; 44 int ports; 45}; 46static TAILQ_HEAD(, entry) entries = TAILQ_HEAD_INITIALIZER(entries); 47 48static struct entry *get_entry(unsigned char *ea); 49static struct entry *add_entry(unsigned char *ea, int ports); 50static void del_entry(unsigned char *ea, int *ports); 51 52static struct entry *get_entry(unsigned char *ea) 53{ 54 static int port = 0; 55 struct entry *entry; 56 57 TAILQ_FOREACH(entry, &entries, link) { 58 if (memcmp(ea, entry->ea, ETHER_ADDR_LEN) == 0) 59 break; 60 } 61 if (!entry && (ea[0] & 1) == 0) { 62 entry = calloc(1, sizeof(*entry)); 63 if (!entry) 64 return NULL; 65 memcpy(entry->ea, ea, ETHER_ADDR_LEN); 66 entry->ports = port++ % 4; 67 TAILQ_INSERT_TAIL(&entries, entry, link); 68 } 69 70 return entry; 71} 72 73static struct entry *add_entry(unsigned char *ea, int ports) 74{ 75 struct entry *entry = get_entry(ea); 76 77 if (!entry) { 78 entry = calloc(1, sizeof(*entry)); 79 if (!entry) 80 return NULL; 81 memcpy(entry->ea, ea, ETHER_ADDR_LEN); 82 TAILQ_INSERT_TAIL(&entries, entry, link); 83 } 84 entry->ports = ports; 85 86 return entry; 87} 88 89static void del_entry(unsigned char *ea, int *ports) 90{ 91 struct entry *entry = get_entry(ea); 92 93 if (ports) 94 *ports = entry ? entry->ports : -1; 95 if (entry) { 96 TAILQ_REMOVE(&entries, entry, link); 97 free(entry); 98 } 99} 100 101static void free_entries(void) 102{ 103 struct entry *entry, *next; 104 105 TAILQ_FOREACH_SAFE(entry, &entries, link, next) { 106 TAILQ_REMOVE(&entries, entry, link); 107 free(entry); 108 } 109} 110 111int switch_init(char *ifname) 112{ 113 log_switch("%-5s [" FMT_EA "] = 0x%x on %s interface", "init", 114 ARG_EA(ifhwaddr), PORT_CPU, ifname); 115 116 return add_entry(ifhwaddr, PORT_CPU) ? 0 : -1; 117} 118 119void switch_done(void) 120{ 121 free_entries(); 122} 123 124int switch_get_port(unsigned char *haddr) 125{ 126 struct entry *entry = get_entry(haddr); 127 int ports = entry ? entry->ports : -1; 128 129 log_switch("%-5s [" FMT_EA "] = 0x%x", "read", 130 ARG_EA(haddr), ports); 131 132 return ports; 133} 134 135int switch_add_portmap(unsigned char *maddr, int portmap) 136{ 137 struct entry *entry = get_entry(maddr); 138 int value, ports = entry ? entry->ports : 0; 139 140 value = ports | portmap; 141 value |= (1 << PORT_CPU); 142 if (value != ports) { 143 log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "write", 144 ARG_EA(maddr), ARG_PORTS(value)); 145 add_entry(maddr, value); 146 } 147 148 return value; 149} 150 151int switch_del_portmap(unsigned char *maddr, int portmap) 152{ 153 struct entry *entry = get_entry(maddr); 154 int value, ports = entry ? entry->ports : 0; 155 156 value = ports & ~portmap; 157 value |= (1 << PORT_CPU); 158 if (value != ports) { 159 log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "write", 160 ARG_EA(maddr), ARG_PORTS(value)); 161 add_entry(maddr, value); 162 } 163 164 return value; 165} 166 167int switch_clr_portmap(unsigned char *maddr) 168{ 169 int ports; 170 171 del_entry(maddr, &ports); 172 log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "clear", 173 ARG_EA(maddr), ARG_PORTS(ports)); 174 175 return ports; 176} 177 178#ifdef TEST 179int main(int argc, char *argv[]) 180{ 181 return 0; 182} 183#endif 184