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