1
2
3
4
5
6
7
8
9
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
25struct 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
34static 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
54struct 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
60int 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
65static int mnlg_cb_noop(const struct nlmsghdr *nlh, void *data)
66{
67 return MNL_CB_OK;
68}
69
70static int mnlg_cb_error(const struct nlmsghdr *nlh, void *data)
71{
72 const struct nlmsgerr *err = mnl_nlmsg_get_payload(nlh);
73
74
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
86static 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
98static 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
105int 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
122struct group_info {
123 bool found;
124 uint32_t id;
125 const char *name;
126};
127
128static 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
150static 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
173static 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
188static 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
201int 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
234static 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
249static 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
262struct 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
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
306err_mnlg_socket_recv_run:
307err_mnlg_socket_send:
308err_mnl_socket_bind:
309 mnl_socket_close(nlg->nl);
310err_mnl_socket_open:
311 free(nlg->buf);
312err_buf_alloc:
313 free(nlg);
314 return NULL;
315}
316
317void mnlg_socket_close(struct mnlg_socket *nlg)
318{
319 mnl_socket_close(nlg->nl);
320 free(nlg->buf);
321 free(nlg);
322}
323
324int mnlg_socket_get_fd(struct mnlg_socket *nlg)
325{
326 return mnl_socket_get_fd(nlg->nl);
327}
328
