wrs_vlans.c 29.9 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24
/*
 * Copyright (c) 2014, CERN
 *
 * Author: Grzegorz Daniluk <grzegorz.daniluk@cern.ch>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version
 * 2 of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <unistd.h>
#include <getopt.h>
#include <stdlib.h>
25
#include <stddef.h>
26
#include <string.h>
27
#include <errno.h>
28 29 30
#include <sys/ioctl.h>
#include <minipc.h>
#include <rtud_exports.h>
31 32
#include "regs/endpoint-regs.h"

33
#include <libwr/switch_hw.h>
34
#include "fpga_io.h"
35
#include "wrs_vlans.h"
36

37 38
#include <libwr/shmem.h>
#include <libwr/rtu_shmem.h>
39
#include <libwr/config.h>
40
#include <libwr/wrs-msg.h>
41

42 43
static struct minipc_ch *rtud_ch;
static struct rtu_vlans_t *rtu_vlans = NULL;
44 45

/* runtime options */
46
static struct option ropts[] = {
47 48 49
	{"help", 0, NULL, OPT_HELP},
	{"clear", 0, NULL, OPT_CLEAR},
	{"list", 0, NULL, OPT_LIST},
50 51 52 53
	{"port", 1, NULL, OPT_P_PORT},
	{"pmode", 1, NULL, OPT_P_QMODE},
	{"pvid", 1, NULL, OPT_P_VID},
	{"pprio", 1, NULL, OPT_P_PRIO},
54
	{"puntag", 1, NULL, OPT_P_UNTAG},
55
	{"plist", 0, NULL, OPT_P_LIST},
56 57 58 59 60 61
	{"rvid", 1, NULL, OPT_RTU_VID},
	{"rfid", 1, NULL, OPT_RTU_FID},
	{"rmask", 1, NULL, OPT_RTU_PMASK},
	{"rdrop", 1, NULL, OPT_RTU_DROP},
	{"rprio", 1, NULL, OPT_RTU_PRIO},
	{"del", 0, NULL, OPT_RTU_DEL},
62
	{"file", 1, NULL, OPT_FILE_READ},
63
	{"hpmask", 1, NULL, OPT_RTU_HP_MASK},
64 65
	{0,}};
/*******************/
66 67 68
static struct vlan_sets dot_config_vlan_sets[] = {
	{"VLANS_ENABLE_SET1", 0, 22},
	{"VLANS_ENABLE_SET2", 23, 100},
69
	{"VLANS_ENABLE_SET3", 101, 4094},
70 71
	{NULL, 0, 0}
};
72
static struct s_port_vlans vlans[NPORTS];
73

74 75
static unsigned long portmask;

76
static void set_p_pmode(int ep, int arg_mode);
77 78
static void set_p_vid(int ep, char *arg_vid);
static void set_p_prio(int ep, char *arg_prio);
79
static void set_p_untag(int ep, int arg_untag);
80
static void set_hp_mask(char *mask_str);
81
static int check_rtu(char *name, char *arg_val, int min, int max);
82
static int check_rtu_prio(char *arg_val);
83
static int print_help(char *prgname);
84 85
static void print_config_rtu(struct s_port_vlans *vlans);
static void print_config_vlan(void);
86
static int apply_settings(struct s_port_vlans *vlans);
87
static void default_vlan_config(void);
88
static int clear_all(void);
89 90 91 92
static int set_rtu_vlan(int vid, int fid, int pmask, int drop, int prio,
			int del, int flags);
static void free_rtu_vlans(struct rtu_vlans_t *ptr);
static void list_rtu_vlans(void);
93
static void list_p_vlans(void);
94
static void print_hp_mask(void);
95
static int rtu_find_vlan(struct rtu_vlan_table_entry *rtu_vlan_entry, int vid,
96 97
					 int fid);
static int config_rtud(void);
98 99
static int read_dot_config(char *dot_config_file);
static void read_dot_config_vlans(int vlan_min, int vlan_max);
100

101 102 103
struct rtu_vlan_table_entry *vlan_tab_shm;
struct wrs_shm_head *rtu_port_shmem;

104
static inline int nextport(int i, unsigned long pmask) /* helper for for_each_port() below */
105 106
{
	while (++i < NPORTS)
107
		if (pmask & (1 << i))
108 109 110 111
			return i;
	return -1;
}

112 113 114
#define iterate_ports(i, pmask) \
	for (i = -1; (i = nextport(i, pmask)) >= 0;)

115
#define for_each_port(i) \
116
	iterate_ports(i, portmask)
117

118
static int parse_mask(char *arg, unsigned long *pmask)
119 120 121 122 123
{
	int p1, p2;
	char c, *newarg, *s;

	newarg = strdup(arg);
124
	while ( (s = strtok(newarg, ",;")) ) {
125 126 127 128 129 130 131 132 133
		newarg = NULL; /* for next iteration */
		switch (sscanf(s, "%i-%i%c", &p1, &p2, &c)) {
		case 1:
			p2 = p1;
		case 2:
			break;
		default:
			return -1;
		}
134 135 136 137
		/* parameter --port should be from the range 1..18,
		 * but internally we use 0..17 */
		p1--;
		p2--;
138 139
		if ((p1 > p2) || (p1 < 0) || (p2 >= NPORTS))
			return -1;
140 141 142
		for (; p1 <= p2; p1++) {
			*pmask |= (1 << p1);
		}
143
	}
144
	if (wrs_msg_level < LOG_DEBUG)
145 146
		return 0;

147
	printf("working on ports:\n");
148
	iterate_ports(p1, *pmask)
149
		printf(" %i", p1 + 1);
150
	printf("\n");
151
	return 0;
152 153
}

154 155
int main(int argc, char *argv[])
{
156
	int c, i, arg;
157
	unsigned long conf_pmask = 0; /* current '--port' port mask */
158
	struct rtu_shmem_header *rtu_hdr;
159 160
	int n_wait = 0;
	int ret;
161
	char *prgname;
162

163
	wrs_msg_level = LOG_WARNING;
164
	wrs_msg_init(argc, argv, LOG_USER);
165 166
	prgname = argv[0];

167 168 169 170 171 172
	if (NPORTS > 8 * sizeof(portmask)) {
		/* build error: too big maxports */
		static __attribute__((used)) int
			array[8 * sizeof(portmask) - NPORTS];
	}

173 174 175 176
	if (argc == 1) {
		print_help(prgname);
		exit(0);
	}
177 178 179 180 181 182

	n_wait = 0;
	/* connect to the RTUd mini-rpc */
	while((rtud_ch = minipc_client_create("rtud", 0)) == 0) {
		n_wait++;
		if (n_wait > 10) {
183
			pr_error("Can't connect to RTUd mini-rpc server\n");
184 185 186
			exit(1);
		}
		sleep(1);
187 188
	}

189
	n_wait = 0;
190
	/* open rtu shm */
191 192 193
	while ((ret = wrs_shm_get_and_check(wrs_shm_rtu, &rtu_port_shmem)) != 0) {
		n_wait++;
		if (n_wait > 10) {
194
			if (ret == WRS_SHM_OPEN_FAILED) {
195
				pr_error("Unable to open RTUd's shmem!\n");
196
			}
197
			if (ret == WRS_SHM_WRONG_VERSION) {
198
				pr_error("Unable to read RTUd's version!\n");
199
			}
200 201 202 203
			if (ret == WRS_SHM_INCONSISTENT_DATA) {
				pr_error("Unable to read consistent data from "
					 "RTUd's shmem!\n");
			}
204 205 206
			exit(1);
		}
		sleep(1);
207
	}
208

209 210
	/* check rtu shm version */
	if (rtu_port_shmem->version != RTU_SHMEM_VERSION) {
211 212
		pr_error("unknown version %i (known is %i)\n",
			 rtu_port_shmem->version, RTU_SHMEM_VERSION);
213 214 215
		exit(1);
	}

216 217


218 219 220 221 222
	/* get vlans array */
	rtu_hdr = (void *)rtu_port_shmem + rtu_port_shmem->data_off;
	vlan_tab_shm = wrs_shm_follow(rtu_port_shmem, rtu_hdr->vlans);

	if (!vlan_tab_shm) {
223
		pr_error("cannot follow pointer to vlans in RTU's shmem\n");
224 225 226
		exit(1);
	}

227
	if (shw_fpga_mmap_init() < 0) {
228
		pr_error("Can't access device memory\n");
229
		exit(1);
230 231 232
	}

	/*parse parameters*/
233
	while ((c = getopt_long(argc, argv, "hf:vq", ropts, NULL)) != -1) {
234
		switch(c) {
235 236 237 238
		case OPT_P_PORT:
			/* port number */
			conf_pmask = 0;
			if (parse_mask(optarg, &conf_pmask) < 0) {
239
				pr_error("wrong port mask \"%s\"\n", optarg);
240 241
				exit(1);
			}
242 243 244
			/* add this set of ports to the global list of ports
			 * that are configured */
			portmask |= conf_pmask;
245
			break;
246

247
		case OPT_P_QMODE:
248
			/* pmode for port */
249 250
			arg = check_rtu("pmode", optarg, 0, 3);
			if (arg < 0)
251 252
				exit(1);
			iterate_ports(i, conf_pmask) {
253
				set_p_pmode(i, arg);
254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270
			}
			break;

		case OPT_P_PRIO:
			/* priority value for port, forces fix_prio=1 */
			iterate_ports(i, conf_pmask) {
				set_p_prio(i, optarg);
			}
			break;

		case OPT_P_VID:
			/* VID for port */
			iterate_ports(i, conf_pmask) {
				set_p_vid(i, optarg);
			}
			break;

271
		case OPT_P_UNTAG:
272 273
			/* untag mask -- currently 0 or 1. Overrides default
			 * set in QMODE above */
274 275
			arg = check_rtu("umask bit", optarg, 0, 1);
			if (arg < 0)
276 277
				exit(1);
			iterate_ports(i, conf_pmask) {
278
				set_p_untag(i, arg);
279 280 281 282 283
			}
			break;

		case OPT_P_LIST:
			/* list endpoint stuff */
284
			print_hp_mask();
285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304
			list_p_vlans();
			break;

	  /****************************************************/
		/* RTU settings */
		case OPT_RTU_VID:
			ret = check_rtu("rtu vid", optarg, RTU_VID_MIN,
					RTU_VID_MAX);
			if (ret < 0)
				exit(1);
			set_rtu_vlan(ret, -1, -1, 0, -1, 0, VALID_VID);
			break;
		case OPT_RTU_FID:
			ret = check_rtu("rtu fid", optarg, RTU_FID_MIN,
					RTU_FID_MAX);
			if (ret < 0)
				exit(1);
			set_rtu_vlan(-1, ret, -1, 0, -1, 0, VALID_FID);
			break;
		case OPT_RTU_PMASK:
305 306 307 308 309
			ret = check_rtu("pmask", optarg, RTU_PMASK_MIN,
					RTU_PMASK_MAX);
			if (ret < 0)
				exit(1);
			set_rtu_vlan(-1, -1, ret, 0, -1, 0, VALID_PMASK);
310 311 312 313 314 315 316 317
			break;
		case OPT_RTU_DROP:
			ret = check_rtu("rtu drop", optarg, 0, 1);
			if (ret < 0)
				exit(1);
			set_rtu_vlan(-1, -1, -1, ret, -1, 0, VALID_DROP);
			break;
		case OPT_RTU_PRIO:
318 319
			ret = check_rtu_prio(optarg);
			if (ret < RTU_PRIO_DISABLE)
320 321 322 323 324 325
				exit(1);
			set_rtu_vlan(-1, -1, -1, 0, ret, 0, VALID_PRIO);
			break;
		case OPT_RTU_DEL:
			set_rtu_vlan(-1, -1, -1, 0, -1, 1, 0);
			break;
326 327 328
		case OPT_RTU_HP_MASK:
			set_hp_mask(optarg);
			break;
329 330 331 332 333
	  /****************************************************/
		/* Other settings */
		case OPT_CLEAR:
			if (clear_all())
				exit(1); /* message already printed */
334
			default_vlan_config();
335 336 337 338
			break;
		case 0:
			break;
		case OPT_LIST:
339
			if (wrs_msg_level >= LOG_DEBUG)
340 341 342 343
				minipc_set_logfile(rtud_ch, stderr);
			list_rtu_vlans();
			break;
		case OPT_FILE_READ:
344 345 346 347
			if (wrs_msg_level >= LOG_DEBUG)
				pr_info("Using file %s as dot-config\n",
					optarg);

348 349 350 351 352 353
			/* read dot-config */
			ret = read_dot_config(optarg);
			if (ret < 0)
				exit(-ret);

			break;
354 355
		case 'q': break; /* done in wrs_msg_init() */
		case 'v': break; /* done in wrs_msg_init() */
356 357 358 359 360
		case '?':
		case OPT_HELP:
		default:
			print_help(prgname);
			exit(0);
361 362 363
		}
	}

364
	if (wrs_msg_level >= LOG_DEBUG)
365 366
		minipc_set_logfile(rtud_ch, stderr);

367
	if (wrs_msg_level >= LOG_INFO) {
368 369
		print_config_rtu(vlans);
		print_config_vlan();
370
	}
371
	/* apply vlans and rtu_vlans */
372 373 374 375 376 377
	apply_settings(vlans);
	free_rtu_vlans(rtu_vlans);

	return 0;
}

378
static void set_p_pmode(int ep, int arg_mode)
379
{
380
	vlans[ep].pmode = arg_mode;
381 382 383 384 385 386 387 388 389
	vlans[ep].valid_mask |= VALID_QMODE;
	/* untag is all-or-nothing: default untag if access mode */
	if ((vlans[ep].valid_mask & VALID_UNTAG) == 0)
		vlans[ep].untag_mask = (arg_mode == 0);
}

static void set_p_vid(int ep, char *arg_vid)
{
	int vid;
390 391 392 393 394 395 396 397 398
	char *endptr;

	vid = strtol(arg_vid, &endptr, 0);
	if (*arg_vid == '\0' /* empty string */
	    || *endptr != '\0' /* more chars after number in the string */
	    || vid < PORT_VID_MIN || vid > PORT_VID_MAX
	   ) {
		pr_error("invalid vid \"%s\" for port %d\n",
			 arg_vid, ep + 1);
399 400
		exit(1);
	}
401
	vlans[ep].vid = vid;
402 403 404 405 406 407
	vlans[ep].valid_mask |= VALID_VID;
}

static void set_p_prio(int ep, char *arg_prio)
{
	int prio;
408
	char *endptr;
409

410 411 412 413 414 415 416 417 418
	prio = strtol(arg_prio, &endptr, 0);

	if (*arg_prio == '\0' /* empty string */
	    || *endptr != '\0' /* more chars after number in the string */
	    || ((prio < PORT_PRIO_MIN || prio > PORT_PRIO_MAX)
	        && (prio != PORT_PRIO_DISABLE))
	   ) {
		pr_error("invalid priority \"%s\" for port %d\n",
			 arg_prio, ep + 1);
419 420
		exit(1);
	}
421
	/* prio was touched, so set VALID_PRIO */
422
	vlans[ep].valid_mask |= VALID_PRIO;
423 424 425 426 427 428 429
	if (prio == PORT_PRIO_DISABLE) {
		vlans[ep].prio_val = PORT_PRIO_MIN;
		vlans[ep].fix_prio = 0;
	} else {
		vlans[ep].prio_val = prio;
		vlans[ep].fix_prio = 1;
	}
430 431
}

432
static void set_p_untag(int ep, int arg_untag)
433
{
434
	vlans[ep].untag_mask = arg_untag;
435 436 437
	vlans[ep].valid_mask |= VALID_UNTAG;
}

438 439 440 441 442
static void set_hp_mask(char *mask_str)
{
	int hp_mask;
	int val;
	int ret;
443
	char *endptr;
444

445
	hp_mask = strtol(mask_str, &endptr, 0);
446

447 448 449
	if (*mask_str == '\0' /* empty string */
	    || *endptr != '\0' /* more chars after number in the string */
	    || hp_mask >= (1 << 8)) {
450 451 452 453 454 455 456 457 458 459 460 461 462 463 464
		pr_error("Wrong HP mask %s\n", mask_str);
		exit(1);
	}

	ret = minipc_call(rtud_ch, MINIPC_TIMEOUT,
			  &rtud_export_hp_mask, &val,
			  RTU_SET_HP_MASK, hp_mask);
	ret = (ret < 0) ? ret : val;
	if (ret < 0) {
		pr_error("failed to set HP mask 0x%x (%s), ret %d\n",
			  hp_mask, mask_str, ret);
		exit(1);
	}

}
465

466 467 468
static int check_rtu(char *name, char *arg_val, int min, int max)
{
	int val;
469 470 471
	char *endptr;

	val = strtol(arg_val, &endptr, 0);
472

473 474 475 476
	if (*arg_val == '\0' /* empty string */
	    || *endptr != '\0' /* more chars after number in the string */
	    || val < min || val > max) {
		pr_error("invalid %s \"%s\"\n", name, arg_val);
477 478 479 480 481
		return -1;
	}
	return val;
}

482 483 484
static int check_rtu_prio(char *arg_val)
{
	int val;
485
	char *endptr;
486

487 488 489 490 491 492 493 494
	val = strtol(arg_val, &endptr, 0);

	if (*arg_val == '\0' /* empty string */
	    || *endptr != '\0' /* more chars after number in the string */
	    || ((val < RTU_PRIO_MIN || val > RTU_PRIO_MAX)
	        && (val != RTU_PRIO_DISABLE))
	   ) {
		pr_error("invalid rtu prio \"%s\"\n", arg_val);
495 496 497 498 499
		return -2;
	}
	return val;
}

500
static int print_help(char *prgname)
501
{
502
	fprintf(stderr, "Use: %s [-v] [-q] [--hpmask <mask>]"
503
			"[--port <port number 1..18> <port options> "
504 505
			"--port <port number> <port options> ...] "
			"[--rvid <vid> --rfid <fid> --rmask <mask> --rdrop "
506 507
			"--rprio <prio> --rvid <vid>...]  "
			"[-f|--file <dot-config>]\n", prgname);
508 509

	fprintf(stderr,
510
			"Port options:\n"
511 512 513
			"\t --port <1..18>     apply following options to particular set of ports\n"
			"\t                    for example 1-3,5-6 will apply settings to ports 1,2,3,5,6\n"
			"\t --pmode <0..3>     sets pmode for a port, possible values:\n"
514
			"\t \t 0: ACCESS           - tags untagged frames, drops tagged frames not belonging to configured VLAN\n"
515
			"\t \t 1: TRUNK            - passes only tagged frames, drops all untagged frames\n"
516
			"\t \t 2: VLANs disabled   - passes all frames as is\n"
517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538
			"\t \t 3: Unqualified port - passes all frames regardless of VLAN config\n");
	fprintf(stderr, "\t --pprio <%d|%d..%d>  sets priority for retagging; -1 disables retagging;\n",
			PORT_PRIO_DISABLE, PORT_PRIO_MIN, PORT_PRIO_MAX);
	fprintf(stderr, "\t --pvid <%d..%d>   sets VLAN Id for port\n",
			PORT_VID_MIN, PORT_VID_MAX);
	fprintf(stderr, "\t --puntag <0|1>     if 1 untag all vlan tags on a port\n"
			"\t --plist            lists current ports configuration\n"
			"RTU options:\n");
	fprintf(stderr, "\t --rvid <%d..%d>       VID value for which the other parameters are set in rtud\n",
			RTU_VID_MIN, RTU_VID_MAX);
	fprintf(stderr, "\t --del                  delete selected VLAN from rtud\n");
	fprintf(stderr, "\t --rfid <%d..%d>       assign fid to configured VLAN\n",
			RTU_FID_MIN, RTU_FID_MAX);
	fprintf(stderr, "\t --rmask <0x%x..0x%x> ports belonging to configured VLAN\n",
			RTU_PMASK_MIN, RTU_PMASK_MAX);
	fprintf(stderr, "\t --rdrop <0|1>          don't drop or drop frames on VLAN (note that frame can belong\n"
			"\t                        to a VID as a consequence of per-port Endpoint configuration)\n");
	fprintf(stderr, "\t --rprio <%d|%d..%d>      force priority for VLAN; -1 cancels priority override\n",
			RTU_PRIO_DISABLE, RTU_PRIO_MIN, RTU_PRIO_MAX);
	fprintf(stderr, "Other options:\n"
			"\t --hpmask <mask>   Set the mask which priorities are considered High Priority\n"
			"\t                   (this only concerns the traffic which is fast-forwarded)\n"
539
			"\t --clear           clears ports' and VLAN configuration\n"
540
			"\t --list            prints the content of RTUd VLAN table\n"
541 542
			"\t -f|--file <file>  clears configuration, then applies configuration from the provided\n"
			"\t                   dot-config file\n"
543 544 545
			"\t -v                be more verbose (can be used 1 or 2 times)\n"
			"\t -q                be less verbose\n"
			"\t --help            prints this help message\n");
546 547 548
	return 0;
}

549
static void print_config_rtu(struct s_port_vlans *vlans)
550 551 552
{
	int i;

553
	for_each_port(i) {
554
		printf("port: %2d, pmode: %d, pmode_valid: %d, fix_prio: %d, "
555 556
		       "prio_val: %d, prio_valid: %d, vid: %2d, vid_valid: %d,"
		       " untag_mask: 0x%X, untag_valid: %d\n",
557
		       i + 1,
558
		       vlans[i].pmode,
559 560 561 562 563 564 565 566 567
		       ((vlans[i].valid_mask & VALID_QMODE) != 0),
		       vlans[i].fix_prio,
		       vlans[i].prio_val,
		       ((vlans[i].valid_mask & VALID_PRIO) != 0),
		       vlans[i].vid,
		       ((vlans[i].valid_mask & VALID_VID) != 0),
		       vlans[i].untag_mask,
		       ((vlans[i].valid_mask & VALID_UNTAG) != 0)
		       );
568 569 570
	}
}

571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608
static void print_config_vlan(void)
{
	struct rtu_vlans_t *cur;
	cur = rtu_vlans;
	while (cur) {
		printf("vid: ");
		if (cur->flags & VALID_VID)
			printf("%4d ", cur->vid);
		else
			printf("     ");

		printf("fid: ");
		if (cur->flags & VALID_FID)
			printf("%4d ", cur->fid);
		else
			printf("     ");

		printf("port_mask: ");
		if (cur->flags & VALID_PMASK)
			printf("0x%05x ", cur->pmask);
		else
			printf("        ");

		printf("drop: ");
		if (cur->flags & VALID_DROP)
			printf("%1d ", cur->drop);
		else
			printf("  ");

		printf("prio: ");
		if (cur->flags & VALID_PRIO)
			printf("%1d", cur->prio);

		printf("\n");
		cur = cur->next;
	}
}

609
static uint32_t ep_read(int ep, int offset)
610
{
611 612
	return _fpga_readl(0x30000 + ep * 0x400 + offset);
}
613

614 615 616 617
static void ep_write(int ep, int offset, uint32_t value)
{
	_fpga_writel(0x30000 + ep * 0x400 + offset, value);
}
618

619
static int apply_settings(struct s_port_vlans *vlans)
620 621 622 623 624 625 626 627 628
{
	int ep;
	uint32_t v, r;

	for_each_port(ep) {
		/* VCR0 */
		r = offsetof(struct EP_WB, VCR0);
		v = ep_read(ep, r);
		if (vlans[ep].valid_mask & VALID_QMODE)
629
			v = (v & ~EP_VCR0_QMODE_MASK) | EP_VCR0_QMODE_W(vlans[ep].pmode);
630
		if (vlans[ep].valid_mask & VALID_PRIO) {
631 632 633 634
			if (vlans[ep].fix_prio)
				v |= EP_VCR0_FIX_PRIO;
			else
				v &= ~EP_VCR0_FIX_PRIO;
635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652
			v = (v & ~EP_VCR0_PRIO_VAL_MASK) | EP_VCR0_PRIO_VAL_W(vlans[ep].prio_val);
		}
		if (vlans[ep].valid_mask & VALID_VID)
			v = (v & ~EP_VCR0_PVID_MASK) | EP_VCR0_PVID_W(vlans[ep].vid);
		ep_write(ep, r, v);
		/* VCR1: loop over the whole bitmask */
		if (vlans[ep].untag_mask) {
			int i;

			r = offsetof(struct EP_WB, VCR1);
			for (i = 0;i < 4096/16; i++) {
				if (vlans[ep].untag_mask)
					ep_write(ep, r, (0xffff << 10) | i);
				else
					ep_write(ep, r, (0x0000 << 10) | i);
			}
		}
	}
653 654 655 656 657
	config_rtud();

	return 0;
}

658
static int config_rtud(void)
659 660
{
	struct rtu_vlans_t *cur;
661
	struct rtu_vlan_table_entry rtu_vlan_entry;
662
	int val;
663 664 665

	cur = rtu_vlans;
	while(cur) {
666 667 668
		if (rtu_find_vlan(&rtu_vlan_entry, cur->vid,
				  (cur->flags & VALID_FID) ? cur->fid : -1)) {

669
		/*preserve previous settings if not overwritten*/
670 671 672 673 674 675 676 677 678 679 680 681
			if (!(cur->flags & VALID_FID))
				cur->fid = rtu_vlan_entry.fid;
			if (!(cur->flags & VALID_PMASK))
				cur->pmask = rtu_vlan_entry.port_mask;
			if (!(cur->flags & VALID_DROP))
				cur->drop = rtu_vlan_entry.drop;
			if (!(cur->flags & VALID_PRIO)) {
				cur->prio = rtu_vlan_entry.prio;
				cur->has_prio = rtu_vlan_entry.has_prio;
				cur->prio_override =
						rtu_vlan_entry.prio_override;
			}
682
		}
683

684
		minipc_call(rtud_ch, MINIPC_TIMEOUT, &rtud_export_vlan_entry, &val,
685 686 687 688 689 690 691 692
				cur->vid, cur->fid, cur->pmask, cur->drop, cur->prio, cur->has_prio,
				cur->prio_override);
		cur = cur->next;
	}

	return 0;
}

693
static void list_rtu_vlans(void)
694
{
695 696 697
	unsigned ii;
	unsigned retries = 0;
	static struct rtu_vlan_table_entry vlan_tab_local[NUM_VLANS];
698

699
	/* read data, with the sequential lock to have all data consistent */
700
	while (1) {
701 702 703 704 705
		ii = wrs_shm_seqbegin(rtu_port_shmem);
		memcpy(&vlan_tab_local, vlan_tab_shm,
		       NUM_VLANS * sizeof(*vlan_tab_shm));
		retries++;
		if (retries > 100) {
706 707
			pr_error("couldn't read consistent data from RTU's "
				 "shmem. Use inconsistent\n");
708 709
			break; /* use inconsistent data */
			}
710 711
		if (!wrs_shm_seqretry(rtu_port_shmem, ii))
			break; /* consistent read */
712
		usleep(1000);
713
	}
714 715 716 717

	printf("# VID    FID       MASK       DROP    PRIO    PRIO_OVERRIDE\n");
	printf("#----------------------------------------------------------\n");

718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739
	for (ii = 0; ii < NUM_VLANS; ii++) {
		/* ignore entires that are not active */
		if ((vlan_tab_local[ii].drop != 0)
		    && (vlan_tab_local[ii].port_mask == 0x0))
			continue;
		printf("%4d   %4d      0x%08x    ", ii, vlan_tab_local[ii].fid,
		       vlan_tab_local[ii].port_mask);

		if (vlan_tab_local[ii].drop == 0)
			printf("NO ");
		else
			printf("YES");

		if (vlan_tab_local[ii].has_prio == 0)
			printf("     --    ");
		else
			printf("     %1d    ", vlan_tab_local[ii].prio);

		if (vlan_tab_local[ii].prio_override == 0)
			printf("     NO ");
		else
			printf("     YES ");
740 741 742

		printf("\n");
	}
743
	printf("\n");
744

745 746
}

747
static void list_p_vlans(void)
748 749 750 751 752
{
	uint32_t v, r;
	int ep;
	static char *names[] = {"ACCESS", "TRUNK", "disabled", "unqualified"};

753 754
	printf("#        QMODE    FIX_PRIO  PRIO    PVID     MAC\n");
	printf("#-----------------------------------------------\n");
755 756 757
	for (ep = 0; ep < NPORTS; ep++) {
		r = offsetof(struct EP_WB, VCR0);
		v = ep_read(ep, r);
758 759
		printf("wri%-2i    %i %6.6s     %i      %i     %4i    %04x%08x\n",
		       ep + 1, v & 3, names[v & 3],
760 761 762
		       v & EP_VCR0_FIX_PRIO ? 1 : 0,
		       EP_VCR0_PRIO_VAL_R(v),
		       EP_VCR0_PVID_R(v),
763 764 765 766 767 768
		       (int)ep_read(ep, offsetof(struct EP_WB, MACH)),
		       (int)ep_read(ep, offsetof(struct EP_WB, MACL)));
	}
	return;
}

769 770 771 772 773 774 775 776 777 778 779 780 781 782 783
static void print_hp_mask(void)
{
	int hp_mask;
	int ret;
	ret = minipc_call(rtud_ch, MINIPC_TIMEOUT, &rtud_export_hp_mask,
			  &hp_mask, RTU_GET_HP_MASK, NULL);
	if (ret < 0) {
		pr_error("failed to read HP mask, ret %d\n", ret);
		exit(1);
	}
	printf("#-----------------------------------------------\n");
	printf("# HP mask: 0x%02x\n", hp_mask);
	printf("#-----------------------------------------------\n");
}

784 785 786 787 788 789 790 791
static void default_vlan_config(void)
{
	int val;
	/* Create VLAN 0 reserved for untagged packets */
	minipc_call(rtud_ch, MINIPC_TIMEOUT, &rtud_export_vlan_entry,
		    &val, 0, 0, 0xffffffff, 0, 0, 0, 0);
}

792
static int clear_all(void)
793
{
794
	uint32_t r;
795 796
	int val, i;
	int ep;
797

798
	for (i = 0; i < NUM_VLANS; i++) {
799 800 801 802 803 804 805
		if ((vlan_tab_shm[i].drop != 0)
		    && (vlan_tab_shm[i].port_mask == 0x0))
			continue;
		minipc_call(rtud_ch, MINIPC_TIMEOUT,
				    &rtud_export_vlan_entry, &val, i,
				    vlan_tab_shm[i].fid, 0, 1, 0, 0, 0);
		}
806

807 808 809 810
	/* cancel tagging/untagging in all endpoints*/
	for (ep = 0; ep < NPORTS; ep++) {
		r = offsetof(struct EP_WB, VCR0);
		ep_write(ep, r, 0x3 /* QMODE */);
811

812 813 814 815 816
		r = offsetof(struct EP_WB, VCR1);
		for (i = 0;i < 4096/16; i++) {
			ep_write(ep, r, (0x0000 << 10) | i); /* no untag */
		}
	}
817 818 819
	return 0;
}

820 821
static int set_rtu_vlan(int vid, int fid, int pmask, int drop, int prio,
			int del, int flags)
822
{
823
	struct rtu_vlans_t *cur = rtu_vlans;;
824

825
	if (!rtu_vlans && vid < 0) {
826
		pr_error("missing \"--rvid <vid>\" before rtu cmd\n");
827
		return -1;
828
	}
829

830
	if (vid >= 0) {
831 832
		cur = calloc(1, sizeof(*cur));
		if (!cur) {
833
			pr_error("%s\n", strerror(errno));
834 835 836 837 838
			return -1;
		}
		cur->vid = vid;
		cur->fid = vid;
		cur->flags |= VALID_VID;
839 840 841 842

		/* link to the list, next time head is "cur" */
		cur->next = rtu_vlans;
		rtu_vlans = cur;
843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874
	}
	if(flags & VALID_FID)
		cur->fid = fid;
	if(flags & VALID_PMASK)
		cur->pmask = pmask;
	if(flags & VALID_DROP)
		cur->drop = drop;
	if(flags & VALID_PRIO && prio >= 0) {
		cur->prio = prio;
		cur->has_prio = 1;
		cur->prio_override = 1;
	}
	else if(flags & VALID_PRIO) {
		cur->prio = prio;
		cur->has_prio = 0;
		cur->prio_override = 0;
	}
	if(del) {
		/*delete this vlan*/
		cur->pmask = 0;
		cur->drop = 1;
		cur->prio = 0;
		cur->has_prio = 0;
		cur->prio_override = 0;
		flags |= (VALID_PMASK | VALID_DROP | VALID_PRIO);
	}

	cur->flags |= flags;

	return 0;
}

875
static void free_rtu_vlans(struct rtu_vlans_t *ptr)
876 877 878 879 880 881 882 883 884 885
{
	struct rtu_vlans_t *next = NULL;

	while(ptr) {
		next = ptr->next;
		free(ptr);
		ptr = next;
	}
}

886
static int rtu_find_vlan(struct rtu_vlan_table_entry *rtu_vlan_entry, int vid,
887
					 int fid)
888
{
889 890
	unsigned ii;
	unsigned retries = 0;
891 892 893

	/* copy data no mater if it will be used later, with the sequential
	 * lock to have all data consistent */
894
	while (1) {
895 896 897 898 899
		ii = wrs_shm_seqbegin(rtu_port_shmem);
		memcpy(rtu_vlan_entry, &vlan_tab_shm[vid],
			sizeof(*rtu_vlan_entry));
		retries++;
		if (retries > 100) {
900 901
			pr_error("couldn't read consistent data from RTU's "
				 "shmem. Use inconsistent\n");
902 903
			break; /* use inconsistent data */
			}
904 905
		if (!wrs_shm_seqretry(rtu_port_shmem, ii))
			break; /* consistent read */
906
		usleep(1000);
907
	}
908

909
	/* Ignore entires that are not active */
910 911
	if ((rtu_vlan_entry->drop != 0)
	    && (rtu_vlan_entry->port_mask == 0x0))
912
		return 0;
913

914
	if ((fid == rtu_vlan_entry->fid) || (fid == -1))
915 916
		return 1;
	return 0;
917
}
918 919 920 921 922 923 924 925 926

static int read_dot_config(char *dot_config_file)
{
	int line;
	int port;
	char *ret;
	char buff[60];
	char *val_ch;
	int i;
927
	int mode;
928
	int vlan0_port_mask = 0;
929 930

	if (access(dot_config_file, R_OK)) {
931 932
		pr_error("Unable to read dot-config file %s\n",
			 dot_config_file);
933 934 935 936 937 938
		return -1;
	}

	line = libwr_cfg_read_file(dot_config_file);

	if (line == -1) {
939 940
		pr_error("Unable to read dot-config file %s or error in "
			"line 1\n", dot_config_file);
941 942
		return -1;
	} else if (line) {
943
		pr_error("Error in dot-config file %s, error in line %d\n",
944 945 946 947
			 dot_config_file, -line);
		return -1;
	}

948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963
	ret = libwr_cfg_get("RTU_HP_MASK_ENABLE");
	if (ret && !strcmp(ret, "y")) {
		if (wrs_msg_level >= LOG_DEBUG)
			printf("Setting HP mask\n");

		ret = libwr_cfg_get("RTU_HP_MASK_VAL");
		if (ret) {
			if (wrs_msg_level >= LOG_DEBUG)
				printf("Set RTU_HP_MASK_VAL %s\n", ret);
			set_hp_mask(ret);
		} else {
			pr_error("Unable to get RTU_HP_MASK_VAL\n");
			exit(1);
		}
	}

964 965
	/* clear ports and VLANs configuration */
	clear_all();
966 967

	/* read VLANs related configuration */
968 969
	ret = libwr_cfg_get("VLANS_ENABLE");
	if (!ret || strcmp(ret, "y")) {
970
		if (wrs_msg_level >= LOG_DEBUG)
971
			printf("VLANS not enabled\n");
972 973 974
		/* All VLANs were removed, since VLANs are disabled set the
		 * default configuration */
		default_vlan_config();
975 976 977 978 979
		return -2;
	}

	for (port = 1; port <= NPORTS; port++) {
		portmask = portmask | (1 << (port - 1));
980
		mode = QMODE_INVALID;
981 982 983 984

		sprintf(buff, "VLANS_PORT%02d_MODE_ACCESS", port);
		ret = libwr_cfg_get(buff);
		if (ret && !strcmp(ret, "y")) {
985
			if (wrs_msg_level >= LOG_DEBUG)
986
				printf("Found %s\n", buff);
987
			set_p_pmode(port - 1, QMODE_ACCESS);
988
			mode = QMODE_ACCESS;
989 990 991 992
		}
		sprintf(buff, "VLANS_PORT%02d_MODE_TRUNK", port);
		ret = libwr_cfg_get(buff);
		if (ret && !strcmp(ret, "y")) {
993
			if (wrs_msg_level >= LOG_DEBUG)
994
				printf("Found %s\n", buff);
995
			set_p_pmode(port - 1, QMODE_TRUNK);
996
			mode = QMODE_TRUNK;
997 998 999 1000
		}
		sprintf(buff, "VLANS_PORT%02d_MODE_DISABLED", port);
		ret = libwr_cfg_get(buff);
		if (ret && !strcmp(ret, "y")) {
1001
			if (wrs_msg_level >= LOG_DEBUG)
1002
				printf("Found %s\n", buff);
1003
			set_p_pmode(port - 1, QMODE_DISABLED);
1004
			mode = QMODE_DISABLED;
1005 1006 1007 1008
		}
		sprintf(buff, "VLANS_PORT%02d_MODE_UNQUALIFIED", port);
		ret = libwr_cfg_get(buff);
		if (ret && !strcmp(ret, "y")) {
1009
			if (wrs_msg_level >= LOG_DEBUG)
1010
				printf("Found %s\n", buff);
1011
			set_p_pmode(port - 1, QMODE_UNQ);
1012
			mode = QMODE_UNQ;
1013
		}
1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034
		
		/* check UNTAG all or none only for ACCESS
		 * for other modes use untag none by default */
		if (mode == QMODE_ACCESS) {
			sprintf(buff, "VLANS_PORT%02d_UNTAG_ALL", port);
			ret = libwr_cfg_get(buff);
			if (ret && !strcmp(ret, "y")) {
				if (wrs_msg_level >= LOG_DEBUG)
					printf("Found %s\n", buff);
				set_p_untag(port - 1, 1);
			}

			sprintf(buff, "VLANS_PORT%02d_UNTAG_NONE", port);
			ret = libwr_cfg_get(buff);
			if (ret && !strcmp(ret, "y")) {
				if (wrs_msg_level >= LOG_DEBUG)
					printf("Found %s\n", buff);
				set_p_untag(port - 1, 0);
			}
		} else {
			/* for other modes "untag none" by default */
1035
			if (wrs_msg_level >= LOG_DEBUG)
1036 1037
				printf("Setting port %d to untag none\n",
				       port);
1038
			set_p_untag(port - 1, 0);
1039
		}
1040

1041 1042 1043 1044 1045
		/* update a mask for vlan0 */
		if (mode != QMODE_ACCESS && mode != QMODE_TRUNK) {
			vlan0_port_mask |= 1 << (port - 1);
		}

1046 1047 1048
		sprintf(buff, "VLANS_PORT%02d_PRIO", port);
		val_ch = libwr_cfg_get(buff);
		if (val_ch) {
1049
			if (wrs_msg_level >= LOG_DEBUG)
1050 1051 1052
				printf("Found %s=%s\n", buff, val_ch);
			set_p_prio(port - 1, val_ch);
		}
1053 1054 1055 1056 1057 1058 1059
		if (mode == QMODE_ACCESS) {
			sprintf(buff, "VLANS_PORT%02d_VID", port);
			val_ch = libwr_cfg_get(buff);
			if (val_ch) {
				if (wrs_msg_level >= LOG_DEBUG)
					printf("Found %s=%s\n", buff, val_ch);
				set_p_vid(port - 1, val_ch);
1060 1061 1062 1063
			} else {
				pr_error("VID not defined for the port (%d) in"
					 " ACCESS mode!\n", port);
				exit(1);
1064
			}
1065 1066 1067
		}
	}

1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079
	/* If VLANS_VLAN0000 is empty => ports mask is not provided for VLAN0
	 * then add to VLAN0 ports that are not ACCESS nor TRUNK */
	val_ch = libwr_cfg_get("VLANS_VLAN0000");
	if (!val_ch || !strnlen(val_ch, 10)) {
		if (wrs_msg_level >= LOG_DEBUG)
			printf("Vlan0 not configured: Using port mask based on"
			       " configured port modes (0x%05x)\n",
			       vlan0_port_mask);
		set_rtu_vlan(0, 0, vlan0_port_mask, -1, -1, 0,
			     VALID_VID | VALID_FID | VALID_PMASK);
	}

1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091
	for (i = 0; dot_config_vlan_sets[i].name; i++) {
		ret = libwr_cfg_get(dot_config_vlan_sets[i].name);
		if (ret && !strcmp(ret, "y")) {
			read_dot_config_vlans(dot_config_vlan_sets[i].min,
					      dot_config_vlan_sets[i].max);
		}
	}
	return 0;
}

static void read_dot_config_vlans(int vlan_min, int vlan_max)
{
1092 1093
	int fid, prio, drop;
	unsigned long pmask;
1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109
	int vlan_flags;
	int vlan;
	char buff[60];

	for (vlan = vlan_min; vlan <= vlan_max; vlan++) {
		vlan_flags = 0;
		fid = -1;
		prio = -1;
		drop = -1;
		pmask = 0;
		if (!libwr_cfg_convert2("VLANS_VLAN%04d", "fid", LIBWR_STRING,
					buff, vlan)) {
			fid = check_rtu("rtu vid", buff, RTU_FID_MIN,
					RTU_FID_MAX);
			if (fid < 0)
				exit(1);
1110
			if (wrs_msg_level >= LOG_DEBUG)
1111 1112 1113 1114 1115 1116 1117
				printf("Vlan %4d: Found fid=%d\n",
				       vlan, fid);
			vlan_flags |= VALID_FID;
		}

		if (!libwr_cfg_convert2("VLANS_VLAN%04d", "prio", LIBWR_STRING,
					buff, vlan)) {
1118 1119
			prio = check_rtu_prio(buff);
			if (prio < RTU_PRIO_DISABLE)
1120 1121
				exit(1);
			vlan_flags |= VALID_PRIO;
1122
			if (wrs_msg_level >= LOG_DEBUG)
1123 1124 1125 1126 1127
				printf("Vlan %4d: Found prio=%d\n",
				       vlan, prio);
		}
		if (!libwr_cfg_convert2("VLANS_VLAN%04d", "drop", LIBWR_STRING,
					buff, vlan)) {
1128 1129 1130
			if (!strcmp(buff, "y") /* "y", "yes" or "1" */
			    || !strcmp(buff, "yes")
			    || !strcmp(buff, "1"))
1131
				sprintf(buff, "1");
1132 1133 1134
			else if (!strcmp(buff, "n") /* "n", "no" or "0" */
				 || !strcmp(buff, "no")
				 || !strcmp(buff, "0"))
1135 1136
				sprintf(buff, "0");
			else {
1137 1138
				pr_error("invalid drop parameter \"%s\" in "
					 "VLANS_VLAN%04d\n", buff, vlan);
1139 1140 1141 1142 1143
				exit(1);
			}
			drop = check_rtu("rtu drop", buff, 0, 1);
			if (drop < 0)
				exit(1);
1144
			if (wrs_msg_level >= LOG_DEBUG)
1145 1146 1147 1148 1149 1150
				printf("Vlan %4d: Found drop=%d\n",
				       vlan, drop);
			vlan_flags |= VALID_DROP;
		}
		if (!libwr_cfg_convert2("VLANS_VLAN%04d", "ports",
					LIBWR_STRING, buff, vlan)) {
1151
			parse_mask(buff, &pmask);
1152
			if (pmask < RTU_PMASK_MIN || pmask > RTU_PMASK_MAX) {
1153 1154
				pr_error("invalid port mask 0x%lx (\"%s\") for"
					 " vlan %4d\n", pmask, buff, vlan);
1155 1156
				exit(1);
			}
1157
			if (wrs_msg_level >= LOG_DEBUG)
1158
				printf("Vlan %4d: Port mask 0x%05lx\n",
1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170
				       vlan, pmask);
			vlan_flags |= VALID_PMASK;
		}
		if (vlan_flags) {
			/* at least one parameter is present, so trigger
			 * the fill */
			vlan_flags |= VALID_VID;
			set_rtu_vlan(vlan, fid, pmask, drop, prio, 0,
				     vlan_flags);
		}
	}
}