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