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 <unistd.h>
25#include <fcntl.h>
26#include <errno.h>
27#include <sys/ioctl.h>
28#include <sys/socket.h>
29#include <netinet/in.h>
30#include <netinet/ip.h>
31#include <netinet/ether.h>
32#include <net/if.h>
33#include <linux/sockios.h>
34#include <linux/if_vlan.h>
35
36#include <typedefs.h>
37#include <etioctl.h>
38
39#include "snooper.h"
40
41#ifdef DEBUG_SWITCH
42#define log_switch(fmt, args...) log_debug("%s::" fmt, "switch", ##args)
43#else
44#define log_switch(...) do {} while (0)
45#endif
46#ifdef TEST
47#define logger(level, fmt, args...) printf(fmt, ##args)
48#endif
49
50#define REG_MII_PAGE		0x10 /* MII Page register */
51#define REG_MII_ADDR		0x11 /* MII Address register */
52#define REG_MII_DATA0		0x18 /* MII Data register 0 */
53#define REG_MII_PAGE_ENABLE	1
54#define REG_MII_ADDR_WRITE	1
55#define REG_MII_ADDR_READ	2
56
57#define ROBO_PHY_ADDR		0x1e /* Robo Switch PHY Address */
58#define PAGE_CTRL		0x00 /* Control registers */
59#define PAGE_VTBL		0x05 /* ARL Access Registers */
60#define PAGE_VLAN		0x34 /* VLAN Registers */
61#define REG_CTRL_MCAST		0x21 /* Multicast Control */
62#define REG_VTBL_CTRL		0x00 /* ARL Read/Write Control */
63#define REG_VTBL_MINDX		0x02 /* MAC Address Index */
64#define REG_VTBL_VINDX		0x08 /* VID Table Address Index */
65#define REG_VTBL_ACCESS		0x80 /* VLAN Table Access */
66#define REG_VTBL_INDX		0x81 /* VLAN Table Address Index */
67#define REG_VTBL_ENTRY		0x83 /* VLAN Table Entry */
68#define REG_VLAN_ACCESS		0x06 /* VLAN Table Access register */
69#define REG_VLAN_READ		0x0c /* VLAN Read register */
70
71enum {
72	SWITCH_UNKNOWN,
73	SWITCH_BCM5325,
74	SWITCH_BCM53115,
75	SWITCH_BCM53125,
76	SWITCH_BCM5301x,
77} model;
78static int fd;
79static struct ifreq ifr;
80static int vlan;
81static int cpu_portmap;
82
83static int robo_ioctl(int write, int page, int reg, uint16_t *value, int count)
84{
85	static const int __ioctl_args[2] = { SIOCGETCROBORD, SIOCSETCROBOWR };
86	int ret, vecarg[5];
87	uint16_t *valp;
88
89	memset(&vecarg, 0, sizeof(vecarg));
90	vecarg[0] = (page << 16) | reg;
91	if (model == SWITCH_BCM5301x) {
92		vecarg[1] = count * 2;
93		valp = (uint16_t *) &vecarg[2];
94	} else
95		valp = (uint16_t *) &vecarg[1];
96
97	if (write)
98		memcpy(valp, value, count * 2);
99	ifr.ifr_data = (caddr_t) vecarg;
100	ret = ioctl(fd, __ioctl_args[write], (caddr_t) &ifr);
101	if (!write)
102		memcpy(value, valp, count * 2);
103
104	return ret;
105}
106
107static int phy_ioctl(int write, int phy, int reg, uint16_t *value)
108{
109	static const int __ioctl_args[2] = { SIOCGETCPHYRD2, SIOCSETCPHYWR2 };
110	int ret, vecarg[2];
111
112	if (model == SWITCH_BCM5301x)
113		return robo_ioctl(write, 0x10 + phy, reg, value, 1);
114
115	memset(&vecarg, 0, sizeof(vecarg));
116	vecarg[0] = (phy << 16) | reg;
117	if (write)
118		vecarg[1] = *value;
119	ifr.ifr_data = (caddr_t) vecarg;
120	ret = ioctl(fd, __ioctl_args[write], (caddr_t) &ifr);
121	if (!write)
122		*value = vecarg[1];
123
124	return ret;
125}
126
127static int phy_select(int page, int reg, int op)
128{
129	uint16_t value;
130	int i = 3;
131
132	value = (page << 8) | REG_MII_PAGE_ENABLE;
133	phy_ioctl(1, ROBO_PHY_ADDR, REG_MII_PAGE, &value);
134	value = (reg << 8) | op;
135	phy_ioctl(1, ROBO_PHY_ADDR, REG_MII_ADDR, &value);
136	while (i--) {
137		if (phy_ioctl(0, ROBO_PHY_ADDR, REG_MII_ADDR, &value) == 0 && (value & 3) == 0)
138			return 0;
139	}
140
141	return -1;
142}
143
144static int robo_read(int page, int reg, uint16_t *value, int count)
145{
146	int i, ret;
147
148	if (model == SWITCH_BCM5301x)
149		return robo_ioctl(0, page, reg, value, count);
150
151	ret = 0;
152	for (i = 0; i < count; i++) {
153		ret |= phy_select(page, reg, REG_MII_ADDR_READ);
154		phy_ioctl(0, ROBO_PHY_ADDR, REG_MII_DATA0 + i, &value[i]);
155	}
156
157	return ret;
158}
159
160static inline uint16_t robo_read16(int page, int reg)
161{
162	uint16_t value = 0;
163	robo_read(page, reg, &value, 1);
164	return value;
165}
166
167static inline uint32_t robo_read32(int page, int reg)
168{
169	uint32_t value = 0;
170	robo_read(page, reg, (uint16_t *) &value, 2);
171	return value;
172}
173
174static int robo_write(int page, int reg, uint16_t *value, int count)
175{
176	int i;
177
178	if (model == SWITCH_BCM5301x)
179		return robo_ioctl(1, page, reg, value, count);
180
181	for (i = 0; i < count; i++)
182		phy_ioctl(1, ROBO_PHY_ADDR, REG_MII_DATA0 + i, &value[i]);
183	return phy_select(page, reg, REG_MII_ADDR_WRITE);
184}
185
186static inline int robo_write16(int page, int reg, uint16_t value)
187{
188	return robo_write(page, reg, &value, 1);
189}
190
191static inline int robo_write32(int page, int reg, uint32_t value)
192{
193	return robo_write(page, reg, (uint16_t *) &value, 2);
194}
195
196static int get_model(void)
197{
198	et_var_t var;
199	int ret, devid = 0;
200
201	memset(&var, 0, sizeof(var));
202	var.set = 0;
203	var.cmd = IOV_ET_ROBO_DEVID;
204	var.buf = &devid;
205	var.len = sizeof(devid);
206
207	ifr.ifr_data = (caddr_t) &var;
208	ret = ioctl(fd, SIOCSETGETVAR, (caddr_t)&ifr);
209	if (ret < 0)
210		devid = robo_read16(0x02, 0x30);
211
212	if (devid == 0x25)
213		return SWITCH_BCM5325;
214	else if (devid == 0x3115)
215		return SWITCH_BCM53115;
216	else if (devid == 0x3125)
217		return SWITCH_BCM53125;
218	else if ((devid & 0xfffffff0) == 0x53010)
219		return SWITCH_BCM5301x;
220
221	return SWITCH_UNKNOWN;
222}
223
224static int read_entry(uint16_t *hwaddr, int vlan, uint16_t entry[6], int *index)
225{
226	int i, valid, portmap, vid;
227
228	vlan &= (model == SWITCH_BCM5325) ? 0x0f : 0x0fff;
229
230	robo_write(PAGE_VTBL, REG_VTBL_MINDX, hwaddr, 3);
231	robo_write16(PAGE_VTBL, REG_VTBL_VINDX, vlan);
232
233	robo_read(PAGE_VTBL, REG_VTBL_MINDX, &entry[0], 4);
234	robo_read(PAGE_VTBL, REG_VTBL_VINDX, &entry[4], 2);
235	robo_write16(PAGE_VTBL, REG_VTBL_CTRL, 0x81);
236
237	for (i = 0; i < (model == SWITCH_BCM5325 ? 2 : 4); i++) {
238		switch (model) {
239		case SWITCH_BCM5325:
240			robo_read(PAGE_VTBL, 0x10 + 0x08 * i, &entry[0], 4);
241			robo_read(PAGE_VTBL, 0x30 + 0x02 * i, &entry[4], 1);
242			valid = entry[3] & 0x8000;
243			portmap = entry[3] & 0x7f;
244			vid = entry[4] & 0x0f;
245			break;
246		case SWITCH_BCM53115:
247		case SWITCH_BCM53125:
248		case SWITCH_BCM5301x:
249			robo_read(PAGE_VTBL, 0x10 + 0x10 * i, &entry[0], 4);
250			robo_read(PAGE_VTBL, 0x18 + 0x10 * i, &entry[4], 2);
251			valid = entry[5] & 0x1;
252			portmap = entry[4] & 0x1ff;
253			vid = entry[3] & 0x0fff;
254			break;
255		default:
256			return -1;
257		}
258		if (valid && memcmp(entry, hwaddr, ETHER_ADDR_LEN) == 0 && vid == vlan) {
259			*index = i;
260			return portmap;
261		} else if (!valid)
262			*index = i;
263	}
264
265	memset(entry, 0, sizeof(entry));
266
267	return -1;
268}
269
270static int write_entry(uint16_t hwaddr[3], int vlan, uint16_t entry[6], int index, int portmap)
271{
272	memcpy(entry, hwaddr, ETHER_ADDR_LEN);
273	switch (model) {
274	case SWITCH_BCM5325:
275		entry[4] = vlan & 0x0f;
276		if (portmap >= 0) {
277			entry[3] &= ~0x7f;
278			entry[3] |= 0xc000 | (portmap & 0x7f);
279		} else
280			entry[3] &= ~0x8000;
281		portmap = entry[3] & 0x7f;
282		robo_write(PAGE_VTBL, 0x10 + 0x08 * index, &entry[0], 4);
283		robo_write(PAGE_VTBL, 0x30 + 0x02 * index, &entry[4], 1);
284		break;
285	case SWITCH_BCM53115:
286	case SWITCH_BCM53125:
287	case SWITCH_BCM5301x:
288		entry[3] = vlan & 0x0fff;
289		if (portmap >= 0) {
290			entry[4] &= ~0x1ff;
291			entry[4] |= 0x8000 | (portmap & 0x1ff);
292			entry[5] |= 0x01;
293		} else
294			entry[5] &= ~0x01;
295		portmap = entry[4] & 0x1ff;
296		robo_write(PAGE_VTBL, 0x10 + 0x10 * index, &entry[0], 4);
297		robo_write(PAGE_VTBL, 0x18 + 0x10 * index, &entry[4], 2);
298		break;
299	default:
300		return -1;
301	}
302	robo_write16(PAGE_VTBL, REG_VTBL_CTRL, 0x80);
303
304	return portmap;
305}
306
307static int read_portmap(int vlan)
308{
309	uint32_t entry;
310	int i, portmap = 0;
311
312	switch (model) {
313	case SWITCH_BCM5325:
314		for (i = vlan; i < vlan + 16; i++) {
315			robo_write16(PAGE_VLAN, REG_VLAN_ACCESS,
316			    0x2000 | (vlan & 0x0ff0) | (i & 0x0f));
317			entry = robo_read32(PAGE_VLAN, REG_VLAN_READ);
318			if (((entry & 0x1000000)) &&
319			    ((entry & 0x0fff000) >> 12) == (vlan & 0x0fff)) {
320				portmap = entry & 0x2f;
321				break;
322			}
323		}
324		break;
325	case SWITCH_BCM53115:
326	case SWITCH_BCM53125:
327	case SWITCH_BCM5301x:
328		robo_write16(PAGE_VTBL, REG_VTBL_INDX, vlan & 0x0fff);
329		robo_write16(PAGE_VTBL, REG_VTBL_ACCESS, 0x81);
330		entry = robo_read32(PAGE_VTBL, REG_VTBL_ENTRY);
331		portmap = entry & 0x1ff;
332		break;
333	default:
334		return -1;
335	}
336
337	return portmap;
338}
339
340int switch_init(char *ifname)
341{
342	struct vlan_ioctl_args ifv;
343	int portmap;
344
345#ifdef SOCK_CLOEXEC
346	fd = socket(AF_INET, SOCK_DGRAM | SOCK_CLOEXEC | SOCK_NONBLOCK, 0);
347	if (fd < 0) {
348		log_error("socket: %s", strerror(errno));
349		return -1;
350	}
351#else
352	int value;
353
354	fd = socket(AF_INET, SOCK_DGRAM, 0);
355	if (fd < 0) {
356		log_error("socket: %s", strerror(errno));
357		return -1;
358	}
359
360	value = fcntl(fd, F_GETFD, 0);
361	if (value < 0 || fcntl(fd, F_SETFD, value | FD_CLOEXEC) < 0) {
362		log_error("fcntl::FD_CLOEXEC: %s", strerror(errno));
363		goto error;
364	}
365
366	value = fcntl(fd, F_GETFL, 0);
367	if (value < 0 || fcntl(fd, F_SETFL, value | O_NONBLOCK) < 0) {
368		log_error("fcntl::O_NONBLOCK: %s", strerror(errno));
369		goto error;
370	}
371#endif
372
373	memset(&ifr, 0, sizeof(ifr));
374	memset(&ifv, 0, sizeof(ifv));
375	strncpy(ifv.device1, ifname, sizeof(ifv.device1));
376	ifv.cmd = GET_VLAN_REALDEV_NAME_CMD;
377	if (ioctl(fd, SIOCGIFVLAN, &ifv) >= 0) {
378		strncpy(ifr.ifr_name, ifv.u.device2, sizeof(ifr.ifr_name));
379		ifv.cmd = GET_VLAN_VID_CMD;
380		if (ioctl(fd, SIOCGIFVLAN, &ifv) >= 0)
381			vlan = ifv.u.VID;
382	} else if (sscanf(ifname, "vlan%u", &vlan) == 1) {
383		strncpy(ifr.ifr_name, "eth0", sizeof(ifr.ifr_name));
384	} else if (sscanf(ifname, "%16[^.].%u", ifr.ifr_name, &vlan) != 2) {
385		strncpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name));
386		vlan = 0;
387	}
388
389	model = get_model();
390	switch (model) {
391	case SWITCH_BCM5325:
392		cpu_portmap = (1 << 5);
393		break;
394	case SWITCH_BCM53115:
395	case SWITCH_BCM53125:
396		cpu_portmap = (1 << 5) | (1 << 8);
397		break;
398	case SWITCH_BCM5301x:
399		cpu_portmap = (1 << 5) | (1 << 7) | (1 << 8);
400		break;
401	default:
402		log_error("unsupported switch: %s", strerror(errno));
403		goto error;
404	}
405
406	portmap = read_portmap(vlan);
407	log_switch("%-5s map@vlan%u = " FMT_PORTS, "read",
408	    vlan, ARG_PORTS(portmap));
409
410	if (portmap & cpu_portmap)
411		cpu_portmap &= portmap;
412	log_switch("%-5s cpu@vlan%u = " FMT_PORTS, "init",
413	    vlan, ARG_PORTS(cpu_portmap));
414
415	if (model == SWITCH_BCM5325)
416		robo_write16(PAGE_CTRL, REG_CTRL_MCAST, 1);
417
418	return 0;
419
420error:
421	if (fd >= 0)
422		close(fd);
423	return -1;
424}
425
426void switch_done(void)
427{
428	if (model == SWITCH_BCM5325)
429		robo_write16(PAGE_CTRL, REG_CTRL_MCAST, 0);
430	if (fd >= 0)
431		close(fd);
432}
433
434int switch_get_port(unsigned char *haddr)
435{
436	uint16_t hwaddr[3], entry[6];
437	int value, index = 0;
438
439	hwaddr[2] = (haddr[0] << 8) | haddr[1];
440	hwaddr[1] = (haddr[2] << 8) | haddr[3];
441	hwaddr[0] = (haddr[4] << 8) | haddr[5];
442
443	value = read_entry(hwaddr, vlan, entry, &index);
444	if (model == SWITCH_BCM5325 && value == 8)
445		value = 5;
446
447	log_switch("%-5s [" FMT_EA "] = 0x%x", "read",
448	    ARG_EA(haddr), value);
449
450	return value;
451}
452
453int switch_add_portmap(unsigned char *maddr, int portmap)
454{
455	uint16_t hwaddr[3], entry[6];
456	int value, index = 0;
457
458	if ((maddr[0] & 0x01) == 0)
459		return -1;
460
461	hwaddr[2] = (maddr[0] << 8) | maddr[1];
462	hwaddr[1] = (maddr[2] << 8) | maddr[3];
463	hwaddr[0] = (maddr[4] << 8) | maddr[5];
464
465	value = read_entry(hwaddr, vlan, entry, &index);
466	log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "read",
467	    ARG_EA(maddr), ARG_PORTS(value));
468
469	if (value < 0)
470		value = 0;
471	portmap = (value | portmap) | cpu_portmap;
472	if (portmap != value) {
473		log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "write",
474		    ARG_EA(maddr), ARG_PORTS(portmap));
475		write_entry(hwaddr, vlan, entry, index, portmap);
476	}
477
478	return portmap;
479}
480
481int switch_del_portmap(unsigned char *maddr, int portmap)
482{
483	uint16_t hwaddr[3], entry[6];
484	int value, index = 0;
485
486	if ((maddr[0] & 0x01) == 0)
487		return -1;
488
489	hwaddr[2] = (maddr[0] << 8) | maddr[1];
490	hwaddr[1] = (maddr[2] << 8) | maddr[3];
491	hwaddr[0] = (maddr[4] << 8) | maddr[5];
492
493	value = read_entry(hwaddr, vlan, entry, &index);
494	log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "read",
495	    ARG_EA(maddr), ARG_PORTS(value));
496
497	if (value < 0)
498		value = 0;
499	portmap = (value & ~portmap) | cpu_portmap;
500	if (portmap != value) {
501		log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "write",
502		    ARG_EA(maddr), ARG_PORTS(portmap));
503		write_entry(hwaddr, vlan, entry, index, portmap);
504	}
505
506	return portmap;
507}
508
509int switch_clr_portmap(unsigned char *maddr)
510{
511	uint16_t hwaddr[3], entry[6];
512	int value, index = 0;
513
514	if ((maddr[0] & 0x01) == 0)
515		return -1;
516
517	hwaddr[2] = (maddr[0] << 8) | maddr[1];
518	hwaddr[1] = (maddr[2] << 8) | maddr[3];
519	hwaddr[0] = (maddr[4] << 8) | maddr[5];
520
521	value = read_entry(hwaddr, vlan, entry, &index);
522	log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "read",
523	    ARG_EA(maddr), ARG_PORTS(value));
524
525	if (value >= 0) {
526		log_switch("%-5s [" FMT_EA "] = " FMT_PORTS, "clear",
527		    ARG_EA(maddr), ARG_PORTS(value));
528		write_entry(hwaddr, vlan, entry, index, -1);
529	}
530
531	return value;
532}
533
534#ifdef TEST
535int main(int argc, char *argv[])
536{
537	uint16_t value[4] = { 0 };
538	int i, write, page, reg, count;
539
540	if (!argv[1] || !argv[2] || !argv[3])
541		return 0;
542
543	write = (strncmp(argv[1], "robowr", 6) == 0 ? 1 :
544		 strncmp(argv[1], "robord", 6) == 0 ? 0 : -1);
545	if (write < 0 || (write && !argv[4]))
546		return 0;
547	count = atoi(argv[1] + 6);
548	count = count ? count / 16 : 0;
549	count = count ? : 1;
550
551	if (switch_init("eth0") < 0) {
552		perror("switch");
553		return -1;
554	}
555
556	page = strtoul(argv[2], NULL, 0);
557	reg = strtoul(argv[3], NULL, 0);
558
559	if (write) {
560		unsigned long long val = strtoull(argv[4], NULL, 0);
561		memcpy(value, &val, count * 2);
562		robo_write(page, reg, value, count);
563	}
564
565	robo_read(page, reg, value, count);
566	printf("page %02x reg %02x = ", page, reg);
567	for (i = 0; i < count * 2; i++)
568		printf("%02x", ((unsigned char *) value)[i]);
569	printf("\n");
570
571	switch_done();
572
573	return 0;
574}
575#endif
576