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