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 <libmnl/libmnl.h>
18#include <linux/genetlink.h>
19
20#include "libnetlink.h"
21#include "mnl_utils.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};
32
33static struct nlmsghdr *__mnlg_msg_prepare(struct mnlg_socket *nlg, uint8_t cmd,
34 uint16_t flags, uint32_t id,
35 uint8_t version)
36{
37 struct genlmsghdr genl = {
38 .cmd = cmd,
39 .version = version,
40 };
41 struct nlmsghdr *nlh;
42
43 nlh = mnlu_msg_prepare(nlg->buf, id, flags, &genl, sizeof(genl));
44 nlg->seq = nlh->nlmsg_seq;
45 return nlh;
46}
47
48struct nlmsghdr *mnlg_msg_prepare(struct mnlg_socket *nlg, uint8_t cmd,
49 uint16_t flags)
50{
51 return __mnlg_msg_prepare(nlg, cmd, flags, nlg->id, nlg->version);
52}
53
54int mnlg_socket_send(struct mnlg_socket *nlg, const struct nlmsghdr *nlh)
55{
56 return mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
57}
58
59int mnlg_socket_recv_run(struct mnlg_socket *nlg, mnl_cb_t data_cb, void *data)
60{
61 return mnlu_socket_recv_run(nlg->nl, nlg->seq, nlg->buf, MNL_SOCKET_BUFFER_SIZE,
62 data_cb, data);
63}
64
65struct group_info {
66 bool found;
67 uint32_t id;
68 const char *name;
69};
70
71static int parse_mc_grps_cb(const struct nlattr *attr, void *data)
72{
73 const struct nlattr **tb = data;
74 int type = mnl_attr_get_type(attr);
75
76 if (mnl_attr_type_valid(attr, CTRL_ATTR_MCAST_GRP_MAX) < 0)
77 return MNL_CB_OK;
78
79 switch (type) {
80 case CTRL_ATTR_MCAST_GRP_ID:
81 if (mnl_attr_validate(attr, MNL_TYPE_U32) < 0)
82 return MNL_CB_ERROR;
83 break;
84 case CTRL_ATTR_MCAST_GRP_NAME:
85 if (mnl_attr_validate(attr, MNL_TYPE_STRING) < 0)
86 return MNL_CB_ERROR;
87 break;
88 }
89 tb[type] = attr;
90 return MNL_CB_OK;
91}
92
93static void parse_genl_mc_grps(struct nlattr *nested,
94 struct group_info *group_info)
95{
96 struct nlattr *pos;
97 const char *name;
98
99 mnl_attr_for_each_nested(pos, nested) {
100 struct nlattr *tb[CTRL_ATTR_MCAST_GRP_MAX + 1] = {};
101
102 mnl_attr_parse_nested(pos, parse_mc_grps_cb, tb);
103 if (!tb[CTRL_ATTR_MCAST_GRP_NAME] ||
104 !tb[CTRL_ATTR_MCAST_GRP_ID])
105 continue;
106
107 name = mnl_attr_get_str(tb[CTRL_ATTR_MCAST_GRP_NAME]);
108 if (strcmp(name, group_info->name) != 0)
109 continue;
110
111 group_info->id = mnl_attr_get_u32(tb[CTRL_ATTR_MCAST_GRP_ID]);
112 group_info->found = true;
113 }
114}
115
116static int get_group_id_attr_cb(const struct nlattr *attr, void *data)
117{
118 const struct nlattr **tb = data;
119 int type = mnl_attr_get_type(attr);
120
121 if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0)
122 return MNL_CB_ERROR;
123
124 if (type == CTRL_ATTR_MCAST_GROUPS &&
125 mnl_attr_validate(attr, MNL_TYPE_NESTED) < 0)
126 return MNL_CB_ERROR;
127 tb[type] = attr;
128 return MNL_CB_OK;
129}
130
131static int get_group_id_cb(const struct nlmsghdr *nlh, void *data)
132{
133 struct group_info *group_info = data;
134 struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
135 struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
136
137 mnl_attr_parse(nlh, sizeof(*genl), get_group_id_attr_cb, tb);
138 if (!tb[CTRL_ATTR_MCAST_GROUPS])
139 return MNL_CB_ERROR;
140 parse_genl_mc_grps(tb[CTRL_ATTR_MCAST_GROUPS], group_info);
141 return MNL_CB_OK;
142}
143
144int mnlg_socket_group_add(struct mnlg_socket *nlg, const char *group_name)
145{
146 struct nlmsghdr *nlh;
147 struct group_info group_info;
148 int err;
149
150 nlh = __mnlg_msg_prepare(nlg, CTRL_CMD_GETFAMILY,
151 NLM_F_REQUEST | NLM_F_ACK, GENL_ID_CTRL, 1);
152 mnl_attr_put_u16(nlh, CTRL_ATTR_FAMILY_ID, nlg->id);
153
154 err = mnlg_socket_send(nlg, nlh);
155 if (err < 0)
156 return err;
157
158 group_info.found = false;
159 group_info.name = group_name;
160 err = mnlg_socket_recv_run(nlg, get_group_id_cb, &group_info);
161 if (err < 0)
162 return err;
163
164 if (!group_info.found) {
165 errno = ENOENT;
166 return -1;
167 }
168
169 err = mnl_socket_setsockopt(nlg->nl, NETLINK_ADD_MEMBERSHIP,
170 &group_info.id, sizeof(group_info.id));
171 if (err < 0)
172 return err;
173
174 return 0;
175}
176
177static int get_family_id_attr_cb(const struct nlattr *attr, void *data)
178{
179 const struct nlattr **tb = data;
180 int type = mnl_attr_get_type(attr);
181
182 if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0)
183 return MNL_CB_ERROR;
184
185 if (type == CTRL_ATTR_FAMILY_ID &&
186 mnl_attr_validate(attr, MNL_TYPE_U16) < 0)
187 return MNL_CB_ERROR;
188 tb[type] = attr;
189 return MNL_CB_OK;
190}
191
192static int get_family_id_cb(const struct nlmsghdr *nlh, void *data)
193{
194 uint32_t *p_id = data;
195 struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
196 struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
197
198 mnl_attr_parse(nlh, sizeof(*genl), get_family_id_attr_cb, tb);
199 if (!tb[CTRL_ATTR_FAMILY_ID])
200 return MNL_CB_ERROR;
201 *p_id = mnl_attr_get_u16(tb[CTRL_ATTR_FAMILY_ID]);
202 return MNL_CB_OK;
203}
204
205struct mnlg_socket *mnlg_socket_open(const char *family_name, uint8_t version)
206{
207 struct mnlg_socket *nlg;
208 struct nlmsghdr *nlh;
209 int err;
210
211 nlg = malloc(sizeof(*nlg));
212 if (!nlg)
213 return NULL;
214
215 nlg->buf = malloc(MNL_SOCKET_BUFFER_SIZE);
216 if (!nlg->buf)
217 goto err_buf_alloc;
218
219 nlg->nl = mnlu_socket_open(NETLINK_GENERIC);
220 if (!nlg->nl)
221 goto err_socket_open;
222
223 nlh = __mnlg_msg_prepare(nlg, CTRL_CMD_GETFAMILY,
224 NLM_F_REQUEST | NLM_F_ACK, GENL_ID_CTRL, 1);
225 mnl_attr_put_strz(nlh, CTRL_ATTR_FAMILY_NAME, family_name);
226
227 err = mnlg_socket_send(nlg, nlh);
228 if (err < 0)
229 goto err_mnlg_socket_send;
230
231 err = mnlg_socket_recv_run(nlg, get_family_id_cb, &nlg->id);
232 if (err < 0)
233 goto err_mnlg_socket_recv_run;
234
235 nlg->version = version;
236 return nlg;
237
238err_mnlg_socket_recv_run:
239err_mnlg_socket_send:
240 mnl_socket_close(nlg->nl);
241err_socket_open:
242 free(nlg->buf);
243err_buf_alloc:
244 free(nlg);
245 return NULL;
246}
247
248void mnlg_socket_close(struct mnlg_socket *nlg)
249{
250 mnl_socket_close(nlg->nl);
251 free(nlg->buf);
252 free(nlg);
253}
254
255int mnlg_socket_get_fd(struct mnlg_socket *nlg)
256{
257 return mnl_socket_get_fd(nlg->nl);
258}
259
