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