1 /* $NetBSD: npf_mbuf.c,v 1.6 2011/01/18 20:33:46 rmind Exp $ */ 2 3 /*- 4 * Copyright (c) 2009-2011 The NetBSD Foundation, Inc. 5 * All rights reserved. 6 * 7 * This material is based upon work partially supported by The 8 * NetBSD Foundation under a contract with Mindaugas Rasiukevicius. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 1. Redistributions of source code must retain the above copyright 14 * notice, this list of conditions and the following disclaimer. 15 * 2. Redistributions in binary form must reproduce the above copyright 16 * notice, this list of conditions and the following disclaimer in the 17 * documentation and/or other materials provided with the distribution. 18 * 19 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS 20 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 21 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 22 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS 23 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 * POSSIBILITY OF SUCH DAMAGE. 30 */ 31 32 /* 33 * NPF network buffer management interface. 34 * 35 * Network buffer in NetBSD is mbuf. Internal mbuf structures are 36 * abstracted within this source. 37 */ 38 39 #include <sys/cdefs.h> 40 __KERNEL_RCSID(0, "$NetBSD: npf_mbuf.c,v 1.6 2011/01/18 20:33:46 rmind Exp $"); 41 42 #include <sys/param.h> 43 #include <sys/mbuf.h> 44 45 #include "npf_impl.h" 46 47 /* 48 * nbuf_dataptr: return a pointer to data in nbuf. 49 */ 50 void * 51 nbuf_dataptr(nbuf_t *nbuf) 52 { 53 const struct mbuf *m = nbuf; 54 55 return mtod(m, void *); 56 } 57 58 /* 59 * nbuf_advance: advance in mbuf or chain by specified amount of bytes. 60 * 61 * => Returns new pointer to data in mbuf and NULL if offset gets invalid. 62 * => Sets nbuf to current (after advance) mbuf in the chain. 63 */ 64 void * 65 nbuf_advance(nbuf_t **nbuf, void *n_ptr, u_int n) 66 { 67 struct mbuf *m = *nbuf; 68 u_int off, wmark; 69 uint8_t *d; 70 71 /* Offset with amount to advance. */ 72 off = (uintptr_t)n_ptr - mtod(m, uintptr_t) + n; 73 wmark = m->m_len; 74 75 /* Find the mbuf according to offset. */ 76 while (__predict_false(wmark <= off)) { 77 m = m->m_next; 78 if (__predict_false(m == NULL)) { 79 /* 80 * If out of chain, then offset is 81 * higher than packet length. 82 */ 83 return NULL; 84 } 85 wmark += m->m_len; 86 } 87 88 /* Offset in mbuf data. */ 89 d = mtod(m, uint8_t *); 90 KASSERT(off >= (wmark - m->m_len)); 91 d += (off - (wmark - m->m_len)); 92 93 *nbuf = (void *)m; 94 return d; 95 } 96 97 /* 98 * nbuf_rw_datum: read or write a datum of specified length at current 99 * offset in the nbuf chain and copy datum into passed buffer. 100 * 101 * => Datum is allowed to overlap between two or more mbufs. 102 * => Note: all data in nbuf is in network byte order. 103 * => Returns 0 on success, error code on failure. 104 * 105 * Note: this function must be static inline with constant operation 106 * parameter - we expect constant propagation. 107 */ 108 109 #define NBUF_DATA_READ 0 110 #define NBUF_DATA_WRITE 1 111 112 static inline int 113 nbuf_rw_datum(const int wr, struct mbuf *m, void *n_ptr, size_t len, void *buf) 114 { 115 uint8_t *d = n_ptr, *b = buf; 116 u_int off, wmark, end; 117 118 /* Current offset in mbuf. */ 119 off = (uintptr_t)n_ptr - mtod(m, uintptr_t); 120 KASSERT(off < m->m_len); 121 wmark = m->m_len; 122 123 /* Is datum overlapping? */ 124 end = off + len; 125 while (__predict_false(end > wmark)) { 126 u_int l; 127 128 /* Get the part of current mbuf. */ 129 l = m->m_len - off; 130 KASSERT(l < len); 131 len -= l; 132 if (wr == NBUF_DATA_WRITE) { 133 while (l--) 134 *d++ = *b++; 135 } else { 136 KASSERT(wr == NBUF_DATA_READ); 137 while (l--) 138 *b++ = *d++; 139 } 140 KASSERT(len > 0); 141 142 /* Take next mbuf and continue. */ 143 m = m->m_next; 144 if (__predict_false(m == NULL)) { 145 /* 146 * If out of chain, then offset with datum 147 * length exceed the packet length. 148 */ 149 return EINVAL; 150 } 151 wmark += m->m_len; 152 d = mtod(m, uint8_t *); 153 off = 0; 154 } 155 KASSERT(n_ptr == d || mtod(m, uint8_t *) == d); 156 KASSERT(len <= m->m_len); 157 158 /* Non-overlapping case: fetch the actual data. */ 159 if (wr == NBUF_DATA_WRITE) { 160 while (len--) 161 *d++ = *b++; 162 } else { 163 KASSERT(wr == NBUF_DATA_READ); 164 while (len--) 165 *b++ = *d++; 166 } 167 return 0; 168 } 169 170 /* 171 * nbuf_{fetch|store}_datum: read/write absraction calls on nbuf_rw_datum(). 172 */ 173 int 174 nbuf_fetch_datum(nbuf_t *nbuf, void *n_ptr, size_t len, void *buf) 175 { 176 struct mbuf *m = nbuf; 177 178 return nbuf_rw_datum(NBUF_DATA_READ, m, n_ptr, len, buf); 179 } 180 181 int 182 nbuf_store_datum(nbuf_t *nbuf, void *n_ptr, size_t len, void *buf) 183 { 184 struct mbuf *m = nbuf; 185 186 KASSERT((m->m_flags & M_PKTHDR) != 0 || !M_READONLY(m)); 187 return nbuf_rw_datum(NBUF_DATA_WRITE, m, n_ptr, len, buf); 188 } 189 190 /* 191 * nbuf_advfetch: advance and fetch the datum. 192 */ 193 int 194 nbuf_advfetch(nbuf_t **nbuf, void **n_ptr, u_int n, size_t len, void *buf) 195 { 196 nbuf_t *orig_nbuf = *nbuf; 197 void *orig_nptr = *n_ptr; 198 int error; 199 200 *n_ptr = nbuf_advance(nbuf, *n_ptr, n); 201 if (__predict_false(*n_ptr != NULL)) { 202 error = nbuf_fetch_datum(*nbuf, *n_ptr, len, buf); 203 } else { 204 error = EINVAL; 205 } 206 if (__predict_false(error)) { 207 *nbuf = orig_nbuf; 208 *n_ptr = orig_nptr; 209 } 210 return error; 211 } 212 213 /* 214 * nbuf_advstore: advance and store the datum. 215 */ 216 int 217 nbuf_advstore(nbuf_t **nbuf, void **n_ptr, u_int n, size_t len, void *buf) 218 { 219 nbuf_t *orig_nbuf = *nbuf; 220 void *orig_nptr = *n_ptr; 221 int error; 222 223 *n_ptr = nbuf_advance(nbuf, *n_ptr, n); 224 if (__predict_false(*n_ptr != NULL)) { 225 error = nbuf_store_datum(*nbuf, *n_ptr, len, buf); 226 } else { 227 error = EINVAL; 228 } 229 if (__predict_false(error)) { 230 *nbuf = orig_nbuf; 231 *n_ptr = orig_nptr; 232 } 233 return error; 234 } 235 236 /* 237 * nbuf_add_tag: add a tag to specified network buffer. 238 * 239 * => Returns 0 on success, or errno on failure. 240 */ 241 int 242 nbuf_add_tag(nbuf_t *nbuf, uint32_t key, uint32_t val) 243 { 244 struct mbuf *m = nbuf; 245 struct m_tag *mt; 246 uint32_t *dat; 247 248 mt = m_tag_get(PACKET_TAG_NPF, sizeof(uint32_t), M_NOWAIT); 249 if (__predict_false(mt == NULL)) { 250 return ENOMEM; 251 } 252 dat = (uint32_t *)(mt + 1); 253 *dat = val; 254 m_tag_prepend(m, mt); 255 return 0; 256 } 257 258 /* 259 * nbuf_find_tag: find a tag in specified network buffer. 260 * 261 * => Returns 0 on success, or errno on failure. 262 */ 263 int 264 nbuf_find_tag(nbuf_t *nbuf, uint32_t key, void **data) 265 { 266 struct mbuf *m = nbuf; 267 struct m_tag *mt; 268 269 mt = m_tag_find(m, PACKET_TAG_NPF, NULL); 270 if (__predict_false(mt == NULL)) { 271 return EINVAL; 272 } 273 *data = (void *)(mt + 1); 274 return 0; 275 } 276