1
2
3
4
5
6#include <errno.h>
7#include <string.h>
8#include <time.h>
9#include <libmnl/libmnl.h>
10#include <linux/genetlink.h>
11
12#include "libnetlink.h"
13#include "mnl_utils.h"
14#include "utils.h"
15
16struct mnl_socket *mnlu_socket_open(int bus)
17{
18 struct mnl_socket *nl;
19 int one = 1;
20
21 nl = mnl_socket_open(bus);
22 if (nl == NULL)
23 return NULL;
24
25 mnl_socket_setsockopt(nl, NETLINK_CAP_ACK, &one, sizeof(one));
26 mnl_socket_setsockopt(nl, NETLINK_EXT_ACK, &one, sizeof(one));
27
28 if (mnl_socket_bind(nl, 0, MNL_SOCKET_AUTOPID) < 0)
29 goto err_bind;
30
31 return nl;
32
33err_bind:
34 mnl_socket_close(nl);
35 return NULL;
36}
37
38struct nlmsghdr *mnlu_msg_prepare(void *buf, uint32_t nlmsg_type, uint16_t flags,
39 void *extra_header, size_t extra_header_size)
40{
41 struct nlmsghdr *nlh;
42 void *eh;
43
44 nlh = mnl_nlmsg_put_header(buf);
45 nlh->nlmsg_type = nlmsg_type;
46 nlh->nlmsg_flags = flags;
47 nlh->nlmsg_seq = time(NULL);
48
49 eh = mnl_nlmsg_put_extra_header(nlh, extra_header_size);
50 memcpy(eh, extra_header, extra_header_size);
51
52 return nlh;
53}
54
55static int mnlu_cb_noop(const struct nlmsghdr *nlh, void *data)
56{
57 return MNL_CB_OK;
58}
59
60static int mnlu_cb_error(const struct nlmsghdr *nlh, void *data)
61{
62 const struct nlmsgerr *err = mnl_nlmsg_get_payload(nlh);
63
64
65 if (err->error < 0)
66 errno = -err->error;
67 else
68 errno = err->error;
69
70 if (nl_dump_ext_ack(nlh, NULL))
71 return MNL_CB_ERROR;
72
73 return err->error == 0 ? MNL_CB_STOP : MNL_CB_ERROR;
74}
75
76static int mnlu_cb_stop(const struct nlmsghdr *nlh, void *data)
77{
78 int len = *(int *)NLMSG_DATA(nlh);
79
80 if (len < 0) {
81 errno = -len;
82 nl_dump_ext_ack_done(nlh, len);
83 return MNL_CB_ERROR;
84 }
85 return MNL_CB_STOP;
86}
87
88static mnl_cb_t mnlu_cb_array[NLMSG_MIN_TYPE] = {
89 [NLMSG_NOOP] = mnlu_cb_noop,
90 [NLMSG_ERROR] = mnlu_cb_error,
91 [NLMSG_DONE] = mnlu_cb_stop,
92 [NLMSG_OVERRUN] = mnlu_cb_noop,
93};
94
95int mnlu_socket_recv_run(struct mnl_socket *nl, unsigned int seq, void *buf, size_t buf_size,
96 mnl_cb_t cb, void *data)
97{
98 unsigned int portid = mnl_socket_get_portid(nl);
99 int err;
100
101 do {
102 err = mnl_socket_recvfrom(nl, buf, buf_size);
103 if (err <= 0)
104 break;
105 err = mnl_cb_run2(buf, err, seq, portid,
106 cb, data, mnlu_cb_array,
107 ARRAY_SIZE(mnlu_cb_array));
108 } while (err > 0);
109
110 return err;
111}
112
113static int get_family_id_attr_cb(const struct nlattr *attr, void *data)
114{
115 int type = mnl_attr_get_type(attr);
116 const struct nlattr **tb = data;
117
118 if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0)
119 return MNL_CB_ERROR;
120
121 if (type == CTRL_ATTR_FAMILY_ID &&
122 mnl_attr_validate(attr, MNL_TYPE_U16) < 0)
123 return MNL_CB_ERROR;
124 tb[type] = attr;
125 return MNL_CB_OK;
126}
127
128static int get_family_id_cb(const struct nlmsghdr *nlh, void *data)
129{
130 struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
131 struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
132 uint32_t *p_id = data;
133
134 mnl_attr_parse(nlh, sizeof(*genl), get_family_id_attr_cb, tb);
135 if (!tb[CTRL_ATTR_FAMILY_ID])
136 return MNL_CB_ERROR;
137 *p_id = mnl_attr_get_u16(tb[CTRL_ATTR_FAMILY_ID]);
138 return MNL_CB_OK;
139}
140
141static int family_get(struct mnlu_gen_socket *nlg, const char *family_name)
142{
143 struct genlmsghdr hdr = {};
144 struct nlmsghdr *nlh;
145 int err;
146
147 hdr.cmd = CTRL_CMD_GETFAMILY;
148 hdr.version = 0x1;
149
150 nlh = mnlu_msg_prepare(nlg->buf, GENL_ID_CTRL,
151 NLM_F_REQUEST | NLM_F_ACK,
152 &hdr, sizeof(hdr));
153
154 mnl_attr_put_strz(nlh, CTRL_ATTR_FAMILY_NAME, family_name);
155
156 err = mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
157 if (err < 0)
158 return err;
159
160 err = mnlu_socket_recv_run(nlg->nl, nlh->nlmsg_seq, nlg->buf,
161 MNL_SOCKET_BUFFER_SIZE,
162 get_family_id_cb, &nlg->family);
163 return err;
164}
165
166int mnlu_gen_socket_open(struct mnlu_gen_socket *nlg, const char *family_name,
167 uint8_t version)
168{
169 int err;
170
171 nlg->buf = malloc(MNL_SOCKET_BUFFER_SIZE);
172 if (!nlg->buf)
173 goto err_buf_alloc;
174
175 nlg->nl = mnlu_socket_open(NETLINK_GENERIC);
176 if (!nlg->nl)
177 goto err_socket_open;
178
179 err = family_get(nlg, family_name);
180 if (err)
181 goto err_socket;
182
183 return 0;
184
185err_socket:
186 mnl_socket_close(nlg->nl);
187err_socket_open:
188 free(nlg->buf);
189err_buf_alloc:
190 return -1;
191}
192
193void mnlu_gen_socket_close(struct mnlu_gen_socket *nlg)
194{
195 mnl_socket_close(nlg->nl);
196 free(nlg->buf);
197}
198
199struct nlmsghdr *
200_mnlu_gen_socket_cmd_prepare(struct mnlu_gen_socket *nlg,
201 uint8_t cmd, uint16_t flags,
202 uint32_t id, uint8_t version)
203{
204 struct genlmsghdr hdr = {};
205 struct nlmsghdr *nlh;
206
207 hdr.cmd = cmd;
208 hdr.version = version;
209 nlh = mnlu_msg_prepare(nlg->buf, id, flags, &hdr, sizeof(hdr));
210 nlg->seq = nlh->nlmsg_seq;
211 return nlh;
212}
213
214struct nlmsghdr *mnlu_gen_socket_cmd_prepare(struct mnlu_gen_socket *nlg,
215 uint8_t cmd, uint16_t flags)
216{
217 return _mnlu_gen_socket_cmd_prepare(nlg, cmd, flags, nlg->family,
218 nlg->version);
219}
220
221int mnlu_gen_socket_sndrcv(struct mnlu_gen_socket *nlg, const struct nlmsghdr *nlh,
222 mnl_cb_t data_cb, void *data)
223{
224 int err;
225
226 err = mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
227 if (err < 0) {
228 perror("Failed to send data");
229 return -errno;
230 }
231
232 err = mnlu_socket_recv_run(nlg->nl, nlh->nlmsg_seq, nlg->buf,
233 MNL_SOCKET_BUFFER_SIZE,
234 data_cb, data);
235 if (err < 0) {
236 fprintf(stderr, "kernel answers: %s\n", strerror(errno));
237 return -errno;
238 }
239 return 0;
240}
241
242int mnlu_gen_socket_recv_run(struct mnlu_gen_socket *nlg, mnl_cb_t cb,
243 void *data)
244{
245 return mnlu_socket_recv_run(nlg->nl, nlg->seq, nlg->buf,
246 MNL_SOCKET_BUFFER_SIZE,
247 cb, data);
248}
249
