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