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