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