1 /*-
2  * SPDX-License-Identifier: BSD-2-Clause
3  *
4  * Copyright (c) 2022 Alexander V. Chernikov <melifaro@FreeBSD.org>
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  * 1. Redistributions of source code must retain the above copyright
10  *    notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  *    notice, this list of conditions and the following disclaimer in the
13  *    documentation and/or other materials provided with the distribution.
14  *
15  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25  * SUCH DAMAGE.
26  */
27 
28 #include <sys/param.h>
29 #include <sys/malloc.h>
30 #include <sys/lock.h>
31 #include <sys/rmlock.h>
32 #include <sys/mbuf.h>
33 #include <sys/socket.h>
34 #include <sys/socketvar.h>
35 #include <sys/syslog.h>
36 
37 #include <netlink/netlink.h>
38 #include <netlink/netlink_ctl.h>
39 #include <netlink/netlink_linux.h>
40 #include <netlink/netlink_var.h>
41 
42 #define	DEBUG_MOD_NAME	nl_writer
43 #define	DEBUG_MAX_LEVEL	LOG_DEBUG3
44 #include <netlink/netlink_debug.h>
45 _DECLARE_DEBUG(LOG_INFO);
46 
47 static bool
48 nlmsg_get_buf(struct nl_writer *nw, u_int len, bool waitok)
49 {
50 	const int mflag = waitok ? M_WAITOK : M_NOWAIT;
51 
52 	MPASS(nw->buf == NULL);
53 
54 	NL_LOG(LOG_DEBUG3, "Setting up nw %p len %u %s", nw, len,
55 	    waitok ? "wait" : "nowait");
56 
57 	nw->buf = nl_buf_alloc(len, mflag);
58 	if (__predict_false(nw->buf == NULL))
59 		return (false);
60 	nw->hdr = NULL;
61 	nw->malloc_flag = mflag;
62 	nw->num_messages = 0;
63 	nw->enomem = false;
64 
65 	return (true);
66 }
67 
68 static bool
69 nl_send_one(struct nl_writer *nw)
70 {
71 
72 	return (nl_send(nw, nw->nlp));
73 }
74 
75 bool
76 _nlmsg_get_unicast_writer(struct nl_writer *nw, int size, struct nlpcb *nlp)
77 {
78 	nw->nlp = nlp;
79 	nw->cb = nl_send_one;
80 
81 	return (nlmsg_get_buf(nw, size, false));
82 }
83 
84 bool
85 _nlmsg_get_group_writer(struct nl_writer *nw, int size, int protocol, int group_id)
86 {
87 	nw->group.proto = protocol;
88 	nw->group.id = group_id;
89 	nw->cb = nl_send_group;
90 
91 	return (nlmsg_get_buf(nw, size, false));
92 }
93 
94 void
95 _nlmsg_ignore_limit(struct nl_writer *nw)
96 {
97 	nw->ignore_limit = true;
98 }
99 
100 bool
101 _nlmsg_flush(struct nl_writer *nw)
102 {
103 
104 	if (__predict_false(nw->hdr != NULL)) {
105 		/* Last message has not been completed, skip it. */
106 		int completed_len = (char *)nw->hdr - nw->buf->data;
107 		/* Send completed messages */
108 		nw->buf->datalen -= nw->buf->datalen - completed_len;
109 		nw->hdr = NULL;
110         }
111 
112 	NL_LOG(LOG_DEBUG2, "OUT");
113 	bool result = nw->cb(nw);
114 	nw->num_messages = 0;
115 
116 	if (!result) {
117 		NL_LOG(LOG_DEBUG, "nw %p flush with %p() failed", nw, nw->cb);
118 	}
119 
120 	return (result);
121 }
122 
123 /*
124  * Flushes previous data and allocates new underlying storage
125  *  sufficient for holding at least @required_len bytes.
126  * Return true on success.
127  */
128 bool
129 _nlmsg_refill_buffer(struct nl_writer *nw, u_int required_len)
130 {
131 	struct nl_buf *new;
132 	u_int completed_len, new_len, last_len;
133 
134 	MPASS(nw->buf != NULL);
135 
136 	if (nw->enomem)
137 		return (false);
138 
139 	NL_LOG(LOG_DEBUG3, "no space at offset %u/%u (want %u), trying to "
140 	    "reclaim", nw->buf->datalen, nw->buf->buflen, required_len);
141 
142 	/* Calculate new buffer size and allocate it. */
143 	completed_len = (nw->hdr != NULL) ?
144 	    (char *)nw->hdr - nw->buf->data : nw->buf->datalen;
145 	if (completed_len > 0 && required_len < NLMBUFSIZE) {
146 		/* We already ran out of space, use largest effective size. */
147 		new_len = max(nw->buf->buflen, NLMBUFSIZE);
148 	} else {
149 		if (nw->buf->buflen < NLMBUFSIZE)
150 			/* XXXGL: does this happen? */
151 			new_len = NLMBUFSIZE;
152 		else
153 			new_len = nw->buf->buflen * 2;
154 		while (new_len < required_len)
155 			new_len *= 2;
156 	}
157 
158 	new = nl_buf_alloc(new_len, nw->malloc_flag | M_ZERO);
159 	if (__predict_false(new == NULL)) {
160 		nw->enomem = true;
161 		NL_LOG(LOG_DEBUG, "getting new buf failed, setting ENOMEM");
162 		return (false);
163 	}
164 
165 	/* Copy last (unfinished) header to the new storage. */
166 	last_len = nw->buf->datalen - completed_len;
167 	if (last_len > 0) {
168 		memcpy(new->data, nw->hdr, last_len);
169 		new->datalen = last_len;
170 	}
171 
172 	NL_LOG(LOG_DEBUG2, "completed: %u bytes, copied: %u bytes",
173 	    completed_len, last_len);
174 
175 	if (completed_len > 0) {
176 		nlmsg_flush(nw);
177 		MPASS(nw->buf == NULL);
178 	} else
179 		nl_buf_free(nw->buf);
180 	nw->buf = new;
181 	nw->hdr = (last_len > 0) ? (struct nlmsghdr *)new->data : NULL;
182 	NL_LOG(LOG_DEBUG2, "switched buffer: used %u/%u bytes",
183 	    new->datalen, new->buflen);
184 
185 	return (true);
186 }
187 
188 bool
189 _nlmsg_add(struct nl_writer *nw, uint32_t portid, uint32_t seq, uint16_t type,
190     uint16_t flags, uint32_t len)
191 {
192 	struct nl_buf *nb = nw->buf;
193 	struct nlmsghdr *hdr;
194 	u_int required_len;
195 
196 	MPASS(nw->hdr == NULL);
197 
198 	required_len = NETLINK_ALIGN(len + sizeof(struct nlmsghdr));
199 	if (__predict_false(nb->datalen + required_len > nb->buflen)) {
200 		if (!nlmsg_refill_buffer(nw, required_len))
201 			return (false);
202 		nb = nw->buf;
203 	}
204 
205 	hdr = (struct nlmsghdr *)(&nb->data[nb->datalen]);
206 
207 	hdr->nlmsg_len = len;
208 	hdr->nlmsg_type = type;
209 	hdr->nlmsg_flags = flags;
210 	hdr->nlmsg_seq = seq;
211 	hdr->nlmsg_pid = portid;
212 
213 	nw->hdr = hdr;
214 	nb->datalen += sizeof(struct nlmsghdr);
215 
216 	return (true);
217 }
218 
219 bool
220 _nlmsg_end(struct nl_writer *nw)
221 {
222 	struct nl_buf *nb = nw->buf;
223 
224 	MPASS(nw->hdr != NULL);
225 
226 	if (nw->enomem) {
227 		NL_LOG(LOG_DEBUG, "ENOMEM when dumping message");
228 		nlmsg_abort(nw);
229 		return (false);
230 	}
231 
232 	nw->hdr->nlmsg_len = nb->data + nb->datalen - (char *)nw->hdr;
233 	NL_LOG(LOG_DEBUG2, "wrote msg len: %u type: %d: flags: 0x%X seq: %u pid: %u",
234 	    nw->hdr->nlmsg_len, nw->hdr->nlmsg_type, nw->hdr->nlmsg_flags,
235 	    nw->hdr->nlmsg_seq, nw->hdr->nlmsg_pid);
236 	nw->hdr = NULL;
237 	nw->num_messages++;
238 	return (true);
239 }
240 
241 void
242 _nlmsg_abort(struct nl_writer *nw)
243 {
244 	struct nl_buf *nb = nw->buf;
245 
246 	if (nw->hdr != NULL) {
247 		nb->datalen = (char *)nw->hdr - nb->data;
248 		nw->hdr = NULL;
249 	}
250 }
251 
252 void
253 nlmsg_ack(struct nlpcb *nlp, int error, struct nlmsghdr *hdr,
254     struct nl_pstate *npt)
255 {
256 	struct nlmsgerr *errmsg;
257 	int payload_len;
258 	uint32_t flags = nlp->nl_flags;
259 	struct nl_writer *nw = npt->nw;
260 	bool cap_ack;
261 
262 	payload_len = sizeof(struct nlmsgerr);
263 
264 	/*
265 	 * The only case when we send the full message in the
266 	 * reply is when there is an error and NETLINK_CAP_ACK
267 	 * is not set.
268 	 */
269 	cap_ack = (error == 0) || (flags & NLF_CAP_ACK);
270 	if (!cap_ack)
271 		payload_len += hdr->nlmsg_len - sizeof(struct nlmsghdr);
272 	payload_len = NETLINK_ALIGN(payload_len);
273 
274 	uint16_t nl_flags = cap_ack ? NLM_F_CAPPED : 0;
275 	if ((npt->err_msg || npt->err_off) && nlp->nl_flags & NLF_EXT_ACK)
276 		nl_flags |= NLM_F_ACK_TLVS;
277 
278 	NL_LOG(LOG_DEBUG3, "acknowledging message type %d seq %d",
279 	    hdr->nlmsg_type, hdr->nlmsg_seq);
280 
281 	if (!nlmsg_add(nw, nlp->nl_port, hdr->nlmsg_seq, NLMSG_ERROR, nl_flags, payload_len))
282 		goto enomem;
283 
284 	errmsg = nlmsg_reserve_data(nw, payload_len, struct nlmsgerr);
285 	errmsg->error = error;
286 	/* In case of error copy the whole message, else just the header */
287 	memcpy(&errmsg->msg, hdr, cap_ack ? sizeof(*hdr) : hdr->nlmsg_len);
288 
289 	if (npt->err_msg != NULL && nlp->nl_flags & NLF_EXT_ACK)
290 		nlattr_add_string(nw, NLMSGERR_ATTR_MSG, npt->err_msg);
291 	if (npt->err_off != 0 && nlp->nl_flags & NLF_EXT_ACK)
292 		nlattr_add_u32(nw, NLMSGERR_ATTR_OFFS, npt->err_off);
293 	if (npt->cookie != NULL)
294 		nlattr_add_raw(nw, npt->cookie);
295 
296 	if (nlmsg_end(nw))
297 		return;
298 enomem:
299 	NLP_LOG(LOG_DEBUG, nlp, "error allocating ack data for message %d seq %u",
300 	    hdr->nlmsg_type, hdr->nlmsg_seq);
301 	nlmsg_abort(nw);
302 }
303 
304 bool
305 _nlmsg_end_dump(struct nl_writer *nw, int error, struct nlmsghdr *hdr)
306 {
307 	if (!nlmsg_add(nw, hdr->nlmsg_pid, hdr->nlmsg_seq, NLMSG_DONE, 0, sizeof(int))) {
308 		NL_LOG(LOG_DEBUG, "Error finalizing table dump");
309 		return (false);
310 	}
311 	/* Save operation result */
312 	int *perror = nlmsg_reserve_object(nw, int);
313 	NL_LOG(LOG_DEBUG2, "record error=%d at off %d (%p)", error,
314 	    nw->buf->datalen, perror);
315 	*perror = error;
316 	nlmsg_end(nw);
317 	nw->suppress_ack = true;
318 
319 	return (true);
320 }
321 
322 /*
323  * KPI functions.
324  */
325 
326 u_int
327 nlattr_save_offset(const struct nl_writer *nw)
328 {
329 	return (nw->buf->datalen - ((char *)nw->hdr - nw->buf->data));
330 }
331 
332 void *
333 nlmsg_reserve_data_raw(struct nl_writer *nw, size_t sz)
334 {
335 	struct nl_buf *nb = nw->buf;
336 	void *data;
337 
338 	sz = NETLINK_ALIGN(sz);
339 	if (__predict_false(nb->datalen + sz > nb->buflen)) {
340 		if (!nlmsg_refill_buffer(nw, sz))
341 			return (NULL);
342 		nb = nw->buf;
343 	}
344 
345 	data = &nb->data[nb->datalen];
346 	bzero(data, sz);
347 	nb->datalen += sz;
348 
349 	return (data);
350 }
351 
352 bool
353 nlattr_add(struct nl_writer *nw, int attr_type, int attr_len, const void *data)
354 {
355 	struct nl_buf *nb = nw->buf;
356 	struct nlattr *nla;
357 	u_int required_len;
358 
359 	required_len = NLA_ALIGN(attr_len + sizeof(struct nlattr));
360 	if (__predict_false(nb->datalen + required_len > nb->buflen)) {
361 		if (!nlmsg_refill_buffer(nw, required_len))
362 			return (false);
363 		nb = nw->buf;
364 	}
365 
366 	nla = (struct nlattr *)(&nb->data[nb->datalen]);
367 
368 	nla->nla_len = attr_len + sizeof(struct nlattr);
369 	nla->nla_type = attr_type;
370 	if (attr_len > 0) {
371 		if ((attr_len % 4) != 0) {
372 			/* clear padding bytes */
373 			bzero((char *)nla + required_len - 4, 4);
374 		}
375 		memcpy((nla + 1), data, attr_len);
376 	}
377 	nb->datalen += required_len;
378 	return (true);
379 }
380 
381 #include <netlink/ktest_netlink_message_writer.h>
382