sys/netlink/netlink_message_writer.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   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/cdefs.h>
  29 __FBSDID("$FreeBSD$");
  30 #include <sys/param.h>
  31 #include <sys/malloc.h>
  32 #include <sys/lock.h>
  33 #include <sys/rmlock.h>
  34 #include <sys/mbuf.h>
  35 #include <sys/ck.h>
  36 #include <sys/socket.h>
  37 #include <sys/socketvar.h>
  38 #include <sys/syslog.h>
  39
  40 #include <netlink/netlink.h>
  41 #include <netlink/netlink_ctl.h>
  42 #include <netlink/netlink_linux.h>
  43 #include <netlink/netlink_var.h>
  44
  45 #define DEBUG_MOD_NAME  nl_writer
  46 #define DEBUG_MAX_LEVEL LOG_DEBUG3
  47 #include <netlink/netlink_debug.h>
  48 _DECLARE_DEBUG(LOG_DEBUG);
  49
  50 /*
  51  * The goal of this file is to provide convenient message writing KPI on top of
  52  * different storage methods (mbufs, uio, temporary memory chunks).
  53  *
  54  * The main KPI guarantee is the the (last) message always resides in the contiguous
  55  *  memory buffer, so one is able to update the header after writing the entire message.
  56  *
  57  * This guarantee comes with a side effect of potentially reallocating underlying
  58  *  buffer, so one needs to update the desired pointers after something is added
  59  *  to the header.
  60  *
  61  * Messaging layer contains hooks performing transparent Linux translation for the messages.
  62  *
  63  * There are 3 types of supported targets:
  64  *  * socket (adds mbufs to the socket buffer, used for message replies)
  65  *  * group (sends mbuf/chain to the specified groups, used for the notifications)
  66  *  * chain (returns mbuf chain, used in Linux message translation code)
  67  *
  68  * There are 3 types of storage:
  69  * * NS_WRITER_TYPE_MBUF (mbuf-based, most efficient, used when a single message
  70  *    fits in MCLBYTES)
  71  * * NS_WRITER_TYPE_BUF (fallback, malloc-based, used when a single message needs
  72  *    to be larger than one supported by NS_WRITER_TYPE_MBUF)
  73  * * NS_WRITER_TYPE_LBUF (malloc-based, similar to NS_WRITER_TYPE_BUF, used for
  74  *    Linux sockets, calls translation hook prior to sending messages to the socket).
  75  *
  76  * Internally, KPI switches between different types of storage when memory requirements
  77  *  change. It happens transparently to the caller.
  78  */
  79
  80
  81 typedef bool nlwriter_op_init(struct nl_writer *nw, int size, bool waitok);
  82 typedef bool nlwriter_op_write(struct nl_writer *nw, void *buf, int buflen, int cnt);
  83
  84 struct nlwriter_ops {
  85         nlwriter_op_init        *init;
  86         nlwriter_op_write       *write_socket;
  87         nlwriter_op_write       *write_group;
  88         nlwriter_op_write       *write_chain;
  89 };
  90
  91 /*
  92  * NS_WRITER_TYPE_BUF
  93  * Writes message to a temporary memory buffer,
  94  * flushing to the socket/group when buffer size limit is reached
  95  */
  96 static bool
  97 nlmsg_get_ns_buf(struct nl_writer *nw, int size, bool waitok)
  98 {
  99         int mflag = waitok ? M_WAITOK : M_NOWAIT;
 100         nw->_storage = malloc(size, M_NETLINK, mflag | M_ZERO);
 101         if (__predict_false(nw->_storage == NULL))
 102                 return (false);
 103         nw->alloc_len = size;
 104         nw->offset = 0;
 105         nw->hdr = NULL;
 106         nw->data = nw->_storage;
 107         nw->writer_type = NS_WRITER_TYPE_BUF;
 108         nw->malloc_flag = mflag;
 109         nw->num_messages = 0;
 110         nw->enomem = false;
 111         return (true);
 112 }
 113
 114 static bool
 115 nlmsg_write_socket_buf(struct nl_writer *nw, void *buf, int datalen, int cnt)
 116 {
 117         NL_LOG(LOG_DEBUG2, "IN: ptr: %p len: %d arg: %p", buf, datalen, nw);
 118         if (__predict_false(datalen == 0)) {
 119                 free(buf, M_NETLINK);
 120                 return (true);
 121         }
 122
 123         struct mbuf *m = m_getm2(NULL, datalen, nw->malloc_flag, MT_DATA, M_PKTHDR);
 124         if (__predict_false(m == NULL)) {
 125                 /* XXX: should we set sorcverr? */
 126                 free(buf, M_NETLINK);
 127                 return (false);
 128         }
 129         m_append(m, datalen, buf);
 130         free(buf, M_NETLINK);
 131
 132         int io_flags = (nw->ignore_limit) ? NL_IOF_IGNORE_LIMIT : 0;
 133         return (nl_send_one(m, (struct nlpcb *)(nw->arg_ptr), cnt, io_flags));
 134 }
 135
 136 static bool
 137 nlmsg_write_group_buf(struct nl_writer *nw, void *buf, int datalen, int cnt)
 138 {
 139         NL_LOG(LOG_DEBUG2, "IN: ptr: %p len: %d arg: %p", buf, datalen, nw->arg_ptr);
 140         if (__predict_false(datalen == 0)) {
 141                 free(buf, M_NETLINK);
 142                 return (true);
 143         }
 144
 145         struct mbuf *m = m_getm2(NULL, datalen, nw->malloc_flag, MT_DATA, M_PKTHDR);
 146         if (__predict_false(m == NULL)) {
 147                 free(buf, M_NETLINK);
 148                 return (false);
 149         }
 150         bool success = m_append(m, datalen, buf) != 0;
 151         free(buf, M_NETLINK);
 152
 153         if (!success)
 154                 return (false);
 155
 156         nl_send_group(m, cnt, nw->arg_uint >> 16, nw->arg_uint & 0xFFFF);
 157         return (true);
 158 }
 159
 160 static bool
 161 nlmsg_write_chain_buf(struct nl_writer *nw, void *buf, int datalen, int cnt)
 162 {
 163         struct mbuf **m0 = (struct mbuf **)(nw->arg_ptr);
 164         NL_LOG(LOG_DEBUG2, "IN: ptr: %p len: %d arg: %p", buf, datalen, nw->arg_ptr);
 165
 166         if (__predict_false(datalen == 0)) {
 167                 free(buf, M_NETLINK);
 168                 return (true);
 169         }
 170
 171         if (*m0 == NULL) {
 172                 struct mbuf *m;
 173
 174                 m = m_getm2(NULL, datalen, nw->malloc_flag, MT_DATA, M_PKTHDR);
 175                 if (__predict_false(m == NULL)) {
 176                         free(buf, M_NETLINK);
 177                         return (false);
 178                 }
 179                 *m0 = m;
 180         }
 181         if (__predict_false(m_append(*m0, datalen, buf) == 0)) {
 182                 free(buf, M_NETLINK);
 183                 return (false);
 184         }
 185         return (true);
 186 }
 187
 188
 189 /*
 190  * NS_WRITER_TYPE_MBUF
 191  * Writes message to the allocated mbuf,
 192  * flushing to socket/group when mbuf size limit is reached.
 193  * This is the most efficient mechanism as it avoids double-copying.
 194  *
 195  * Allocates a single mbuf suitable to store up to @size bytes of data.
 196  * If size < MHLEN (around 160 bytes), allocates mbuf with pkghdr
 197  * If size <= MCLBYTES (2k), allocate a single mbuf cluster
 198  * Otherwise, return NULL.
 199  */
 200 static bool
 201 nlmsg_get_ns_mbuf(struct nl_writer *nw, int size, bool waitok)
 202 {
 203         struct mbuf *m;
 204
 205         int mflag = waitok ? M_WAITOK : M_NOWAIT;
 206         m = m_get2(size, mflag, MT_DATA, M_PKTHDR);
 207         if (__predict_false(m == NULL))
 208                 return (false);
 209         nw->alloc_len = M_TRAILINGSPACE(m);
 210         nw->offset = 0;
 211         nw->hdr = NULL;
 212         nw->_storage = (void *)m;
 213         nw->data = mtod(m, void *);
 214         nw->writer_type = NS_WRITER_TYPE_MBUF;
 215         nw->malloc_flag = mflag;
 216         nw->num_messages = 0;
 217         nw->enomem = false;
 218         memset(nw->data, 0, size);
 219         NL_LOG(LOG_DEBUG2, "alloc mbuf %p req_len %d alloc_len %d data_ptr %p",
 220             m, size, nw->alloc_len, nw->data);
 221         return (true);
 222 }
 223
 224 static bool
 225 nlmsg_write_socket_mbuf(struct nl_writer *nw, void *buf, int datalen, int cnt)
 226 {
 227         struct mbuf *m = (struct mbuf *)buf;
 228         NL_LOG(LOG_DEBUG2, "IN: ptr: %p len: %d arg: %p", buf, datalen, nw->arg_ptr);
 229
 230         if (__predict_false(datalen == 0)) {
 231                 m_freem(m);
 232                 return (true);
 233         }
 234
 235         m->m_pkthdr.len = datalen;
 236         m->m_len = datalen;
 237         int io_flags = (nw->ignore_limit) ? NL_IOF_IGNORE_LIMIT : 0;
 238         return (nl_send_one(m, (struct nlpcb *)(nw->arg_ptr), cnt, io_flags));
 239 }
 240
 241 static bool
 242 nlmsg_write_group_mbuf(struct nl_writer *nw, void *buf, int datalen, int cnt)
 243 {
 244         struct mbuf *m = (struct mbuf *)buf;
 245         NL_LOG(LOG_DEBUG2, "IN: ptr: %p len: %d arg: %p", buf, datalen, nw->arg_ptr);
 246
 247         if (__predict_false(datalen == 0)) {
 248                 m_freem(m);
 249                 return (true);
 250         }
 251
 252         m->m_pkthdr.len = datalen;
 253         m->m_len = datalen;
 254         nl_send_group(m, cnt, nw->arg_uint >> 16, nw->arg_uint & 0xFFFF);
 255         return (true);
 256 }
 257
 258 static bool
 259 nlmsg_write_chain_mbuf(struct nl_writer *nw, void *buf, int datalen, int cnt)
 260 {
 261         struct mbuf *m_new = (struct mbuf *)buf;
 262         struct mbuf **m0 = (struct mbuf **)(nw->arg_ptr);
 263
 264         NL_LOG(LOG_DEBUG2, "IN: ptr: %p len: %d arg: %p", buf, datalen, nw->arg_ptr);
 265
 266         if (__predict_false(datalen == 0)) {
 267                 m_freem(m_new);
 268                 return (true);
 269         }
 270
 271         m_new->m_pkthdr.len = datalen;
 272         m_new->m_len = datalen;
 273
 274         if (*m0 == NULL) {
 275                 *m0 = m_new;
 276         } else {
 277                 struct mbuf *m_last;
 278                 for (m_last = *m0; m_last->m_next != NULL; m_last = m_last->m_next)
 279                         ;
 280                 m_last->m_next = m_new;
 281                 (*m0)->m_pkthdr.len += datalen;
 282         }
 283
 284         return (true);
 285 }
 286
 287 /*
 288  * NS_WRITER_TYPE_LBUF
 289  * Writes message to the allocated memory buffer,
 290  * flushing to socket/group when mbuf size limit is reached.
 291  * Calls linux handler to rewrite messages before sending to the socket.
 292  */
 293 static bool
 294 nlmsg_get_ns_lbuf(struct nl_writer *nw, int size, bool waitok)
 295 {
 296         int mflag = waitok ? M_WAITOK : M_NOWAIT;
 297         size = roundup2(size, sizeof(void *));
 298         int add_size = sizeof(struct linear_buffer) + SCRATCH_BUFFER_SIZE;
 299         char *buf = malloc(add_size + size * 2, M_NETLINK, mflag | M_ZERO);
 300         if (__predict_false(buf == NULL))
 301                 return (false);
 302
 303         /* Fill buffer header first */
 304         struct linear_buffer *lb = (struct linear_buffer *)buf;
 305         lb->base = &buf[sizeof(struct linear_buffer) + size];
 306         lb->size = size + SCRATCH_BUFFER_SIZE;
 307
 308         nw->alloc_len = size;
 309         nw->offset = 0;
 310         nw->hdr = NULL;
 311         nw->_storage = buf;
 312         nw->data = (char *)(lb + 1);
 313         nw->malloc_flag = mflag;
 314         nw->writer_type = NS_WRITER_TYPE_LBUF;
 315         nw->num_messages = 0;
 316         nw->enomem = false;
 317         return (true);
 318 }
 319
 320
 321 static bool
 322 nlmsg_write_socket_lbuf(struct nl_writer *nw, void *buf, int datalen, int cnt)
 323 {
 324         struct linear_buffer *lb = (struct linear_buffer *)buf;
 325         char *data = (char *)(lb + 1);
 326         struct nlpcb *nlp = (struct nlpcb *)(nw->arg_ptr);
 327
 328         if (__predict_false(datalen == 0)) {
 329                 free(buf, M_NETLINK);
 330                 return (true);
 331         }
 332
 333         struct mbuf *m = NULL;
 334         if (linux_netlink_p != NULL)
 335                 m = linux_netlink_p->msgs_to_linux(nlp->nl_proto, data, datalen, nlp);
 336         free(buf, M_NETLINK);
 337
 338         if (__predict_false(m == NULL)) {
 339                 /* XXX: should we set sorcverr? */
 340                 return (false);
 341         }
 342
 343         int io_flags = (nw->ignore_limit) ? NL_IOF_IGNORE_LIMIT : 0;
 344         return (nl_send_one(m, nlp, cnt, io_flags));
 345 }
 346
 347 /* Shouldn't be called (maybe except Linux code originating message) */
 348 static bool
 349 nlmsg_write_group_lbuf(struct nl_writer *nw, void *buf, int datalen, int cnt)
 350 {
 351         struct linear_buffer *lb = (struct linear_buffer *)buf;
 352         char *data = (char *)(lb + 1);
 353
 354         if (__predict_false(datalen == 0)) {
 355                 free(buf, M_NETLINK);
 356                 return (true);
 357         }
 358
 359         struct mbuf *m = m_getm2(NULL, datalen, nw->malloc_flag, MT_DATA, M_PKTHDR);
 360         if (__predict_false(m == NULL)) {
 361                 free(buf, M_NETLINK);
 362                 return (false);
 363         }
 364         m_append(m, datalen, data);
 365         free(buf, M_NETLINK);
 366
 367         nl_send_group(m, cnt, nw->arg_uint >> 16, nw->arg_uint & 0xFFFF);
 368         return (true);
 369 }
 370
 371 static const struct nlwriter_ops nlmsg_writers[] = {
 372         /* NS_WRITER_TYPE_MBUF */
 373         {
 374                 .init = nlmsg_get_ns_mbuf,
 375                 .write_socket = nlmsg_write_socket_mbuf,
 376                 .write_group = nlmsg_write_group_mbuf,
 377                 .write_chain = nlmsg_write_chain_mbuf,
 378         },
 379         /* NS_WRITER_TYPE_BUF */
 380         {
 381                 .init = nlmsg_get_ns_buf,
 382                 .write_socket = nlmsg_write_socket_buf,
 383                 .write_group = nlmsg_write_group_buf,
 384                 .write_chain = nlmsg_write_chain_buf,
 385         },
 386         /* NS_WRITER_TYPE_LBUF */
 387         {
 388                 .init = nlmsg_get_ns_lbuf,
 389                 .write_socket = nlmsg_write_socket_lbuf,
 390                 .write_group = nlmsg_write_group_lbuf,
 391         },
 392 };
 393
 394 static void
 395 nlmsg_set_callback(struct nl_writer *nw)
 396 {
 397         const struct nlwriter_ops *pops = &nlmsg_writers[nw->writer_type];
 398
 399         switch (nw->writer_target) {
 400         case NS_WRITER_TARGET_SOCKET:
 401                 nw->cb = pops->write_socket;
 402                 break;
 403         case NS_WRITER_TARGET_GROUP:
 404                 nw->cb = pops->write_group;
 405                 break;
 406         case NS_WRITER_TARGET_CHAIN:
 407                 nw->cb = pops->write_chain;
 408                 break;
 409         default:
 410                 panic("not implemented");
 411         }
 412 }
 413
 414 static bool
 415 nlmsg_get_buf_type(struct nl_writer *nw, int size, int type, bool waitok)
 416 {
 417         MPASS(type + 1 <= sizeof(nlmsg_writers) / sizeof(nlmsg_writers[0]));
 418         NL_LOG(LOG_DEBUG3, "Setting up nw %p size %d type %d", nw, size, type);
 419         return (nlmsg_writers[type].init(nw, size, waitok));
 420 }
 421
 422 static bool
 423 nlmsg_get_buf(struct nl_writer *nw, int size, bool waitok, bool is_linux)
 424 {
 425         int type;
 426
 427         if (!is_linux) {
 428                 if (__predict_true(size <= MCLBYTES))
 429                         type = NS_WRITER_TYPE_MBUF;
 430                 else
 431                         type = NS_WRITER_TYPE_BUF;
 432         } else
 433                 type = NS_WRITER_TYPE_LBUF;
 434         return (nlmsg_get_buf_type(nw, size, type, waitok));
 435 }
 436
 437 bool
 438 nlmsg_get_unicast_writer(struct nl_writer *nw, int size, struct nlpcb *nlp)
 439 {
 440         if (!nlmsg_get_buf(nw, size, false, nlp->nl_linux))
 441                 return (false);
 442         nw->arg_ptr = (void *)nlp;
 443         nw->writer_target = NS_WRITER_TARGET_SOCKET;
 444         nlmsg_set_callback(nw);
 445         return (true);
 446 }
 447
 448 bool
 449 nlmsg_get_group_writer(struct nl_writer *nw, int size, int protocol, int group_id)
 450 {
 451         if (!nlmsg_get_buf(nw, size, false, false))
 452                 return (false);
 453         nw->arg_uint = (uint64_t)protocol << 16 | (uint64_t)group_id;
 454         nw->writer_target = NS_WRITER_TARGET_GROUP;
 455         nlmsg_set_callback(nw);
 456         return (true);
 457 }
 458
 459 bool
 460 nlmsg_get_chain_writer(struct nl_writer *nw, int size, struct mbuf **pm)
 461 {
 462         if (!nlmsg_get_buf(nw, size, false, false))
 463                 return (false);
 464         *pm = NULL;
 465         nw->arg_ptr = (void *)pm;
 466         nw->writer_target = NS_WRITER_TARGET_CHAIN;
 467         nlmsg_set_callback(nw);
 468         NL_LOG(LOG_DEBUG3, "setup cb %p (need %p)", nw->cb, &nlmsg_write_chain_mbuf);
 469         return (true);
 470 }
 471
 472 void
 473 nlmsg_ignore_limit(struct nl_writer *nw)
 474 {
 475         nw->ignore_limit = true;
 476 }
 477
 478 bool
 479 nlmsg_flush(struct nl_writer *nw)
 480 {
 481
 482         if (__predict_false(nw->hdr != NULL)) {
 483                 /* Last message has not been completed, skip it. */
 484                 int completed_len = (char *)nw->hdr - nw->data;
 485                 /* Send completed messages */
 486                 nw->offset -= nw->offset - completed_len;
 487                 nw->hdr = NULL;
 488         }
 489
 490         NL_LOG(LOG_DEBUG2, "OUT");
 491         bool result = nw->cb(nw, nw->_storage, nw->offset, nw->num_messages);
 492         nw->_storage = NULL;
 493
 494         if (!result) {
 495                 NL_LOG(LOG_DEBUG, "nw %p offset %d: flush with %p() failed", nw, nw->offset, nw->cb);
 496         }
 497
 498         return (result);
 499 }
 500
 501 /*
 502  * Flushes previous data and allocates new underlying storage
 503  *  sufficient for holding at least @required_len bytes.
 504  * Return true on success.
 505  */
 506 bool
 507 nlmsg_refill_buffer(struct nl_writer *nw, int required_len)
 508 {
 509         struct nl_writer ns_new = {};
 510         int completed_len, new_len;
 511
 512         if (nw->enomem)
 513                 return (false);
 514
 515         NL_LOG(LOG_DEBUG3, "no space at offset %d/%d (want %d), trying to reclaim",
 516             nw->offset, nw->alloc_len, required_len);
 517
 518         /* Calculated new buffer size and allocate it s*/
 519         completed_len = (nw->hdr != NULL) ? (char *)nw->hdr - nw->data : nw->offset;
 520         if (completed_len > 0 && required_len < MCLBYTES) {
 521                 /* We already ran out of space, use the largest effective size */
 522                 new_len = max(nw->alloc_len, MCLBYTES);
 523         } else {
 524                 if (nw->alloc_len < MCLBYTES)
 525                         new_len = MCLBYTES;
 526                 else
 527                         new_len = nw->alloc_len * 2;
 528                 while (new_len < required_len)
 529                         new_len *= 2;
 530         }
 531         bool waitok = (nw->malloc_flag == M_WAITOK);
 532         bool is_linux = (nw->writer_type == NS_WRITER_TYPE_LBUF);
 533         if (!nlmsg_get_buf(&ns_new, new_len, waitok, is_linux)) {
 534                 nw->enomem = true;
 535                 NL_LOG(LOG_DEBUG, "getting new buf failed, setting ENOMEM");
 536                 return (false);
 537         }
 538         if (nw->ignore_limit)
 539                 nlmsg_ignore_limit(&ns_new);
 540
 541         /* Update callback data */
 542         ns_new.writer_target = nw->writer_target;
 543         nlmsg_set_callback(&ns_new);
 544         ns_new.arg_uint = nw->arg_uint;
 545
 546         /* Copy last (unfinished) header to the new storage */
 547         int last_len = nw->offset - completed_len;
 548         if (last_len > 0) {
 549                 memcpy(ns_new.data, nw->hdr, last_len);
 550                 ns_new.hdr = (struct nlmsghdr *)ns_new.data;
 551                 ns_new.offset = last_len;
 552         }
 553
 554         NL_LOG(LOG_DEBUG2, "completed: %d bytes, copied: %d bytes", completed_len, last_len);
 555
 556         /* Flush completed headers & switch to the new nw */
 557         nlmsg_flush(nw);
 558         memcpy(nw, &ns_new, sizeof(struct nl_writer));
 559         NL_LOG(LOG_DEBUG2, "switched buffer: used %d/%d bytes", nw->offset, nw->alloc_len);
 560
 561         return (true);
 562 }
 563
 564 bool
 565 nlmsg_add(struct nl_writer *nw, uint32_t portid, uint32_t seq, uint16_t type,
 566     uint16_t flags, uint32_t len)
 567 {
 568         struct nlmsghdr *hdr;
 569
 570         MPASS(nw->hdr == NULL);
 571
 572         int required_len = NETLINK_ALIGN(len + sizeof(struct nlmsghdr));
 573         if (__predict_false(nw->offset + required_len > nw->alloc_len)) {
 574                 if (!nlmsg_refill_buffer(nw, required_len))
 575                         return (false);
 576         }
 577
 578         hdr = (struct nlmsghdr *)(&nw->data[nw->offset]);
 579
 580         hdr->nlmsg_len = len;
 581         hdr->nlmsg_type = type;
 582         hdr->nlmsg_flags = flags;
 583         hdr->nlmsg_seq = seq;
 584         hdr->nlmsg_pid = portid;
 585
 586         nw->hdr = hdr;
 587         nw->offset += sizeof(struct nlmsghdr);
 588
 589         return (true);
 590 }
 591
 592 bool
 593 nlmsg_end(struct nl_writer *nw)
 594 {
 595         MPASS(nw->hdr != NULL);
 596
 597         if (nw->enomem) {
 598                 NL_LOG(LOG_DEBUG, "ENOMEM when dumping message");
 599                 nlmsg_abort(nw);
 600                 return (false);
 601         }
 602
 603         nw->hdr->nlmsg_len = (uint32_t)(nw->data + nw->offset - (char *)nw->hdr);
 604         NL_LOG(LOG_DEBUG2, "wrote msg len: %u type: %d: flags: 0x%X seq: %u pid: %u",
 605             nw->hdr->nlmsg_len, nw->hdr->nlmsg_type, nw->hdr->nlmsg_flags,
 606             nw->hdr->nlmsg_seq, nw->hdr->nlmsg_pid);
 607         nw->hdr = NULL;
 608         nw->num_messages++;
 609         return (true);
 610 }
 611
 612 void
 613 nlmsg_abort(struct nl_writer *nw)
 614 {
 615         if (nw->hdr != NULL) {
 616                 nw->offset = (uint32_t)((char *)nw->hdr - nw->data);
 617                 nw->hdr = NULL;
 618         }
 619 }
 620
 621 void
 622 nlmsg_ack(struct nlpcb *nlp, int error, struct nlmsghdr *hdr,
 623     struct nl_pstate *npt)
 624 {
 625         struct nlmsgerr *errmsg;
 626         int payload_len;
 627         uint32_t flags = nlp->nl_flags;
 628         struct nl_writer *nw = npt->nw;
 629         bool cap_ack;
 630
 631         payload_len = sizeof(struct nlmsgerr);
 632
 633         /*
 634          * The only case when we send the full message in the
 635          * reply is when there is an error and NETLINK_CAP_ACK
 636          * is not set.
 637          */
 638         cap_ack = (error == 0) || (flags & NLF_CAP_ACK);
 639         if (!cap_ack)
 640                 payload_len += hdr->nlmsg_len - sizeof(struct nlmsghdr);
 641         payload_len = NETLINK_ALIGN(payload_len);
 642
 643         uint16_t nl_flags = cap_ack ? NLM_F_CAPPED : 0;
 644         if ((npt->err_msg || npt->err_off) && nlp->nl_flags & NLF_EXT_ACK)
 645                 nl_flags |= NLM_F_ACK_TLVS;
 646
 647         /*
 648          * TODO: handle cookies
 649          */
 650
 651         NL_LOG(LOG_DEBUG3, "acknowledging message type %d seq %d",
 652             hdr->nlmsg_type, hdr->nlmsg_seq);
 653
 654         if (!nlmsg_add(nw, nlp->nl_port, hdr->nlmsg_seq, NLMSG_ERROR, nl_flags, payload_len))
 655                 goto enomem;
 656
 657         errmsg = nlmsg_reserve_data(nw, payload_len, struct nlmsgerr);
 658         errmsg->error = error;
 659         /* In case of error copy the whole message, else just the header */
 660         memcpy(&errmsg->msg, hdr, cap_ack ? sizeof(*hdr) : hdr->nlmsg_len);
 661
 662         if (npt->err_msg != NULL && nlp->nl_flags & NLF_EXT_ACK)
 663                 nlattr_add_string(nw, NLMSGERR_ATTR_MSG, npt->err_msg);
 664         if (npt->err_off != 0 && nlp->nl_flags & NLF_EXT_ACK)
 665                 nlattr_add_u32(nw, NLMSGERR_ATTR_OFFS, npt->err_off);
 666
 667         if (nlmsg_end(nw))
 668                 return;
 669 enomem:
 670         NLP_LOG(LOG_DEBUG, nlp, "error allocating ack data for message %d seq %u",
 671             hdr->nlmsg_type, hdr->nlmsg_seq);
 672         nlmsg_abort(nw);
 673 }
 674
 675 bool
 676 nlmsg_end_dump(struct nl_writer *nw, int error, struct nlmsghdr *hdr)
 677 {
 678         if (!nlmsg_add(nw, hdr->nlmsg_pid, hdr->nlmsg_seq, NLMSG_DONE, 0, sizeof(int))) {
 679                 NL_LOG(LOG_DEBUG, "Error finalizing table dump");
 680                 return (false);
 681         }
 682         /* Save operation result */
 683         int *perror = nlmsg_reserve_object(nw, int);
 684         NL_LOG(LOG_DEBUG2, "record error=%d at off %d (%p)", error,
 685             nw->offset, perror);
 686         *perror = error;
 687         nlmsg_end(nw);
 688         nw->suppress_ack = true;
 689
 690         return (true);
 691 }