devlink/mnlg.c

   1 /*
   2  *   mnlg.c     Generic Netlink helpers for libmnl
   3  *
   4  *              This program is free software; you can redistribute it and/or
   5  *              modify it under the terms of the GNU General Public License
   6  *              as published by the Free Software Foundation; either version
   7  *              2 of the License, or (at your option) any later version.
   8  *
   9  * Authors:     Jiri Pirko <jiri@mellanox.com>
  10  */
  11
  12 #include <stdlib.h>
  13 #include <stdbool.h>
  14 #include <string.h>
  15 #include <errno.h>
  16 #include <unistd.h>
  17 #include <time.h>
  18 #include <libmnl/libmnl.h>
  19 #include <linux/genetlink.h>
  20
  21 #include "libnetlink.h"
  22 #include "utils.h"
  23 #include "mnlg.h"
  24
  25 struct mnlg_socket {
  26         struct mnl_socket *nl;
  27         char *buf;
  28         uint32_t id;
  29         uint8_t version;
  30         unsigned int seq;
  31         unsigned int portid;
  32 };
  33
  34 static struct nlmsghdr *__mnlg_msg_prepare(struct mnlg_socket *nlg, uint8_t cmd,
  35                                            uint16_t flags, uint32_t id,
  36                                            uint8_t version)
  37 {
  38         struct nlmsghdr *nlh;
  39         struct genlmsghdr *genl;
  40
  41         nlh = mnl_nlmsg_put_header(nlg->buf);
  42         nlh->nlmsg_type = id;
  43         nlh->nlmsg_flags = flags;
  44         nlg->seq = time(NULL);
  45         nlh->nlmsg_seq = nlg->seq;
  46
  47         genl = mnl_nlmsg_put_extra_header(nlh, sizeof(struct genlmsghdr));
  48         genl->cmd = cmd;
  49         genl->version = version;
  50
  51         return nlh;
  52 }
  53
  54 struct nlmsghdr *mnlg_msg_prepare(struct mnlg_socket *nlg, uint8_t cmd,
  55                                   uint16_t flags)
  56 {
  57         return __mnlg_msg_prepare(nlg, cmd, flags, nlg->id, nlg->version);
  58 }
  59
  60 int mnlg_socket_send(struct mnlg_socket *nlg, const struct nlmsghdr *nlh)
  61 {
  62         return mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
  63 }
  64
  65 static int mnlg_cb_noop(const struct nlmsghdr *nlh, void *data)
  66 {
  67         return MNL_CB_OK;
  68 }
  69
  70 static int mnlg_cb_error(const struct nlmsghdr *nlh, void *data)
  71 {
  72         const struct nlmsgerr *err = mnl_nlmsg_get_payload(nlh);
  73
  74         /* Netlink subsystems returns the errno value with different signess */
  75         if (err->error < 0)
  76                 errno = -err->error;
  77         else
  78                 errno = err->error;
  79
  80         if (nl_dump_ext_ack(nlh, NULL))
  81                 return MNL_CB_ERROR;
  82
  83         return err->error == 0 ? MNL_CB_STOP : MNL_CB_ERROR;
  84 }
  85
  86 static int mnlg_cb_stop(const struct nlmsghdr *nlh, void *data)
  87 {
  88         int len = *(int *)NLMSG_DATA(nlh);
  89
  90         if (len < 0) {
  91                 errno = -len;
  92                 nl_dump_ext_ack_done(nlh, len);
  93                 return MNL_CB_ERROR;
  94         }
  95         return MNL_CB_STOP;
  96 }
  97
  98 static mnl_cb_t mnlg_cb_array[NLMSG_MIN_TYPE] = {
  99         [NLMSG_NOOP]    = mnlg_cb_noop,
 100         [NLMSG_ERROR]   = mnlg_cb_error,
 101         [NLMSG_DONE]    = mnlg_cb_stop,
 102         [NLMSG_OVERRUN] = mnlg_cb_noop,
 103 };
 104
 105 int mnlg_socket_recv_run(struct mnlg_socket *nlg, mnl_cb_t data_cb, void *data)
 106 {
 107         int err;
 108
 109         do {
 110                 err = mnl_socket_recvfrom(nlg->nl, nlg->buf,
 111                                           MNL_SOCKET_BUFFER_SIZE);
 112                 if (err <= 0)
 113                         break;
 114                 err = mnl_cb_run2(nlg->buf, err, nlg->seq, nlg->portid,
 115                                   data_cb, data, mnlg_cb_array,
 116                                   ARRAY_SIZE(mnlg_cb_array));
 117         } while (err > 0);
 118
 119         return err;
 120 }
 121
 122 struct group_info {
 123         bool found;
 124         uint32_t id;
 125         const char *name;
 126 };
 127
 128 static int parse_mc_grps_cb(const struct nlattr *attr, void *data)
 129 {
 130         const struct nlattr **tb = data;
 131         int type = mnl_attr_get_type(attr);
 132
 133         if (mnl_attr_type_valid(attr, CTRL_ATTR_MCAST_GRP_MAX) < 0)
 134                 return MNL_CB_OK;
 135
 136         switch (type) {
 137         case CTRL_ATTR_MCAST_GRP_ID:
 138                 if (mnl_attr_validate(attr, MNL_TYPE_U32) < 0)
 139                         return MNL_CB_ERROR;
 140                 break;
 141         case CTRL_ATTR_MCAST_GRP_NAME:
 142                 if (mnl_attr_validate(attr, MNL_TYPE_STRING) < 0)
 143                         return MNL_CB_ERROR;
 144                 break;
 145         }
 146         tb[type] = attr;
 147         return MNL_CB_OK;
 148 }
 149
 150 static void parse_genl_mc_grps(struct nlattr *nested,
 151                                struct group_info *group_info)
 152 {
 153         struct nlattr *pos;
 154         const char *name;
 155
 156         mnl_attr_for_each_nested(pos, nested) {
 157                 struct nlattr *tb[CTRL_ATTR_MCAST_GRP_MAX + 1] = {};
 158
 159                 mnl_attr_parse_nested(pos, parse_mc_grps_cb, tb);
 160                 if (!tb[CTRL_ATTR_MCAST_GRP_NAME] ||
 161                     !tb[CTRL_ATTR_MCAST_GRP_ID])
 162                         continue;
 163
 164                 name = mnl_attr_get_str(tb[CTRL_ATTR_MCAST_GRP_NAME]);
 165                 if (strcmp(name, group_info->name) != 0)
 166                         continue;
 167
 168                 group_info->id = mnl_attr_get_u32(tb[CTRL_ATTR_MCAST_GRP_ID]);
 169                 group_info->found = true;
 170         }
 171 }
 172
 173 static int get_group_id_attr_cb(const struct nlattr *attr, void *data)
 174 {
 175         const struct nlattr **tb = data;
 176         int type = mnl_attr_get_type(attr);
 177
 178         if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0)
 179                 return MNL_CB_ERROR;
 180
 181         if (type == CTRL_ATTR_MCAST_GROUPS &&
 182             mnl_attr_validate(attr, MNL_TYPE_NESTED) < 0)
 183                 return MNL_CB_ERROR;
 184         tb[type] = attr;
 185         return MNL_CB_OK;
 186 }
 187
 188 static int get_group_id_cb(const struct nlmsghdr *nlh, void *data)
 189 {
 190         struct group_info *group_info = data;
 191         struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
 192         struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
 193
 194         mnl_attr_parse(nlh, sizeof(*genl), get_group_id_attr_cb, tb);
 195         if (!tb[CTRL_ATTR_MCAST_GROUPS])
 196                 return MNL_CB_ERROR;
 197         parse_genl_mc_grps(tb[CTRL_ATTR_MCAST_GROUPS], group_info);
 198         return MNL_CB_OK;
 199 }
 200
 201 int mnlg_socket_group_add(struct mnlg_socket *nlg, const char *group_name)
 202 {
 203         struct nlmsghdr *nlh;
 204         struct group_info group_info;
 205         int err;
 206
 207         nlh = __mnlg_msg_prepare(nlg, CTRL_CMD_GETFAMILY,
 208                                  NLM_F_REQUEST | NLM_F_ACK, GENL_ID_CTRL, 1);
 209         mnl_attr_put_u16(nlh, CTRL_ATTR_FAMILY_ID, nlg->id);
 210
 211         err = mnlg_socket_send(nlg, nlh);
 212         if (err < 0)
 213                 return err;
 214
 215         group_info.found = false;
 216         group_info.name = group_name;
 217         err = mnlg_socket_recv_run(nlg, get_group_id_cb, &group_info);
 218         if (err < 0)
 219                 return err;
 220
 221         if (!group_info.found) {
 222                 errno = ENOENT;
 223                 return -1;
 224         }
 225
 226         err = mnl_socket_setsockopt(nlg->nl, NETLINK_ADD_MEMBERSHIP,
 227                                     &group_info.id, sizeof(group_info.id));
 228         if (err < 0)
 229                 return err;
 230
 231         return 0;
 232 }
 233
 234 static int get_family_id_attr_cb(const struct nlattr *attr, void *data)
 235 {
 236         const struct nlattr **tb = data;
 237         int type = mnl_attr_get_type(attr);
 238
 239         if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0)
 240                 return MNL_CB_ERROR;
 241
 242         if (type == CTRL_ATTR_FAMILY_ID &&
 243             mnl_attr_validate(attr, MNL_TYPE_U16) < 0)
 244                 return MNL_CB_ERROR;
 245         tb[type] = attr;
 246         return MNL_CB_OK;
 247 }
 248
 249 static int get_family_id_cb(const struct nlmsghdr *nlh, void *data)
 250 {
 251         uint32_t *p_id = data;
 252         struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
 253         struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
 254
 255         mnl_attr_parse(nlh, sizeof(*genl), get_family_id_attr_cb, tb);
 256         if (!tb[CTRL_ATTR_FAMILY_ID])
 257                 return MNL_CB_ERROR;
 258         *p_id = mnl_attr_get_u16(tb[CTRL_ATTR_FAMILY_ID]);
 259         return MNL_CB_OK;
 260 }
 261
 262 struct mnlg_socket *mnlg_socket_open(const char *family_name, uint8_t version)
 263 {
 264         struct mnlg_socket *nlg;
 265         struct nlmsghdr *nlh;
 266         int one = 1;
 267         int err;
 268
 269         nlg = malloc(sizeof(*nlg));
 270         if (!nlg)
 271                 return NULL;
 272
 273         nlg->buf = malloc(MNL_SOCKET_BUFFER_SIZE);
 274         if (!nlg->buf)
 275                 goto err_buf_alloc;
 276
 277         nlg->nl = mnl_socket_open(NETLINK_GENERIC);
 278         if (!nlg->nl)
 279                 goto err_mnl_socket_open;
 280
 281         /* Older kernels may no support capped/extended ACK reporting */
 282         mnl_socket_setsockopt(nlg->nl, NETLINK_CAP_ACK, &one, sizeof(one));
 283         mnl_socket_setsockopt(nlg->nl, NETLINK_EXT_ACK, &one, sizeof(one));
 284
 285         err = mnl_socket_bind(nlg->nl, 0, MNL_SOCKET_AUTOPID);
 286         if (err < 0)
 287                 goto err_mnl_socket_bind;
 288
 289         nlg->portid = mnl_socket_get_portid(nlg->nl);
 290
 291         nlh = __mnlg_msg_prepare(nlg, CTRL_CMD_GETFAMILY,
 292                                  NLM_F_REQUEST | NLM_F_ACK, GENL_ID_CTRL, 1);
 293         mnl_attr_put_strz(nlh, CTRL_ATTR_FAMILY_NAME, family_name);
 294
 295         err = mnlg_socket_send(nlg, nlh);
 296         if (err < 0)
 297                 goto err_mnlg_socket_send;
 298
 299         err = mnlg_socket_recv_run(nlg, get_family_id_cb, &nlg->id);
 300         if (err < 0)
 301                 goto err_mnlg_socket_recv_run;
 302
 303         nlg->version = version;
 304         return nlg;
 305
 306 err_mnlg_socket_recv_run:
 307 err_mnlg_socket_send:
 308 err_mnl_socket_bind:
 309         mnl_socket_close(nlg->nl);
 310 err_mnl_socket_open:
 311         free(nlg->buf);
 312 err_buf_alloc:
 313         free(nlg);
 314         return NULL;
 315 }
 316
 317 void mnlg_socket_close(struct mnlg_socket *nlg)
 318 {
 319         mnl_socket_close(nlg->nl);
 320         free(nlg->buf);
 321         free(nlg);
 322 }
 323
 324 int mnlg_socket_get_fd(struct mnlg_socket *nlg)
 325 {
 326         return mnl_socket_get_fd(nlg->nl);
 327 }