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 	bool result;
104 
105 	if (__predict_false(nw->hdr != NULL)) {
106 		/* Last message has not been completed, skip it. */
107 		int completed_len = (char *)nw->hdr - nw->buf->data;
108 		/* Send completed messages */
109 		nw->buf->datalen -= nw->buf->datalen - completed_len;
110 		nw->hdr = NULL;
111         }
112 
113 	if (nw->buf->datalen == 0) {
114 		MPASS(nw->num_messages == 0);
115 		nl_buf_free(nw->buf);
116 		nw->buf = NULL;
117 		return (true);
118 	}
119 
120 	result = nw->cb(nw);
121 	nw->num_messages = 0;
122 
123 	if (!result) {
124 		NL_LOG(LOG_DEBUG, "nw %p flush with %p() failed", nw, nw->cb);
125 	}
126 
127 	return (result);
128 }
129 
130 /*
131  * Flushes previous data and allocates new underlying storage
132  *  sufficient for holding at least @required_len bytes.
133  * Return true on success.
134  */
135 bool
136 _nlmsg_refill_buffer(struct nl_writer *nw, u_int required_len)
137 {
138 	struct nl_buf *new;
139 	u_int completed_len, new_len, last_len;
140 
141 	MPASS(nw->buf != NULL);
142 
143 	if (nw->enomem)
144 		return (false);
145 
146 	NL_LOG(LOG_DEBUG3, "no space at offset %u/%u (want %u), trying to "
147 	    "reclaim", nw->buf->datalen, nw->buf->buflen, required_len);
148 
149 	/* Calculate new buffer size and allocate it. */
150 	completed_len = (nw->hdr != NULL) ?
151 	    (char *)nw->hdr - nw->buf->data : nw->buf->datalen;
152 	if (completed_len > 0 && required_len < NLMBUFSIZE) {
153 		/* We already ran out of space, use largest effective size. */
154 		new_len = max(nw->buf->buflen, NLMBUFSIZE);
155 	} else {
156 		if (nw->buf->buflen < NLMBUFSIZE)
157 			/* XXXGL: does this happen? */
158 			new_len = NLMBUFSIZE;
159 		else
160 			new_len = nw->buf->buflen * 2;
161 		while (new_len < required_len)
162 			new_len *= 2;
163 	}
164 
165 	new = nl_buf_alloc(new_len, nw->malloc_flag | M_ZERO);
166 	if (__predict_false(new == NULL)) {
167 		nw->enomem = true;
168 		NL_LOG(LOG_DEBUG, "getting new buf failed, setting ENOMEM");
169 		return (false);
170 	}
171 
172 	/* Copy last (unfinished) header to the new storage. */
173 	last_len = nw->buf->datalen - completed_len;
174 	if (last_len > 0) {
175 		memcpy(new->data, nw->hdr, last_len);
176 		new->datalen = last_len;
177 	}
178 
179 	NL_LOG(LOG_DEBUG2, "completed: %u bytes, copied: %u bytes",
180 	    completed_len, last_len);
181 
182 	if (completed_len > 0) {
183 		nlmsg_flush(nw);
184 		MPASS(nw->buf == NULL);
185 	} else
186 		nl_buf_free(nw->buf);
187 	nw->buf = new;
188 	nw->hdr = (last_len > 0) ? (struct nlmsghdr *)new->data : NULL;
189 	NL_LOG(LOG_DEBUG2, "switched buffer: used %u/%u bytes",
190 	    new->datalen, new->buflen);
191 
192 	return (true);
193 }
194 
195 bool
196 _nlmsg_add(struct nl_writer *nw, uint32_t portid, uint32_t seq, uint16_t type,
197     uint16_t flags, uint32_t len)
198 {
199 	struct nl_buf *nb = nw->buf;
200 	struct nlmsghdr *hdr;
201 	u_int required_len;
202 
203 	MPASS(nw->hdr == NULL);
204 
205 	required_len = NETLINK_ALIGN(len + sizeof(struct nlmsghdr));
206 	if (__predict_false(nb->datalen + required_len > nb->buflen)) {
207 		if (!nlmsg_refill_buffer(nw, required_len))
208 			return (false);
209 		nb = nw->buf;
210 	}
211 
212 	hdr = (struct nlmsghdr *)(&nb->data[nb->datalen]);
213 
214 	hdr->nlmsg_len = len;
215 	hdr->nlmsg_type = type;
216 	hdr->nlmsg_flags = flags;
217 	hdr->nlmsg_seq = seq;
218 	hdr->nlmsg_pid = portid;
219 
220 	nw->hdr = hdr;
221 	nb->datalen += sizeof(struct nlmsghdr);
222 
223 	return (true);
224 }
225 
226 bool
227 _nlmsg_end(struct nl_writer *nw)
228 {
229 	struct nl_buf *nb = nw->buf;
230 
231 	MPASS(nw->hdr != NULL);
232 
233 	if (nw->enomem) {
234 		NL_LOG(LOG_DEBUG, "ENOMEM when dumping message");
235 		nlmsg_abort(nw);
236 		return (false);
237 	}
238 
239 	nw->hdr->nlmsg_len = nb->data + nb->datalen - (char *)nw->hdr;
240 	NL_LOG(LOG_DEBUG2, "wrote msg len: %u type: %d: flags: 0x%X seq: %u pid: %u",
241 	    nw->hdr->nlmsg_len, nw->hdr->nlmsg_type, nw->hdr->nlmsg_flags,
242 	    nw->hdr->nlmsg_seq, nw->hdr->nlmsg_pid);
243 	nw->hdr = NULL;
244 	nw->num_messages++;
245 	return (true);
246 }
247 
248 void
249 _nlmsg_abort(struct nl_writer *nw)
250 {
251 	struct nl_buf *nb = nw->buf;
252 
253 	if (nw->hdr != NULL) {
254 		nb->datalen = (char *)nw->hdr - nb->data;
255 		nw->hdr = NULL;
256 	}
257 }
258 
259 void
260 nlmsg_ack(struct nlpcb *nlp, int error, struct nlmsghdr *hdr,
261     struct nl_pstate *npt)
262 {
263 	struct nlmsgerr *errmsg;
264 	int payload_len;
265 	uint32_t flags = nlp->nl_flags;
266 	struct nl_writer *nw = npt->nw;
267 	bool cap_ack;
268 
269 	payload_len = sizeof(struct nlmsgerr);
270 
271 	/*
272 	 * The only case when we send the full message in the
273 	 * reply is when there is an error and NETLINK_CAP_ACK
274 	 * is not set.
275 	 */
276 	cap_ack = (error == 0) || (flags & NLF_CAP_ACK);
277 	if (!cap_ack)
278 		payload_len += hdr->nlmsg_len - sizeof(struct nlmsghdr);
279 	payload_len = NETLINK_ALIGN(payload_len);
280 
281 	uint16_t nl_flags = cap_ack ? NLM_F_CAPPED : 0;
282 	if ((npt->err_msg || npt->err_off) && nlp->nl_flags & NLF_EXT_ACK)
283 		nl_flags |= NLM_F_ACK_TLVS;
284 
285 	NL_LOG(LOG_DEBUG3, "acknowledging message type %d seq %d",
286 	    hdr->nlmsg_type, hdr->nlmsg_seq);
287 
288 	if (!nlmsg_add(nw, nlp->nl_port, hdr->nlmsg_seq, NLMSG_ERROR, nl_flags, payload_len))
289 		goto enomem;
290 
291 	errmsg = nlmsg_reserve_data(nw, payload_len, struct nlmsgerr);
292 	errmsg->error = error;
293 	/* In case of error copy the whole message, else just the header */
294 	memcpy(&errmsg->msg, hdr, cap_ack ? sizeof(*hdr) : hdr->nlmsg_len);
295 
296 	if (npt->err_msg != NULL && nlp->nl_flags & NLF_EXT_ACK)
297 		nlattr_add_string(nw, NLMSGERR_ATTR_MSG, npt->err_msg);
298 	if (npt->err_off != 0 && nlp->nl_flags & NLF_EXT_ACK)
299 		nlattr_add_u32(nw, NLMSGERR_ATTR_OFFS, npt->err_off);
300 	if (npt->cookie != NULL)
301 		nlattr_add_raw(nw, npt->cookie);
302 
303 	if (nlmsg_end(nw))
304 		return;
305 enomem:
306 	NLP_LOG(LOG_DEBUG, nlp, "error allocating ack data for message %d seq %u",
307 	    hdr->nlmsg_type, hdr->nlmsg_seq);
308 	nlmsg_abort(nw);
309 }
310 
311 bool
312 _nlmsg_end_dump(struct nl_writer *nw, int error, struct nlmsghdr *hdr)
313 {
314 	if (!nlmsg_add(nw, hdr->nlmsg_pid, hdr->nlmsg_seq, NLMSG_DONE, 0, sizeof(int))) {
315 		NL_LOG(LOG_DEBUG, "Error finalizing table dump");
316 		return (false);
317 	}
318 	/* Save operation result */
319 	int *perror = nlmsg_reserve_object(nw, int);
320 	NL_LOG(LOG_DEBUG2, "record error=%d at off %d (%p)", error,
321 	    nw->buf->datalen, perror);
322 	*perror = error;
323 	nlmsg_end(nw);
324 	nw->suppress_ack = true;
325 
326 	return (true);
327 }
328 
329 /*
330  * KPI functions.
331  */
332 
333 u_int
334 nlattr_save_offset(const struct nl_writer *nw)
335 {
336 	return (nw->buf->datalen - ((char *)nw->hdr - nw->buf->data));
337 }
338 
339 void *
340 nlmsg_reserve_data_raw(struct nl_writer *nw, size_t sz)
341 {
342 	struct nl_buf *nb = nw->buf;
343 	void *data;
344 
345 	sz = NETLINK_ALIGN(sz);
346 	if (__predict_false(nb->datalen + sz > nb->buflen)) {
347 		if (!nlmsg_refill_buffer(nw, sz))
348 			return (NULL);
349 		nb = nw->buf;
350 	}
351 
352 	data = &nb->data[nb->datalen];
353 	bzero(data, sz);
354 	nb->datalen += sz;
355 
356 	return (data);
357 }
358 
359 bool
360 nlattr_add(struct nl_writer *nw, int attr_type, int attr_len, const void *data)
361 {
362 	struct nl_buf *nb = nw->buf;
363 	struct nlattr *nla;
364 	u_int required_len;
365 
366 	required_len = NLA_ALIGN(attr_len + sizeof(struct nlattr));
367 	if (__predict_false(nb->datalen + required_len > nb->buflen)) {
368 		if (!nlmsg_refill_buffer(nw, required_len))
369 			return (false);
370 		nb = nw->buf;
371 	}
372 
373 	nla = (struct nlattr *)(&nb->data[nb->datalen]);
374 
375 	nla->nla_len = attr_len + sizeof(struct nlattr);
376 	nla->nla_type = attr_type;
377 	if (attr_len > 0) {
378 		if ((attr_len % 4) != 0) {
379 			/* clear padding bytes */
380 			bzero((char *)nla + required_len - 4, 4);
381 		}
382 		memcpy((nla + 1), data, attr_len);
383 	}
384 	nb->datalen += required_len;
385 	return (true);
386 }
387 
388 #include <netlink/ktest_netlink_message_writer.h>
389