package/network/config/swconfig/src/swlib.c

   1 /*
   2  * swlib.c: Switch configuration API (user space part)
   3  *
   4  * Copyright (C) 2008 Felix Fietkau <nbd@nbd.name>
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public License
   8  * version 2.1 as published by the Free Software Foundation.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  */
  15
  16 #include <stdio.h>
  17 #include <string.h>
  18 #include <stdlib.h>
  19 #include <ctype.h>
  20 #include <inttypes.h>
  21 #include <errno.h>
  22 #include <stdint.h>
  23 #include <getopt.h>
  24 #include <sys/types.h>
  25 #include <sys/socket.h>
  26 #include <linux/switch.h>
  27 #include "swlib.h"
  28 #include <netlink/netlink.h>
  29 #include <netlink/genl/genl.h>
  30 #include <netlink/genl/family.h>
  31
  32 //#define DEBUG 1
  33 #ifdef DEBUG
  34 #define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__)
  35 #else
  36 #define DPRINTF(fmt, ...) do {} while (0)
  37 #endif
  38
  39 static struct nl_sock *handle;
  40 static struct nl_cache *cache;
  41 static struct genl_family *family;
  42 static struct nlattr *tb[SWITCH_ATTR_MAX + 1];
  43 static int refcount = 0;
  44
  45 static struct nla_policy port_policy[SWITCH_ATTR_MAX] = {
  46         [SWITCH_PORT_ID] = { .type = NLA_U32 },
  47         [SWITCH_PORT_FLAG_TAGGED] = { .type = NLA_FLAG },
  48 };
  49
  50 static struct nla_policy portmap_policy[SWITCH_PORTMAP_MAX] = {
  51         [SWITCH_PORTMAP_SEGMENT] = { .type = NLA_STRING },
  52         [SWITCH_PORTMAP_VIRT] = { .type = NLA_U32 },
  53 };
  54
  55 static struct nla_policy link_policy[SWITCH_LINK_ATTR_MAX] = {
  56         [SWITCH_LINK_FLAG_LINK] = { .type = NLA_FLAG },
  57         [SWITCH_LINK_FLAG_DUPLEX] = { .type = NLA_FLAG },
  58         [SWITCH_LINK_FLAG_ANEG] = { .type = NLA_FLAG },
  59         [SWITCH_LINK_SPEED] = { .type = NLA_U32 },
  60         [SWITCH_LINK_FLAG_EEE_100BASET] = { .type = NLA_FLAG },
  61         [SWITCH_LINK_FLAG_EEE_1000BASET] = { .type = NLA_FLAG },
  62 };
  63
  64 static inline void *
  65 swlib_alloc(size_t size)
  66 {
  67         void *ptr;
  68
  69         ptr = malloc(size);
  70         if (!ptr)
  71                 goto done;
  72         memset(ptr, 0, size);
  73
  74 done:
  75         return ptr;
  76 }
  77
  78 static int
  79 wait_handler(struct nl_msg *msg, void *arg)
  80 {
  81         int *finished = arg;
  82
  83         *finished = 1;
  84         return NL_STOP;
  85 }
  86
  87 /* helper function for performing netlink requests */
  88 static int
  89 swlib_call(int cmd, int (*call)(struct nl_msg *, void *),
  90                 int (*data)(struct nl_msg *, void *), void *arg)
  91 {
  92         struct nl_msg *msg;
  93         struct nl_cb *cb = NULL;
  94         int finished;
  95         int flags = 0;
  96         int err = 0;
  97
  98         msg = nlmsg_alloc();
  99         if (!msg) {
 100                 fprintf(stderr, "Out of memory!\n");
 101                 exit(1);
 102         }
 103
 104         if (!data)
 105                 flags |= NLM_F_DUMP;
 106
 107         genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, genl_family_get_id(family), 0, flags, cmd, 0);
 108         if (data) {
 109                 err = data(msg, arg);
 110                 if (err < 0)
 111                         goto nla_put_failure;
 112         }
 113
 114         cb = nl_cb_alloc(NL_CB_CUSTOM);
 115         if (!cb) {
 116                 fprintf(stderr, "nl_cb_alloc failed.\n");
 117                 exit(1);
 118         }
 119
 120         err = nl_send_auto_complete(handle, msg);
 121         if (err < 0) {
 122                 fprintf(stderr, "nl_send_auto_complete failed: %d\n", err);
 123                 goto out;
 124         }
 125
 126         finished = 0;
 127
 128         if (call)
 129                 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, call, arg);
 130
 131         if (data)
 132                 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, wait_handler, &finished);
 133         else
 134                 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, wait_handler, &finished);
 135
 136         err = nl_recvmsgs(handle, cb);
 137         if (err < 0) {
 138                 goto out;
 139         }
 140
 141         if (!finished)
 142                 err = nl_wait_for_ack(handle);
 143
 144 out:
 145         if (cb)
 146                 nl_cb_put(cb);
 147 nla_put_failure:
 148         nlmsg_free(msg);
 149         return err;
 150 }
 151
 152 static int
 153 send_attr(struct nl_msg *msg, void *arg)
 154 {
 155         struct switch_val *val = arg;
 156         struct switch_attr *attr = val->attr;
 157
 158         NLA_PUT_U32(msg, SWITCH_ATTR_ID, attr->dev->id);
 159         NLA_PUT_U32(msg, SWITCH_ATTR_OP_ID, attr->id);
 160         switch(attr->atype) {
 161         case SWLIB_ATTR_GROUP_PORT:
 162                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_PORT, val->port_vlan);
 163                 break;
 164         case SWLIB_ATTR_GROUP_VLAN:
 165                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_VLAN, val->port_vlan);
 166                 break;
 167         default:
 168                 break;
 169         }
 170
 171         return 0;
 172
 173 nla_put_failure:
 174         return -1;
 175 }
 176
 177 static int
 178 store_port_val(struct nl_msg *msg, struct nlattr *nla, struct switch_val *val)
 179 {
 180         struct nlattr *p;
 181         int ports = val->attr->dev->ports;
 182         int err = 0;
 183         int remaining;
 184
 185         if (!val->value.ports)
 186                 val->value.ports = malloc(sizeof(struct switch_port) * ports);
 187
 188         nla_for_each_nested(p, nla, remaining) {
 189                 struct nlattr *tb[SWITCH_PORT_ATTR_MAX+1];
 190                 struct switch_port *port;
 191
 192                 if (val->len >= ports)
 193                         break;
 194
 195                 err = nla_parse_nested(tb, SWITCH_PORT_ATTR_MAX, p, port_policy);
 196                 if (err < 0)
 197                         goto out;
 198
 199                 if (!tb[SWITCH_PORT_ID])
 200                         continue;
 201
 202                 port = &val->value.ports[val->len];
 203                 port->id = nla_get_u32(tb[SWITCH_PORT_ID]);
 204                 port->flags = 0;
 205                 if (tb[SWITCH_PORT_FLAG_TAGGED])
 206                         port->flags |= SWLIB_PORT_FLAG_TAGGED;
 207
 208                 val->len++;
 209         }
 210
 211 out:
 212         return err;
 213 }
 214
 215 static int
 216 store_link_val(struct nl_msg *msg, struct nlattr *nla, struct switch_val *val)
 217 {
 218         struct nlattr *tb[SWITCH_LINK_ATTR_MAX + 1];
 219         struct switch_port_link *link;
 220         int err = 0;
 221
 222         if (!val->value.link)
 223                 val->value.link = malloc(sizeof(struct switch_port_link));
 224
 225         err = nla_parse_nested(tb, SWITCH_LINK_ATTR_MAX, nla, link_policy);
 226         if (err < 0)
 227                 goto out;
 228
 229         link = val->value.link;
 230         link->link = !!tb[SWITCH_LINK_FLAG_LINK];
 231         link->duplex = !!tb[SWITCH_LINK_FLAG_DUPLEX];
 232         link->aneg = !!tb[SWITCH_LINK_FLAG_ANEG];
 233         link->tx_flow = !!tb[SWITCH_LINK_FLAG_TX_FLOW];
 234         link->rx_flow = !!tb[SWITCH_LINK_FLAG_RX_FLOW];
 235         link->speed = nla_get_u32(tb[SWITCH_LINK_SPEED]);
 236         link->eee = 0;
 237         if (tb[SWITCH_LINK_FLAG_EEE_100BASET])
 238                 link->eee |= SWLIB_LINK_FLAG_EEE_100BASET;
 239         if (tb[SWITCH_LINK_FLAG_EEE_1000BASET])
 240                 link->eee |= SWLIB_LINK_FLAG_EEE_1000BASET;
 241
 242 out:
 243         return err;
 244 }
 245
 246 static int
 247 store_val(struct nl_msg *msg, void *arg)
 248 {
 249         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 250         struct switch_val *val = arg;
 251
 252         if (!val)
 253                 goto error;
 254
 255         if (nla_parse(tb, SWITCH_ATTR_MAX - 1, genlmsg_attrdata(gnlh, 0),
 256                         genlmsg_attrlen(gnlh, 0), NULL) < 0) {
 257                 goto error;
 258         }
 259
 260         if (tb[SWITCH_ATTR_OP_VALUE_INT])
 261                 val->value.i = nla_get_u32(tb[SWITCH_ATTR_OP_VALUE_INT]);
 262         else if (tb[SWITCH_ATTR_OP_VALUE_STR])
 263                 val->value.s = strdup(nla_get_string(tb[SWITCH_ATTR_OP_VALUE_STR]));
 264         else if (tb[SWITCH_ATTR_OP_VALUE_PORTS])
 265                 val->err = store_port_val(msg, tb[SWITCH_ATTR_OP_VALUE_PORTS], val);
 266         else if (tb[SWITCH_ATTR_OP_VALUE_LINK])
 267                 val->err = store_link_val(msg, tb[SWITCH_ATTR_OP_VALUE_LINK], val);
 268
 269         val->err = 0;
 270         return 0;
 271
 272 error:
 273         return NL_SKIP;
 274 }
 275
 276 int
 277 swlib_get_attr(struct switch_dev *dev, struct switch_attr *attr, struct switch_val *val)
 278 {
 279         int cmd;
 280         int err;
 281
 282         switch(attr->atype) {
 283         case SWLIB_ATTR_GROUP_GLOBAL:
 284                 cmd = SWITCH_CMD_GET_GLOBAL;
 285                 break;
 286         case SWLIB_ATTR_GROUP_PORT:
 287                 cmd = SWITCH_CMD_GET_PORT;
 288                 break;
 289         case SWLIB_ATTR_GROUP_VLAN:
 290                 cmd = SWITCH_CMD_GET_VLAN;
 291                 break;
 292         default:
 293                 return -EINVAL;
 294         }
 295
 296         memset(&val->value, 0, sizeof(val->value));
 297         val->len = 0;
 298         val->attr = attr;
 299         val->err = -EINVAL;
 300         err = swlib_call(cmd, store_val, send_attr, val);
 301         if (!err)
 302                 err = val->err;
 303
 304         return err;
 305 }
 306
 307 static int
 308 send_attr_ports(struct nl_msg *msg, struct switch_val *val)
 309 {
 310         struct nlattr *n;
 311         int i;
 312
 313         /* TODO implement multipart? */
 314         if (val->len == 0)
 315                 goto done;
 316         n = nla_nest_start(msg, SWITCH_ATTR_OP_VALUE_PORTS);
 317         if (!n)
 318                 goto nla_put_failure;
 319         for (i = 0; i < val->len; i++) {
 320                 struct switch_port *port = &val->value.ports[i];
 321                 struct nlattr *np;
 322
 323                 np = nla_nest_start(msg, SWITCH_ATTR_PORT);
 324                 if (!np)
 325                         goto nla_put_failure;
 326
 327                 NLA_PUT_U32(msg, SWITCH_PORT_ID, port->id);
 328                 if (port->flags & SWLIB_PORT_FLAG_TAGGED)
 329                         NLA_PUT_FLAG(msg, SWITCH_PORT_FLAG_TAGGED);
 330
 331                 nla_nest_end(msg, np);
 332         }
 333         nla_nest_end(msg, n);
 334 done:
 335         return 0;
 336
 337 nla_put_failure:
 338         return -1;
 339 }
 340
 341 static int
 342 send_attr_link(struct nl_msg *msg, struct switch_val *val)
 343 {
 344         struct switch_port_link *link = val->value.link;
 345         struct nlattr *n;
 346
 347         n = nla_nest_start(msg, SWITCH_ATTR_OP_VALUE_LINK);
 348         if (!n)
 349                 goto nla_put_failure;
 350
 351         if (link->duplex)
 352                 NLA_PUT_FLAG(msg, SWITCH_LINK_FLAG_DUPLEX);
 353         if (link->aneg)
 354                 NLA_PUT_FLAG(msg, SWITCH_LINK_FLAG_ANEG);
 355         NLA_PUT_U32(msg, SWITCH_LINK_SPEED, link->speed);
 356
 357         nla_nest_end(msg, n);
 358
 359         return 0;
 360
 361 nla_put_failure:
 362         return -1;
 363 }
 364
 365 static int
 366 send_attr_val(struct nl_msg *msg, void *arg)
 367 {
 368         struct switch_val *val = arg;
 369         struct switch_attr *attr = val->attr;
 370
 371         if (send_attr(msg, arg))
 372                 goto nla_put_failure;
 373
 374         switch(attr->type) {
 375         case SWITCH_TYPE_NOVAL:
 376                 break;
 377         case SWITCH_TYPE_INT:
 378                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_VALUE_INT, val->value.i);
 379                 break;
 380         case SWITCH_TYPE_STRING:
 381                 if (!val->value.s)
 382                         goto nla_put_failure;
 383                 NLA_PUT_STRING(msg, SWITCH_ATTR_OP_VALUE_STR, val->value.s);
 384                 break;
 385         case SWITCH_TYPE_PORTS:
 386                 if (send_attr_ports(msg, val) < 0)
 387                         goto nla_put_failure;
 388                 break;
 389         case SWITCH_TYPE_LINK:
 390                 if (send_attr_link(msg, val))
 391                         goto nla_put_failure;
 392                 break;
 393         default:
 394                 goto nla_put_failure;
 395         }
 396         return 0;
 397
 398 nla_put_failure:
 399         return -1;
 400 }
 401
 402 int
 403 swlib_set_attr(struct switch_dev *dev, struct switch_attr *attr, struct switch_val *val)
 404 {
 405         int cmd;
 406
 407         switch(attr->atype) {
 408         case SWLIB_ATTR_GROUP_GLOBAL:
 409                 cmd = SWITCH_CMD_SET_GLOBAL;
 410                 break;
 411         case SWLIB_ATTR_GROUP_PORT:
 412                 cmd = SWITCH_CMD_SET_PORT;
 413                 break;
 414         case SWLIB_ATTR_GROUP_VLAN:
 415                 cmd = SWITCH_CMD_SET_VLAN;
 416                 break;
 417         default:
 418                 return -EINVAL;
 419         }
 420
 421         val->attr = attr;
 422         return swlib_call(cmd, NULL, send_attr_val, val);
 423 }
 424
 425 enum {
 426         CMD_NONE,
 427         CMD_DUPLEX,
 428         CMD_ANEG,
 429         CMD_SPEED,
 430 };
 431
 432 int swlib_set_attr_string(struct switch_dev *dev, struct switch_attr *a, int port_vlan, const char *str)
 433 {
 434         struct switch_port *ports;
 435         struct switch_port_link *link;
 436         struct switch_val val;
 437         char *ptr;
 438         int cmd = CMD_NONE;
 439
 440         memset(&val, 0, sizeof(val));
 441         val.port_vlan = port_vlan;
 442         switch(a->type) {
 443         case SWITCH_TYPE_INT:
 444                 val.value.i = atoi(str);
 445                 break;
 446         case SWITCH_TYPE_STRING:
 447                 val.value.s = (char *)str;
 448                 break;
 449         case SWITCH_TYPE_PORTS:
 450                 ports = alloca(sizeof(struct switch_port) * dev->ports);
 451                 memset(ports, 0, sizeof(struct switch_port) * dev->ports);
 452                 val.len = 0;
 453                 ptr = (char *)str;
 454                 while(ptr && *ptr)
 455                 {
 456                         while(*ptr && isspace(*ptr))
 457                                 ptr++;
 458
 459                         if (!*ptr)
 460                                 break;
 461
 462                         if (!isdigit(*ptr))
 463                                 return -1;
 464
 465                         if (val.len >= dev->ports)
 466                                 return -1;
 467
 468                         ports[val.len].flags = 0;
 469                         ports[val.len].id = strtoul(ptr, &ptr, 10);
 470                         while(*ptr && !isspace(*ptr)) {
 471                                 if (*ptr == 't')
 472                                         ports[val.len].flags |= SWLIB_PORT_FLAG_TAGGED;
 473                                 else
 474                                         return -1;
 475
 476                                 ptr++;
 477                         }
 478                         if (*ptr)
 479                                 ptr++;
 480                         val.len++;
 481                 }
 482                 val.value.ports = ports;
 483                 break;
 484         case SWITCH_TYPE_LINK:
 485                 link = malloc(sizeof(struct switch_port_link));
 486                 memset(link, 0, sizeof(struct switch_port_link));
 487                 ptr = (char *)str;
 488                 for (ptr = strtok(ptr," "); ptr; ptr = strtok(NULL, " ")) {
 489                         switch (cmd) {
 490                         case CMD_NONE:
 491                                 if (!strcmp(ptr, "duplex"))
 492                                         cmd = CMD_DUPLEX;
 493                                 else if (!strcmp(ptr, "autoneg"))
 494                                         cmd = CMD_ANEG;
 495                                 else if (!strcmp(ptr, "speed"))
 496                                         cmd = CMD_SPEED;
 497                                 else
 498                                         fprintf(stderr, "Unsupported option %s\n", ptr);
 499                                 break;
 500                         case CMD_DUPLEX:
 501                                 if (!strcmp(ptr, "half"))
 502                                         link->duplex = 0;
 503                                 else if (!strcmp(ptr, "full"))
 504                                         link->duplex = 1;
 505                                 else
 506                                         fprintf(stderr, "Unsupported value %s\n", ptr);
 507                                 cmd = CMD_NONE;
 508                                 break;
 509                         case CMD_ANEG:
 510                                 if (!strcmp(ptr, "on"))
 511                                         link->aneg = 1;
 512                                 else if (!strcmp(ptr, "off"))
 513                                         link->aneg = 0;
 514                                 else
 515                                         fprintf(stderr, "Unsupported value %s\n", ptr);
 516                                 cmd = CMD_NONE;
 517                                 break;
 518                         case CMD_SPEED:
 519                                 link->speed = atoi(ptr);
 520                                 cmd = CMD_NONE;
 521                                 break;
 522                         }
 523                 }
 524                 val.value.link = link;
 525                 break;
 526         case SWITCH_TYPE_NOVAL:
 527                 if (str && !strcmp(str, "0"))
 528                         return 0;
 529
 530                 break;
 531         default:
 532                 return -1;
 533         }
 534         return swlib_set_attr(dev, a, &val);
 535 }
 536
 537
 538 struct attrlist_arg {
 539         int id;
 540         int atype;
 541         struct switch_dev *dev;
 542         struct switch_attr *prev;
 543         struct switch_attr **head;
 544 };
 545
 546 static int
 547 add_id(struct nl_msg *msg, void *arg)
 548 {
 549         struct attrlist_arg *l = arg;
 550
 551         NLA_PUT_U32(msg, SWITCH_ATTR_ID, l->id);
 552
 553         return 0;
 554 nla_put_failure:
 555         return -1;
 556 }
 557
 558 static int
 559 add_attr(struct nl_msg *msg, void *ptr)
 560 {
 561         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 562         struct attrlist_arg *arg = ptr;
 563         struct switch_attr *new;
 564
 565         if (nla_parse(tb, SWITCH_ATTR_MAX - 1, genlmsg_attrdata(gnlh, 0),
 566                         genlmsg_attrlen(gnlh, 0), NULL) < 0)
 567                 goto done;
 568
 569         new = swlib_alloc(sizeof(struct switch_attr));
 570         if (!new)
 571                 goto done;
 572
 573         new->dev = arg->dev;
 574         new->atype = arg->atype;
 575         if (arg->prev) {
 576                 arg->prev->next = new;
 577         } else {
 578                 arg->prev = *arg->head;
 579         }
 580         *arg->head = new;
 581         arg->head = &new->next;
 582
 583         if (tb[SWITCH_ATTR_OP_ID])
 584                 new->id = nla_get_u32(tb[SWITCH_ATTR_OP_ID]);
 585         if (tb[SWITCH_ATTR_OP_TYPE])
 586                 new->type = nla_get_u32(tb[SWITCH_ATTR_OP_TYPE]);
 587         if (tb[SWITCH_ATTR_OP_NAME])
 588                 new->name = strdup(nla_get_string(tb[SWITCH_ATTR_OP_NAME]));
 589         if (tb[SWITCH_ATTR_OP_DESCRIPTION])
 590                 new->description = strdup(nla_get_string(tb[SWITCH_ATTR_OP_DESCRIPTION]));
 591
 592 done:
 593         return NL_SKIP;
 594 }
 595
 596 int
 597 swlib_scan(struct switch_dev *dev)
 598 {
 599         struct attrlist_arg arg;
 600
 601         if (dev->ops || dev->port_ops || dev->vlan_ops)
 602                 return 0;
 603
 604         arg.atype = SWLIB_ATTR_GROUP_GLOBAL;
 605         arg.dev = dev;
 606         arg.id = dev->id;
 607         arg.prev = NULL;
 608         arg.head = &dev->ops;
 609         swlib_call(SWITCH_CMD_LIST_GLOBAL, add_attr, add_id, &arg);
 610
 611         arg.atype = SWLIB_ATTR_GROUP_PORT;
 612         arg.prev = NULL;
 613         arg.head = &dev->port_ops;
 614         swlib_call(SWITCH_CMD_LIST_PORT, add_attr, add_id, &arg);
 615
 616         arg.atype = SWLIB_ATTR_GROUP_VLAN;
 617         arg.prev = NULL;
 618         arg.head = &dev->vlan_ops;
 619         swlib_call(SWITCH_CMD_LIST_VLAN, add_attr, add_id, &arg);
 620
 621         return 0;
 622 }
 623
 624 struct switch_attr *swlib_lookup_attr(struct switch_dev *dev,
 625                 enum swlib_attr_group atype, const char *name)
 626 {
 627         struct switch_attr *head;
 628
 629         if (!name || !dev)
 630                 return NULL;
 631
 632         switch(atype) {
 633         case SWLIB_ATTR_GROUP_GLOBAL:
 634                 head = dev->ops;
 635                 break;
 636         case SWLIB_ATTR_GROUP_PORT:
 637                 head = dev->port_ops;
 638                 break;
 639         case SWLIB_ATTR_GROUP_VLAN:
 640                 head = dev->vlan_ops;
 641                 break;
 642         }
 643         while(head) {
 644                 if (!strcmp(name, head->name))
 645                         return head;
 646                 head = head->next;
 647         }
 648
 649         return NULL;
 650 }
 651
 652 static void
 653 swlib_priv_free(void)
 654 {
 655         if (family)
 656                 nl_object_put((struct nl_object*)family);
 657         if (cache)
 658                 nl_cache_free(cache);
 659         if (handle)
 660                 nl_socket_free(handle);
 661         family = NULL;
 662         handle = NULL;
 663         cache = NULL;
 664 }
 665
 666 static int
 667 swlib_priv_init(void)
 668 {
 669         int ret;
 670
 671         handle = nl_socket_alloc();
 672         if (!handle) {
 673                 DPRINTF("Failed to create handle\n");
 674                 goto err;
 675         }
 676
 677         if (genl_connect(handle)) {
 678                 DPRINTF("Failed to connect to generic netlink\n");
 679                 goto err;
 680         }
 681
 682         ret = genl_ctrl_alloc_cache(handle, &cache);
 683         if (ret < 0) {
 684                 DPRINTF("Failed to allocate netlink cache\n");
 685                 goto err;
 686         }
 687
 688         family = genl_ctrl_search_by_name(cache, "switch");
 689         if (!family) {
 690                 DPRINTF("Switch API not present\n");
 691                 goto err;
 692         }
 693         return 0;
 694
 695 err:
 696         swlib_priv_free();
 697         return -EINVAL;
 698 }
 699
 700 struct swlib_scan_arg {
 701         const char *name;
 702         struct switch_dev *head;
 703         struct switch_dev *ptr;
 704 };
 705
 706 static int
 707 add_port_map(struct switch_dev *dev, struct nlattr *nla)
 708 {
 709         struct nlattr *p;
 710         int err = 0, idx = 0;
 711         int remaining;
 712
 713         dev->maps = malloc(sizeof(struct switch_portmap) * dev->ports);
 714         if (!dev->maps)
 715                 return -1;
 716         memset(dev->maps, 0, sizeof(struct switch_portmap) * dev->ports);
 717
 718         nla_for_each_nested(p, nla, remaining) {
 719                 struct nlattr *tb[SWITCH_PORTMAP_MAX+1];
 720
 721                 if (idx >= dev->ports)
 722                         continue;
 723
 724                 err = nla_parse_nested(tb, SWITCH_PORTMAP_MAX, p, portmap_policy);
 725                 if (err < 0)
 726                         continue;
 727
 728
 729                 if (tb[SWITCH_PORTMAP_SEGMENT] && tb[SWITCH_PORTMAP_VIRT]) {
 730                         dev->maps[idx].segment = strdup(nla_get_string(tb[SWITCH_PORTMAP_SEGMENT]));
 731                         dev->maps[idx].virt = nla_get_u32(tb[SWITCH_PORTMAP_VIRT]);
 732                 }
 733                 idx++;
 734         }
 735
 736 out:
 737         return err;
 738 }
 739
 740
 741 static int
 742 add_switch(struct nl_msg *msg, void *arg)
 743 {
 744         struct swlib_scan_arg *sa = arg;
 745         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 746         struct switch_dev *dev;
 747         const char *name;
 748         const char *alias;
 749
 750         if (nla_parse(tb, SWITCH_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL) < 0)
 751                 goto done;
 752
 753         if (!tb[SWITCH_ATTR_DEV_NAME])
 754                 goto done;
 755
 756         name = nla_get_string(tb[SWITCH_ATTR_DEV_NAME]);
 757         alias = nla_get_string(tb[SWITCH_ATTR_ALIAS]);
 758
 759         if (sa->name && (strcmp(name, sa->name) != 0) && (strcmp(alias, sa->name) != 0))
 760                 goto done;
 761
 762         dev = swlib_alloc(sizeof(struct switch_dev));
 763         if (!dev)
 764                 goto done;
 765
 766         strncpy(dev->dev_name, name, IFNAMSIZ - 1);
 767         dev->alias = strdup(alias);
 768         if (tb[SWITCH_ATTR_ID])
 769                 dev->id = nla_get_u32(tb[SWITCH_ATTR_ID]);
 770         if (tb[SWITCH_ATTR_NAME])
 771                 dev->name = strdup(nla_get_string(tb[SWITCH_ATTR_NAME]));
 772         if (tb[SWITCH_ATTR_PORTS])
 773                 dev->ports = nla_get_u32(tb[SWITCH_ATTR_PORTS]);
 774         if (tb[SWITCH_ATTR_VLANS])
 775                 dev->vlans = nla_get_u32(tb[SWITCH_ATTR_VLANS]);
 776         if (tb[SWITCH_ATTR_CPU_PORT])
 777                 dev->cpu_port = nla_get_u32(tb[SWITCH_ATTR_CPU_PORT]);
 778         if (tb[SWITCH_ATTR_PORTMAP])
 779                 add_port_map(dev, tb[SWITCH_ATTR_PORTMAP]);
 780
 781         if (!sa->head) {
 782                 sa->head = dev;
 783                 sa->ptr = dev;
 784         } else {
 785                 sa->ptr->next = dev;
 786                 sa->ptr = dev;
 787         }
 788
 789         refcount++;
 790 done:
 791         return NL_SKIP;
 792 }
 793
 794 static int
 795 list_switch(struct nl_msg *msg, void *arg)
 796 {
 797         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 798
 799         if (nla_parse(tb, SWITCH_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL) < 0)
 800                 goto done;
 801
 802         if (!tb[SWITCH_ATTR_DEV_NAME] || !tb[SWITCH_ATTR_NAME])
 803                 goto done;
 804
 805         printf("Found: %s - %s\n", nla_get_string(tb[SWITCH_ATTR_DEV_NAME]),
 806                 nla_get_string(tb[SWITCH_ATTR_ALIAS]));
 807
 808 done:
 809         return NL_SKIP;
 810 }
 811
 812 void
 813 swlib_list(void)
 814 {
 815         if (swlib_priv_init() < 0)
 816                 return;
 817         swlib_call(SWITCH_CMD_GET_SWITCH, list_switch, NULL, NULL);
 818         swlib_priv_free();
 819 }
 820
 821 void
 822 swlib_print_portmap(struct switch_dev *dev, char *segment)
 823 {
 824         int i;
 825
 826         if (segment) {
 827                 if (!strcmp(segment, "cpu")) {
 828                         printf("%d ", dev->cpu_port);
 829                 } else if (!strcmp(segment, "disabled")) {
 830                         for (i = 0; i < dev->ports; i++)
 831                                 if (!dev->maps[i].segment)
 832                                         printf("%d ", i);
 833                 } else for (i = 0; i < dev->ports; i++) {
 834                         if (dev->maps[i].segment && !strcmp(dev->maps[i].segment, segment))
 835                                 printf("%d ", i);
 836                 }
 837         } else {
 838                 printf("%s - %s\n", dev->dev_name, dev->name);
 839                 for (i = 0; i < dev->ports; i++)
 840                         if (i == dev->cpu_port)
 841                                 printf("port%d:\tcpu\n", i);
 842                         else if (dev->maps[i].segment)
 843                                 printf("port%d:\t%s.%d\n", i, dev->maps[i].segment, dev->maps[i].virt);
 844                         else
 845                                 printf("port%d:\tdisabled\n", i);
 846         }
 847 }
 848
 849 struct switch_dev *
 850 swlib_connect(const char *name)
 851 {
 852         struct swlib_scan_arg arg;
 853
 854         if (!refcount) {
 855                 if (swlib_priv_init() < 0)
 856                         return NULL;
 857         };
 858
 859         arg.head = NULL;
 860         arg.ptr = NULL;
 861         arg.name = name;
 862         swlib_call(SWITCH_CMD_GET_SWITCH, add_switch, NULL, &arg);
 863
 864         if (!refcount)
 865                 swlib_priv_free();
 866
 867         return arg.head;
 868 }
 869
 870 static void
 871 swlib_free_attributes(struct switch_attr **head)
 872 {
 873         struct switch_attr *a = *head;
 874         struct switch_attr *next;
 875
 876         while (a) {
 877                 next = a->next;
 878                 free(a->name);
 879                 free(a->description);
 880                 free(a);
 881                 a = next;
 882         }
 883         *head = NULL;
 884 }
 885
 886 static void
 887 swlib_free_port_map(struct switch_dev *dev)
 888 {
 889         int i;
 890
 891         if (!dev || !dev->maps)
 892                 return;
 893
 894         for (i = 0; i < dev->ports; i++)
 895                 free(dev->maps[i].segment);
 896         free(dev->maps);
 897 }
 898
 899 void
 900 swlib_free(struct switch_dev *dev)
 901 {
 902         swlib_free_attributes(&dev->ops);
 903         swlib_free_attributes(&dev->port_ops);
 904         swlib_free_attributes(&dev->vlan_ops);
 905         swlib_free_port_map(dev);
 906         free(dev->name);
 907         free(dev->alias);
 908         free(dev);
 909
 910         if (--refcount == 0)
 911                 swlib_priv_free();
 912 }
 913
 914 void
 915 swlib_free_all(struct switch_dev *dev)
 916 {
 917         struct switch_dev *p;
 918
 919         while (dev) {
 920                 p = dev->next;
 921                 swlib_free(dev);
 922                 dev = p;
 923         }
 924 }