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