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