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 DEBUG 1
  29 #ifdef DEBUG
  30 #define DPRINTF(format, ...) printk("%s: " format, __func__, ##__VA_ARGS__)
  31 #else
  32 #define DPRINTF(...) do {} while(0)
  33 #endif
  34
  35 #define SWCONFIG_DEVNAME        "switch%d"
  36
  37 #include "swconfig_leds.c"
  38
  39 MODULE_AUTHOR("Felix Fietkau <nbd@openwrt.org>");
  40 MODULE_LICENSE("GPL");
  41
  42 static int swdev_id = 0;
  43 static struct list_head swdevs;
  44 static DEFINE_SPINLOCK(swdevs_lock);
  45 struct swconfig_callback;
  46
  47 struct swconfig_callback
  48 {
  49         struct sk_buff *msg;
  50         struct genlmsghdr *hdr;
  51         struct genl_info *info;
  52         int cmd;
  53
  54         /* callback for filling in the message data */
  55         int (*fill)(struct swconfig_callback *cb, void *arg);
  56
  57         /* callback for closing the message before sending it */
  58         int (*close)(struct swconfig_callback *cb, void *arg);
  59
  60         struct nlattr *nest[4];
  61         int args[4];
  62 };
  63
  64 /* defaults */
  65
  66 static int
  67 swconfig_get_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
  68 {
  69         int ret;
  70         if (val->port_vlan >= dev->vlans)
  71                 return -EINVAL;
  72
  73         if (!dev->ops->get_vlan_ports)
  74                 return -EOPNOTSUPP;
  75
  76         ret = dev->ops->get_vlan_ports(dev, val);
  77         return ret;
  78 }
  79
  80 static int
  81 swconfig_set_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
  82 {
  83         struct switch_port *ports = val->value.ports;
  84         const struct switch_dev_ops *ops = dev->ops;
  85         int i;
  86
  87         if (val->port_vlan >= dev->vlans)
  88                 return -EINVAL;
  89
  90         /* validate ports */
  91         if (val->len > dev->ports)
  92                 return -EINVAL;
  93
  94         if (!ops->set_vlan_ports)
  95                 return -EOPNOTSUPP;
  96
  97         for (i = 0; i < val->len; i++) {
  98                 if (ports[i].id >= dev->ports)
  99                         return -EINVAL;
 100
 101                 if (ops->set_port_pvid &&
 102                     !(ports[i].flags & (1 << SWITCH_PORT_FLAG_TAGGED)))
 103                         ops->set_port_pvid(dev, ports[i].id, val->port_vlan);
 104         }
 105
 106         return ops->set_vlan_ports(dev, val);
 107 }
 108
 109 static int
 110 swconfig_set_pvid(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
 111 {
 112         if (val->port_vlan >= dev->ports)
 113                 return -EINVAL;
 114
 115         if (!dev->ops->set_port_pvid)
 116                 return -EOPNOTSUPP;
 117
 118         return dev->ops->set_port_pvid(dev, val->port_vlan, val->value.i);
 119 }
 120
 121 static int
 122 swconfig_get_pvid(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
 123 {
 124         if (val->port_vlan >= dev->ports)
 125                 return -EINVAL;
 126
 127         if (!dev->ops->get_port_pvid)
 128                 return -EOPNOTSUPP;
 129
 130         return dev->ops->get_port_pvid(dev, val->port_vlan, &val->value.i);
 131 }
 132
 133 static const char *
 134 swconfig_speed_str(enum switch_port_speed speed)
 135 {
 136         switch (speed) {
 137         case SWITCH_PORT_SPEED_10:
 138                 return "10baseT";
 139         case SWITCH_PORT_SPEED_100:
 140                 return "100baseT";
 141         case SWITCH_PORT_SPEED_1000:
 142                 return "1000baseT";
 143         default:
 144                 break;
 145         }
 146
 147         return "unknown";
 148 }
 149
 150 static int
 151 swconfig_get_link(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
 152 {
 153         struct switch_port_link link;
 154         int len;
 155         int ret;
 156
 157         if (val->port_vlan >= dev->ports)
 158                 return -EINVAL;
 159
 160         if (!dev->ops->get_port_link)
 161                 return -EOPNOTSUPP;
 162
 163         memset(&link, 0, sizeof(link));
 164         ret = dev->ops->get_port_link(dev, val->port_vlan, &link);
 165         if (ret)
 166                 return ret;
 167
 168         memset(dev->buf, 0, sizeof(dev->buf));
 169
 170         if (link.link)
 171                 len = snprintf(dev->buf, sizeof(dev->buf),
 172                                "port:%d link:up speed:%s %s-duplex %s%s%s",
 173                                val->port_vlan,
 174                                swconfig_speed_str(link.speed),
 175                                link.duplex ? "full" : "half",
 176                                link.tx_flow ? "txflow ": "",
 177                                link.rx_flow ?   "rxflow " : "",
 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, struct switch_val *val)
 191 {
 192         /* don't complain if not supported by the switch driver */
 193         if (!dev->ops->apply_config)
 194                 return 0;
 195
 196         return dev->ops->apply_config(dev);
 197 }
 198
 199 static int
 200 swconfig_reset_switch(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
 201 {
 202         /* don't complain if not supported by the switch driver */
 203         if (!dev->ops->reset_switch)
 204                 return 0;
 205
 206         return dev->ops->reset_switch(dev);
 207 }
 208
 209 enum global_defaults {
 210         GLOBAL_APPLY,
 211         GLOBAL_RESET,
 212 };
 213
 214 enum vlan_defaults {
 215         VLAN_PORTS,
 216 };
 217
 218 enum port_defaults {
 219         PORT_PVID,
 220         PORT_LINK,
 221 };
 222
 223 static struct switch_attr default_global[] = {
 224         [GLOBAL_APPLY] = {
 225                 .type = SWITCH_TYPE_NOVAL,
 226                 .name = "apply",
 227                 .description = "Activate changes in the hardware",
 228                 .set = swconfig_apply_config,
 229         },
 230         [GLOBAL_RESET] = {
 231                 .type = SWITCH_TYPE_NOVAL,
 232                 .name = "reset",
 233                 .description = "Reset the switch",
 234                 .set = swconfig_reset_switch,
 235         }
 236 };
 237
 238 static struct switch_attr default_port[] = {
 239         [PORT_PVID] = {
 240                 .type = SWITCH_TYPE_INT,
 241                 .name = "pvid",
 242                 .description = "Primary VLAN ID",
 243                 .set = swconfig_set_pvid,
 244                 .get = swconfig_get_pvid,
 245         },
 246         [PORT_LINK] = {
 247                 .type = SWITCH_TYPE_STRING,
 248                 .name = "link",
 249                 .description = "Get port link information",
 250                 .set = NULL,
 251                 .get = swconfig_get_link,
 252         }
 253 };
 254
 255 static struct switch_attr default_vlan[] = {
 256         [VLAN_PORTS] = {
 257                 .type = SWITCH_TYPE_PORTS,
 258                 .name = "ports",
 259                 .description = "VLAN port mapping",
 260                 .set = swconfig_set_vlan_ports,
 261                 .get = swconfig_get_vlan_ports,
 262         },
 263 };
 264
 265 static const struct switch_attr *
 266 swconfig_find_attr_by_name(const struct switch_attrlist *alist, 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                 DPRINTF("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                 DPRINTF("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], &val, dev->ports);
 678                         if (err < 0)
 679                                 goto error;
 680                 } else {
 681                         val.len = 0;
 682                         err = 0;
 683                 }
 684                 break;
 685         default:
 686                 goto error;
 687         }
 688
 689         err = attr->set(dev, attr, &val);
 690 error:
 691         swconfig_put_dev(dev);
 692         return err;
 693 }
 694
 695 static int
 696 swconfig_close_portlist(struct swconfig_callback *cb, void *arg)
 697 {
 698         if (cb->nest[0])
 699                 nla_nest_end(cb->msg, cb->nest[0]);
 700         return 0;
 701 }
 702
 703 static int
 704 swconfig_send_port(struct swconfig_callback *cb, void *arg)
 705 {
 706         const struct switch_port *port = arg;
 707         struct nlattr *p = NULL;
 708
 709         if (!cb->nest[0]) {
 710                 cb->nest[0] = nla_nest_start(cb->msg, cb->cmd);
 711                 if (!cb->nest[0])
 712                         return -1;
 713         }
 714
 715         p = nla_nest_start(cb->msg, SWITCH_ATTR_PORT);
 716         if (!p)
 717                 goto error;
 718
 719         if (nla_put_u32(cb->msg, SWITCH_PORT_ID, port->id))
 720                 goto nla_put_failure;
 721         if (port->flags & (1 << SWITCH_PORT_FLAG_TAGGED)) {
 722                 if (nla_put_flag(cb->msg, SWITCH_PORT_FLAG_TAGGED))
 723                         goto nla_put_failure;
 724         }
 725
 726         nla_nest_end(cb->msg, p);
 727         return 0;
 728
 729 nla_put_failure:
 730                 nla_nest_cancel(cb->msg, p);
 731 error:
 732         nla_nest_cancel(cb->msg, cb->nest[0]);
 733         return -1;
 734 }
 735
 736 static int
 737 swconfig_send_ports(struct sk_buff **msg, struct genl_info *info, int attr,
 738                 const struct switch_val *val)
 739 {
 740         struct swconfig_callback cb;
 741         int err = 0;
 742         int i;
 743
 744         if (!val->value.ports)
 745                 return -EINVAL;
 746
 747         memset(&cb, 0, sizeof(cb));
 748         cb.cmd = attr;
 749         cb.msg = *msg;
 750         cb.info = info;
 751         cb.fill = swconfig_send_port;
 752         cb.close = swconfig_close_portlist;
 753
 754         cb.nest[0] = nla_nest_start(cb.msg, cb.cmd);
 755         for (i = 0; i < val->len; i++) {
 756                 err = swconfig_send_multipart(&cb, &val->value.ports[i]);
 757                 if (err)
 758                         goto done;
 759         }
 760         err = val->len;
 761         swconfig_close_portlist(&cb, NULL);
 762         *msg = cb.msg;
 763
 764 done:
 765         return err;
 766 }
 767
 768 static int
 769 swconfig_get_attr(struct sk_buff *skb, struct genl_info *info)
 770 {
 771         struct genlmsghdr *hdr = nlmsg_data(info->nlhdr);
 772         const struct switch_attr *attr;
 773         struct switch_dev *dev;
 774         struct sk_buff *msg = NULL;
 775         struct switch_val val;
 776         int err = -EINVAL;
 777         int cmd = hdr->cmd;
 778
 779         dev = swconfig_get_dev(info);
 780         if (!dev)
 781                 return -EINVAL;
 782
 783         memset(&val, 0, sizeof(val));
 784         attr = swconfig_lookup_attr(dev, info, &val);
 785         if (!attr || !attr->get)
 786                 goto error;
 787
 788         if (attr->type == SWITCH_TYPE_PORTS) {
 789                 val.value.ports = dev->portbuf;
 790                 memset(dev->portbuf, 0,
 791                         sizeof(struct switch_port) * dev->ports);
 792         }
 793
 794         err = attr->get(dev, attr, &val);
 795         if (err)
 796                 goto error;
 797
 798         msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
 799         if (!msg)
 800                 goto error;
 801
 802         hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq, &switch_fam,
 803                         0, cmd);
 804         if (IS_ERR(hdr))
 805                 goto nla_put_failure;
 806
 807         switch(attr->type) {
 808         case SWITCH_TYPE_INT:
 809                 if (nla_put_u32(msg, SWITCH_ATTR_OP_VALUE_INT, val.value.i))
 810                         goto nla_put_failure;
 811                 break;
 812         case SWITCH_TYPE_STRING:
 813                 if (nla_put_string(msg, SWITCH_ATTR_OP_VALUE_STR, val.value.s))
 814                         goto nla_put_failure;
 815                 break;
 816         case SWITCH_TYPE_PORTS:
 817                 err = swconfig_send_ports(&msg, info,
 818                                 SWITCH_ATTR_OP_VALUE_PORTS, &val);
 819                 if (err < 0)
 820                         goto nla_put_failure;
 821                 break;
 822         default:
 823                 DPRINTF("invalid type in attribute\n");
 824                 err = -EINVAL;
 825                 goto error;
 826         }
 827         err = genlmsg_end(msg, hdr);
 828         if (err < 0)
 829                 goto nla_put_failure;
 830
 831         swconfig_put_dev(dev);
 832         return genlmsg_reply(msg, info);
 833
 834 nla_put_failure:
 835         if (msg)
 836                 nlmsg_free(msg);
 837 error:
 838         swconfig_put_dev(dev);
 839         if (!err)
 840                 err = -ENOMEM;
 841         return err;
 842 }
 843
 844 static int
 845 swconfig_send_switch(struct sk_buff *msg, u32 pid, u32 seq, int flags,
 846                 const struct switch_dev *dev)
 847 {
 848         struct nlattr *p = NULL, *m = NULL;
 849         void *hdr;
 850         int i;
 851
 852         hdr = genlmsg_put(msg, pid, seq, &switch_fam, flags,
 853                         SWITCH_CMD_NEW_ATTR);
 854         if (IS_ERR(hdr))
 855                 return -1;
 856
 857         if (nla_put_u32(msg, SWITCH_ATTR_ID, dev->id))
 858                 goto nla_put_failure;
 859         if (nla_put_string(msg, SWITCH_ATTR_DEV_NAME, dev->devname))
 860                 goto nla_put_failure;
 861         if (nla_put_string(msg, SWITCH_ATTR_ALIAS, dev->alias))
 862                 goto nla_put_failure;
 863         if (nla_put_string(msg, SWITCH_ATTR_NAME, dev->name))
 864                 goto nla_put_failure;
 865         if (nla_put_u32(msg, SWITCH_ATTR_VLANS, dev->vlans))
 866                 goto nla_put_failure;
 867         if (nla_put_u32(msg, SWITCH_ATTR_PORTS, dev->ports))
 868                 goto nla_put_failure;
 869         if (nla_put_u32(msg, SWITCH_ATTR_CPU_PORT, dev->cpu_port))
 870                 goto nla_put_failure;
 871
 872         m = nla_nest_start(msg, SWITCH_ATTR_PORTMAP);
 873         if (!m)
 874                 goto nla_put_failure;
 875         for (i = 0; i < dev->ports; i++) {
 876                 p = nla_nest_start(msg, SWITCH_ATTR_PORTS);
 877                 if (!p)
 878                         continue;
 879                 if (dev->portmap[i].s) {
 880                         if (nla_put_string(msg, SWITCH_PORTMAP_SEGMENT, dev->portmap[i].s))
 881                                 goto nla_put_failure;
 882                         if (nla_put_u32(msg, SWITCH_PORTMAP_VIRT, dev->portmap[i].virt))
 883                                 goto nla_put_failure;
 884                 }
 885                 nla_nest_end(msg, p);
 886         }
 887         nla_nest_end(msg, m);
 888         return genlmsg_end(msg, hdr);
 889 nla_put_failure:
 890         genlmsg_cancel(msg, hdr);
 891         return -EMSGSIZE;
 892 }
 893
 894 static int swconfig_dump_switches(struct sk_buff *skb,
 895                 struct netlink_callback *cb)
 896 {
 897         struct switch_dev *dev;
 898         int start = cb->args[0];
 899         int idx = 0;
 900
 901         swconfig_lock();
 902         list_for_each_entry(dev, &swdevs, dev_list) {
 903                 if (++idx <= start)
 904                         continue;
 905                 if (swconfig_send_switch(skb, NETLINK_CB(cb->skb).portid,
 906                                 cb->nlh->nlmsg_seq, NLM_F_MULTI,
 907                                 dev) < 0)
 908                         break;
 909         }
 910         swconfig_unlock();
 911         cb->args[0] = idx;
 912
 913         return skb->len;
 914 }
 915
 916 static int
 917 swconfig_done(struct netlink_callback *cb)
 918 {
 919         return 0;
 920 }
 921
 922 static struct genl_ops swconfig_ops[] = {
 923         {
 924                 .cmd = SWITCH_CMD_LIST_GLOBAL,
 925                 .doit = swconfig_list_attrs,
 926                 .policy = switch_policy,
 927         },
 928         {
 929                 .cmd = SWITCH_CMD_LIST_VLAN,
 930                 .doit = swconfig_list_attrs,
 931                 .policy = switch_policy,
 932         },
 933         {
 934                 .cmd = SWITCH_CMD_LIST_PORT,
 935                 .doit = swconfig_list_attrs,
 936                 .policy = switch_policy,
 937         },
 938         {
 939                 .cmd = SWITCH_CMD_GET_GLOBAL,
 940                 .doit = swconfig_get_attr,
 941                 .policy = switch_policy,
 942         },
 943         {
 944                 .cmd = SWITCH_CMD_GET_VLAN,
 945                 .doit = swconfig_get_attr,
 946                 .policy = switch_policy,
 947         },
 948         {
 949                 .cmd = SWITCH_CMD_GET_PORT,
 950                 .doit = swconfig_get_attr,
 951                 .policy = switch_policy,
 952         },
 953         {
 954                 .cmd = SWITCH_CMD_SET_GLOBAL,
 955                 .doit = swconfig_set_attr,
 956                 .policy = switch_policy,
 957         },
 958         {
 959                 .cmd = SWITCH_CMD_SET_VLAN,
 960                 .doit = swconfig_set_attr,
 961                 .policy = switch_policy,
 962         },
 963         {
 964                 .cmd = SWITCH_CMD_SET_PORT,
 965                 .doit = swconfig_set_attr,
 966                 .policy = switch_policy,
 967         },
 968         {
 969                 .cmd = SWITCH_CMD_GET_SWITCH,
 970                 .dumpit = swconfig_dump_switches,
 971                 .policy = switch_policy,
 972                 .done = swconfig_done,
 973         }
 974 };
 975
 976 #ifdef CONFIG_OF
 977 void
 978 of_switch_load_portmap(struct switch_dev *dev)
 979 {
 980         struct device_node *port;
 981
 982         if (!dev->of_node)
 983                 return;
 984
 985         for_each_child_of_node(dev->of_node, port) {
 986                 const __be32 *prop;
 987                 const char *segment;
 988                 int size, phys;
 989
 990                 if (of_device_is_compatible(port, "swconfig,port"))
 991                         continue;
 992
 993                 if (of_property_read_string(port, "swconfig,segment", &segment))
 994                         continue;
 995
 996                 prop = of_get_property(port, "swconfig,portmap", &size);
 997                 if (!prop)
 998                         continue;
 999
1000                 if (size != (2 * sizeof(*prop))) {
1001                         pr_err("%s: failed to parse port mapping\n", port->name);
1002                         continue;
1003                 }
1004
1005                 phys = be32_to_cpup(prop++);
1006                 if ((phys < 0) | (phys >= dev->ports)) {
1007                         pr_err("%s: physical port index out of range\n", port->name);
1008                         continue;
1009                 }
1010
1011                 dev->portmap[phys].s = kstrdup(segment, GFP_KERNEL);
1012                 dev->portmap[phys].virt = be32_to_cpup(prop);
1013                 pr_debug("Found port: %s, physical: %d, virtual: %d\n",
1014                         segment, phys, dev->portmap[phys].virt);
1015         }
1016 }
1017 #endif
1018
1019 int
1020 register_switch(struct switch_dev *dev, struct net_device *netdev)
1021 {
1022         struct switch_dev *sdev;
1023         const int max_switches = 8 * sizeof(unsigned long);
1024         unsigned long in_use = 0;
1025         int err;
1026         int i;
1027
1028         INIT_LIST_HEAD(&dev->dev_list);
1029         if (netdev) {
1030                 dev->netdev = netdev;
1031                 if (!dev->alias)
1032                         dev->alias = netdev->name;
1033         }
1034         BUG_ON(!dev->alias);
1035
1036         if (dev->ports > 0) {
1037                 dev->portbuf = kzalloc(sizeof(struct switch_port) * dev->ports,
1038                                 GFP_KERNEL);
1039                 if (!dev->portbuf)
1040                         return -ENOMEM;
1041                 dev->portmap = kzalloc(sizeof(struct switch_portmap) * dev->ports,
1042                                 GFP_KERNEL);
1043                 if (!dev->portmap) {
1044                         kfree(dev->portbuf);
1045                         return -ENOMEM;
1046                 }
1047         }
1048         swconfig_defaults_init(dev);
1049         mutex_init(&dev->sw_mutex);
1050         swconfig_lock();
1051         dev->id = ++swdev_id;
1052
1053         list_for_each_entry(sdev, &swdevs, dev_list) {
1054                 if (!sscanf(sdev->devname, SWCONFIG_DEVNAME, &i))
1055                         continue;
1056                 if (i < 0 || i > max_switches)
1057                         continue;
1058
1059                 set_bit(i, &in_use);
1060         }
1061         i = find_first_zero_bit(&in_use, max_switches);
1062
1063         if (i == max_switches) {
1064                 swconfig_unlock();
1065                 return -ENFILE;
1066         }
1067
1068 #ifdef CONFIG_OF
1069         if (dev->ports)
1070                 of_switch_load_portmap(dev);
1071 #endif
1072
1073         /* fill device name */
1074         snprintf(dev->devname, IFNAMSIZ, SWCONFIG_DEVNAME, i);
1075
1076         list_add(&dev->dev_list, &swdevs);
1077         swconfig_unlock();
1078
1079         err = swconfig_create_led_trigger(dev);
1080         if (err)
1081                 return err;
1082
1083         return 0;
1084 }
1085 EXPORT_SYMBOL_GPL(register_switch);
1086
1087 void
1088 unregister_switch(struct switch_dev *dev)
1089 {
1090         swconfig_destroy_led_trigger(dev);
1091         kfree(dev->portbuf);
1092         mutex_lock(&dev->sw_mutex);
1093         swconfig_lock();
1094         list_del(&dev->dev_list);
1095         swconfig_unlock();
1096         mutex_unlock(&dev->sw_mutex);
1097 }
1098 EXPORT_SYMBOL_GPL(unregister_switch);
1099
1100
1101 static int __init
1102 swconfig_init(void)
1103 {
1104         int i, err;
1105
1106         INIT_LIST_HEAD(&swdevs);
1107         err = genl_register_family(&switch_fam);
1108         if (err)
1109                 return err;
1110
1111         for (i = 0; i < ARRAY_SIZE(swconfig_ops); i++) {
1112                 err = genl_register_ops(&switch_fam, &swconfig_ops[i]);
1113                 if (err)
1114                         goto unregister;
1115         }
1116
1117         return 0;
1118
1119 unregister:
1120         genl_unregister_family(&switch_fam);
1121         return err;
1122 }
1123
1124 static void __exit
1125 swconfig_exit(void)
1126 {
1127         genl_unregister_family(&switch_fam);
1128 }
1129
1130 module_init(swconfig_init);
1131 module_exit(swconfig_exit);
1132