Annotation of sys/netinet/ip_esp.c, Revision 1.1
1.1 ! nbrk 1: /* $OpenBSD: ip_esp.c,v 1.100 2006/12/15 09:32:30 otto Exp $ */
! 2: /*
! 3: * The authors of this code are John Ioannidis (ji@tla.org),
! 4: * Angelos D. Keromytis (kermit@csd.uch.gr) and
! 5: * Niels Provos (provos@physnet.uni-hamburg.de).
! 6: *
! 7: * The original version of this code was written by John Ioannidis
! 8: * for BSD/OS in Athens, Greece, in November 1995.
! 9: *
! 10: * Ported to OpenBSD and NetBSD, with additional transforms, in December 1996,
! 11: * by Angelos D. Keromytis.
! 12: *
! 13: * Additional transforms and features in 1997 and 1998 by Angelos D. Keromytis
! 14: * and Niels Provos.
! 15: *
! 16: * Additional features in 1999 by Angelos D. Keromytis.
! 17: *
! 18: * Copyright (C) 1995, 1996, 1997, 1998, 1999 by John Ioannidis,
! 19: * Angelos D. Keromytis and Niels Provos.
! 20: * Copyright (c) 2001 Angelos D. Keromytis.
! 21: *
! 22: * Permission to use, copy, and modify this software with or without fee
! 23: * is hereby granted, provided that this entire notice is included in
! 24: * all copies of any software which is or includes a copy or
! 25: * modification of this software.
! 26: * You may use this code under the GNU public license if you so wish. Please
! 27: * contribute changes back to the authors under this freer than GPL license
! 28: * so that we may further the use of strong encryption without limitations to
! 29: * all.
! 30: *
! 31: * THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR
! 32: * IMPLIED WARRANTY. IN PARTICULAR, NONE OF THE AUTHORS MAKES ANY
! 33: * REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE
! 34: * MERCHANTABILITY OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR
! 35: * PURPOSE.
! 36: */
! 37:
! 38: #include "pfsync.h"
! 39:
! 40: #include <sys/param.h>
! 41: #include <sys/systm.h>
! 42: #include <sys/mbuf.h>
! 43: #include <sys/socket.h>
! 44:
! 45: #include <net/if.h>
! 46: #include <net/bpf.h>
! 47:
! 48: #include <dev/rndvar.h>
! 49:
! 50: #ifdef INET
! 51: #include <netinet/in.h>
! 52: #include <netinet/in_systm.h>
! 53: #include <netinet/ip.h>
! 54: #include <netinet/ip_var.h>
! 55: #endif /* INET */
! 56:
! 57: #ifdef INET6
! 58: #ifndef INET
! 59: #include <netinet/in.h>
! 60: #endif
! 61: #include <netinet/ip6.h>
! 62: #endif /* INET6 */
! 63:
! 64: #include <netinet/ip_ipsp.h>
! 65: #include <netinet/ip_esp.h>
! 66: #include <net/pfkeyv2.h>
! 67: #include <net/if_enc.h>
! 68:
! 69: #if NPFSYNC > 0
! 70: #include <net/pfvar.h>
! 71: #include <net/if_pfsync.h>
! 72: #endif /* NPFSYNC > 0 */
! 73:
! 74: #include <crypto/cryptodev.h>
! 75: #include <crypto/xform.h>
! 76:
! 77: #include "bpfilter.h"
! 78:
! 79: #ifdef ENCDEBUG
! 80: #define DPRINTF(x) if (encdebug) printf x
! 81: #else
! 82: #define DPRINTF(x)
! 83: #endif
! 84:
! 85: struct espstat espstat;
! 86:
! 87: /*
! 88: * esp_attach() is called from the transformation initialization code.
! 89: */
! 90: int
! 91: esp_attach()
! 92: {
! 93: return 0;
! 94: }
! 95:
! 96: /*
! 97: * esp_init() is called when an SPI is being set up.
! 98: */
! 99: int
! 100: esp_init(struct tdb *tdbp, struct xformsw *xsp, struct ipsecinit *ii)
! 101: {
! 102: struct enc_xform *txform = NULL;
! 103: struct auth_hash *thash = NULL;
! 104: struct cryptoini cria, crie;
! 105:
! 106: if (!ii->ii_encalg && !ii->ii_authalg) {
! 107: DPRINTF(("esp_init(): neither authentication nor encryption "
! 108: "algorithm given"));
! 109: return EINVAL;
! 110: }
! 111:
! 112: if (ii->ii_encalg) {
! 113: switch (ii->ii_encalg) {
! 114: case SADB_EALG_NULL:
! 115: txform = &enc_xform_null;
! 116: break;
! 117:
! 118: case SADB_EALG_DESCBC:
! 119: txform = &enc_xform_des;
! 120: break;
! 121:
! 122: case SADB_EALG_3DESCBC:
! 123: txform = &enc_xform_3des;
! 124: break;
! 125:
! 126: case SADB_X_EALG_AES:
! 127: txform = &enc_xform_rijndael128;
! 128: break;
! 129:
! 130: case SADB_X_EALG_AESCTR:
! 131: txform = &enc_xform_aes_ctr;
! 132: break;
! 133:
! 134: case SADB_X_EALG_BLF:
! 135: txform = &enc_xform_blf;
! 136: break;
! 137:
! 138: case SADB_X_EALG_CAST:
! 139: txform = &enc_xform_cast5;
! 140: break;
! 141:
! 142: case SADB_X_EALG_SKIPJACK:
! 143: txform = &enc_xform_skipjack;
! 144: break;
! 145:
! 146: default:
! 147: DPRINTF(("esp_init(): unsupported encryption algorithm %d specified\n", ii->ii_encalg));
! 148: return EINVAL;
! 149: }
! 150:
! 151: if (ii->ii_enckeylen < txform->minkey) {
! 152: DPRINTF(("esp_init(): keylength %d too small (min length is %d) for algorithm %s\n", ii->ii_enckeylen, txform->minkey, txform->name));
! 153: return EINVAL;
! 154: }
! 155:
! 156: if (ii->ii_enckeylen > txform->maxkey) {
! 157: DPRINTF(("esp_init(): keylength %d too large (max length is %d) for algorithm %s\n", ii->ii_enckeylen, txform->maxkey, txform->name));
! 158: return EINVAL;
! 159: }
! 160:
! 161: tdbp->tdb_encalgxform = txform;
! 162:
! 163: DPRINTF(("esp_init(): initialized TDB with enc algorithm %s\n",
! 164: txform->name));
! 165:
! 166: tdbp->tdb_ivlen = txform->ivsize;
! 167: if (tdbp->tdb_flags & TDBF_HALFIV)
! 168: tdbp->tdb_ivlen /= 2;
! 169: }
! 170:
! 171: if (ii->ii_authalg) {
! 172: switch (ii->ii_authalg) {
! 173: case SADB_AALG_MD5HMAC:
! 174: thash = &auth_hash_hmac_md5_96;
! 175: break;
! 176:
! 177: case SADB_AALG_SHA1HMAC:
! 178: thash = &auth_hash_hmac_sha1_96;
! 179: break;
! 180:
! 181: case SADB_X_AALG_RIPEMD160HMAC:
! 182: thash = &auth_hash_hmac_ripemd_160_96;
! 183: break;
! 184:
! 185: case SADB_X_AALG_SHA2_256:
! 186: thash = &auth_hash_hmac_sha2_256_96;
! 187: break;
! 188:
! 189: case SADB_X_AALG_SHA2_384:
! 190: thash = &auth_hash_hmac_sha2_384_96;
! 191: break;
! 192:
! 193: case SADB_X_AALG_SHA2_512:
! 194: thash = &auth_hash_hmac_sha2_512_96;
! 195: break;
! 196:
! 197: default:
! 198: DPRINTF(("esp_init(): unsupported authentication algorithm %d specified\n", ii->ii_authalg));
! 199: return EINVAL;
! 200: }
! 201:
! 202: if (ii->ii_authkeylen != thash->keysize) {
! 203: DPRINTF(("esp_init(): keylength %d doesn't match algorithm %s keysize (%d)\n", ii->ii_authkeylen, thash->name, thash->keysize));
! 204: return EINVAL;
! 205: }
! 206:
! 207: tdbp->tdb_authalgxform = thash;
! 208:
! 209: DPRINTF(("esp_init(): initialized TDB with hash algorithm %s\n",
! 210: thash->name));
! 211: }
! 212:
! 213: tdbp->tdb_xform = xsp;
! 214: tdbp->tdb_bitmap = 0;
! 215: tdbp->tdb_rpl = AH_HMAC_INITIAL_RPL;
! 216:
! 217: /* Initialize crypto session */
! 218: if (tdbp->tdb_encalgxform) {
! 219: /* Save the raw keys */
! 220: tdbp->tdb_emxkeylen = ii->ii_enckeylen;
! 221: MALLOC(tdbp->tdb_emxkey, u_int8_t *, tdbp->tdb_emxkeylen,
! 222: M_XDATA, M_WAITOK);
! 223: bcopy(ii->ii_enckey, tdbp->tdb_emxkey, tdbp->tdb_emxkeylen);
! 224:
! 225: bzero(&crie, sizeof(crie));
! 226:
! 227: crie.cri_alg = tdbp->tdb_encalgxform->type;
! 228:
! 229: if (tdbp->tdb_authalgxform)
! 230: crie.cri_next = &cria;
! 231: else
! 232: crie.cri_next = NULL;
! 233:
! 234: crie.cri_klen = ii->ii_enckeylen * 8;
! 235: crie.cri_key = ii->ii_enckey;
! 236: /* XXX Rounds ? */
! 237: }
! 238:
! 239: if (tdbp->tdb_authalgxform) {
! 240: /* Save the raw keys */
! 241: tdbp->tdb_amxkeylen = ii->ii_authkeylen;
! 242: MALLOC(tdbp->tdb_amxkey, u_int8_t *, tdbp->tdb_amxkeylen, M_XDATA,
! 243: M_WAITOK);
! 244: bcopy(ii->ii_authkey, tdbp->tdb_amxkey, tdbp->tdb_amxkeylen);
! 245:
! 246: bzero(&cria, sizeof(cria));
! 247:
! 248: cria.cri_alg = tdbp->tdb_authalgxform->type;
! 249: cria.cri_next = NULL;
! 250: cria.cri_klen = ii->ii_authkeylen * 8;
! 251: cria.cri_key = ii->ii_authkey;
! 252: }
! 253:
! 254: return crypto_newsession(&tdbp->tdb_cryptoid,
! 255: (tdbp->tdb_encalgxform ? &crie : &cria), 0);
! 256: }
! 257:
! 258: /*
! 259: * Paranoia.
! 260: */
! 261: int
! 262: esp_zeroize(struct tdb *tdbp)
! 263: {
! 264: int err;
! 265:
! 266: if (tdbp->tdb_amxkey) {
! 267: bzero(tdbp->tdb_amxkey, tdbp->tdb_amxkeylen);
! 268: FREE(tdbp->tdb_amxkey, M_XDATA);
! 269: tdbp->tdb_amxkey = NULL;
! 270: }
! 271:
! 272: if (tdbp->tdb_emxkey) {
! 273: bzero(tdbp->tdb_emxkey, tdbp->tdb_emxkeylen);
! 274: FREE(tdbp->tdb_emxkey, M_XDATA);
! 275: tdbp->tdb_emxkey = NULL;
! 276: }
! 277:
! 278: err = crypto_freesession(tdbp->tdb_cryptoid);
! 279: tdbp->tdb_cryptoid = 0;
! 280: return err;
! 281: }
! 282:
! 283: #define MAXBUFSIZ (AH_ALEN_MAX > ESP_MAX_IVS ? AH_ALEN_MAX : ESP_MAX_IVS)
! 284:
! 285: /*
! 286: * ESP input processing, called (eventually) through the protocol switch.
! 287: */
! 288: int
! 289: esp_input(struct mbuf *m, struct tdb *tdb, int skip, int protoff)
! 290: {
! 291: struct auth_hash *esph = (struct auth_hash *) tdb->tdb_authalgxform;
! 292: struct enc_xform *espx = (struct enc_xform *) tdb->tdb_encalgxform;
! 293: struct tdb_crypto *tc;
! 294: int plen, alen, hlen;
! 295: struct m_tag *mtag;
! 296: u_int32_t btsx;
! 297:
! 298: struct cryptodesc *crde = NULL, *crda = NULL;
! 299: struct cryptop *crp;
! 300:
! 301: /* Determine the ESP header length */
! 302: if (tdb->tdb_flags & TDBF_NOREPLAY)
! 303: hlen = sizeof(u_int32_t) + tdb->tdb_ivlen; /* "old" ESP */
! 304: else
! 305: hlen = 2 * sizeof(u_int32_t) + tdb->tdb_ivlen; /* "new" ESP */
! 306:
! 307: if (esph)
! 308: alen = AH_HMAC_HASHLEN;
! 309: else
! 310: alen = 0;
! 311:
! 312: plen = m->m_pkthdr.len - (skip + hlen + alen);
! 313: if (plen <= 0) {
! 314: DPRINTF(("esp_input: invalid payload length\n"));
! 315: espstat.esps_badilen++;
! 316: m_freem(m);
! 317: return EINVAL;
! 318: }
! 319:
! 320: if (espx) {
! 321: /*
! 322: * Verify payload length is multiple of encryption algorithm
! 323: * block size.
! 324: */
! 325: if (plen & (espx->blocksize - 1)) {
! 326: DPRINTF(("esp_input(): payload of %d octets not a multiple of %d octets, SA %s/%08x\n", plen, espx->blocksize, ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 327: espstat.esps_badilen++;
! 328: m_freem(m);
! 329: return EINVAL;
! 330: }
! 331: }
! 332:
! 333: /* Replay window checking, if appropriate -- no value commitment. */
! 334: if ((tdb->tdb_wnd > 0) && (!(tdb->tdb_flags & TDBF_NOREPLAY))) {
! 335: m_copydata(m, skip + sizeof(u_int32_t), sizeof(u_int32_t),
! 336: (unsigned char *) &btsx);
! 337: btsx = ntohl(btsx);
! 338:
! 339: switch (checkreplaywindow32(btsx, 0, &(tdb->tdb_rpl),
! 340: tdb->tdb_wnd, &(tdb->tdb_bitmap), 0)) {
! 341: case 0: /* All's well */
! 342: break;
! 343:
! 344: case 1:
! 345: m_freem(m);
! 346: DPRINTF(("esp_input(): replay counter wrapped for SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 347: espstat.esps_wrap++;
! 348: return EACCES;
! 349:
! 350: case 2:
! 351: case 3:
! 352: DPRINTF(("esp_input(): duplicate packet received in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 353: m_freem(m);
! 354: return EACCES;
! 355:
! 356: default:
! 357: m_freem(m);
! 358: DPRINTF(("esp_input(): bogus value from checkreplaywindow32() in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 359: espstat.esps_replay++;
! 360: return EACCES;
! 361: }
! 362: }
! 363:
! 364: /* Update the counters */
! 365: tdb->tdb_cur_bytes += m->m_pkthdr.len - skip - hlen - alen;
! 366: espstat.esps_ibytes += m->m_pkthdr.len - skip - hlen - alen;
! 367:
! 368: /* Hard expiration */
! 369: if ((tdb->tdb_flags & TDBF_BYTES) &&
! 370: (tdb->tdb_cur_bytes >= tdb->tdb_exp_bytes)) {
! 371: pfkeyv2_expire(tdb, SADB_EXT_LIFETIME_HARD);
! 372: tdb_delete(tdb);
! 373: m_freem(m);
! 374: return ENXIO;
! 375: }
! 376:
! 377: /* Notify on soft expiration */
! 378: if ((tdb->tdb_flags & TDBF_SOFT_BYTES) &&
! 379: (tdb->tdb_cur_bytes >= tdb->tdb_soft_bytes)) {
! 380: pfkeyv2_expire(tdb, SADB_EXT_LIFETIME_SOFT);
! 381: tdb->tdb_flags &= ~TDBF_SOFT_BYTES; /* Turn off checking */
! 382: }
! 383:
! 384: #ifdef notyet
! 385: /* Find out if we've already done crypto */
! 386: for (mtag = m_tag_find(m, PACKET_TAG_IPSEC_IN_CRYPTO_DONE, NULL);
! 387: mtag != NULL;
! 388: mtag = m_tag_find(m, PACKET_TAG_IPSEC_IN_CRYPTO_DONE, mtag)) {
! 389: struct tdb_ident *tdbi;
! 390:
! 391: tdbi = (struct tdb_ident *) (mtag + 1);
! 392: if (tdbi->proto == tdb->tdb_sproto && tdbi->spi == tdb->tdb_spi &&
! 393: !bcmp(&tdbi->dst, &tdb->tdb_dst, sizeof(union sockaddr_union)))
! 394: break;
! 395: }
! 396: #else
! 397: mtag = NULL;
! 398: #endif
! 399:
! 400: /* Get crypto descriptors */
! 401: crp = crypto_getreq(esph && espx ? 2 : 1);
! 402: if (crp == NULL) {
! 403: m_freem(m);
! 404: DPRINTF(("esp_input(): failed to acquire crypto descriptors\n"));
! 405: espstat.esps_crypto++;
! 406: return ENOBUFS;
! 407: }
! 408:
! 409: /* Get IPsec-specific opaque pointer */
! 410: if (esph == NULL || mtag != NULL)
! 411: MALLOC(tc, struct tdb_crypto *, sizeof(struct tdb_crypto),
! 412: M_XDATA, M_NOWAIT);
! 413: else
! 414: MALLOC(tc, struct tdb_crypto *,
! 415: sizeof(struct tdb_crypto) + alen, M_XDATA, M_NOWAIT);
! 416: if (tc == NULL) {
! 417: m_freem(m);
! 418: crypto_freereq(crp);
! 419: DPRINTF(("esp_input(): failed to allocate tdb_crypto\n"));
! 420: espstat.esps_crypto++;
! 421: return ENOBUFS;
! 422: }
! 423:
! 424: bzero(tc, sizeof(struct tdb_crypto));
! 425: tc->tc_ptr = (caddr_t) mtag;
! 426:
! 427: if (esph) {
! 428: crda = crp->crp_desc;
! 429: crde = crda->crd_next;
! 430:
! 431: /* Authentication descriptor */
! 432: crda->crd_skip = skip;
! 433: crda->crd_len = m->m_pkthdr.len - (skip + alen);
! 434: crda->crd_inject = m->m_pkthdr.len - alen;
! 435:
! 436: crda->crd_alg = esph->type;
! 437: crda->crd_key = tdb->tdb_amxkey;
! 438: crda->crd_klen = tdb->tdb_amxkeylen * 8;
! 439:
! 440: /* Copy the authenticator */
! 441: if (mtag == NULL)
! 442: m_copydata(m, m->m_pkthdr.len - alen, alen, (caddr_t) (tc + 1));
! 443: } else
! 444: crde = crp->crp_desc;
! 445:
! 446: /* Crypto operation descriptor */
! 447: crp->crp_ilen = m->m_pkthdr.len; /* Total input length */
! 448: crp->crp_flags = CRYPTO_F_IMBUF;
! 449: crp->crp_buf = (caddr_t) m;
! 450: crp->crp_callback = (int (*) (struct cryptop *)) esp_input_cb;
! 451: crp->crp_sid = tdb->tdb_cryptoid;
! 452: crp->crp_opaque = (caddr_t) tc;
! 453:
! 454: /* These are passed as-is to the callback */
! 455: tc->tc_skip = skip;
! 456: tc->tc_protoff = protoff;
! 457: tc->tc_spi = tdb->tdb_spi;
! 458: tc->tc_proto = tdb->tdb_sproto;
! 459: bcopy(&tdb->tdb_dst, &tc->tc_dst, sizeof(union sockaddr_union));
! 460:
! 461: /* Decryption descriptor */
! 462: if (espx) {
! 463: crde->crd_skip = skip + hlen;
! 464: crde->crd_len = m->m_pkthdr.len - (skip + hlen + alen);
! 465: crde->crd_inject = skip + hlen - tdb->tdb_ivlen;
! 466:
! 467: if (tdb->tdb_flags & TDBF_HALFIV) {
! 468: /* Copy half-IV from packet */
! 469: m_copydata(m, crde->crd_inject, tdb->tdb_ivlen, crde->crd_iv);
! 470:
! 471: /* Cook IV */
! 472: for (btsx = 0; btsx < tdb->tdb_ivlen; btsx++)
! 473: crde->crd_iv[tdb->tdb_ivlen + btsx] = ~crde->crd_iv[btsx];
! 474:
! 475: crde->crd_flags |= CRD_F_IV_EXPLICIT;
! 476: }
! 477:
! 478: crde->crd_alg = espx->type;
! 479: crde->crd_key = tdb->tdb_emxkey;
! 480: crde->crd_klen = tdb->tdb_emxkeylen * 8;
! 481: /* XXX Rounds ? */
! 482: }
! 483:
! 484: if (mtag == NULL)
! 485: return crypto_dispatch(crp);
! 486: else
! 487: return esp_input_cb(crp);
! 488: }
! 489:
! 490: /*
! 491: * ESP input callback, called directly by the crypto driver.
! 492: */
! 493: int
! 494: esp_input_cb(void *op)
! 495: {
! 496: u_int8_t lastthree[3], aalg[AH_HMAC_HASHLEN];
! 497: int s, hlen, roff, skip, protoff, error;
! 498: struct mbuf *m1, *mo, *m;
! 499: struct auth_hash *esph;
! 500: struct tdb_crypto *tc;
! 501: struct cryptop *crp;
! 502: struct m_tag *mtag;
! 503: struct tdb *tdb;
! 504: u_int32_t btsx;
! 505: caddr_t ptr;
! 506:
! 507: crp = (struct cryptop *) op;
! 508:
! 509: tc = (struct tdb_crypto *) crp->crp_opaque;
! 510: skip = tc->tc_skip;
! 511: protoff = tc->tc_protoff;
! 512: mtag = (struct m_tag *) tc->tc_ptr;
! 513:
! 514: m = (struct mbuf *) crp->crp_buf;
! 515: if (m == NULL) {
! 516: /* Shouldn't happen... */
! 517: FREE(tc, M_XDATA);
! 518: crypto_freereq(crp);
! 519: espstat.esps_crypto++;
! 520: DPRINTF(("esp_input_cb(): bogus returned buffer from crypto\n"));
! 521: return (EINVAL);
! 522: }
! 523:
! 524: s = spltdb();
! 525:
! 526: tdb = gettdb(tc->tc_spi, &tc->tc_dst, tc->tc_proto);
! 527: if (tdb == NULL) {
! 528: FREE(tc, M_XDATA);
! 529: espstat.esps_notdb++;
! 530: DPRINTF(("esp_input_cb(): TDB is expired while in crypto"));
! 531: error = EPERM;
! 532: goto baddone;
! 533: }
! 534:
! 535: esph = (struct auth_hash *) tdb->tdb_authalgxform;
! 536:
! 537: /* Check for crypto errors */
! 538: if (crp->crp_etype) {
! 539: if (crp->crp_etype == EAGAIN) {
! 540: /* Reset the session ID */
! 541: if (tdb->tdb_cryptoid != 0)
! 542: tdb->tdb_cryptoid = crp->crp_sid;
! 543: splx(s);
! 544: return crypto_dispatch(crp);
! 545: }
! 546: FREE(tc, M_XDATA);
! 547: espstat.esps_noxform++;
! 548: DPRINTF(("esp_input_cb(): crypto error %d\n", crp->crp_etype));
! 549: error = crp->crp_etype;
! 550: goto baddone;
! 551: }
! 552:
! 553: /* If authentication was performed, check now. */
! 554: if (esph != NULL) {
! 555: /*
! 556: * If we have a tag, it means an IPsec-aware NIC did the verification
! 557: * for us.
! 558: */
! 559: if (mtag == NULL) {
! 560: /* Copy the authenticator from the packet */
! 561: m_copydata(m, m->m_pkthdr.len - esph->authsize,
! 562: esph->authsize, aalg);
! 563:
! 564: ptr = (caddr_t) (tc + 1);
! 565:
! 566: /* Verify authenticator */
! 567: if (bcmp(ptr, aalg, esph->authsize)) {
! 568: FREE(tc, M_XDATA);
! 569: DPRINTF(("esp_input_cb(): authentication failed for packet in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 570: espstat.esps_badauth++;
! 571: error = EACCES;
! 572: goto baddone;
! 573: }
! 574: }
! 575:
! 576: /* Remove trailing authenticator */
! 577: m_adj(m, -(esph->authsize));
! 578: }
! 579: FREE(tc, M_XDATA);
! 580:
! 581: /* Replay window checking, if appropriate */
! 582: if ((tdb->tdb_wnd > 0) && (!(tdb->tdb_flags & TDBF_NOREPLAY))) {
! 583: m_copydata(m, skip + sizeof(u_int32_t), sizeof(u_int32_t),
! 584: (unsigned char *) &btsx);
! 585: btsx = ntohl(btsx);
! 586:
! 587: switch (checkreplaywindow32(btsx, 0, &(tdb->tdb_rpl),
! 588: tdb->tdb_wnd, &(tdb->tdb_bitmap), 1)) {
! 589: case 0: /* All's well */
! 590: #if NPFSYNC > 0
! 591: pfsync_update_tdb(tdb,0);
! 592: #endif
! 593: break;
! 594:
! 595: case 1:
! 596: DPRINTF(("esp_input_cb(): replay counter wrapped for SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 597: espstat.esps_wrap++;
! 598: error = EACCES;
! 599: goto baddone;
! 600:
! 601: case 2:
! 602: case 3:
! 603: DPRINTF(("esp_input_cb(): duplicate packet received in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 604: error = EACCES;
! 605: goto baddone;
! 606:
! 607: default:
! 608: DPRINTF(("esp_input_cb(): bogus value from checkreplaywindow32() in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 609: espstat.esps_replay++;
! 610: error = EACCES;
! 611: goto baddone;
! 612: }
! 613: }
! 614:
! 615: /* Release the crypto descriptors */
! 616: crypto_freereq(crp);
! 617:
! 618: /* Determine the ESP header length */
! 619: if (tdb->tdb_flags & TDBF_NOREPLAY)
! 620: hlen = sizeof(u_int32_t) + tdb->tdb_ivlen; /* "old" ESP */
! 621: else
! 622: hlen = 2 * sizeof(u_int32_t) + tdb->tdb_ivlen; /* "new" ESP */
! 623:
! 624: /* Find beginning of ESP header */
! 625: m1 = m_getptr(m, skip, &roff);
! 626: if (m1 == NULL) {
! 627: espstat.esps_hdrops++;
! 628: splx(s);
! 629: DPRINTF(("esp_input_cb(): bad mbuf chain, SA %s/%08x\n",
! 630: ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 631: m_freem(m);
! 632: return EINVAL;
! 633: }
! 634:
! 635: /* Remove the ESP header and IV from the mbuf. */
! 636: if (roff == 0) {
! 637: /* The ESP header was conveniently at the beginning of the mbuf */
! 638: m_adj(m1, hlen);
! 639: if (!(m1->m_flags & M_PKTHDR))
! 640: m->m_pkthdr.len -= hlen;
! 641: } else if (roff + hlen >= m1->m_len) {
! 642: /*
! 643: * Part or all of the ESP header is at the end of this mbuf, so
! 644: * first let's remove the remainder of the ESP header from the
! 645: * beginning of the remainder of the mbuf chain, if any.
! 646: */
! 647: if (roff + hlen > m1->m_len) {
! 648: /* Adjust the next mbuf by the remainder */
! 649: m_adj(m1->m_next, roff + hlen - m1->m_len);
! 650:
! 651: /* The second mbuf is guaranteed not to have a pkthdr... */
! 652: m->m_pkthdr.len -= (roff + hlen - m1->m_len);
! 653: }
! 654:
! 655: /* Now, let's unlink the mbuf chain for a second...*/
! 656: mo = m1->m_next;
! 657: m1->m_next = NULL;
! 658:
! 659: /* ...and trim the end of the first part of the chain...sick */
! 660: m_adj(m1, -(m1->m_len - roff));
! 661: if (!(m1->m_flags & M_PKTHDR))
! 662: m->m_pkthdr.len -= (m1->m_len - roff);
! 663:
! 664: /* Finally, let's relink */
! 665: m1->m_next = mo;
! 666: } else {
! 667: /*
! 668: * The ESP header lies in the "middle" of the mbuf...do an
! 669: * overlapping copy of the remainder of the mbuf over the ESP
! 670: * header.
! 671: */
! 672: bcopy(mtod(m1, u_char *) + roff + hlen,
! 673: mtod(m1, u_char *) + roff, m1->m_len - (roff + hlen));
! 674: m1->m_len -= hlen;
! 675: m->m_pkthdr.len -= hlen;
! 676: }
! 677:
! 678: /* Save the last three bytes of decrypted data */
! 679: m_copydata(m, m->m_pkthdr.len - 3, 3, lastthree);
! 680:
! 681: /* Verify pad length */
! 682: if (lastthree[1] + 2 > m->m_pkthdr.len - skip) {
! 683: espstat.esps_badilen++;
! 684: splx(s);
! 685: DPRINTF(("esp_input_cb(): invalid padding length %d for packet in SA %s/%08x\n", lastthree[1], ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 686: m_freem(m);
! 687: return EINVAL;
! 688: }
! 689:
! 690: /* Verify correct decryption by checking the last padding bytes */
! 691: if (!(tdb->tdb_flags & TDBF_RANDOMPADDING)) {
! 692: if ((lastthree[1] != lastthree[0]) && (lastthree[1] != 0)) {
! 693: espstat.esps_badenc++;
! 694: splx(s);
! 695: DPRINTF(("esp_input(): decryption failed for packet in SA %s/%08x\n", ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 696: m_freem(m);
! 697: return EINVAL;
! 698: }
! 699: }
! 700:
! 701: /* Trim the mbuf chain to remove the trailing authenticator and padding */
! 702: m_adj(m, -(lastthree[1] + 2));
! 703:
! 704: /* Restore the Next Protocol field */
! 705: m_copyback(m, protoff, sizeof(u_int8_t), lastthree + 2);
! 706:
! 707: /* Back to generic IPsec input processing */
! 708: error = ipsec_common_input_cb(m, tdb, skip, protoff, mtag);
! 709: splx(s);
! 710: return (error);
! 711:
! 712: baddone:
! 713: splx(s);
! 714:
! 715: if (m != NULL)
! 716: m_freem(m);
! 717:
! 718: crypto_freereq(crp);
! 719:
! 720: return (error);
! 721: }
! 722:
! 723: /*
! 724: * ESP output routine, called by ipsp_process_packet().
! 725: */
! 726: int
! 727: esp_output(struct mbuf *m, struct tdb *tdb, struct mbuf **mp, int skip,
! 728: int protoff)
! 729: {
! 730: struct enc_xform *espx = (struct enc_xform *) tdb->tdb_encalgxform;
! 731: struct auth_hash *esph = (struct auth_hash *) tdb->tdb_authalgxform;
! 732: int ilen, hlen, rlen, padding, blks, alen;
! 733: struct mbuf *mi, *mo = (struct mbuf *) NULL;
! 734: struct tdb_crypto *tc;
! 735: unsigned char *pad;
! 736: u_int8_t prot;
! 737:
! 738: struct cryptodesc *crde = NULL, *crda = NULL;
! 739: struct cryptop *crp;
! 740: #if NBPFILTER > 0
! 741: struct ifnet *ifn = &(encif[0].sc_if);
! 742:
! 743: ifn->if_opackets++;
! 744: ifn->if_obytes += m->m_pkthdr.len;
! 745:
! 746: if (ifn->if_bpf) {
! 747: struct enchdr hdr;
! 748:
! 749: bzero (&hdr, sizeof(hdr));
! 750:
! 751: hdr.af = tdb->tdb_dst.sa.sa_family;
! 752: hdr.spi = tdb->tdb_spi;
! 753: if (espx)
! 754: hdr.flags |= M_CONF;
! 755: if (esph)
! 756: hdr.flags |= M_AUTH;
! 757:
! 758: bpf_mtap_hdr(ifn->if_bpf, (char *)&hdr, ENC_HDRLEN, m,
! 759: BPF_DIRECTION_OUT);
! 760: }
! 761: #endif
! 762:
! 763: if (tdb->tdb_flags & TDBF_NOREPLAY)
! 764: hlen = sizeof(u_int32_t) + tdb->tdb_ivlen;
! 765: else
! 766: hlen = 2 * sizeof(u_int32_t) + tdb->tdb_ivlen;
! 767:
! 768: rlen = m->m_pkthdr.len - skip; /* Raw payload length. */
! 769: if (espx)
! 770: blks = espx->blocksize;
! 771: else
! 772: blks = 4; /* If no encryption, we have to be 4-byte aligned. */
! 773:
! 774: padding = ((blks - ((rlen + 2) % blks)) % blks) + 2;
! 775:
! 776: if (esph)
! 777: alen = AH_HMAC_HASHLEN;
! 778: else
! 779: alen = 0;
! 780:
! 781: espstat.esps_output++;
! 782:
! 783: switch (tdb->tdb_dst.sa.sa_family) {
! 784: #ifdef INET
! 785: case AF_INET:
! 786: /* Check for IP maximum packet size violations. */
! 787: if (skip + hlen + rlen + padding + alen > IP_MAXPACKET) {
! 788: DPRINTF(("esp_output(): packet in SA %s/%08x got "
! 789: "too big\n", ipsp_address(tdb->tdb_dst),
! 790: ntohl(tdb->tdb_spi)));
! 791: m_freem(m);
! 792: espstat.esps_toobig++;
! 793: return EMSGSIZE;
! 794: }
! 795: break;
! 796: #endif /* INET */
! 797:
! 798: #ifdef INET6
! 799: case AF_INET6:
! 800: /* Check for IPv6 maximum packet size violations. */
! 801: if (skip + hlen + rlen + padding + alen > IPV6_MAXPACKET) {
! 802: DPRINTF(("esp_output(): packet in SA %s/%08x got too "
! 803: "big\n", ipsp_address(tdb->tdb_dst),
! 804: ntohl(tdb->tdb_spi)));
! 805: m_freem(m);
! 806: espstat.esps_toobig++;
! 807: return EMSGSIZE;
! 808: }
! 809: break;
! 810: #endif /* INET6 */
! 811:
! 812: default:
! 813: DPRINTF(("esp_output(): unknown/unsupported protocol "
! 814: "family %d, SA %s/%08x\n", tdb->tdb_dst.sa.sa_family
! 815: , ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 816: m_freem(m);
! 817: espstat.esps_nopf++;
! 818: return EPFNOSUPPORT;
! 819: }
! 820:
! 821: /* Update the counters. */
! 822: tdb->tdb_cur_bytes += m->m_pkthdr.len - skip;
! 823: espstat.esps_obytes += m->m_pkthdr.len - skip;
! 824:
! 825: /* Hard byte expiration. */
! 826: if (tdb->tdb_flags & TDBF_BYTES &&
! 827: tdb->tdb_cur_bytes >= tdb->tdb_exp_bytes) {
! 828: pfkeyv2_expire(tdb, SADB_EXT_LIFETIME_HARD);
! 829: tdb_delete(tdb);
! 830: m_freem(m);
! 831: return EINVAL;
! 832: }
! 833:
! 834: /* Soft byte expiration. */
! 835: if (tdb->tdb_flags & TDBF_SOFT_BYTES &&
! 836: tdb->tdb_cur_bytes >= tdb->tdb_soft_bytes) {
! 837: pfkeyv2_expire(tdb, SADB_EXT_LIFETIME_SOFT);
! 838: tdb->tdb_flags &= ~TDBF_SOFT_BYTES; /* Turn off checking. */
! 839: }
! 840:
! 841: /*
! 842: * Loop through mbuf chain; if we find a readonly mbuf,
! 843: * replace the rest of the chain.
! 844: */
! 845: mo = NULL;
! 846: mi = m;
! 847: while (mi != NULL && !M_READONLY(mi)) {
! 848: mo = mi;
! 849: mi = mi->m_next;
! 850: }
! 851:
! 852: if (mi != NULL) {
! 853: /* Replace the rest of the mbuf chain. */
! 854: struct mbuf *n = m_copym2(mi, 0, M_COPYALL, M_DONTWAIT);
! 855:
! 856: if (n == NULL) {
! 857: DPRINTF(("esp_output(): bad mbuf chain, SA %s/%08x\n",
! 858: ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 859: espstat.esps_hdrops++;
! 860: m_freem(m);
! 861: return ENOBUFS;
! 862: }
! 863:
! 864: if (mo != NULL)
! 865: mo->m_next = n;
! 866: else
! 867: m = n;
! 868:
! 869: m_freem(mi);
! 870: }
! 871:
! 872: /* Inject ESP header. */
! 873: mo = m_inject(m, skip, hlen, M_DONTWAIT);
! 874: if (mo == NULL) {
! 875: DPRINTF(("esp_output(): failed to inject ESP header for "
! 876: "SA %s/%08x\n", ipsp_address(tdb->tdb_dst),
! 877: ntohl(tdb->tdb_spi)));
! 878: m_freem(m);
! 879: espstat.esps_hdrops++;
! 880: return ENOBUFS;
! 881: }
! 882:
! 883: /* Initialize ESP header. */
! 884: bcopy((caddr_t) &tdb->tdb_spi, mtod(mo, caddr_t), sizeof(u_int32_t));
! 885: if (!(tdb->tdb_flags & TDBF_NOREPLAY)) {
! 886: u_int32_t replay = htonl(tdb->tdb_rpl++);
! 887: bcopy((caddr_t) &replay, mtod(mo, caddr_t) + sizeof(u_int32_t),
! 888: sizeof(u_int32_t));
! 889: #if NPFSYNC > 0
! 890: pfsync_update_tdb(tdb,1);
! 891: #endif
! 892: }
! 893:
! 894: /*
! 895: * Add padding -- better to do it ourselves than use the crypto engine,
! 896: * although if/when we support compression, we'd have to do that.
! 897: */
! 898: pad = (u_char *) m_pad(m, padding + alen);
! 899: if (pad == NULL) {
! 900: DPRINTF(("esp_output(): m_pad() failed for SA %s/%08x\n",
! 901: ipsp_address(tdb->tdb_dst), ntohl(tdb->tdb_spi)));
! 902: return ENOBUFS;
! 903: }
! 904:
! 905: /* Self-describing or random padding ? */
! 906: if (!(tdb->tdb_flags & TDBF_RANDOMPADDING))
! 907: for (ilen = 0; ilen < padding - 2; ilen++)
! 908: pad[ilen] = ilen + 1;
! 909: else
! 910: arc4random_bytes((void *) pad, padding - 2);
! 911:
! 912: /* Fix padding length and Next Protocol in padding itself. */
! 913: pad[padding - 2] = padding - 2;
! 914: m_copydata(m, protoff, sizeof(u_int8_t), pad + padding - 1);
! 915:
! 916: /* Fix Next Protocol in IPv4/IPv6 header. */
! 917: prot = IPPROTO_ESP;
! 918: m_copyback(m, protoff, sizeof(u_int8_t), &prot);
! 919:
! 920: /* Get crypto descriptors. */
! 921: crp = crypto_getreq(esph && espx ? 2 : 1);
! 922: if (crp == NULL) {
! 923: m_freem(m);
! 924: DPRINTF(("esp_output(): failed to acquire crypto "
! 925: "descriptors\n"));
! 926: espstat.esps_crypto++;
! 927: return ENOBUFS;
! 928: }
! 929:
! 930: if (espx) {
! 931: crde = crp->crp_desc;
! 932: crda = crde->crd_next;
! 933:
! 934: /* Encryption descriptor. */
! 935: crde->crd_skip = skip + hlen;
! 936: crde->crd_len = m->m_pkthdr.len - (skip + hlen + alen);
! 937: crde->crd_flags = CRD_F_ENCRYPT;
! 938: crde->crd_inject = skip + hlen - tdb->tdb_ivlen;
! 939:
! 940: if (tdb->tdb_flags & TDBF_HALFIV) {
! 941: /* Copy half-iv in the packet. */
! 942: m_copyback(m, crde->crd_inject, tdb->tdb_ivlen,
! 943: tdb->tdb_iv);
! 944:
! 945: /* Cook half-iv. */
! 946: bcopy(tdb->tdb_iv, crde->crd_iv, tdb->tdb_ivlen);
! 947: for (ilen = 0; ilen < tdb->tdb_ivlen; ilen++)
! 948: crde->crd_iv[tdb->tdb_ivlen + ilen] =
! 949: ~crde->crd_iv[ilen];
! 950:
! 951: crde->crd_flags |=
! 952: CRD_F_IV_PRESENT | CRD_F_IV_EXPLICIT;
! 953: }
! 954:
! 955: /* Encryption operation. */
! 956: crde->crd_alg = espx->type;
! 957: crde->crd_key = tdb->tdb_emxkey;
! 958: crde->crd_klen = tdb->tdb_emxkeylen * 8;
! 959: /* XXX Rounds ? */
! 960: } else
! 961: crda = crp->crp_desc;
! 962:
! 963: /* IPsec-specific opaque crypto info. */
! 964: MALLOC(tc, struct tdb_crypto *, sizeof(struct tdb_crypto),
! 965: M_XDATA, M_NOWAIT);
! 966: if (tc == NULL) {
! 967: m_freem(m);
! 968: crypto_freereq(crp);
! 969: DPRINTF(("esp_output(): failed to allocate tdb_crypto\n"));
! 970: espstat.esps_crypto++;
! 971: return ENOBUFS;
! 972: }
! 973:
! 974: bzero(tc, sizeof(struct tdb_crypto));
! 975: tc->tc_spi = tdb->tdb_spi;
! 976: tc->tc_proto = tdb->tdb_sproto;
! 977: bcopy(&tdb->tdb_dst, &tc->tc_dst, sizeof(union sockaddr_union));
! 978:
! 979: /* Crypto operation descriptor. */
! 980: crp->crp_ilen = m->m_pkthdr.len; /* Total input length. */
! 981: crp->crp_flags = CRYPTO_F_IMBUF;
! 982: crp->crp_buf = (caddr_t) m;
! 983: crp->crp_callback = (int (*) (struct cryptop *)) esp_output_cb;
! 984: crp->crp_opaque = (caddr_t) tc;
! 985: crp->crp_sid = tdb->tdb_cryptoid;
! 986:
! 987: if (esph) {
! 988: /* Authentication descriptor. */
! 989: crda->crd_skip = skip;
! 990: crda->crd_len = m->m_pkthdr.len - (skip + alen);
! 991: crda->crd_inject = m->m_pkthdr.len - alen;
! 992:
! 993: /* Authentication operation. */
! 994: crda->crd_alg = esph->type;
! 995: crda->crd_key = tdb->tdb_amxkey;
! 996: crda->crd_klen = tdb->tdb_amxkeylen * 8;
! 997: }
! 998:
! 999: if ((tdb->tdb_flags & TDBF_SKIPCRYPTO) == 0)
! 1000: return crypto_dispatch(crp);
! 1001: else
! 1002: return esp_output_cb(crp);
! 1003: }
! 1004:
! 1005: /*
! 1006: * ESP output callback, called directly by the crypto driver.
! 1007: */
! 1008: int
! 1009: esp_output_cb(void *op)
! 1010: {
! 1011: struct cryptop *crp = (struct cryptop *) op;
! 1012: struct tdb_crypto *tc;
! 1013: struct tdb *tdb;
! 1014: struct mbuf *m;
! 1015: int error, s;
! 1016:
! 1017: tc = (struct tdb_crypto *) crp->crp_opaque;
! 1018:
! 1019: m = (struct mbuf *) crp->crp_buf;
! 1020: if (m == NULL) {
! 1021: /* Shouldn't happen... */
! 1022: FREE(tc, M_XDATA);
! 1023: crypto_freereq(crp);
! 1024: espstat.esps_crypto++;
! 1025: DPRINTF(("esp_output_cb(): bogus returned buffer from "
! 1026: "crypto\n"));
! 1027: return (EINVAL);
! 1028: }
! 1029:
! 1030:
! 1031: s = spltdb();
! 1032:
! 1033: tdb = gettdb(tc->tc_spi, &tc->tc_dst, tc->tc_proto);
! 1034: if (tdb == NULL) {
! 1035: FREE(tc, M_XDATA);
! 1036: espstat.esps_notdb++;
! 1037: DPRINTF(("esp_output_cb(): TDB is expired while in crypto\n"));
! 1038: error = EPERM;
! 1039: goto baddone;
! 1040: }
! 1041:
! 1042: /* Check for crypto errors. */
! 1043: if (crp->crp_etype) {
! 1044: if (crp->crp_etype == EAGAIN) {
! 1045: /* Reset the session ID */
! 1046: if (tdb->tdb_cryptoid != 0)
! 1047: tdb->tdb_cryptoid = crp->crp_sid;
! 1048: splx(s);
! 1049: return crypto_dispatch(crp);
! 1050: }
! 1051: FREE(tc, M_XDATA);
! 1052: espstat.esps_noxform++;
! 1053: DPRINTF(("esp_output_cb(): crypto error %d\n",
! 1054: crp->crp_etype));
! 1055: error = crp->crp_etype;
! 1056: goto baddone;
! 1057: }
! 1058: FREE(tc, M_XDATA);
! 1059:
! 1060: /* Release crypto descriptors. */
! 1061: crypto_freereq(crp);
! 1062:
! 1063: /*
! 1064: * If we're doing half-iv, keep a copy of the last few bytes of the
! 1065: * encrypted part, for use as the next IV. Note that HALF-IV is only
! 1066: * supposed to be used without authentication (the old ESP specs).
! 1067: */
! 1068: if (tdb->tdb_flags & TDBF_HALFIV)
! 1069: m_copydata(m, m->m_pkthdr.len - tdb->tdb_ivlen, tdb->tdb_ivlen,
! 1070: tdb->tdb_iv);
! 1071:
! 1072: /* Call the IPsec input callback. */
! 1073: error = ipsp_process_done(m, tdb);
! 1074: splx(s);
! 1075: return error;
! 1076:
! 1077: baddone:
! 1078: splx(s);
! 1079:
! 1080: if (m != NULL)
! 1081: m_freem(m);
! 1082:
! 1083: crypto_freereq(crp);
! 1084:
! 1085: return error;
! 1086: }
! 1087:
! 1088: /*
! 1089: * return 0 on success
! 1090: * return 1 for counter == 0
! 1091: * return 2 for very old packet
! 1092: * return 3 for packet within current window but already received
! 1093: */
! 1094: int
! 1095: checkreplaywindow32(u_int32_t seq, u_int32_t initial, u_int32_t *lastseq,
! 1096: u_int32_t window, u_int32_t *bitmap, int commit)
! 1097: {
! 1098: u_int32_t diff, llseq, lbitmap;
! 1099:
! 1100: /* Just do the checking, without "committing" any changes. */
! 1101: if (commit == 0) {
! 1102: llseq = *lastseq;
! 1103: lbitmap = *bitmap;
! 1104:
! 1105: lastseq = &llseq;
! 1106: bitmap = &lbitmap;
! 1107: }
! 1108:
! 1109: seq -= initial;
! 1110:
! 1111: if (seq == 0)
! 1112: return 1;
! 1113:
! 1114: if (seq > *lastseq - initial) {
! 1115: diff = seq - (*lastseq - initial);
! 1116: if (diff < window)
! 1117: *bitmap = ((*bitmap) << diff) | 1;
! 1118: else
! 1119: *bitmap = 1;
! 1120: *lastseq = seq + initial;
! 1121: return 0;
! 1122: }
! 1123:
! 1124: diff = *lastseq - initial - seq;
! 1125: if (diff >= window) {
! 1126: espstat.esps_wrap++;
! 1127: return 2;
! 1128: }
! 1129:
! 1130: if ((*bitmap) & (((u_int32_t) 1) << diff)) {
! 1131: espstat.esps_replay++;
! 1132: return 3;
! 1133: }
! 1134:
! 1135: *bitmap |= (((u_int32_t) 1) << diff);
! 1136: return 0;
! 1137: }
! 1138:
! 1139: /*
! 1140: * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header
! 1141: * length is updated, and a pointer to the first byte of the padding
! 1142: * (which is guaranteed to be all in one mbuf) is returned.
! 1143: */
! 1144:
! 1145: caddr_t
! 1146: m_pad(struct mbuf *m, int n)
! 1147: {
! 1148: struct mbuf *m0, *m1;
! 1149: int len, pad;
! 1150: caddr_t retval;
! 1151:
! 1152: if (n <= 0) { /* No stupid arguments. */
! 1153: DPRINTF(("m_pad(): pad length invalid (%d)\n", n));
! 1154: m_freem(m);
! 1155: return NULL;
! 1156: }
! 1157:
! 1158: len = m->m_pkthdr.len;
! 1159: pad = n;
! 1160: m0 = m;
! 1161:
! 1162: while (m0->m_len < len) {
! 1163: len -= m0->m_len;
! 1164: m0 = m0->m_next;
! 1165: }
! 1166:
! 1167: if (m0->m_len != len) {
! 1168: DPRINTF(("m_pad(): length mismatch (should be %d instead of "
! 1169: "%d)\n", m->m_pkthdr.len,
! 1170: m->m_pkthdr.len + m0->m_len - len));
! 1171:
! 1172: m_freem(m);
! 1173: return NULL;
! 1174: }
! 1175:
! 1176: /* Check for zero-length trailing mbufs, and find the last one. */
! 1177: for (m1 = m0; m1->m_next; m1 = m1->m_next) {
! 1178: if (m1->m_next->m_len != 0) {
! 1179: DPRINTF(("m_pad(): length mismatch (should be %d "
! 1180: "instead of %d)\n", m->m_pkthdr.len,
! 1181: m->m_pkthdr.len + m1->m_next->m_len));
! 1182:
! 1183: m_freem(m);
! 1184: return NULL;
! 1185: }
! 1186:
! 1187: m0 = m1->m_next;
! 1188: }
! 1189:
! 1190: if ((m0->m_flags & M_EXT) ||
! 1191: m0->m_data + m0->m_len + pad >= &(m0->m_dat[MLEN])) {
! 1192: /* Add an mbuf to the chain. */
! 1193: MGET(m1, M_DONTWAIT, MT_DATA);
! 1194: if (m1 == 0) {
! 1195: m_freem(m0);
! 1196: DPRINTF(("m_pad(): cannot append\n"));
! 1197: return NULL;
! 1198: }
! 1199:
! 1200: m0->m_next = m1;
! 1201: m0 = m1;
! 1202: m0->m_len = 0;
! 1203: }
! 1204:
! 1205: retval = m0->m_data + m0->m_len;
! 1206: m0->m_len += pad;
! 1207: m->m_pkthdr.len += pad;
! 1208:
! 1209: return retval;
! 1210: }
CVSweb
![[BACK]](/icons/back.gif){kind=icon}
Return to ip_esp.c CVS log ![[TXT]](/icons/text.gif){kind=icon}
![[DIR]](/icons/dir.gif){kind=icon}
Up to [local] / sys / netinet