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