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@openwrt.org>
   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 <inttypes.h>
  20 #include <errno.h>
  21 #include <stdint.h>
  22 #include <getopt.h>
  23 #include <sys/types.h>
  24 #include <sys/socket.h>
  25 #include <linux/switch.h>
  26 #include "swlib.h"
  27 #include <netlink/netlink.h>
  28 #include <netlink/genl/genl.h>
  29 #include <netlink/genl/family.h>
  30
  31 //#define DEBUG 1
  32 #ifdef DEBUG
  33 #define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__)
  34 #else
  35 #define DPRINTF(fmt, ...) do {} while (0)
  36 #endif
  37
  38 static struct nl_sock *handle;
  39 static struct nl_cache *cache;
  40 static struct genl_family *family;
  41 static struct nlattr *tb[SWITCH_ATTR_MAX + 1];
  42 static int refcount = 0;
  43
  44 static struct nla_policy port_policy[] = {
  45         [SWITCH_PORT_ID] = { .type = NLA_U32 },
  46         [SWITCH_PORT_FLAG_TAGGED] = { .type = NLA_FLAG },
  47 };
  48
  49 static inline void *
  50 swlib_alloc(size_t size)
  51 {
  52         void *ptr;
  53
  54         ptr = malloc(size);
  55         if (!ptr)
  56                 goto done;
  57         memset(ptr, 0, size);
  58
  59 done:
  60         return ptr;
  61 }
  62
  63 static int
  64 wait_handler(struct nl_msg *msg, void *arg)
  65 {
  66         int *finished = arg;
  67
  68         *finished = 1;
  69         return NL_STOP;
  70 }
  71
  72 /* helper function for performing netlink requests */
  73 static int
  74 swlib_call(int cmd, int (*call)(struct nl_msg *, void *),
  75                 int (*data)(struct nl_msg *, void *), void *arg)
  76 {
  77         struct nl_msg *msg;
  78         struct nl_cb *cb = NULL;
  79         int finished;
  80         int flags = 0;
  81         int err;
  82
  83         msg = nlmsg_alloc();
  84         if (!msg) {
  85                 fprintf(stderr, "Out of memory!\n");
  86                 exit(1);
  87         }
  88
  89         if (!data)
  90                 flags |= NLM_F_DUMP;
  91
  92         genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, genl_family_get_id(family), 0, flags, cmd, 0);
  93         if (data) {
  94                 if (data(msg, arg) < 0)
  95                         goto nla_put_failure;
  96         }
  97
  98         cb = nl_cb_alloc(NL_CB_CUSTOM);
  99         if (!cb) {
 100                 fprintf(stderr, "nl_cb_alloc failed.\n");
 101                 exit(1);
 102         }
 103
 104         err = nl_send_auto_complete(handle, msg);
 105         if (err < 0) {
 106                 fprintf(stderr, "nl_send_auto_complete failed: %d\n", err);
 107                 goto out;
 108         }
 109
 110         finished = 0;
 111
 112         if (call)
 113                 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, call, arg);
 114
 115         if (data)
 116                 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, wait_handler, &finished);
 117         else
 118                 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, wait_handler, &finished);
 119
 120         err = nl_recvmsgs(handle, cb);
 121         if (err < 0) {
 122                 goto out;
 123         }
 124
 125         if (!finished)
 126                 err = nl_wait_for_ack(handle);
 127
 128 out:
 129         if (cb)
 130                 nl_cb_put(cb);
 131 nla_put_failure:
 132         nlmsg_free(msg);
 133         return err;
 134 }
 135
 136 static int
 137 send_attr(struct nl_msg *msg, void *arg)
 138 {
 139         struct switch_val *val = arg;
 140         struct switch_attr *attr = val->attr;
 141
 142         NLA_PUT_U32(msg, SWITCH_ATTR_ID, attr->dev->id);
 143         NLA_PUT_U32(msg, SWITCH_ATTR_OP_ID, attr->id);
 144         switch(attr->atype) {
 145         case SWLIB_ATTR_GROUP_PORT:
 146                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_PORT, val->port_vlan);
 147                 break;
 148         case SWLIB_ATTR_GROUP_VLAN:
 149                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_VLAN, val->port_vlan);
 150                 break;
 151         default:
 152                 break;
 153         }
 154
 155         return 0;
 156
 157 nla_put_failure:
 158         return -1;
 159 }
 160
 161 static int
 162 store_port_val(struct nl_msg *msg, struct nlattr *nla, struct switch_val *val)
 163 {
 164         struct nlattr *p;
 165         int ports = val->attr->dev->ports;
 166         int err = 0;
 167         int remaining;
 168
 169         if (!val->value.ports)
 170                 val->value.ports = malloc(sizeof(struct switch_port) * ports);
 171
 172         nla_for_each_nested(p, nla, remaining) {
 173                 struct nlattr *tb[SWITCH_PORT_ATTR_MAX+1];
 174                 struct switch_port *port;
 175
 176                 if (val->len >= ports)
 177                         break;
 178
 179                 err = nla_parse_nested(tb, SWITCH_PORT_ATTR_MAX, p, port_policy);
 180                 if (err < 0)
 181                         goto out;
 182
 183                 if (!tb[SWITCH_PORT_ID])
 184                         continue;
 185
 186                 port = &val->value.ports[val->len];
 187                 port->id = nla_get_u32(tb[SWITCH_PORT_ID]);
 188                 port->flags = 0;
 189                 if (tb[SWITCH_PORT_FLAG_TAGGED])
 190                         port->flags |= SWLIB_PORT_FLAG_TAGGED;
 191
 192                 val->len++;
 193         }
 194
 195 out:
 196         return err;
 197 }
 198
 199 static int
 200 store_val(struct nl_msg *msg, void *arg)
 201 {
 202         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 203         struct switch_val *val = arg;
 204         struct switch_attr *attr = val->attr;
 205
 206         if (!val)
 207                 goto error;
 208
 209         if (nla_parse(tb, SWITCH_ATTR_MAX - 1, genlmsg_attrdata(gnlh, 0),
 210                         genlmsg_attrlen(gnlh, 0), NULL) < 0) {
 211                 goto error;
 212         }
 213
 214         if (tb[SWITCH_ATTR_OP_VALUE_INT])
 215                 val->value.i = nla_get_u32(tb[SWITCH_ATTR_OP_VALUE_INT]);
 216         else if (tb[SWITCH_ATTR_OP_VALUE_STR])
 217                 val->value.s = strdup(nla_get_string(tb[SWITCH_ATTR_OP_VALUE_STR]));
 218         else if (tb[SWITCH_ATTR_OP_VALUE_PORTS])
 219                 val->err = store_port_val(msg, tb[SWITCH_ATTR_OP_VALUE_PORTS], val);
 220
 221         val->err = 0;
 222         return 0;
 223
 224 error:
 225         return NL_SKIP;
 226 }
 227
 228 int
 229 swlib_get_attr(struct switch_dev *dev, struct switch_attr *attr, struct switch_val *val)
 230 {
 231         int cmd;
 232         int err;
 233
 234         switch(attr->atype) {
 235         case SWLIB_ATTR_GROUP_GLOBAL:
 236                 cmd = SWITCH_CMD_GET_GLOBAL;
 237                 break;
 238         case SWLIB_ATTR_GROUP_PORT:
 239                 cmd = SWITCH_CMD_GET_PORT;
 240                 break;
 241         case SWLIB_ATTR_GROUP_VLAN:
 242                 cmd = SWITCH_CMD_GET_VLAN;
 243                 break;
 244         default:
 245                 return -EINVAL;
 246         }
 247
 248         memset(&val->value, 0, sizeof(val->value));
 249         val->len = 0;
 250         val->attr = attr;
 251         val->err = -EINVAL;
 252         err = swlib_call(cmd, store_val, send_attr, val);
 253         if (!err)
 254                 err = val->err;
 255
 256         return err;
 257 }
 258
 259 static int
 260 send_attr_ports(struct nl_msg *msg, struct switch_val *val)
 261 {
 262         struct nlattr *n;
 263         int i;
 264
 265         /* TODO implement multipart? */
 266         if (val->len == 0)
 267                 goto done;
 268         n = nla_nest_start(msg, SWITCH_ATTR_OP_VALUE_PORTS);
 269         if (!n)
 270                 goto nla_put_failure;
 271         for (i = 0; i < val->len; i++) {
 272                 struct switch_port *port = &val->value.ports[i];
 273                 struct nlattr *np;
 274
 275                 np = nla_nest_start(msg, SWITCH_ATTR_PORT);
 276                 if (!np)
 277                         goto nla_put_failure;
 278
 279                 NLA_PUT_U32(msg, SWITCH_PORT_ID, port->id);
 280                 if (port->flags & SWLIB_PORT_FLAG_TAGGED)
 281                         NLA_PUT_FLAG(msg, SWITCH_PORT_FLAG_TAGGED);
 282
 283                 nla_nest_end(msg, np);
 284         }
 285         nla_nest_end(msg, n);
 286 done:
 287         return 0;
 288
 289 nla_put_failure:
 290         return -1;
 291 }
 292
 293 static int
 294 send_attr_val(struct nl_msg *msg, void *arg)
 295 {
 296         struct switch_val *val = arg;
 297         struct switch_attr *attr = val->attr;
 298
 299         if (send_attr(msg, arg))
 300                 goto nla_put_failure;
 301
 302         switch(attr->type) {
 303         case SWITCH_TYPE_NOVAL:
 304                 break;
 305         case SWITCH_TYPE_INT:
 306                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_VALUE_INT, val->value.i);
 307                 break;
 308         case SWITCH_TYPE_STRING:
 309                 if (!val->value.s)
 310                         goto nla_put_failure;
 311                 NLA_PUT_STRING(msg, SWITCH_ATTR_OP_VALUE_STR, val->value.s);
 312                 break;
 313         case SWITCH_TYPE_PORTS:
 314                 if (send_attr_ports(msg, val) < 0)
 315                         goto nla_put_failure;
 316                 break;
 317         default:
 318                 goto nla_put_failure;
 319         }
 320         return 0;
 321
 322 nla_put_failure:
 323         return -1;
 324 }
 325
 326 int
 327 swlib_set_attr(struct switch_dev *dev, struct switch_attr *attr, struct switch_val *val)
 328 {
 329         int cmd;
 330
 331         switch(attr->atype) {
 332         case SWLIB_ATTR_GROUP_GLOBAL:
 333                 cmd = SWITCH_CMD_SET_GLOBAL;
 334                 break;
 335         case SWLIB_ATTR_GROUP_PORT:
 336                 cmd = SWITCH_CMD_SET_PORT;
 337                 break;
 338         case SWLIB_ATTR_GROUP_VLAN:
 339                 cmd = SWITCH_CMD_SET_VLAN;
 340                 break;
 341         default:
 342                 return -EINVAL;
 343         }
 344
 345         val->attr = attr;
 346         return swlib_call(cmd, NULL, send_attr_val, val);
 347 }
 348
 349 int swlib_set_attr_string(struct switch_dev *dev, struct switch_attr *a, int port_vlan, const char *str)
 350 {
 351         struct switch_port *ports;
 352         struct switch_val val;
 353         char *ptr;
 354
 355         memset(&val, 0, sizeof(val));
 356         val.port_vlan = port_vlan;
 357         switch(a->type) {
 358         case SWITCH_TYPE_INT:
 359                 val.value.i = atoi(str);
 360                 break;
 361         case SWITCH_TYPE_STRING:
 362                 val.value.s = str;
 363                 break;
 364         case SWITCH_TYPE_PORTS:
 365                 ports = alloca(sizeof(struct switch_port) * dev->ports);
 366                 memset(ports, 0, sizeof(struct switch_port) * dev->ports);
 367                 val.len = 0;
 368                 ptr = (char *)str;
 369                 while(ptr && *ptr)
 370                 {
 371                         while(*ptr && isspace(*ptr))
 372                                 ptr++;
 373
 374                         if (!*ptr)
 375                                 break;
 376
 377                         if (!isdigit(*ptr))
 378                                 return -1;
 379
 380                         if (val.len >= dev->ports)
 381                                 return -1;
 382
 383                         ports[val.len].flags = 0;
 384                         ports[val.len].id = strtoul(ptr, &ptr, 10);
 385                         while(*ptr && !isspace(*ptr)) {
 386                                 if (*ptr == 't')
 387                                         ports[val.len].flags |= SWLIB_PORT_FLAG_TAGGED;
 388                                 else
 389                                         return -1;
 390
 391                                 ptr++;
 392                         }
 393                         if (*ptr)
 394                                 ptr++;
 395                         val.len++;
 396                 }
 397                 val.value.ports = ports;
 398                 break;
 399         case SWITCH_TYPE_NOVAL:
 400                 if (str && !strcmp(str, "0"))
 401                         return 0;
 402
 403                 break;
 404         default:
 405                 return -1;
 406         }
 407         return swlib_set_attr(dev, a, &val);
 408 }
 409
 410
 411 struct attrlist_arg {
 412         int id;
 413         int atype;
 414         struct switch_dev *dev;
 415         struct switch_attr *prev;
 416         struct switch_attr **head;
 417 };
 418
 419 static int
 420 add_id(struct nl_msg *msg, void *arg)
 421 {
 422         struct attrlist_arg *l = arg;
 423
 424         NLA_PUT_U32(msg, SWITCH_ATTR_ID, l->id);
 425
 426         return 0;
 427 nla_put_failure:
 428         return -1;
 429 }
 430
 431 static int
 432 add_attr(struct nl_msg *msg, void *ptr)
 433 {
 434         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 435         struct attrlist_arg *arg = ptr;
 436         struct switch_attr *new;
 437
 438         if (nla_parse(tb, SWITCH_ATTR_MAX - 1, genlmsg_attrdata(gnlh, 0),
 439                         genlmsg_attrlen(gnlh, 0), NULL) < 0)
 440                 goto done;
 441
 442         new = swlib_alloc(sizeof(struct switch_attr));
 443         if (!new)
 444                 goto done;
 445
 446         new->dev = arg->dev;
 447         new->atype = arg->atype;
 448         if (arg->prev) {
 449                 arg->prev->next = new;
 450         } else {
 451                 arg->prev = *arg->head;
 452         }
 453         *arg->head = new;
 454         arg->head = &new->next;
 455
 456         if (tb[SWITCH_ATTR_OP_ID])
 457                 new->id = nla_get_u32(tb[SWITCH_ATTR_OP_ID]);
 458         if (tb[SWITCH_ATTR_OP_TYPE])
 459                 new->type = nla_get_u32(tb[SWITCH_ATTR_OP_TYPE]);
 460         if (tb[SWITCH_ATTR_OP_NAME])
 461                 new->name = strdup(nla_get_string(tb[SWITCH_ATTR_OP_NAME]));
 462         if (tb[SWITCH_ATTR_OP_DESCRIPTION])
 463                 new->description = strdup(nla_get_string(tb[SWITCH_ATTR_OP_DESCRIPTION]));
 464
 465 done:
 466         return NL_SKIP;
 467 }
 468
 469 int
 470 swlib_scan(struct switch_dev *dev)
 471 {
 472         struct attrlist_arg arg;
 473
 474         if (dev->ops || dev->port_ops || dev->vlan_ops)
 475                 return 0;
 476
 477         arg.atype = SWLIB_ATTR_GROUP_GLOBAL;
 478         arg.dev = dev;
 479         arg.id = dev->id;
 480         arg.prev = NULL;
 481         arg.head = &dev->ops;
 482         swlib_call(SWITCH_CMD_LIST_GLOBAL, add_attr, add_id, &arg);
 483
 484         arg.atype = SWLIB_ATTR_GROUP_PORT;
 485         arg.prev = NULL;
 486         arg.head = &dev->port_ops;
 487         swlib_call(SWITCH_CMD_LIST_PORT, add_attr, add_id, &arg);
 488
 489         arg.atype = SWLIB_ATTR_GROUP_VLAN;
 490         arg.prev = NULL;
 491         arg.head = &dev->vlan_ops;
 492         swlib_call(SWITCH_CMD_LIST_VLAN, add_attr, add_id, &arg);
 493
 494         return 0;
 495 }
 496
 497 struct switch_attr *swlib_lookup_attr(struct switch_dev *dev,
 498                 enum swlib_attr_group atype, const char *name)
 499 {
 500         struct switch_attr *head;
 501
 502         if (!name || !dev)
 503                 return NULL;
 504
 505         switch(atype) {
 506         case SWLIB_ATTR_GROUP_GLOBAL:
 507                 head = dev->ops;
 508                 break;
 509         case SWLIB_ATTR_GROUP_PORT:
 510                 head = dev->port_ops;
 511                 break;
 512         case SWLIB_ATTR_GROUP_VLAN:
 513                 head = dev->vlan_ops;
 514                 break;
 515         }
 516         while(head) {
 517                 if (!strcmp(name, head->name))
 518                         return head;
 519                 head = head->next;
 520         }
 521
 522         return NULL;
 523 }
 524
 525 static void
 526 swlib_priv_free(void)
 527 {
 528         if (cache)
 529                 nl_cache_free(cache);
 530         if (handle)
 531                 nl_socket_free(handle);
 532         handle = NULL;
 533         cache = NULL;
 534 }
 535
 536 static int
 537 swlib_priv_init(void)
 538 {
 539         int ret;
 540
 541         handle = nl_socket_alloc();
 542         if (!handle) {
 543                 DPRINTF("Failed to create handle\n");
 544                 goto err;
 545         }
 546
 547         if (genl_connect(handle)) {
 548                 DPRINTF("Failed to connect to generic netlink\n");
 549                 goto err;
 550         }
 551
 552         ret = genl_ctrl_alloc_cache(handle, &cache);
 553         if (ret < 0) {
 554                 DPRINTF("Failed to allocate netlink cache\n");
 555                 goto err;
 556         }
 557
 558         family = genl_ctrl_search_by_name(cache, "switch");
 559         if (!family) {
 560                 DPRINTF("Switch API not present\n");
 561                 goto err;
 562         }
 563         return 0;
 564
 565 err:
 566         swlib_priv_free();
 567         return -EINVAL;
 568 }
 569
 570 struct swlib_scan_arg {
 571         const char *name;
 572         struct switch_dev *head;
 573         struct switch_dev *ptr;
 574 };
 575
 576 static int
 577 add_switch(struct nl_msg *msg, void *arg)
 578 {
 579         struct swlib_scan_arg *sa = arg;
 580         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 581         struct switch_dev *dev;
 582         const char *name;
 583         const char *alias;
 584
 585         if (nla_parse(tb, SWITCH_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL) < 0)
 586                 goto done;
 587
 588         if (!tb[SWITCH_ATTR_DEV_NAME])
 589                 goto done;
 590
 591         name = nla_get_string(tb[SWITCH_ATTR_DEV_NAME]);
 592         alias = nla_get_string(tb[SWITCH_ATTR_ALIAS]);
 593
 594         if (sa->name && (strcmp(name, sa->name) != 0) && (strcmp(alias, sa->name) != 0))
 595                 goto done;
 596
 597         dev = swlib_alloc(sizeof(struct switch_dev));
 598         if (!dev)
 599                 goto done;
 600
 601         strncpy(dev->dev_name, name, IFNAMSIZ - 1);
 602         dev->alias = strdup(alias);
 603         if (tb[SWITCH_ATTR_ID])
 604                 dev->id = nla_get_u32(tb[SWITCH_ATTR_ID]);
 605         if (tb[SWITCH_ATTR_NAME])
 606                 dev->name = strdup(nla_get_string(tb[SWITCH_ATTR_NAME]));
 607         if (tb[SWITCH_ATTR_PORTS])
 608                 dev->ports = nla_get_u32(tb[SWITCH_ATTR_PORTS]);
 609         if (tb[SWITCH_ATTR_VLANS])
 610                 dev->vlans = nla_get_u32(tb[SWITCH_ATTR_VLANS]);
 611         if (tb[SWITCH_ATTR_CPU_PORT])
 612                 dev->cpu_port = nla_get_u32(tb[SWITCH_ATTR_CPU_PORT]);
 613
 614         if (!sa->head) {
 615                 sa->head = dev;
 616                 sa->ptr = dev;
 617         } else {
 618                 sa->ptr->next = dev;
 619                 sa->ptr = dev;
 620         }
 621
 622         refcount++;
 623 done:
 624         return NL_SKIP;
 625 }
 626
 627
 628 struct switch_dev *
 629 swlib_connect(const char *name)
 630 {
 631         struct swlib_scan_arg arg;
 632         int err;
 633
 634         if (!refcount) {
 635                 if (swlib_priv_init() < 0)
 636                         return NULL;
 637         };
 638
 639         arg.head = NULL;
 640         arg.ptr = NULL;
 641         arg.name = name;
 642         swlib_call(SWITCH_CMD_GET_SWITCH, add_switch, NULL, &arg);
 643
 644         if (!refcount)
 645                 swlib_priv_free();
 646
 647         return arg.head;
 648 }
 649
 650 static void
 651 swlib_free_attributes(struct switch_attr **head)
 652 {
 653         struct switch_attr *a = *head;
 654         struct switch_attr *next;
 655
 656         while (a) {
 657                 next = a->next;
 658                 free(a);
 659                 a = next;
 660         }
 661         *head = NULL;
 662 }
 663
 664 void
 665 swlib_free(struct switch_dev *dev)
 666 {
 667         swlib_free_attributes(&dev->ops);
 668         swlib_free_attributes(&dev->port_ops);
 669         swlib_free_attributes(&dev->vlan_ops);
 670         free(dev);
 671
 672         if (--refcount == 0)
 673                 swlib_priv_free();
 674 }
 675
 676 void
 677 swlib_free_all(struct switch_dev *dev)
 678 {
 679         struct switch_dev *p;
 680
 681         while (dev) {
 682                 p = dev->next;
 683                 swlib_free(dev);
 684                 dev = p;
 685         }
 686 }