ef218bd984bf0692522a876b694d088fcb79e292
[openwrt/staging/yousong.git] / target / linux / generic / files / drivers / net / phy / swconfig.c
1 /*
2 * swconfig.c: Switch configuration API
3 *
4 * Copyright (C) 2008 Felix Fietkau <nbd@openwrt.org>
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the 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 General Public License for more details.
15 */
16
17 #include <linux/types.h>
18 #include <linux/module.h>
19 #include <linux/init.h>
20 #include <linux/list.h>
21 #include <linux/if.h>
22 #include <linux/if_ether.h>
23 #include <linux/capability.h>
24 #include <linux/skbuff.h>
25 #include <linux/switch.h>
26 #include <linux/of.h>
27 #include <linux/version.h>
28
29 #define SWCONFIG_DEVNAME "switch%d"
30
31 #include "swconfig_leds.c"
32
33 MODULE_AUTHOR("Felix Fietkau <nbd@openwrt.org>");
34 MODULE_LICENSE("GPL");
35
36 static int swdev_id;
37 static struct list_head swdevs;
38 static DEFINE_SPINLOCK(swdevs_lock);
39 struct swconfig_callback;
40
41 struct swconfig_callback {
42 struct sk_buff *msg;
43 struct genlmsghdr *hdr;
44 struct genl_info *info;
45 int cmd;
46
47 /* callback for filling in the message data */
48 int (*fill)(struct swconfig_callback *cb, void *arg);
49
50 /* callback for closing the message before sending it */
51 int (*close)(struct swconfig_callback *cb, void *arg);
52
53 struct nlattr *nest[4];
54 int args[4];
55 };
56
57 /* defaults */
58
59 static int
60 swconfig_get_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr,
61 struct switch_val *val)
62 {
63 int ret;
64 if (val->port_vlan >= dev->vlans)
65 return -EINVAL;
66
67 if (!dev->ops->get_vlan_ports)
68 return -EOPNOTSUPP;
69
70 ret = dev->ops->get_vlan_ports(dev, val);
71 return ret;
72 }
73
74 static int
75 swconfig_set_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr,
76 struct switch_val *val)
77 {
78 struct switch_port *ports = val->value.ports;
79 const struct switch_dev_ops *ops = dev->ops;
80 int i;
81
82 if (val->port_vlan >= dev->vlans)
83 return -EINVAL;
84
85 /* validate ports */
86 if (val->len > dev->ports)
87 return -EINVAL;
88
89 if (!ops->set_vlan_ports)
90 return -EOPNOTSUPP;
91
92 for (i = 0; i < val->len; i++) {
93 if (ports[i].id >= dev->ports)
94 return -EINVAL;
95
96 if (ops->set_port_pvid &&
97 !(ports[i].flags & (1 << SWITCH_PORT_FLAG_TAGGED)))
98 ops->set_port_pvid(dev, ports[i].id, val->port_vlan);
99 }
100
101 return ops->set_vlan_ports(dev, val);
102 }
103
104 static int
105 swconfig_set_pvid(struct switch_dev *dev, const struct switch_attr *attr,
106 struct switch_val *val)
107 {
108 if (val->port_vlan >= dev->ports)
109 return -EINVAL;
110
111 if (!dev->ops->set_port_pvid)
112 return -EOPNOTSUPP;
113
114 return dev->ops->set_port_pvid(dev, val->port_vlan, val->value.i);
115 }
116
117 static int
118 swconfig_get_pvid(struct switch_dev *dev, const struct switch_attr *attr,
119 struct switch_val *val)
120 {
121 if (val->port_vlan >= dev->ports)
122 return -EINVAL;
123
124 if (!dev->ops->get_port_pvid)
125 return -EOPNOTSUPP;
126
127 return dev->ops->get_port_pvid(dev, val->port_vlan, &val->value.i);
128 }
129
130 static const char *
131 swconfig_speed_str(enum switch_port_speed speed)
132 {
133 switch (speed) {
134 case SWITCH_PORT_SPEED_10:
135 return "10baseT";
136 case SWITCH_PORT_SPEED_100:
137 return "100baseT";
138 case SWITCH_PORT_SPEED_1000:
139 return "1000baseT";
140 default:
141 break;
142 }
143
144 return "unknown";
145 }
146
147 static int
148 swconfig_get_link(struct switch_dev *dev, const struct switch_attr *attr,
149 struct switch_val *val)
150 {
151 struct switch_port_link link;
152 int len;
153 int ret;
154
155 if (val->port_vlan >= dev->ports)
156 return -EINVAL;
157
158 if (!dev->ops->get_port_link)
159 return -EOPNOTSUPP;
160
161 memset(&link, 0, sizeof(link));
162 ret = dev->ops->get_port_link(dev, val->port_vlan, &link);
163 if (ret)
164 return ret;
165
166 memset(dev->buf, 0, sizeof(dev->buf));
167
168 if (link.link)
169 len = snprintf(dev->buf, sizeof(dev->buf),
170 "port:%d link:up speed:%s %s-duplex %s%s%s%s%s",
171 val->port_vlan,
172 swconfig_speed_str(link.speed),
173 link.duplex ? "full" : "half",
174 link.tx_flow ? "txflow " : "",
175 link.rx_flow ? "rxflow " : "",
176 link.eee & ADVERTISED_100baseT_Full ? "eee100 " : "",
177 link.eee & ADVERTISED_1000baseT_Full ? "eee1000 " : "",
178 link.aneg ? "auto" : "");
179 else
180 len = snprintf(dev->buf, sizeof(dev->buf), "port:%d link:down",
181 val->port_vlan);
182
183 val->value.s = dev->buf;
184 val->len = len;
185
186 return 0;
187 }
188
189 static int
190 swconfig_apply_config(struct switch_dev *dev, const struct switch_attr *attr,
191 struct switch_val *val)
192 {
193 /* don't complain if not supported by the switch driver */
194 if (!dev->ops->apply_config)
195 return 0;
196
197 return dev->ops->apply_config(dev);
198 }
199
200 static int
201 swconfig_reset_switch(struct switch_dev *dev, const struct switch_attr *attr,
202 struct switch_val *val)
203 {
204 /* don't complain if not supported by the switch driver */
205 if (!dev->ops->reset_switch)
206 return 0;
207
208 return dev->ops->reset_switch(dev);
209 }
210
211 enum global_defaults {
212 GLOBAL_APPLY,
213 GLOBAL_RESET,
214 };
215
216 enum vlan_defaults {
217 VLAN_PORTS,
218 };
219
220 enum port_defaults {
221 PORT_PVID,
222 PORT_LINK,
223 };
224
225 static struct switch_attr default_global[] = {
226 [GLOBAL_APPLY] = {
227 .type = SWITCH_TYPE_NOVAL,
228 .name = "apply",
229 .description = "Activate changes in the hardware",
230 .set = swconfig_apply_config,
231 },
232 [GLOBAL_RESET] = {
233 .type = SWITCH_TYPE_NOVAL,
234 .name = "reset",
235 .description = "Reset the switch",
236 .set = swconfig_reset_switch,
237 }
238 };
239
240 static struct switch_attr default_port[] = {
241 [PORT_PVID] = {
242 .type = SWITCH_TYPE_INT,
243 .name = "pvid",
244 .description = "Primary VLAN ID",
245 .set = swconfig_set_pvid,
246 .get = swconfig_get_pvid,
247 },
248 [PORT_LINK] = {
249 .type = SWITCH_TYPE_STRING,
250 .name = "link",
251 .description = "Get port link information",
252 .set = NULL,
253 .get = swconfig_get_link,
254 }
255 };
256
257 static struct switch_attr default_vlan[] = {
258 [VLAN_PORTS] = {
259 .type = SWITCH_TYPE_PORTS,
260 .name = "ports",
261 .description = "VLAN port mapping",
262 .set = swconfig_set_vlan_ports,
263 .get = swconfig_get_vlan_ports,
264 },
265 };
266
267 static const struct switch_attr *
268 swconfig_find_attr_by_name(const struct switch_attrlist *alist,
269 const char *name)
270 {
271 int i;
272
273 for (i = 0; i < alist->n_attr; i++)
274 if (strcmp(name, alist->attr[i].name) == 0)
275 return &alist->attr[i];
276
277 return NULL;
278 }
279
280 static void swconfig_defaults_init(struct switch_dev *dev)
281 {
282 const struct switch_dev_ops *ops = dev->ops;
283
284 dev->def_global = 0;
285 dev->def_vlan = 0;
286 dev->def_port = 0;
287
288 if (ops->get_vlan_ports || ops->set_vlan_ports)
289 set_bit(VLAN_PORTS, &dev->def_vlan);
290
291 if (ops->get_port_pvid || ops->set_port_pvid)
292 set_bit(PORT_PVID, &dev->def_port);
293
294 if (ops->get_port_link &&
295 !swconfig_find_attr_by_name(&ops->attr_port, "link"))
296 set_bit(PORT_LINK, &dev->def_port);
297
298 /* always present, can be no-op */
299 set_bit(GLOBAL_APPLY, &dev->def_global);
300 set_bit(GLOBAL_RESET, &dev->def_global);
301 }
302
303
304 static struct genl_family switch_fam = {
305 .id = GENL_ID_GENERATE,
306 .name = "switch",
307 .hdrsize = 0,
308 .version = 1,
309 .maxattr = SWITCH_ATTR_MAX,
310 };
311
312 static const struct nla_policy switch_policy[SWITCH_ATTR_MAX+1] = {
313 [SWITCH_ATTR_ID] = { .type = NLA_U32 },
314 [SWITCH_ATTR_OP_ID] = { .type = NLA_U32 },
315 [SWITCH_ATTR_OP_PORT] = { .type = NLA_U32 },
316 [SWITCH_ATTR_OP_VLAN] = { .type = NLA_U32 },
317 [SWITCH_ATTR_OP_VALUE_INT] = { .type = NLA_U32 },
318 [SWITCH_ATTR_OP_VALUE_STR] = { .type = NLA_NUL_STRING },
319 [SWITCH_ATTR_OP_VALUE_PORTS] = { .type = NLA_NESTED },
320 [SWITCH_ATTR_TYPE] = { .type = NLA_U32 },
321 };
322
323 static const struct nla_policy port_policy[SWITCH_PORT_ATTR_MAX+1] = {
324 [SWITCH_PORT_ID] = { .type = NLA_U32 },
325 [SWITCH_PORT_FLAG_TAGGED] = { .type = NLA_FLAG },
326 };
327
328 static inline void
329 swconfig_lock(void)
330 {
331 spin_lock(&swdevs_lock);
332 }
333
334 static inline void
335 swconfig_unlock(void)
336 {
337 spin_unlock(&swdevs_lock);
338 }
339
340 static struct switch_dev *
341 swconfig_get_dev(struct genl_info *info)
342 {
343 struct switch_dev *dev = NULL;
344 struct switch_dev *p;
345 int id;
346
347 if (!info->attrs[SWITCH_ATTR_ID])
348 goto done;
349
350 id = nla_get_u32(info->attrs[SWITCH_ATTR_ID]);
351 swconfig_lock();
352 list_for_each_entry(p, &swdevs, dev_list) {
353 if (id != p->id)
354 continue;
355
356 dev = p;
357 break;
358 }
359 if (dev)
360 mutex_lock(&dev->sw_mutex);
361 else
362 pr_debug("device %d not found\n", id);
363 swconfig_unlock();
364 done:
365 return dev;
366 }
367
368 static inline void
369 swconfig_put_dev(struct switch_dev *dev)
370 {
371 mutex_unlock(&dev->sw_mutex);
372 }
373
374 static int
375 swconfig_dump_attr(struct swconfig_callback *cb, void *arg)
376 {
377 struct switch_attr *op = arg;
378 struct genl_info *info = cb->info;
379 struct sk_buff *msg = cb->msg;
380 int id = cb->args[0];
381 void *hdr;
382
383 hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq, &switch_fam,
384 NLM_F_MULTI, SWITCH_CMD_NEW_ATTR);
385 if (IS_ERR(hdr))
386 return -1;
387
388 if (nla_put_u32(msg, SWITCH_ATTR_OP_ID, id))
389 goto nla_put_failure;
390 if (nla_put_u32(msg, SWITCH_ATTR_OP_TYPE, op->type))
391 goto nla_put_failure;
392 if (nla_put_string(msg, SWITCH_ATTR_OP_NAME, op->name))
393 goto nla_put_failure;
394 if (op->description)
395 if (nla_put_string(msg, SWITCH_ATTR_OP_DESCRIPTION,
396 op->description))
397 goto nla_put_failure;
398
399 genlmsg_end(msg, hdr);
400 return msg->len;
401 nla_put_failure:
402 genlmsg_cancel(msg, hdr);
403 return -EMSGSIZE;
404 }
405
406 /* spread multipart messages across multiple message buffers */
407 static int
408 swconfig_send_multipart(struct swconfig_callback *cb, void *arg)
409 {
410 struct genl_info *info = cb->info;
411 int restart = 0;
412 int err;
413
414 do {
415 if (!cb->msg) {
416 cb->msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
417 if (cb->msg == NULL)
418 goto error;
419 }
420
421 if (!(cb->fill(cb, arg) < 0))
422 break;
423
424 /* fill failed, check if this was already the second attempt */
425 if (restart)
426 goto error;
427
428 /* try again in a new message, send the current one */
429 restart = 1;
430 if (cb->close) {
431 if (cb->close(cb, arg) < 0)
432 goto error;
433 }
434 err = genlmsg_reply(cb->msg, info);
435 cb->msg = NULL;
436 if (err < 0)
437 goto error;
438
439 } while (restart);
440
441 return 0;
442
443 error:
444 if (cb->msg)
445 nlmsg_free(cb->msg);
446 return -1;
447 }
448
449 static int
450 swconfig_list_attrs(struct sk_buff *skb, struct genl_info *info)
451 {
452 struct genlmsghdr *hdr = nlmsg_data(info->nlhdr);
453 const struct switch_attrlist *alist;
454 struct switch_dev *dev;
455 struct swconfig_callback cb;
456 int err = -EINVAL;
457 int i;
458
459 /* defaults */
460 struct switch_attr *def_list;
461 unsigned long *def_active;
462 int n_def;
463
464 dev = swconfig_get_dev(info);
465 if (!dev)
466 return -EINVAL;
467
468 switch (hdr->cmd) {
469 case SWITCH_CMD_LIST_GLOBAL:
470 alist = &dev->ops->attr_global;
471 def_list = default_global;
472 def_active = &dev->def_global;
473 n_def = ARRAY_SIZE(default_global);
474 break;
475 case SWITCH_CMD_LIST_VLAN:
476 alist = &dev->ops->attr_vlan;
477 def_list = default_vlan;
478 def_active = &dev->def_vlan;
479 n_def = ARRAY_SIZE(default_vlan);
480 break;
481 case SWITCH_CMD_LIST_PORT:
482 alist = &dev->ops->attr_port;
483 def_list = default_port;
484 def_active = &dev->def_port;
485 n_def = ARRAY_SIZE(default_port);
486 break;
487 default:
488 WARN_ON(1);
489 goto out;
490 }
491
492 memset(&cb, 0, sizeof(cb));
493 cb.info = info;
494 cb.fill = swconfig_dump_attr;
495 for (i = 0; i < alist->n_attr; i++) {
496 if (alist->attr[i].disabled)
497 continue;
498 cb.args[0] = i;
499 err = swconfig_send_multipart(&cb, (void *) &alist->attr[i]);
500 if (err < 0)
501 goto error;
502 }
503
504 /* defaults */
505 for (i = 0; i < n_def; i++) {
506 if (!test_bit(i, def_active))
507 continue;
508 cb.args[0] = SWITCH_ATTR_DEFAULTS_OFFSET + i;
509 err = swconfig_send_multipart(&cb, (void *) &def_list[i]);
510 if (err < 0)
511 goto error;
512 }
513 swconfig_put_dev(dev);
514
515 if (!cb.msg)
516 return 0;
517
518 return genlmsg_reply(cb.msg, info);
519
520 error:
521 if (cb.msg)
522 nlmsg_free(cb.msg);
523 out:
524 swconfig_put_dev(dev);
525 return err;
526 }
527
528 static const struct switch_attr *
529 swconfig_lookup_attr(struct switch_dev *dev, struct genl_info *info,
530 struct switch_val *val)
531 {
532 struct genlmsghdr *hdr = nlmsg_data(info->nlhdr);
533 const struct switch_attrlist *alist;
534 const struct switch_attr *attr = NULL;
535 int attr_id;
536
537 /* defaults */
538 struct switch_attr *def_list;
539 unsigned long *def_active;
540 int n_def;
541
542 if (!info->attrs[SWITCH_ATTR_OP_ID])
543 goto done;
544
545 switch (hdr->cmd) {
546 case SWITCH_CMD_SET_GLOBAL:
547 case SWITCH_CMD_GET_GLOBAL:
548 alist = &dev->ops->attr_global;
549 def_list = default_global;
550 def_active = &dev->def_global;
551 n_def = ARRAY_SIZE(default_global);
552 break;
553 case SWITCH_CMD_SET_VLAN:
554 case SWITCH_CMD_GET_VLAN:
555 alist = &dev->ops->attr_vlan;
556 def_list = default_vlan;
557 def_active = &dev->def_vlan;
558 n_def = ARRAY_SIZE(default_vlan);
559 if (!info->attrs[SWITCH_ATTR_OP_VLAN])
560 goto done;
561 val->port_vlan = nla_get_u32(info->attrs[SWITCH_ATTR_OP_VLAN]);
562 if (val->port_vlan >= dev->vlans)
563 goto done;
564 break;
565 case SWITCH_CMD_SET_PORT:
566 case SWITCH_CMD_GET_PORT:
567 alist = &dev->ops->attr_port;
568 def_list = default_port;
569 def_active = &dev->def_port;
570 n_def = ARRAY_SIZE(default_port);
571 if (!info->attrs[SWITCH_ATTR_OP_PORT])
572 goto done;
573 val->port_vlan = nla_get_u32(info->attrs[SWITCH_ATTR_OP_PORT]);
574 if (val->port_vlan >= dev->ports)
575 goto done;
576 break;
577 default:
578 WARN_ON(1);
579 goto done;
580 }
581
582 if (!alist)
583 goto done;
584
585 attr_id = nla_get_u32(info->attrs[SWITCH_ATTR_OP_ID]);
586 if (attr_id >= SWITCH_ATTR_DEFAULTS_OFFSET) {
587 attr_id -= SWITCH_ATTR_DEFAULTS_OFFSET;
588 if (attr_id >= n_def)
589 goto done;
590 if (!test_bit(attr_id, def_active))
591 goto done;
592 attr = &def_list[attr_id];
593 } else {
594 if (attr_id >= alist->n_attr)
595 goto done;
596 attr = &alist->attr[attr_id];
597 }
598
599 if (attr->disabled)
600 attr = NULL;
601
602 done:
603 if (!attr)
604 pr_debug("attribute lookup failed\n");
605 val->attr = attr;
606 return attr;
607 }
608
609 static int
610 swconfig_parse_ports(struct sk_buff *msg, struct nlattr *head,
611 struct switch_val *val, int max)
612 {
613 struct nlattr *nla;
614 int rem;
615
616 val->len = 0;
617 nla_for_each_nested(nla, head, rem) {
618 struct nlattr *tb[SWITCH_PORT_ATTR_MAX+1];
619 struct switch_port *port = &val->value.ports[val->len];
620
621 if (val->len >= max)
622 return -EINVAL;
623
624 if (nla_parse_nested(tb, SWITCH_PORT_ATTR_MAX, nla,
625 port_policy))
626 return -EINVAL;
627
628 if (!tb[SWITCH_PORT_ID])
629 return -EINVAL;
630
631 port->id = nla_get_u32(tb[SWITCH_PORT_ID]);
632 if (tb[SWITCH_PORT_FLAG_TAGGED])
633 port->flags |= (1 << SWITCH_PORT_FLAG_TAGGED);
634 val->len++;
635 }
636
637 return 0;
638 }
639
640 static int
641 swconfig_set_attr(struct sk_buff *skb, struct genl_info *info)
642 {
643 const struct switch_attr *attr;
644 struct switch_dev *dev;
645 struct switch_val val;
646 int err = -EINVAL;
647
648 dev = swconfig_get_dev(info);
649 if (!dev)
650 return -EINVAL;
651
652 memset(&val, 0, sizeof(val));
653 attr = swconfig_lookup_attr(dev, info, &val);
654 if (!attr || !attr->set)
655 goto error;
656
657 val.attr = attr;
658 switch (attr->type) {
659 case SWITCH_TYPE_NOVAL:
660 break;
661 case SWITCH_TYPE_INT:
662 if (!info->attrs[SWITCH_ATTR_OP_VALUE_INT])
663 goto error;
664 val.value.i =
665 nla_get_u32(info->attrs[SWITCH_ATTR_OP_VALUE_INT]);
666 break;
667 case SWITCH_TYPE_STRING:
668 if (!info->attrs[SWITCH_ATTR_OP_VALUE_STR])
669 goto error;
670 val.value.s =
671 nla_data(info->attrs[SWITCH_ATTR_OP_VALUE_STR]);
672 break;
673 case SWITCH_TYPE_PORTS:
674 val.value.ports = dev->portbuf;
675 memset(dev->portbuf, 0,
676 sizeof(struct switch_port) * dev->ports);
677
678 /* TODO: implement multipart? */
679 if (info->attrs[SWITCH_ATTR_OP_VALUE_PORTS]) {
680 err = swconfig_parse_ports(skb,
681 info->attrs[SWITCH_ATTR_OP_VALUE_PORTS],
682 &val, dev->ports);
683 if (err < 0)
684 goto error;
685 } else {
686 val.len = 0;
687 err = 0;
688 }
689 break;
690 default:
691 goto error;
692 }
693
694 err = attr->set(dev, attr, &val);
695 error:
696 swconfig_put_dev(dev);
697 return err;
698 }
699
700 static int
701 swconfig_close_portlist(struct swconfig_callback *cb, void *arg)
702 {
703 if (cb->nest[0])
704 nla_nest_end(cb->msg, cb->nest[0]);
705 return 0;
706 }
707
708 static int
709 swconfig_send_port(struct swconfig_callback *cb, void *arg)
710 {
711 const struct switch_port *port = arg;
712 struct nlattr *p = NULL;
713
714 if (!cb->nest[0]) {
715 cb->nest[0] = nla_nest_start(cb->msg, cb->cmd);
716 if (!cb->nest[0])
717 return -1;
718 }
719
720 p = nla_nest_start(cb->msg, SWITCH_ATTR_PORT);
721 if (!p)
722 goto error;
723
724 if (nla_put_u32(cb->msg, SWITCH_PORT_ID, port->id))
725 goto nla_put_failure;
726 if (port->flags & (1 << SWITCH_PORT_FLAG_TAGGED)) {
727 if (nla_put_flag(cb->msg, SWITCH_PORT_FLAG_TAGGED))
728 goto nla_put_failure;
729 }
730
731 nla_nest_end(cb->msg, p);
732 return 0;
733
734 nla_put_failure:
735 nla_nest_cancel(cb->msg, p);
736 error:
737 nla_nest_cancel(cb->msg, cb->nest[0]);
738 return -1;
739 }
740
741 static int
742 swconfig_send_ports(struct sk_buff **msg, struct genl_info *info, int attr,
743 const struct switch_val *val)
744 {
745 struct swconfig_callback cb;
746 int err = 0;
747 int i;
748
749 if (!val->value.ports)
750 return -EINVAL;
751
752 memset(&cb, 0, sizeof(cb));
753 cb.cmd = attr;
754 cb.msg = *msg;
755 cb.info = info;
756 cb.fill = swconfig_send_port;
757 cb.close = swconfig_close_portlist;
758
759 cb.nest[0] = nla_nest_start(cb.msg, cb.cmd);
760 for (i = 0; i < val->len; i++) {
761 err = swconfig_send_multipart(&cb, &val->value.ports[i]);
762 if (err)
763 goto done;
764 }
765 err = val->len;
766 swconfig_close_portlist(&cb, NULL);
767 *msg = cb.msg;
768
769 done:
770 return err;
771 }
772
773 static int
774 swconfig_send_link(struct sk_buff *msg, struct genl_info *info, int attr,
775 const struct switch_port_link *link)
776 {
777 struct nlattr *p = NULL;
778 int err = 0;
779
780 p = nla_nest_start(msg, attr);
781 if (link->link) {
782 if (nla_put_flag(msg, SWITCH_LINK_FLAG_LINK))
783 goto nla_put_failure;
784 }
785 if (link->duplex) {
786 if (nla_put_flag(msg, SWITCH_LINK_FLAG_DUPLEX))
787 goto nla_put_failure;
788 }
789 if (link->aneg) {
790 if (nla_put_flag(msg, SWITCH_LINK_FLAG_ANEG))
791 goto nla_put_failure;
792 }
793 if (link->tx_flow) {
794 if (nla_put_flag(msg, SWITCH_LINK_FLAG_TX_FLOW))
795 goto nla_put_failure;
796 }
797 if (link->rx_flow) {
798 if (nla_put_flag(msg, SWITCH_LINK_FLAG_RX_FLOW))
799 goto nla_put_failure;
800 }
801 if (nla_put_u32(msg, SWITCH_LINK_SPEED, link->speed))
802 goto nla_put_failure;
803 if (link->eee & ADVERTISED_100baseT_Full) {
804 if (nla_put_flag(msg, SWITCH_LINK_FLAG_EEE_100BASET))
805 goto nla_put_failure;
806 }
807 if (link->eee & ADVERTISED_1000baseT_Full) {
808 if (nla_put_flag(msg, SWITCH_LINK_FLAG_EEE_1000BASET))
809 goto nla_put_failure;
810 }
811 nla_nest_end(msg, p);
812
813 return err;
814
815 nla_put_failure:
816 nla_nest_cancel(msg, p);
817 return -1;
818 }
819
820 static int
821 swconfig_get_attr(struct sk_buff *skb, struct genl_info *info)
822 {
823 struct genlmsghdr *hdr = nlmsg_data(info->nlhdr);
824 const struct switch_attr *attr;
825 struct switch_dev *dev;
826 struct sk_buff *msg = NULL;
827 struct switch_val val;
828 int err = -EINVAL;
829 int cmd = hdr->cmd;
830
831 dev = swconfig_get_dev(info);
832 if (!dev)
833 return -EINVAL;
834
835 memset(&val, 0, sizeof(val));
836 attr = swconfig_lookup_attr(dev, info, &val);
837 if (!attr || !attr->get)
838 goto error;
839
840 if (attr->type == SWITCH_TYPE_PORTS) {
841 val.value.ports = dev->portbuf;
842 memset(dev->portbuf, 0,
843 sizeof(struct switch_port) * dev->ports);
844 } else if (attr->type == SWITCH_TYPE_LINK) {
845 val.value.link = &dev->linkbuf;
846 memset(&dev->linkbuf, 0, sizeof(struct switch_port_link));
847 }
848
849 err = attr->get(dev, attr, &val);
850 if (err)
851 goto error;
852
853 msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
854 if (!msg)
855 goto error;
856
857 hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq, &switch_fam,
858 0, cmd);
859 if (IS_ERR(hdr))
860 goto nla_put_failure;
861
862 switch (attr->type) {
863 case SWITCH_TYPE_INT:
864 if (nla_put_u32(msg, SWITCH_ATTR_OP_VALUE_INT, val.value.i))
865 goto nla_put_failure;
866 break;
867 case SWITCH_TYPE_STRING:
868 if (nla_put_string(msg, SWITCH_ATTR_OP_VALUE_STR, val.value.s))
869 goto nla_put_failure;
870 break;
871 case SWITCH_TYPE_PORTS:
872 err = swconfig_send_ports(&msg, info,
873 SWITCH_ATTR_OP_VALUE_PORTS, &val);
874 if (err < 0)
875 goto nla_put_failure;
876 break;
877 case SWITCH_TYPE_LINK:
878 err = swconfig_send_link(msg, info,
879 SWITCH_ATTR_OP_VALUE_LINK, val.value.link);
880 if (err < 0)
881 goto nla_put_failure;
882 break;
883 default:
884 pr_debug("invalid type in attribute\n");
885 err = -EINVAL;
886 goto error;
887 }
888 genlmsg_end(msg, hdr);
889 err = msg->len;
890 if (err < 0)
891 goto nla_put_failure;
892
893 swconfig_put_dev(dev);
894 return genlmsg_reply(msg, info);
895
896 nla_put_failure:
897 if (msg)
898 nlmsg_free(msg);
899 error:
900 swconfig_put_dev(dev);
901 if (!err)
902 err = -ENOMEM;
903 return err;
904 }
905
906 static int
907 swconfig_send_switch(struct sk_buff *msg, u32 pid, u32 seq, int flags,
908 const struct switch_dev *dev)
909 {
910 struct nlattr *p = NULL, *m = NULL;
911 void *hdr;
912 int i;
913
914 hdr = genlmsg_put(msg, pid, seq, &switch_fam, flags,
915 SWITCH_CMD_NEW_ATTR);
916 if (IS_ERR(hdr))
917 return -1;
918
919 if (nla_put_u32(msg, SWITCH_ATTR_ID, dev->id))
920 goto nla_put_failure;
921 if (nla_put_string(msg, SWITCH_ATTR_DEV_NAME, dev->devname))
922 goto nla_put_failure;
923 if (nla_put_string(msg, SWITCH_ATTR_ALIAS, dev->alias))
924 goto nla_put_failure;
925 if (nla_put_string(msg, SWITCH_ATTR_NAME, dev->name))
926 goto nla_put_failure;
927 if (nla_put_u32(msg, SWITCH_ATTR_VLANS, dev->vlans))
928 goto nla_put_failure;
929 if (nla_put_u32(msg, SWITCH_ATTR_PORTS, dev->ports))
930 goto nla_put_failure;
931 if (nla_put_u32(msg, SWITCH_ATTR_CPU_PORT, dev->cpu_port))
932 goto nla_put_failure;
933
934 m = nla_nest_start(msg, SWITCH_ATTR_PORTMAP);
935 if (!m)
936 goto nla_put_failure;
937 for (i = 0; i < dev->ports; i++) {
938 p = nla_nest_start(msg, SWITCH_ATTR_PORTS);
939 if (!p)
940 continue;
941 if (dev->portmap[i].s) {
942 if (nla_put_string(msg, SWITCH_PORTMAP_SEGMENT,
943 dev->portmap[i].s))
944 goto nla_put_failure;
945 if (nla_put_u32(msg, SWITCH_PORTMAP_VIRT,
946 dev->portmap[i].virt))
947 goto nla_put_failure;
948 }
949 nla_nest_end(msg, p);
950 }
951 nla_nest_end(msg, m);
952 genlmsg_end(msg, hdr);
953 return msg->len;
954 nla_put_failure:
955 genlmsg_cancel(msg, hdr);
956 return -EMSGSIZE;
957 }
958
959 static int swconfig_dump_switches(struct sk_buff *skb,
960 struct netlink_callback *cb)
961 {
962 struct switch_dev *dev;
963 int start = cb->args[0];
964 int idx = 0;
965
966 swconfig_lock();
967 list_for_each_entry(dev, &swdevs, dev_list) {
968 if (++idx <= start)
969 continue;
970 if (swconfig_send_switch(skb, NETLINK_CB(cb->skb).portid,
971 cb->nlh->nlmsg_seq, NLM_F_MULTI,
972 dev) < 0)
973 break;
974 }
975 swconfig_unlock();
976 cb->args[0] = idx;
977
978 return skb->len;
979 }
980
981 static int
982 swconfig_done(struct netlink_callback *cb)
983 {
984 return 0;
985 }
986
987 static struct genl_ops swconfig_ops[] = {
988 {
989 .cmd = SWITCH_CMD_LIST_GLOBAL,
990 .doit = swconfig_list_attrs,
991 .policy = switch_policy,
992 },
993 {
994 .cmd = SWITCH_CMD_LIST_VLAN,
995 .doit = swconfig_list_attrs,
996 .policy = switch_policy,
997 },
998 {
999 .cmd = SWITCH_CMD_LIST_PORT,
1000 .doit = swconfig_list_attrs,
1001 .policy = switch_policy,
1002 },
1003 {
1004 .cmd = SWITCH_CMD_GET_GLOBAL,
1005 .doit = swconfig_get_attr,
1006 .policy = switch_policy,
1007 },
1008 {
1009 .cmd = SWITCH_CMD_GET_VLAN,
1010 .doit = swconfig_get_attr,
1011 .policy = switch_policy,
1012 },
1013 {
1014 .cmd = SWITCH_CMD_GET_PORT,
1015 .doit = swconfig_get_attr,
1016 .policy = switch_policy,
1017 },
1018 {
1019 .cmd = SWITCH_CMD_SET_GLOBAL,
1020 .doit = swconfig_set_attr,
1021 .policy = switch_policy,
1022 },
1023 {
1024 .cmd = SWITCH_CMD_SET_VLAN,
1025 .doit = swconfig_set_attr,
1026 .policy = switch_policy,
1027 },
1028 {
1029 .cmd = SWITCH_CMD_SET_PORT,
1030 .doit = swconfig_set_attr,
1031 .policy = switch_policy,
1032 },
1033 {
1034 .cmd = SWITCH_CMD_GET_SWITCH,
1035 .dumpit = swconfig_dump_switches,
1036 .policy = switch_policy,
1037 .done = swconfig_done,
1038 }
1039 };
1040
1041 #ifdef CONFIG_OF
1042 void
1043 of_switch_load_portmap(struct switch_dev *dev)
1044 {
1045 struct device_node *port;
1046
1047 if (!dev->of_node)
1048 return;
1049
1050 for_each_child_of_node(dev->of_node, port) {
1051 const __be32 *prop;
1052 const char *segment;
1053 int size, phys;
1054
1055 if (!of_device_is_compatible(port, "swconfig,port"))
1056 continue;
1057
1058 if (of_property_read_string(port, "swconfig,segment", &segment))
1059 continue;
1060
1061 prop = of_get_property(port, "swconfig,portmap", &size);
1062 if (!prop)
1063 continue;
1064
1065 if (size != (2 * sizeof(*prop))) {
1066 pr_err("%s: failed to parse port mapping\n",
1067 port->name);
1068 continue;
1069 }
1070
1071 phys = be32_to_cpup(prop++);
1072 if ((phys < 0) | (phys >= dev->ports)) {
1073 pr_err("%s: physical port index out of range\n",
1074 port->name);
1075 continue;
1076 }
1077
1078 dev->portmap[phys].s = kstrdup(segment, GFP_KERNEL);
1079 dev->portmap[phys].virt = be32_to_cpup(prop);
1080 pr_debug("Found port: %s, physical: %d, virtual: %d\n",
1081 segment, phys, dev->portmap[phys].virt);
1082 }
1083 }
1084 #endif
1085
1086 int
1087 register_switch(struct switch_dev *dev, struct net_device *netdev)
1088 {
1089 struct switch_dev *sdev;
1090 const int max_switches = 8 * sizeof(unsigned long);
1091 unsigned long in_use = 0;
1092 int err;
1093 int i;
1094
1095 INIT_LIST_HEAD(&dev->dev_list);
1096 if (netdev) {
1097 dev->netdev = netdev;
1098 if (!dev->alias)
1099 dev->alias = netdev->name;
1100 }
1101 BUG_ON(!dev->alias);
1102
1103 if (dev->ports > 0) {
1104 dev->portbuf = kzalloc(sizeof(struct switch_port) *
1105 dev->ports, GFP_KERNEL);
1106 if (!dev->portbuf)
1107 return -ENOMEM;
1108 dev->portmap = kzalloc(sizeof(struct switch_portmap) *
1109 dev->ports, GFP_KERNEL);
1110 if (!dev->portmap) {
1111 kfree(dev->portbuf);
1112 return -ENOMEM;
1113 }
1114 }
1115 swconfig_defaults_init(dev);
1116 mutex_init(&dev->sw_mutex);
1117 swconfig_lock();
1118 dev->id = ++swdev_id;
1119
1120 list_for_each_entry(sdev, &swdevs, dev_list) {
1121 if (!sscanf(sdev->devname, SWCONFIG_DEVNAME, &i))
1122 continue;
1123 if (i < 0 || i > max_switches)
1124 continue;
1125
1126 set_bit(i, &in_use);
1127 }
1128 i = find_first_zero_bit(&in_use, max_switches);
1129
1130 if (i == max_switches) {
1131 swconfig_unlock();
1132 return -ENFILE;
1133 }
1134
1135 #ifdef CONFIG_OF
1136 if (dev->ports)
1137 of_switch_load_portmap(dev);
1138 #endif
1139
1140 /* fill device name */
1141 snprintf(dev->devname, IFNAMSIZ, SWCONFIG_DEVNAME, i);
1142
1143 list_add_tail(&dev->dev_list, &swdevs);
1144 swconfig_unlock();
1145
1146 err = swconfig_create_led_trigger(dev);
1147 if (err)
1148 return err;
1149
1150 return 0;
1151 }
1152 EXPORT_SYMBOL_GPL(register_switch);
1153
1154 void
1155 unregister_switch(struct switch_dev *dev)
1156 {
1157 swconfig_destroy_led_trigger(dev);
1158 kfree(dev->portbuf);
1159 mutex_lock(&dev->sw_mutex);
1160 swconfig_lock();
1161 list_del(&dev->dev_list);
1162 swconfig_unlock();
1163 mutex_unlock(&dev->sw_mutex);
1164 }
1165 EXPORT_SYMBOL_GPL(unregister_switch);
1166
1167
1168 static int __init
1169 swconfig_init(void)
1170 {
1171 int err;
1172 #if (LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0))
1173 int i;
1174 #endif
1175
1176 INIT_LIST_HEAD(&swdevs);
1177
1178 #if (LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0))
1179 err = genl_register_family(&switch_fam);
1180 if (err)
1181 return err;
1182
1183 for (i = 0; i < ARRAY_SIZE(swconfig_ops); i++) {
1184 err = genl_register_ops(&switch_fam, &swconfig_ops[i]);
1185 if (err)
1186 goto unregister;
1187 }
1188 return 0;
1189
1190 unregister:
1191 genl_unregister_family(&switch_fam);
1192 return err;
1193 #else
1194 err = genl_register_family_with_ops(&switch_fam, swconfig_ops);
1195 if (err)
1196 return err;
1197 return 0;
1198 #endif
1199 }
1200
1201 static void __exit
1202 swconfig_exit(void)
1203 {
1204 genl_unregister_family(&switch_fam);
1205 }
1206
1207 module_init(swconfig_init);
1208 module_exit(swconfig_exit);
1209