sys/netinet6/ipsec.c

   1 /*      $FreeBSD: src/sys/netinet6/ipsec.c,v 1.3.2.12 2003/05/06 06:46:58 suz Exp $     */
   2 /*      $DragonFly: src/sys/netinet6/ipsec.c,v 1.22 2008/05/27 01:10:43 dillon Exp $    */
   3 /*      $KAME: ipsec.c,v 1.103 2001/05/24 07:14:18 sakane Exp $ */
   4
   5 /*
   6  * Copyright (C) 1995, 1996, 1997, and 1998 WIDE Project.
   7  * All rights reserved.
   8  *
   9  * Redistribution and use in source and binary forms, with or without
  10  * modification, are permitted provided that the following conditions
  11  * are met:
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions and the following disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in the
  16  *    documentation and/or other materials provided with the distribution.
  17  * 3. Neither the name of the project nor the names of its contributors
  18  *    may be used to endorse or promote products derived from this software
  19  *    without specific prior written permission.
  20  *
  21  * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
  22  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  24  * ARE DISCLAIMED.  IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
  25  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  26  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  27  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  28  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  30  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  31  * SUCH DAMAGE.
  32  */
  33
  34 /*
  35  * IPsec controller part.
  36  */
  37
  38 #include "opt_inet.h"
  39 #include "opt_inet6.h"
  40 #include "opt_ipsec.h"
  41
  42 #include <sys/param.h>
  43 #include <sys/systm.h>
  44 #include <sys/malloc.h>
  45 #include <sys/mbuf.h>
  46 #include <sys/domain.h>
  47 #include <sys/protosw.h>
  48 #include <sys/socket.h>
  49 #include <sys/socketvar.h>
  50 #include <sys/errno.h>
  51 #include <sys/time.h>
  52 #include <sys/kernel.h>
  53 #include <sys/syslog.h>
  54 #include <sys/sysctl.h>
  55 #include <sys/proc.h>
  56 #include <sys/in_cksum.h>
  57 #include <sys/thread2.h>
  58
  59 #include <net/if.h>
  60 #include <net/route.h>
  61
  62 #include <netinet/in.h>
  63 #include <netinet/in_systm.h>
  64 #include <netinet/ip.h>
  65 #include <netinet/ip_var.h>
  66 #include <netinet/in_var.h>
  67 #include <netinet/udp.h>
  68 #include <netinet/udp_var.h>
  69 #include <netinet/ip_ecn.h>
  70 #ifdef INET6
  71 #include <netinet6/ip6_ecn.h>
  72 #endif
  73 #include <netinet/tcp.h>
  74 #include <netinet/udp.h>
  75
  76 #include <netinet/ip6.h>
  77 #ifdef INET6
  78 #include <netinet6/ip6_var.h>
  79 #endif
  80 #include <netinet/in_pcb.h>
  81 #ifdef INET6
  82 #include <netinet/icmp6.h>
  83 #endif
  84
  85 #include <netinet6/ipsec.h>
  86 #ifdef INET6
  87 #include <netinet6/ipsec6.h>
  88 #endif
  89 #include <netinet6/ah.h>
  90 #ifdef INET6
  91 #include <netinet6/ah6.h>
  92 #endif
  93 #ifdef IPSEC_ESP
  94 #include <netinet6/esp.h>
  95 #ifdef INET6
  96 #include <netinet6/esp6.h>
  97 #endif
  98 #endif
  99 #include <netinet6/ipcomp.h>
 100 #ifdef INET6
 101 #include <netinet6/ipcomp6.h>
 102 #endif
 103 #include <netproto/key/key.h>
 104 #include <netproto/key/keydb.h>
 105 #include <netproto/key/key_debug.h>
 106
 107 #include <net/net_osdep.h>
 108
 109 #ifdef IPSEC_DEBUG
 110 int ipsec_debug = 1;
 111 #else
 112 int ipsec_debug = 0;
 113 #endif
 114
 115 struct ipsecstat ipsecstat;
 116 int ip4_ah_cleartos = 1;
 117 int ip4_ah_offsetmask = 0;      /* maybe IP_DF? */
 118 int ip4_ipsec_dfbit = 0;        /* DF bit on encap. 0: clear 1: set 2: copy */
 119 int ip4_esp_trans_deflev = IPSEC_LEVEL_USE;
 120 int ip4_esp_net_deflev = IPSEC_LEVEL_USE;
 121 int ip4_ah_trans_deflev = IPSEC_LEVEL_USE;
 122 int ip4_ah_net_deflev = IPSEC_LEVEL_USE;
 123 struct secpolicy ip4_def_policy;
 124 int ip4_ipsec_ecn = 0;          /* ECN ignore(-1)/forbidden(0)/allowed(1) */
 125 int ip4_esp_randpad = -1;
 126
 127 #ifdef SYSCTL_DECL
 128 SYSCTL_DECL(_net_inet_ipsec);
 129 #ifdef INET6
 130 SYSCTL_DECL(_net_inet6_ipsec6);
 131 #endif
 132 #endif
 133
 134 /* net.inet.ipsec */
 135 SYSCTL_STRUCT(_net_inet_ipsec, IPSECCTL_STATS,
 136         stats, CTLFLAG_RD,      &ipsecstat,     ipsecstat, "");
 137 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_POLICY,
 138         def_policy, CTLFLAG_RW, &ip4_def_policy.policy, 0, "");
 139 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_TRANSLEV, esp_trans_deflev,
 140         CTLFLAG_RW, &ip4_esp_trans_deflev,      0, "");
 141 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_NETLEV, esp_net_deflev,
 142         CTLFLAG_RW, &ip4_esp_net_deflev,        0, "");
 143 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
 144         CTLFLAG_RW, &ip4_ah_trans_deflev,       0, "");
 145 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
 146         CTLFLAG_RW, &ip4_ah_net_deflev, 0, "");
 147 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_AH_CLEARTOS,
 148         ah_cleartos, CTLFLAG_RW,        &ip4_ah_cleartos,       0, "");
 149 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_AH_OFFSETMASK,
 150         ah_offsetmask, CTLFLAG_RW,      &ip4_ah_offsetmask,     0, "");
 151 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DFBIT,
 152         dfbit, CTLFLAG_RW,      &ip4_ipsec_dfbit,       0, "");
 153 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ECN,
 154         ecn, CTLFLAG_RW,        &ip4_ipsec_ecn, 0, "");
 155 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEBUG,
 156         debug, CTLFLAG_RW,      &ipsec_debug,   0, "");
 157 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ESP_RANDPAD,
 158         esp_randpad, CTLFLAG_RW,        &ip4_esp_randpad,       0, "");
 159
 160 #ifdef INET6
 161 struct ipsecstat ipsec6stat;
 162 int ip6_esp_trans_deflev = IPSEC_LEVEL_USE;
 163 int ip6_esp_net_deflev = IPSEC_LEVEL_USE;
 164 int ip6_ah_trans_deflev = IPSEC_LEVEL_USE;
 165 int ip6_ah_net_deflev = IPSEC_LEVEL_USE;
 166 struct secpolicy ip6_def_policy;
 167 int ip6_ipsec_ecn = 0;          /* ECN ignore(-1)/forbidden(0)/allowed(1) */
 168 int ip6_esp_randpad = -1;
 169
 170 /* net.inet6.ipsec6 */
 171 SYSCTL_STRUCT(_net_inet6_ipsec6, IPSECCTL_STATS,
 172         stats, CTLFLAG_RD, &ipsec6stat, ipsecstat, "");
 173 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_POLICY,
 174         def_policy, CTLFLAG_RW, &ip6_def_policy.policy, 0, "");
 175 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_ESP_TRANSLEV, esp_trans_deflev,
 176         CTLFLAG_RW, &ip6_esp_trans_deflev,      0, "");
 177 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_ESP_NETLEV, esp_net_deflev,
 178         CTLFLAG_RW, &ip6_esp_net_deflev,        0, "");
 179 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
 180         CTLFLAG_RW, &ip6_ah_trans_deflev,       0, "");
 181 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
 182         CTLFLAG_RW, &ip6_ah_net_deflev, 0, "");
 183 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ECN,
 184         ecn, CTLFLAG_RW,        &ip6_ipsec_ecn, 0, "");
 185 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEBUG,
 186         debug, CTLFLAG_RW,      &ipsec_debug,   0, "");
 187 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ESP_RANDPAD,
 188         esp_randpad, CTLFLAG_RW,        &ip6_esp_randpad,       0, "");
 189 #endif /* INET6 */
 190
 191 static int ipsec_setspidx_mbuf
 192         (struct secpolicyindex *, u_int, u_int, struct mbuf *, int);
 193 static int ipsec4_setspidx_inpcb (struct mbuf *, struct inpcb *pcb);
 194 #ifdef INET6
 195 static int ipsec6_setspidx_in6pcb (struct mbuf *, struct in6pcb *pcb);
 196 #endif
 197 static int ipsec_setspidx (struct mbuf *, struct secpolicyindex *, int);
 198 static void ipsec4_get_ulp (struct mbuf *m, struct secpolicyindex *, int);
 199 static int ipsec4_setspidx_ipaddr (struct mbuf *, struct secpolicyindex *);
 200 #ifdef INET6
 201 static void ipsec6_get_ulp (struct mbuf *m, struct secpolicyindex *, int);
 202 static int ipsec6_setspidx_ipaddr (struct mbuf *, struct secpolicyindex *);
 203 #endif
 204 static struct inpcbpolicy *ipsec_newpcbpolicy (void);
 205 static void ipsec_delpcbpolicy (struct inpcbpolicy *);
 206 static struct secpolicy *ipsec_deepcopy_policy (struct secpolicy *src);
 207 static int ipsec_set_policy (struct secpolicy **pcb_sp,
 208         int optname, caddr_t request, size_t len, int priv);
 209 static int ipsec_get_policy (struct secpolicy *pcb_sp, struct mbuf **mp);
 210 static void vshiftl (unsigned char *, int, int);
 211 static int ipsec_in_reject (struct secpolicy *, struct mbuf *);
 212 static size_t ipsec_hdrsiz (struct secpolicy *);
 213 #ifdef INET
 214 static struct mbuf *ipsec4_splithdr (struct mbuf *);
 215 #endif
 216 #ifdef INET6
 217 static struct mbuf *ipsec6_splithdr (struct mbuf *);
 218 #endif
 219 #ifdef INET
 220 static int ipsec4_encapsulate (struct mbuf *, struct secasvar *);
 221 #endif
 222 #ifdef INET6
 223 static int ipsec6_encapsulate (struct mbuf *, struct secasvar *);
 224 #endif
 225
 226 /*
 227  * For OUTBOUND packet having a socket. Searching SPD for packet,
 228  * and return a pointer to SP.
 229  * OUT: NULL:   no apropreate SP found, the following value is set to error.
 230  *              0       : bypass
 231  *              EACCES  : discard packet.
 232  *              ENOENT  : ipsec_acquire() in progress, maybe.
 233  *              others  : error occured.
 234  *      others: a pointer to SP
 235  *
 236  * NOTE: IPv6 mapped adddress concern is implemented here.
 237  */
 238 struct secpolicy *
 239 ipsec4_getpolicybysock(struct mbuf *m, u_int dir, struct socket *so, int *error)
 240 {
 241         struct inpcbpolicy *pcbsp = NULL;
 242         struct secpolicy *currsp = NULL;        /* policy on socket */
 243         struct secpolicy *kernsp = NULL;        /* policy on kernel */
 244
 245         /* sanity check */
 246         if (m == NULL || so == NULL || error == NULL)
 247                 panic("ipsec4_getpolicybysock: NULL pointer was passed.");
 248
 249         switch (so->so_proto->pr_domain->dom_family) {
 250         case AF_INET:
 251                 /* set spidx in pcb */
 252                 *error = ipsec4_setspidx_inpcb(m, so->so_pcb);
 253                 break;
 254 #ifdef INET6
 255         case AF_INET6:
 256                 /* set spidx in pcb */
 257                 *error = ipsec6_setspidx_in6pcb(m, so->so_pcb);
 258                 break;
 259 #endif
 260         default:
 261                 panic("ipsec4_getpolicybysock: unsupported address family\n");
 262         }
 263         if (*error)
 264                 return NULL;
 265         switch (so->so_proto->pr_domain->dom_family) {
 266         case AF_INET:
 267                 pcbsp = sotoinpcb(so)->inp_sp;
 268                 break;
 269 #ifdef INET6
 270         case AF_INET6:
 271                 pcbsp = sotoin6pcb(so)->in6p_sp;
 272                 break;
 273 #endif
 274         }
 275
 276         /* sanity check */
 277         if (pcbsp == NULL)
 278                 panic("ipsec4_getpolicybysock: pcbsp is NULL.");
 279
 280         switch (dir) {
 281         case IPSEC_DIR_INBOUND:
 282                 currsp = pcbsp->sp_in;
 283                 break;
 284         case IPSEC_DIR_OUTBOUND:
 285                 currsp = pcbsp->sp_out;
 286                 break;
 287         default:
 288                 panic("ipsec4_getpolicybysock: illegal direction.");
 289         }
 290
 291         /* sanity check */
 292         if (currsp == NULL)
 293                 panic("ipsec4_getpolicybysock: currsp is NULL.");
 294
 295         lwkt_gettoken(&key_token);
 296
 297         /* when privilieged socket */
 298         if (pcbsp->priv) {
 299                 switch (currsp->policy) {
 300                 case IPSEC_POLICY_BYPASS:
 301                         currsp->refcnt++;
 302                         *error = 0;
 303                         lwkt_reltoken(&key_token);
 304                         return currsp;
 305
 306                 case IPSEC_POLICY_ENTRUST:
 307                         /* look for a policy in SPD */
 308                         kernsp = key_allocsp(&currsp->spidx, dir);
 309
 310                         /* SP found */
 311                         if (kernsp != NULL) {
 312                                 KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
 313                                         kprintf("DP ipsec4_getpolicybysock called "
 314                                                "to allocate SP:%p\n", kernsp));
 315                                 *error = 0;
 316                                 lwkt_reltoken(&key_token);
 317                                 return kernsp;
 318                         }
 319
 320                         /* no SP found */
 321                         if (ip4_def_policy.policy != IPSEC_POLICY_DISCARD
 322                          && ip4_def_policy.policy != IPSEC_POLICY_NONE) {
 323                                 ipseclog((LOG_INFO,
 324                                     "fixed system default policy: %d->%d\n",
 325                                     ip4_def_policy.policy, IPSEC_POLICY_NONE));
 326                                 ip4_def_policy.policy = IPSEC_POLICY_NONE;
 327                         }
 328                         ip4_def_policy.refcnt++;
 329                         *error = 0;
 330                         lwkt_reltoken(&key_token);
 331                         return &ip4_def_policy;
 332
 333                 case IPSEC_POLICY_IPSEC:
 334                         currsp->refcnt++;
 335                         *error = 0;
 336                         lwkt_reltoken(&key_token);
 337                         return currsp;
 338
 339                 default:
 340                         ipseclog((LOG_ERR, "ipsec4_getpolicybysock: "
 341                               "Invalid policy for PCB %d\n", currsp->policy));
 342                         *error = EINVAL;
 343                         lwkt_reltoken(&key_token);
 344                         return NULL;
 345                 }
 346                 /* NOTREACHED */
 347         }
 348
 349         /* when non-privilieged socket */
 350         /* look for a policy in SPD */
 351         kernsp = key_allocsp(&currsp->spidx, dir);
 352
 353         /* SP found */
 354         if (kernsp != NULL) {
 355                 KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
 356                         kprintf("DP ipsec4_getpolicybysock called "
 357                                "to allocate SP:%p\n", kernsp));
 358                 *error = 0;
 359                 lwkt_reltoken(&key_token);
 360                 return kernsp;
 361         }
 362
 363         /* no SP found */
 364         switch (currsp->policy) {
 365         case IPSEC_POLICY_BYPASS:
 366                 ipseclog((LOG_ERR, "ipsec4_getpolicybysock: "
 367                        "Illegal policy for non-priviliged defined %d\n",
 368                         currsp->policy));
 369                 *error = EINVAL;
 370                 lwkt_reltoken(&key_token);
 371                 return NULL;
 372
 373         case IPSEC_POLICY_ENTRUST:
 374                 if (ip4_def_policy.policy != IPSEC_POLICY_DISCARD
 375                  && ip4_def_policy.policy != IPSEC_POLICY_NONE) {
 376                         ipseclog((LOG_INFO,
 377                             "fixed system default policy: %d->%d\n",
 378                             ip4_def_policy.policy, IPSEC_POLICY_NONE));
 379                         ip4_def_policy.policy = IPSEC_POLICY_NONE;
 380                 }
 381                 ip4_def_policy.refcnt++;
 382                 *error = 0;
 383                 lwkt_reltoken(&key_token);
 384                 return &ip4_def_policy;
 385
 386         case IPSEC_POLICY_IPSEC:
 387                 currsp->refcnt++;
 388                 *error = 0;
 389                 lwkt_reltoken(&key_token);
 390                 return currsp;
 391
 392         default:
 393                 ipseclog((LOG_ERR, "ipsec4_getpolicybysock: "
 394                    "Invalid policy for PCB %d\n", currsp->policy));
 395                 *error = EINVAL;
 396                 lwkt_reltoken(&key_token);
 397                 return NULL;
 398         }
 399         /* NOTREACHED */
 400 }
 401
 402 /*
 403  * For FORWADING packet or OUTBOUND without a socket. Searching SPD for packet,
 404  * and return a pointer to SP.
 405  * OUT: positive: a pointer to the entry for security policy leaf matched.
 406  *      NULL:   no apropreate SP found, the following value is set to error.
 407  *              0       : bypass
 408  *              EACCES  : discard packet.
 409  *              ENOENT  : ipsec_acquire() in progress, maybe.
 410  *              others  : error occured.
 411  */
 412 struct secpolicy *
 413 ipsec4_getpolicybyaddr(struct mbuf *m, u_int dir, int flag, int *error)
 414 {
 415         struct secpolicy *sp = NULL;
 416         struct secpolicyindex spidx;
 417
 418         /* sanity check */
 419         if (m == NULL || error == NULL)
 420                 panic("ipsec4_getpolicybyaddr: NULL pointer was passed.");
 421
 422         bzero(&spidx, sizeof(spidx));
 423
 424         /* make a index to look for a policy */
 425         *error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET, m,
 426                                      (flag & IP_FORWARDING) ? 0 : 1);
 427
 428         if (*error != 0)
 429                 return NULL;
 430
 431         lwkt_gettoken(&key_token);
 432         sp = key_allocsp(&spidx, dir);
 433
 434         /* SP found */
 435         if (sp != NULL) {
 436                 KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
 437                         kprintf("DP ipsec4_getpolicybyaddr called "
 438                                "to allocate SP:%p\n", sp));
 439                 *error = 0;
 440                 lwkt_reltoken(&key_token);
 441                 return sp;
 442         }
 443
 444         /* no SP found */
 445         if (ip4_def_policy.policy != IPSEC_POLICY_DISCARD
 446          && ip4_def_policy.policy != IPSEC_POLICY_NONE) {
 447                 ipseclog((LOG_INFO, "fixed system default policy:%d->%d\n",
 448                         ip4_def_policy.policy,
 449                         IPSEC_POLICY_NONE));
 450                 ip4_def_policy.policy = IPSEC_POLICY_NONE;
 451         }
 452         ip4_def_policy.refcnt++;
 453         *error = 0;
 454         lwkt_reltoken(&key_token);
 455         return &ip4_def_policy;
 456 }
 457
 458 #ifdef INET6
 459 /*
 460  * For OUTBOUND packet having a socket. Searching SPD for packet,
 461  * and return a pointer to SP.
 462  * OUT: NULL:   no apropreate SP found, the following value is set to error.
 463  *              0       : bypass
 464  *              EACCES  : discard packet.
 465  *              ENOENT  : ipsec_acquire() in progress, maybe.
 466  *              others  : error occured.
 467  *      others: a pointer to SP
 468  */
 469 struct secpolicy *
 470 ipsec6_getpolicybysock(struct mbuf *m, u_int dir, struct socket *so, int *error)
 471 {
 472         struct inpcbpolicy *pcbsp = NULL;
 473         struct secpolicy *currsp = NULL;        /* policy on socket */
 474         struct secpolicy *kernsp = NULL;        /* policy on kernel */
 475
 476         /* sanity check */
 477         if (m == NULL || so == NULL || error == NULL)
 478                 panic("ipsec6_getpolicybysock: NULL pointer was passed.");
 479
 480 #ifdef DIAGNOSTIC
 481         if (so->so_proto->pr_domain->dom_family != AF_INET6)
 482                 panic("ipsec6_getpolicybysock: socket domain != inet6");
 483 #endif
 484
 485         lwkt_gettoken(&key_token);
 486
 487         /* set spidx in pcb */
 488         ipsec6_setspidx_in6pcb(m, so->so_pcb);
 489         pcbsp = sotoin6pcb(so)->in6p_sp;
 490
 491         /* sanity check */
 492         if (pcbsp == NULL)
 493                 panic("ipsec6_getpolicybysock: pcbsp is NULL.");
 494
 495         switch (dir) {
 496         case IPSEC_DIR_INBOUND:
 497                 currsp = pcbsp->sp_in;
 498                 break;
 499         case IPSEC_DIR_OUTBOUND:
 500                 currsp = pcbsp->sp_out;
 501                 break;
 502         default:
 503                 panic("ipsec6_getpolicybysock: illegal direction.");
 504         }
 505
 506         /* sanity check */
 507         if (currsp == NULL)
 508                 panic("ipsec6_getpolicybysock: currsp is NULL.");
 509
 510         /* when privilieged socket */
 511         if (pcbsp->priv) {
 512                 switch (currsp->policy) {
 513                 case IPSEC_POLICY_BYPASS:
 514                         currsp->refcnt++;
 515                         *error = 0;
 516                         break;
 517                 case IPSEC_POLICY_ENTRUST:
 518                         /* look for a policy in SPD */
 519                         kernsp = key_allocsp(&currsp->spidx, dir);
 520
 521                         /* SP found */
 522                         if (kernsp != NULL) {
 523                                 KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
 524                                         kprintf("DP ipsec6_getpolicybysock called "
 525                                                "to allocate SP:%p\n", kernsp));
 526                                 *error = 0;
 527                                 currsp = kernsp;
 528                                 break;
 529                         }
 530
 531                         /* no SP found */
 532                         if (ip6_def_policy.policy != IPSEC_POLICY_DISCARD
 533                          && ip6_def_policy.policy != IPSEC_POLICY_NONE) {
 534                                 ipseclog((LOG_INFO,
 535                                     "fixed system default policy: %d->%d\n",
 536                                     ip6_def_policy.policy, IPSEC_POLICY_NONE));
 537                                 ip6_def_policy.policy = IPSEC_POLICY_NONE;
 538                         }
 539                         currsp = &ip6_def_policy;
 540                         currsp->refcnt++;
 541                         *error = 0;
 542                         break;
 543                 case IPSEC_POLICY_IPSEC:
 544                         currsp->refcnt++;
 545                         *error = 0;
 546                         break;
 547                 default:
 548                         ipseclog((LOG_ERR, "ipsec6_getpolicybysock: "
 549                             "Invalid policy for PCB %d\n", currsp->policy));
 550                         *error = EINVAL;
 551                         lwkt_reltoken(&key_token);
 552                         currsp = NULL;
 553                         break;
 554                 }
 555                 lwkt_reltoken(&key_token);
 556                 return currsp;
 557                 /* NOTREACHED */
 558         }
 559
 560         /* when non-privilieged socket */
 561         /* look for a policy in SPD */
 562         kernsp = key_allocsp(&currsp->spidx, dir);
 563
 564         /* SP found */
 565         if (kernsp != NULL) {
 566                 KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
 567                         kprintf("DP ipsec6_getpolicybysock called "
 568                                "to allocate SP:%p\n", kernsp));
 569                 *error = 0;
 570                 lwkt_reltoken(&key_token);
 571                 return kernsp;
 572         }
 573
 574         /* no SP found */
 575         switch (currsp->policy) {
 576         case IPSEC_POLICY_BYPASS:
 577                 ipseclog((LOG_ERR, "ipsec6_getpolicybysock: "
 578                     "Illegal policy for non-priviliged defined %d\n",
 579                     currsp->policy));
 580                 *error = EINVAL;
 581                 currsp = NULL;
 582                 break;
 583         case IPSEC_POLICY_ENTRUST:
 584                 if (ip6_def_policy.policy != IPSEC_POLICY_DISCARD
 585                  && ip6_def_policy.policy != IPSEC_POLICY_NONE) {
 586                         ipseclog((LOG_INFO,
 587                             "fixed system default policy: %d->%d\n",
 588                             ip6_def_policy.policy, IPSEC_POLICY_NONE));
 589                         ip6_def_policy.policy = IPSEC_POLICY_NONE;
 590                 }
 591                 currsp = &ip6_def_policy;
 592                 currsp->refcnt++;
 593                 *error = 0;
 594                 break;
 595         case IPSEC_POLICY_IPSEC:
 596                 currsp->refcnt++;
 597                 *error = 0;
 598                 break;
 599         default:
 600                 ipseclog((LOG_ERR,
 601                     "ipsec6_policybysock: Invalid policy for PCB %d\n",
 602                     currsp->policy));
 603                 *error = EINVAL;
 604                 currsp = NULL;
 605                 break;
 606         }
 607         lwkt_reltoken(&key_token);
 608         return currsp;
 609 }
 610
 611 /*
 612  * For FORWADING packet or OUTBOUND without a socket. Searching SPD for packet,
 613  * and return a pointer to SP.
 614  * `flag' means that packet is to be forwarded whether or not.
 615  *      flag = 1: forwad
 616  * OUT: positive: a pointer to the entry for security policy leaf matched.
 617  *      NULL:   no apropreate SP found, the following value is set to error.
 618  *              0       : bypass
 619  *              EACCES  : discard packet.
 620  *              ENOENT  : ipsec_acquire() in progress, maybe.
 621  *              others  : error occured.
 622  */
 623 #ifndef IP_FORWARDING
 624 #define IP_FORWARDING 1
 625 #endif
 626
 627 struct secpolicy *
 628 ipsec6_getpolicybyaddr(struct mbuf *m, u_int dir, int flag, int *error)
 629 {
 630         struct secpolicy *sp = NULL;
 631         struct secpolicyindex spidx;
 632
 633         /* sanity check */
 634         if (m == NULL || error == NULL)
 635                 panic("ipsec6_getpolicybyaddr: NULL pointer was passed.");
 636         bzero(&spidx, sizeof(spidx));
 637
 638         lwkt_gettoken(&key_token);
 639
 640         /* make a index to look for a policy */
 641         *error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET6, m,
 642                                      (flag & IP_FORWARDING) ? 0 : 1);
 643         if (*error != 0) {
 644                 lwkt_reltoken(&key_token);
 645                 return NULL;
 646         }
 647
 648         sp = key_allocsp(&spidx, dir);
 649
 650         /* SP found */
 651         if (sp != NULL) {
 652                 KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
 653                         kprintf("DP ipsec6_getpolicybyaddr called "
 654                                "to allocate SP:%p\n", sp));
 655                 *error = 0;
 656                 lwkt_reltoken(&key_token);
 657                 return sp;
 658         }
 659
 660         /* no SP found */
 661         if (ip6_def_policy.policy != IPSEC_POLICY_DISCARD
 662          && ip6_def_policy.policy != IPSEC_POLICY_NONE) {
 663                 ipseclog((LOG_INFO, "fixed system default policy: %d->%d\n",
 664                     ip6_def_policy.policy, IPSEC_POLICY_NONE));
 665                 ip6_def_policy.policy = IPSEC_POLICY_NONE;
 666         }
 667         sp = &ip6_def_policy;
 668         sp->refcnt++;
 669         *error = 0;
 670         lwkt_reltoken(&key_token);
 671
 672         return sp;
 673 }
 674 #endif /* INET6 */
 675
 676 /*
 677  * set IP address into spidx from mbuf.
 678  * When Forwarding packet and ICMP echo reply, this function is used.
 679  *
 680  * IN:  get the followings from mbuf.
 681  *      protocol family, src, dst, next protocol
 682  * OUT:
 683  *      0:      success.
 684  *      other:  failure, and set errno.
 685  */
 686 int
 687 ipsec_setspidx_mbuf(struct secpolicyindex *spidx, u_int dir, u_int family,
 688                     struct mbuf *m, int needport)
 689 {
 690         int error;
 691
 692         /* sanity check */
 693         if (spidx == NULL || m == NULL)
 694                 panic("ipsec_setspidx_mbuf: NULL pointer was passed.");
 695
 696         bzero(spidx, sizeof(*spidx));
 697
 698         error = ipsec_setspidx(m, spidx, needport);
 699         if (error)
 700                 goto bad;
 701         spidx->dir = dir;
 702
 703         return 0;
 704
 705 bad:
 706         /* XXX initialize */
 707         bzero(spidx, sizeof(*spidx));
 708         return EINVAL;
 709 }
 710
 711 static int
 712 ipsec4_setspidx_inpcb(struct mbuf *m, struct inpcb *pcb)
 713 {
 714         struct secpolicyindex *spidx;
 715         int error;
 716
 717         /* sanity check */
 718         if (pcb == NULL)
 719                 panic("ipsec4_setspidx_inpcb: no PCB found.");
 720         if (pcb->inp_sp == NULL)
 721                 panic("ipsec4_setspidx_inpcb: no inp_sp found.");
 722         if (pcb->inp_sp->sp_out == NULL || pcb->inp_sp->sp_in == NULL)
 723                 panic("ipsec4_setspidx_inpcb: no sp_in/out found.");
 724
 725         bzero(&pcb->inp_sp->sp_in->spidx, sizeof(*spidx));
 726         bzero(&pcb->inp_sp->sp_out->spidx, sizeof(*spidx));
 727
 728         spidx = &pcb->inp_sp->sp_in->spidx;
 729         error = ipsec_setspidx(m, spidx, 1);
 730         if (error)
 731                 goto bad;
 732         spidx->dir = IPSEC_DIR_INBOUND;
 733
 734         spidx = &pcb->inp_sp->sp_out->spidx;
 735         error = ipsec_setspidx(m, spidx, 1);
 736         if (error)
 737                 goto bad;
 738         spidx->dir = IPSEC_DIR_OUTBOUND;
 739
 740         return 0;
 741
 742 bad:
 743         bzero(&pcb->inp_sp->sp_in->spidx, sizeof(*spidx));
 744         bzero(&pcb->inp_sp->sp_out->spidx, sizeof(*spidx));
 745         return error;
 746 }
 747
 748 #ifdef INET6
 749 static int
 750 ipsec6_setspidx_in6pcb(struct mbuf *m, struct in6pcb *pcb)
 751 {
 752         struct secpolicyindex *spidx;
 753         int error;
 754
 755         /* sanity check */
 756         if (pcb == NULL)
 757                 panic("ipsec6_setspidx_in6pcb: no PCB found.");
 758         if (pcb->in6p_sp == NULL)
 759                 panic("ipsec6_setspidx_in6pcb: no in6p_sp found.");
 760         if (pcb->in6p_sp->sp_out == NULL || pcb->in6p_sp->sp_in == NULL)
 761                 panic("ipsec6_setspidx_in6pcb: no sp_in/out found.");
 762
 763         bzero(&pcb->in6p_sp->sp_in->spidx, sizeof(*spidx));
 764         bzero(&pcb->in6p_sp->sp_out->spidx, sizeof(*spidx));
 765
 766         spidx = &pcb->in6p_sp->sp_in->spidx;
 767         error = ipsec_setspidx(m, spidx, 1);
 768         if (error)
 769                 goto bad;
 770         spidx->dir = IPSEC_DIR_INBOUND;
 771
 772         spidx = &pcb->in6p_sp->sp_out->spidx;
 773         error = ipsec_setspidx(m, spidx, 1);
 774         if (error)
 775                 goto bad;
 776         spidx->dir = IPSEC_DIR_OUTBOUND;
 777
 778         return 0;
 779
 780 bad:
 781         bzero(&pcb->in6p_sp->sp_in->spidx, sizeof(*spidx));
 782         bzero(&pcb->in6p_sp->sp_out->spidx, sizeof(*spidx));
 783         return error;
 784 }
 785 #endif
 786
 787 /*
 788  * configure security policy index (src/dst/proto/sport/dport)
 789  * by looking at the content of mbuf.
 790  * the caller is responsible for error recovery (like clearing up spidx).
 791  */
 792 static int
 793 ipsec_setspidx(struct mbuf *m, struct secpolicyindex *spidx, int needport)
 794 {
 795         struct ip *ip = NULL;
 796         struct ip ipbuf;
 797         u_int v;
 798         struct mbuf *n;
 799         int len;
 800         int error;
 801
 802         if (m == NULL)
 803                 panic("ipsec_setspidx: m == 0 passed.");
 804
 805         /*
 806          * validate m->m_pkthdr.len.  we see incorrect length if we
 807          * mistakenly call this function with inconsistent mbuf chain
 808          * (like 4.4BSD tcp/udp processing).  XXX should we panic here?
 809          */
 810         len = 0;
 811         for (n = m; n; n = n->m_next)
 812                 len += n->m_len;
 813         if (m->m_pkthdr.len != len) {
 814                 KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
 815                         kprintf("ipsec_setspidx: "
 816                                "total of m_len(%d) != pkthdr.len(%d), "
 817                                "ignored.\n",
 818                                 len, m->m_pkthdr.len));
 819                 return EINVAL;
 820         }
 821
 822         if (m->m_pkthdr.len < sizeof(struct ip)) {
 823                 KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
 824                         kprintf("ipsec_setspidx: "
 825                             "pkthdr.len(%d) < sizeof(struct ip), ignored.\n",
 826                             m->m_pkthdr.len));
 827                 return EINVAL;
 828         }
 829
 830         if (m->m_len >= sizeof(*ip))
 831                 ip = mtod(m, struct ip *);
 832         else {
 833                 m_copydata(m, 0, sizeof(ipbuf), (caddr_t)&ipbuf);
 834                 ip = &ipbuf;
 835         }
 836 #ifdef _IP_VHL
 837         v = _IP_VHL_V(ip->ip_vhl);
 838 #else
 839         v = ip->ip_v;
 840 #endif
 841         switch (v) {
 842         case 4:
 843                 error = ipsec4_setspidx_ipaddr(m, spidx);
 844                 if (error)
 845                         return error;
 846                 ipsec4_get_ulp(m, spidx, needport);
 847                 return 0;
 848 #ifdef INET6
 849         case 6:
 850                 if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
 851                         KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
 852                                 kprintf("ipsec_setspidx: "
 853                                     "pkthdr.len(%d) < sizeof(struct ip6_hdr), "
 854                                     "ignored.\n", m->m_pkthdr.len));
 855                         return EINVAL;
 856                 }
 857                 error = ipsec6_setspidx_ipaddr(m, spidx);
 858                 if (error)
 859                         return error;
 860                 ipsec6_get_ulp(m, spidx, needport);
 861                 return 0;
 862 #endif
 863         default:
 864                 KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
 865                         kprintf("ipsec_setspidx: "
 866                             "unknown IP version %u, ignored.\n", v));
 867                 return EINVAL;
 868         }
 869 }
 870
 871 static void
 872 ipsec4_get_ulp(struct mbuf *m, struct secpolicyindex *spidx, int needport)
 873 {
 874         struct ip ip;
 875         struct ip6_ext ip6e;
 876         u_int8_t nxt;
 877         int off;
 878         struct tcphdr th;
 879         struct udphdr uh;
 880
 881         /* sanity check */
 882         if (m == NULL)
 883                 panic("ipsec4_get_ulp: NULL pointer was passed.");
 884         if (m->m_pkthdr.len < sizeof(ip))
 885                 panic("ipsec4_get_ulp: too short");
 886
 887         /* set default */
 888         spidx->ul_proto = IPSEC_ULPROTO_ANY;
 889         ((struct sockaddr_in *)&spidx->src)->sin_port = IPSEC_PORT_ANY;
 890         ((struct sockaddr_in *)&spidx->dst)->sin_port = IPSEC_PORT_ANY;
 891
 892         m_copydata(m, 0, sizeof(ip), (caddr_t)&ip);
 893         /* ip_input() flips it into host endian XXX need more checking */
 894         if (ip.ip_off & (IP_MF | IP_OFFMASK))
 895                 return;
 896
 897         nxt = ip.ip_p;
 898 #ifdef _IP_VHL
 899         off = _IP_VHL_HL(ip->ip_vhl) << 2;
 900 #else
 901         off = ip.ip_hl << 2;
 902 #endif
 903         while (off < m->m_pkthdr.len) {
 904                 switch (nxt) {
 905                 case IPPROTO_TCP:
 906                         spidx->ul_proto = nxt;
 907                         if (!needport)
 908                                 return;
 909                         if (off + sizeof(struct tcphdr) > m->m_pkthdr.len)
 910                                 return;
 911                         m_copydata(m, off, sizeof(th), (caddr_t)&th);
 912                         ((struct sockaddr_in *)&spidx->src)->sin_port =
 913                             th.th_sport;
 914                         ((struct sockaddr_in *)&spidx->dst)->sin_port =
 915                             th.th_dport;
 916                         return;
 917                 case IPPROTO_UDP:
 918                         spidx->ul_proto = nxt;
 919                         if (!needport)
 920                                 return;
 921                         if (off + sizeof(struct udphdr) > m->m_pkthdr.len)
 922                                 return;
 923                         m_copydata(m, off, sizeof(uh), (caddr_t)&uh);
 924                         ((struct sockaddr_in *)&spidx->src)->sin_port =
 925                             uh.uh_sport;
 926                         ((struct sockaddr_in *)&spidx->dst)->sin_port =
 927                             uh.uh_dport;
 928                         return;
 929                 case IPPROTO_AH:
 930                         if (off + sizeof(ip6e) > m->m_pkthdr.len)
 931                                 return;
 932                         m_copydata(m, off, sizeof(ip6e), (caddr_t)&ip6e);
 933                         off += (ip6e.ip6e_len + 2) << 2;
 934                         nxt = ip6e.ip6e_nxt;
 935                         break;
 936                 case IPPROTO_ICMP:
 937                 default:
 938                         /* XXX intermediate headers??? */
 939                         spidx->ul_proto = nxt;
 940                         return;
 941                 }
 942         }
 943 }
 944
 945 /* assumes that m is sane */
 946 static int
 947 ipsec4_setspidx_ipaddr(struct mbuf *m, struct secpolicyindex *spidx)
 948 {
 949         struct ip *ip = NULL;
 950         struct ip ipbuf;
 951         struct sockaddr_in *sin;
 952
 953         if (m->m_len >= sizeof(*ip))
 954                 ip = mtod(m, struct ip *);
 955         else {
 956                 m_copydata(m, 0, sizeof(ipbuf), (caddr_t)&ipbuf);
 957                 ip = &ipbuf;
 958         }
 959
 960         sin = (struct sockaddr_in *)&spidx->src;
 961         bzero(sin, sizeof(*sin));
 962         sin->sin_family = AF_INET;
 963         sin->sin_len = sizeof(struct sockaddr_in);
 964         bcopy(&ip->ip_src, &sin->sin_addr, sizeof(ip->ip_src));
 965         spidx->prefs = sizeof(struct in_addr) << 3;
 966
 967         sin = (struct sockaddr_in *)&spidx->dst;
 968         bzero(sin, sizeof(*sin));
 969         sin->sin_family = AF_INET;
 970         sin->sin_len = sizeof(struct sockaddr_in);
 971         bcopy(&ip->ip_dst, &sin->sin_addr, sizeof(ip->ip_dst));
 972         spidx->prefd = sizeof(struct in_addr) << 3;
 973         return 0;
 974 }
 975
 976 #ifdef INET6
 977 static void
 978 ipsec6_get_ulp(struct mbuf *m, struct secpolicyindex *spidx, int needport)
 979 {
 980         int off, nxt;
 981         struct tcphdr th;
 982         struct udphdr uh;
 983
 984         /* sanity check */
 985         if (m == NULL)
 986                 panic("ipsec6_get_ulp: NULL pointer was passed.");
 987
 988         KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
 989                 kprintf("ipsec6_get_ulp:\n"); kdebug_mbuf(m));
 990
 991         /* set default */
 992         spidx->ul_proto = IPSEC_ULPROTO_ANY;
 993         ((struct sockaddr_in6 *)&spidx->src)->sin6_port = IPSEC_PORT_ANY;
 994         ((struct sockaddr_in6 *)&spidx->dst)->sin6_port = IPSEC_PORT_ANY;
 995
 996         nxt = -1;
 997         off = ip6_lasthdr(m, 0, IPPROTO_IPV6, &nxt);
 998         if (off < 0 || m->m_pkthdr.len < off)
 999                 return;
1000
1001         switch (nxt) {
1002         case IPPROTO_TCP:
1003                 spidx->ul_proto = nxt;
1004                 if (!needport)
1005                         break;
1006                 if (off + sizeof(struct tcphdr) > m->m_pkthdr.len)
1007                         break;
1008                 m_copydata(m, off, sizeof(th), (caddr_t)&th);
1009                 ((struct sockaddr_in6 *)&spidx->src)->sin6_port = th.th_sport;
1010                 ((struct sockaddr_in6 *)&spidx->dst)->sin6_port = th.th_dport;
1011                 break;
1012         case IPPROTO_UDP:
1013                 spidx->ul_proto = nxt;
1014                 if (!needport)
1015                         break;
1016                 if (off + sizeof(struct udphdr) > m->m_pkthdr.len)
1017                         break;
1018                 m_copydata(m, off, sizeof(uh), (caddr_t)&uh);
1019                 ((struct sockaddr_in6 *)&spidx->src)->sin6_port = uh.uh_sport;
1020                 ((struct sockaddr_in6 *)&spidx->dst)->sin6_port = uh.uh_dport;
1021                 break;
1022         case IPPROTO_ICMPV6:
1023         default:
1024                 /* XXX intermediate headers??? */
1025                 spidx->ul_proto = nxt;
1026                 break;
1027         }
1028 }
1029
1030 /* assumes that m is sane */
1031 static int
1032 ipsec6_setspidx_ipaddr(struct mbuf *m, struct secpolicyindex *spidx)
1033 {
1034         struct ip6_hdr *ip6 = NULL;
1035         struct ip6_hdr ip6buf;
1036         struct sockaddr_in6 *sin6;
1037
1038         if (m->m_len >= sizeof(*ip6))
1039                 ip6 = mtod(m, struct ip6_hdr *);
1040         else {
1041                 m_copydata(m, 0, sizeof(ip6buf), (caddr_t)&ip6buf);
1042                 ip6 = &ip6buf;
1043         }
1044
1045         sin6 = (struct sockaddr_in6 *)&spidx->src;
1046         bzero(sin6, sizeof(*sin6));
1047         sin6->sin6_family = AF_INET6;
1048         sin6->sin6_len = sizeof(struct sockaddr_in6);
1049         bcopy(&ip6->ip6_src, &sin6->sin6_addr, sizeof(ip6->ip6_src));
1050         if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
1051                 sin6->sin6_addr.s6_addr16[1] = 0;
1052                 sin6->sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
1053         }
1054         spidx->prefs = sizeof(struct in6_addr) << 3;
1055
1056         sin6 = (struct sockaddr_in6 *)&spidx->dst;
1057         bzero(sin6, sizeof(*sin6));
1058         sin6->sin6_family = AF_INET6;
1059         sin6->sin6_len = sizeof(struct sockaddr_in6);
1060         bcopy(&ip6->ip6_dst, &sin6->sin6_addr, sizeof(ip6->ip6_dst));
1061         if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
1062                 sin6->sin6_addr.s6_addr16[1] = 0;
1063                 sin6->sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
1064         }
1065         spidx->prefd = sizeof(struct in6_addr) << 3;
1066
1067         return 0;
1068 }
1069 #endif
1070
1071 static struct inpcbpolicy *
1072 ipsec_newpcbpolicy(void)
1073 {
1074         struct inpcbpolicy *p;
1075
1076         p = (struct inpcbpolicy *)kmalloc(sizeof(*p), M_SECA, M_NOWAIT);
1077         return p;
1078 }
1079
1080 static void
1081 ipsec_delpcbpolicy(struct inpcbpolicy *p)
1082 {
1083         kfree(p, M_SECA);
1084 }
1085
1086 /* initialize policy in PCB */
1087 int
1088 ipsec_init_policy(struct socket *so, struct inpcbpolicy **pcb_sp)
1089 {
1090         struct inpcbpolicy *new;
1091
1092         /* sanity check. */
1093         if (so == NULL || pcb_sp == NULL)
1094                 panic("ipsec_init_policy: NULL pointer was passed.");
1095
1096         lwkt_gettoken(&key_token);
1097
1098         new = ipsec_newpcbpolicy();
1099         if (new == NULL) {
1100                 ipseclog((LOG_DEBUG, "ipsec_init_policy: No more memory.\n"));
1101                 lwkt_reltoken(&key_token);
1102                 return ENOBUFS;
1103         }
1104         bzero(new, sizeof(*new));
1105
1106         if (so->so_cred != 0 && so->so_cred->cr_uid == 0)
1107                 new->priv = 1;
1108         else
1109                 new->priv = 0;
1110
1111         if ((new->sp_in = key_newsp()) == NULL) {
1112                 ipsec_delpcbpolicy(new);
1113                 lwkt_reltoken(&key_token);
1114                 return ENOBUFS;
1115         }
1116         new->sp_in->state = IPSEC_SPSTATE_ALIVE;
1117         new->sp_in->policy = IPSEC_POLICY_ENTRUST;
1118
1119         if ((new->sp_out = key_newsp()) == NULL) {
1120                 key_freesp(new->sp_in);
1121                 ipsec_delpcbpolicy(new);
1122                 lwkt_reltoken(&key_token);
1123                 return ENOBUFS;
1124         }
1125         new->sp_out->state = IPSEC_SPSTATE_ALIVE;
1126         new->sp_out->policy = IPSEC_POLICY_ENTRUST;
1127
1128         *pcb_sp = new;
1129         lwkt_reltoken(&key_token);
1130
1131         return 0;
1132 }
1133
1134 /* copy old ipsec policy into new */
1135 int
1136 ipsec_copy_policy(struct inpcbpolicy *old, struct inpcbpolicy *new)
1137 {
1138         struct secpolicy *sp;
1139
1140         lwkt_gettoken(&key_token);
1141         sp = ipsec_deepcopy_policy(old->sp_in);
1142         if (sp) {
1143                 key_freesp(new->sp_in);
1144                 new->sp_in = sp;
1145         } else {
1146                 lwkt_reltoken(&key_token);
1147                 return ENOBUFS;
1148         }
1149
1150         sp = ipsec_deepcopy_policy(old->sp_out);
1151         if (sp) {
1152                 key_freesp(new->sp_out);
1153                 new->sp_out = sp;
1154         } else {
1155                 lwkt_reltoken(&key_token);
1156                 return ENOBUFS;
1157         }
1158
1159         new->priv = old->priv;
1160         lwkt_reltoken(&key_token);
1161
1162         return 0;
1163 }
1164
1165 /* deep-copy a policy in PCB */
1166 static struct secpolicy *
1167 ipsec_deepcopy_policy(struct secpolicy *src)
1168 {
1169         struct ipsecrequest *newchain = NULL;
1170         struct ipsecrequest *p;
1171         struct ipsecrequest **q;
1172         struct ipsecrequest *r;
1173         struct secpolicy *dst;
1174
1175         lwkt_gettoken(&key_token);
1176         dst = key_newsp();
1177         if (src == NULL || dst == NULL) {
1178                 lwkt_reltoken(&key_token);
1179                 return NULL;
1180         }
1181
1182         /*
1183          * deep-copy IPsec request chain.  This is required since struct
1184          * ipsecrequest is not reference counted.
1185          */
1186         q = &newchain;
1187         for (p = src->req; p; p = p->next) {
1188                 *q = (struct ipsecrequest *)kmalloc(sizeof(struct ipsecrequest),
1189                         M_SECA, M_NOWAIT | M_ZERO);
1190                 if (*q == NULL)
1191                         goto fail;
1192                 (*q)->next = NULL;
1193
1194                 (*q)->saidx.proto = p->saidx.proto;
1195                 (*q)->saidx.mode = p->saidx.mode;
1196                 (*q)->level = p->level;
1197                 (*q)->saidx.reqid = p->saidx.reqid;
1198
1199                 bcopy(&p->saidx.src, &(*q)->saidx.src, sizeof((*q)->saidx.src));
1200                 bcopy(&p->saidx.dst, &(*q)->saidx.dst, sizeof((*q)->saidx.dst));
1201
1202                 (*q)->sav = NULL;
1203                 (*q)->sp = dst;
1204
1205                 q = &((*q)->next);
1206         }
1207
1208         dst->req = newchain;
1209         dst->state = src->state;
1210         dst->policy = src->policy;
1211         /* do not touch the refcnt fields */
1212         lwkt_reltoken(&key_token);
1213
1214         return dst;
1215
1216 fail:
1217         lwkt_reltoken(&key_token);
1218         for (p = newchain; p; p = r) {
1219                 r = p->next;
1220                 kfree(p, M_SECA);
1221                 p = NULL;
1222         }
1223         return NULL;
1224 }
1225
1226 /* set policy and ipsec request if present. */
1227 static int
1228 ipsec_set_policy(struct secpolicy **pcb_sp, int optname, caddr_t request,
1229                  size_t len, int priv)
1230 {
1231         struct sadb_x_policy *xpl;
1232         struct secpolicy *newsp = NULL;
1233         int error;
1234
1235         /* sanity check. */
1236         if (pcb_sp == NULL || *pcb_sp == NULL || request == NULL)
1237                 return EINVAL;
1238         if (len < sizeof(*xpl))
1239                 return EINVAL;
1240         xpl = (struct sadb_x_policy *)request;
1241
1242         KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
1243                 kprintf("ipsec_set_policy: passed policy\n");
1244                 kdebug_sadb_x_policy((struct sadb_ext *)xpl));
1245
1246         /* check policy type */
1247         /* ipsec_set_policy() accepts IPSEC, ENTRUST and BYPASS. */
1248         if (xpl->sadb_x_policy_type == IPSEC_POLICY_DISCARD
1249          || xpl->sadb_x_policy_type == IPSEC_POLICY_NONE)
1250                 return EINVAL;
1251
1252         /* check privileged socket */
1253         if (priv == 0 && xpl->sadb_x_policy_type == IPSEC_POLICY_BYPASS)
1254                 return EACCES;
1255
1256         /* allocation new SP entry */
1257         if ((newsp = key_msg2sp(xpl, len, &error)) == NULL)
1258                 return error;
1259
1260         newsp->state = IPSEC_SPSTATE_ALIVE;
1261
1262         /* clear old SP and set new SP */
1263         key_freesp(*pcb_sp);
1264         *pcb_sp = newsp;
1265         KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
1266                 kprintf("ipsec_set_policy: new policy\n");
1267                 kdebug_secpolicy(newsp));
1268
1269         return 0;
1270 }
1271
1272 static int
1273 ipsec_get_policy(struct secpolicy *pcb_sp, struct mbuf **mp)
1274 {
1275
1276         /* sanity check. */
1277         if (pcb_sp == NULL || mp == NULL)
1278                 return EINVAL;
1279
1280         *mp = key_sp2msg(pcb_sp);
1281         if (!*mp) {
1282                 ipseclog((LOG_DEBUG, "ipsec_get_policy: No more memory.\n"));
1283                 return ENOBUFS;
1284         }
1285
1286         KKASSERT((*mp)->m_type == MT_DATA);
1287         KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
1288                 kprintf("ipsec_get_policy:\n");
1289                 kdebug_mbuf(*mp));
1290
1291         return 0;
1292 }
1293
1294 int
1295 ipsec4_set_policy(struct inpcb *inp, int optname, caddr_t request, size_t len,
1296                   int priv)
1297 {
1298         struct sadb_x_policy *xpl;
1299         struct secpolicy **pcb_sp;
1300         int error;
1301
1302         /* sanity check. */
1303         if (inp == NULL || request == NULL)
1304                 return EINVAL;
1305         if (len < sizeof(*xpl))
1306                 return EINVAL;
1307         xpl = (struct sadb_x_policy *)request;
1308
1309         lwkt_gettoken(&key_token);
1310         /* select direction */
1311         switch (xpl->sadb_x_policy_dir) {
1312         case IPSEC_DIR_INBOUND:
1313                 pcb_sp = &inp->inp_sp->sp_in;
1314                 break;
1315         case IPSEC_DIR_OUTBOUND:
1316                 pcb_sp = &inp->inp_sp->sp_out;
1317                 break;
1318         default:
1319                 ipseclog((LOG_ERR, "ipsec4_set_policy: invalid direction=%u\n",
1320                         xpl->sadb_x_policy_dir));
1321                 lwkt_reltoken(&key_token);
1322                 return EINVAL;
1323         }
1324         error = ipsec_set_policy(pcb_sp, optname, request, len, priv);
1325         lwkt_reltoken(&key_token);
1326         return error;
1327 }
1328
1329 int
1330 ipsec4_get_policy(struct inpcb *inp, caddr_t request, size_t len,
1331                   struct mbuf **mp)
1332 {
1333         struct sadb_x_policy *xpl;
1334         struct secpolicy *pcb_sp;
1335         int error;
1336
1337         /* sanity check. */
1338         if (inp == NULL || request == NULL || mp == NULL)
1339                 return EINVAL;
1340         if (inp->inp_sp == NULL)
1341                 panic("policy in PCB is NULL");
1342         if (len < sizeof(*xpl))
1343                 return EINVAL;
1344         xpl = (struct sadb_x_policy *)request;
1345
1346         lwkt_gettoken(&key_token);
1347
1348         /* select direction */
1349         switch (xpl->sadb_x_policy_dir) {
1350         case IPSEC_DIR_INBOUND:
1351                 pcb_sp = inp->inp_sp->sp_in;
1352                 break;
1353         case IPSEC_DIR_OUTBOUND:
1354                 pcb_sp = inp->inp_sp->sp_out;
1355                 break;
1356         default:
1357                 ipseclog((LOG_ERR, "ipsec4_set_policy: invalid direction=%u\n",
1358                         xpl->sadb_x_policy_dir));
1359                 lwkt_reltoken(&key_token);
1360                 return EINVAL;
1361         }
1362         error = ipsec_get_policy(pcb_sp, mp);
1363         lwkt_reltoken(&key_token);
1364         return error;
1365 }
1366
1367 /* delete policy in PCB */
1368 int
1369 ipsec4_delete_pcbpolicy(struct inpcb *inp)
1370 {
1371         struct inpcbpolicy *isp;
1372
1373         /* sanity check. */
1374         if (inp == NULL)
1375                 panic("ipsec4_delete_pcbpolicy: NULL pointer was passed.");
1376
1377         lwkt_gettoken(&key_token);
1378
1379         if ((isp = inp->inp_sp) == NULL) {
1380                 lwkt_reltoken(&key_token);
1381                 return 0;
1382         }
1383
1384         if (isp->sp_in != NULL) {
1385                 key_freesp(isp->sp_in);
1386                 isp->sp_in = NULL;
1387         }
1388
1389         if (isp->sp_out != NULL) {
1390                 key_freesp(isp->sp_out);
1391                 isp->sp_out = NULL;
1392         }
1393         KKASSERT(inp->inp_sp == isp);
1394         inp->inp_sp = NULL;
1395         ipsec_delpcbpolicy(isp);
1396         lwkt_reltoken(&key_token);
1397
1398         return 0;
1399 }
1400
1401 #ifdef INET6
1402 int
1403 ipsec6_set_policy(struct in6pcb *in6p, int optname, caddr_t request, size_t len,
1404                   int priv)
1405 {
1406         struct sadb_x_policy *xpl;
1407         struct secpolicy **pcb_sp;
1408         int error;
1409
1410         /* sanity check. */
1411         if (in6p == NULL || request == NULL)
1412                 return EINVAL;
1413         if (len < sizeof(*xpl))
1414                 return EINVAL;
1415         xpl = (struct sadb_x_policy *)request;
1416
1417         lwkt_gettoken(&key_token);
1418
1419         /* select direction */
1420         switch (xpl->sadb_x_policy_dir) {
1421         case IPSEC_DIR_INBOUND:
1422                 pcb_sp = &in6p->in6p_sp->sp_in;
1423                 break;
1424         case IPSEC_DIR_OUTBOUND:
1425                 pcb_sp = &in6p->in6p_sp->sp_out;
1426                 break;
1427         default:
1428                 ipseclog((LOG_ERR, "ipsec6_set_policy: invalid direction=%u\n",
1429                         xpl->sadb_x_policy_dir));
1430                 lwkt_reltoken(&key_token);
1431                 return EINVAL;
1432         }
1433
1434         error = ipsec_set_policy(pcb_sp, optname, request, len, priv);
1435         lwkt_reltoken(&key_token);
1436         return error;
1437 }
1438
1439 int
1440 ipsec6_get_policy(struct in6pcb *in6p, caddr_t request, size_t len,
1441                   struct mbuf **mp)
1442 {
1443         struct sadb_x_policy *xpl;
1444         struct secpolicy *pcb_sp;
1445         int error;
1446
1447         /* sanity check. */
1448         if (in6p == NULL || request == NULL || mp == NULL)
1449                 return EINVAL;
1450         if (in6p->in6p_sp == NULL)
1451                 panic("policy in PCB is NULL");
1452         if (len < sizeof(*xpl))
1453                 return EINVAL;
1454         xpl = (struct sadb_x_policy *)request;
1455
1456         lwkt_gettoken(&key_token);
1457
1458         /* select direction */
1459         switch (xpl->sadb_x_policy_dir) {
1460         case IPSEC_DIR_INBOUND:
1461                 pcb_sp = in6p->in6p_sp->sp_in;
1462                 break;
1463         case IPSEC_DIR_OUTBOUND:
1464                 pcb_sp = in6p->in6p_sp->sp_out;
1465                 break;
1466         default:
1467                 ipseclog((LOG_ERR, "ipsec6_set_policy: invalid direction=%u\n",
1468                         xpl->sadb_x_policy_dir));
1469                 lwkt_reltoken(&key_token);
1470                 return EINVAL;
1471         }
1472
1473         error = ipsec_get_policy(pcb_sp, mp);
1474         lwkt_reltoken(&key_token);
1475         return error;
1476 }
1477
1478 int
1479 ipsec6_delete_pcbpolicy(struct in6pcb *in6p)
1480 {
1481         struct inpcbpolicy *isp;
1482
1483         /* sanity check. */
1484         if (in6p == NULL)
1485                 panic("ipsec6_delete_pcbpolicy: NULL pointer was passed.");
1486
1487         lwkt_gettoken(&key_token);
1488
1489         if ((isp = in6p->in6p_sp) == NULL) {
1490                 lwkt_reltoken(&key_token);
1491                 return 0;
1492         }
1493
1494         if (isp->sp_in != NULL) {
1495                 key_freesp(isp->sp_in);
1496                 isp->sp_in = NULL;
1497         }
1498
1499         if (isp->sp_out != NULL) {
1500                 key_freesp(isp->sp_out);
1501                 isp->sp_out = NULL;
1502         }
1503         KKASSERT(in6p->in6p_sp == isp);
1504         in6p->in6p_sp = NULL;
1505         ipsec_delpcbpolicy(isp);
1506         lwkt_reltoken(&key_token);
1507
1508         return 0;
1509 }
1510 #endif
1511
1512 /*
1513  * return current level.
1514  * Either IPSEC_LEVEL_USE or IPSEC_LEVEL_REQUIRE are always returned.
1515  */
1516 u_int
1517 ipsec_get_reqlevel(struct ipsecrequest *isr)
1518 {
1519         u_int level = 0;
1520         u_int esp_trans_deflev, esp_net_deflev, ah_trans_deflev, ah_net_deflev;
1521
1522         /* sanity check */
1523         if (isr == NULL || isr->sp == NULL)
1524                 panic("ipsec_get_reqlevel: NULL pointer is passed.");
1525         if (((struct sockaddr *)&isr->sp->spidx.src)->sa_family
1526                         != ((struct sockaddr *)&isr->sp->spidx.dst)->sa_family)
1527                 panic("ipsec_get_reqlevel: family mismatched.");
1528
1529 /* XXX note that we have ipseclog() expanded here - code sync issue */
1530 #define IPSEC_CHECK_DEFAULT(lev) \
1531         (((lev) != IPSEC_LEVEL_USE && (lev) != IPSEC_LEVEL_REQUIRE            \
1532                         && (lev) != IPSEC_LEVEL_UNIQUE)                       \
1533                 ? (ipsec_debug                                                \
1534                         ? log(LOG_INFO, "fixed system default level " #lev ":%d->%d\n",\
1535                                 (lev), IPSEC_LEVEL_REQUIRE)                   \
1536                         : 0),                                                 \
1537                         (lev) = IPSEC_LEVEL_REQUIRE,                          \
1538                         (lev)                                                 \
1539                 : (lev))
1540
1541         /* set default level */
1542         switch (((struct sockaddr *)&isr->sp->spidx.src)->sa_family) {
1543 #ifdef INET
1544         case AF_INET:
1545                 esp_trans_deflev = IPSEC_CHECK_DEFAULT(ip4_esp_trans_deflev);
1546                 esp_net_deflev = IPSEC_CHECK_DEFAULT(ip4_esp_net_deflev);
1547                 ah_trans_deflev = IPSEC_CHECK_DEFAULT(ip4_ah_trans_deflev);
1548                 ah_net_deflev = IPSEC_CHECK_DEFAULT(ip4_ah_net_deflev);
1549                 break;
1550 #endif
1551 #ifdef INET6
1552         case AF_INET6:
1553                 esp_trans_deflev = IPSEC_CHECK_DEFAULT(ip6_esp_trans_deflev);
1554                 esp_net_deflev = IPSEC_CHECK_DEFAULT(ip6_esp_net_deflev);
1555                 ah_trans_deflev = IPSEC_CHECK_DEFAULT(ip6_ah_trans_deflev);
1556                 ah_net_deflev = IPSEC_CHECK_DEFAULT(ip6_ah_net_deflev);
1557                 break;
1558 #endif /* INET6 */
1559         default:
1560                 panic("key_get_reqlevel: Unknown family. %d",
1561                         ((struct sockaddr *)&isr->sp->spidx.src)->sa_family);
1562         }
1563
1564 #undef IPSEC_CHECK_DEFAULT
1565
1566         /* set level */
1567         switch (isr->level) {
1568         case IPSEC_LEVEL_DEFAULT:
1569                 switch (isr->saidx.proto) {
1570                 case IPPROTO_ESP:
1571                         if (isr->saidx.mode == IPSEC_MODE_TUNNEL)
1572                                 level = esp_net_deflev;
1573                         else
1574                                 level = esp_trans_deflev;
1575                         break;
1576                 case IPPROTO_AH:
1577                         if (isr->saidx.mode == IPSEC_MODE_TUNNEL)
1578                                 level = ah_net_deflev;
1579                         else
1580                                 level = ah_trans_deflev;
1581                 case IPPROTO_IPCOMP:
1582                         /*
1583                          * we don't really care, as IPcomp document says that
1584                          * we shouldn't compress small packets
1585                          */
1586                         level = IPSEC_LEVEL_USE;
1587                         break;
1588                 default:
1589                         panic("ipsec_get_reqlevel: "
1590                                 "Illegal protocol defined %u",
1591                                 isr->saidx.proto);
1592                 }
1593                 break;
1594
1595         case IPSEC_LEVEL_USE:
1596         case IPSEC_LEVEL_REQUIRE:
1597                 level = isr->level;
1598                 break;
1599         case IPSEC_LEVEL_UNIQUE:
1600                 level = IPSEC_LEVEL_REQUIRE;
1601                 break;
1602
1603         default:
1604                 panic("ipsec_get_reqlevel: Illegal IPsec level %u",
1605                         isr->level);
1606         }
1607
1608         return level;
1609 }
1610
1611 /*
1612  * Check AH/ESP integrity.
1613  * OUT:
1614  *      0: valid
1615  *      1: invalid
1616  */
1617 static int
1618 ipsec_in_reject(struct secpolicy *sp, struct mbuf *m)
1619 {
1620         struct ipsecrequest *isr;
1621         u_int level;
1622         int need_auth, need_conf, need_icv;
1623
1624         KEYDEBUG(KEYDEBUG_IPSEC_DATA,
1625                 kprintf("ipsec_in_reject: using SP\n");
1626                 kdebug_secpolicy(sp));
1627
1628         /* check policy */
1629         switch (sp->policy) {
1630         case IPSEC_POLICY_DISCARD:
1631                 return 1;
1632         case IPSEC_POLICY_BYPASS:
1633         case IPSEC_POLICY_NONE:
1634                 return 0;
1635
1636         case IPSEC_POLICY_IPSEC:
1637                 break;
1638
1639         case IPSEC_POLICY_ENTRUST:
1640         default:
1641                 panic("ipsec_hdrsiz: Invalid policy found. %d", sp->policy);
1642         }
1643
1644         need_auth = 0;
1645         need_conf = 0;
1646         need_icv = 0;
1647
1648         /* XXX should compare policy against ipsec header history */
1649
1650         for (isr = sp->req; isr != NULL; isr = isr->next) {
1651
1652                 /* get current level */
1653                 level = ipsec_get_reqlevel(isr);
1654
1655                 switch (isr->saidx.proto) {
1656                 case IPPROTO_ESP:
1657                         if (level == IPSEC_LEVEL_REQUIRE) {
1658                                 need_conf++;
1659
1660                                 if (isr->sav != NULL
1661                                  && isr->sav->flags == SADB_X_EXT_NONE
1662                                  && isr->sav->alg_auth != SADB_AALG_NONE)
1663                                         need_icv++;
1664                         }
1665                         break;
1666                 case IPPROTO_AH:
1667                         if (level == IPSEC_LEVEL_REQUIRE) {
1668                                 need_auth++;
1669                                 need_icv++;
1670                         }
1671                         break;
1672                 case IPPROTO_IPCOMP:
1673                         /*
1674                          * we don't really care, as IPcomp document says that
1675                          * we shouldn't compress small packets, IPComp policy
1676                          * should always be treated as being in "use" level.
1677                          */
1678                         break;
1679                 }
1680         }
1681
1682         KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
1683                 kprintf("ipsec_in_reject: auth:%d conf:%d icv:%d m_flags:%x\n",
1684                         need_auth, need_conf, need_icv, m->m_flags));
1685
1686         if ((need_conf && !(m->m_flags & M_DECRYPTED))
1687          || (!need_auth && need_icv && !(m->m_flags & M_AUTHIPDGM))
1688          || (need_auth && !(m->m_flags & M_AUTHIPHDR)))
1689                 return 1;
1690
1691         return 0;
1692 }
1693
1694 /*
1695  * Check AH/ESP integrity.
1696  * This function is called from tcp_input(), udp_input(),
1697  * and {ah,esp}4_input for tunnel mode
1698  */
1699 int
1700 ipsec4_in_reject_so(struct mbuf *m, struct socket *so)
1701 {
1702         struct secpolicy *sp = NULL;
1703         int error;
1704         int result;
1705
1706         /* sanity check */
1707         if (m == NULL)
1708                 return 0;       /* XXX should be panic ? */
1709
1710         /* get SP for this packet.
1711          * When we are called from ip_forward(), we call
1712          * ipsec4_getpolicybyaddr() with IP_FORWARDING flag.
1713          */
1714         lwkt_gettoken(&key_token);
1715         if (so == NULL)
1716                 sp = ipsec4_getpolicybyaddr(m, IPSEC_DIR_INBOUND, IP_FORWARDING, &error);
1717         else
1718                 sp = ipsec4_getpolicybysock(m, IPSEC_DIR_INBOUND, so, &error);
1719
1720         if (sp == NULL) {
1721                 lwkt_reltoken(&key_token);
1722                 return 0;
1723         }
1724
1725         result = ipsec_in_reject(sp, m);
1726         KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
1727                 kprintf("DP ipsec4_in_reject_so call free SP:%p\n", sp));
1728         key_freesp(sp);
1729         lwkt_reltoken(&key_token);
1730
1731         return result;
1732 }
1733
1734 int
1735 ipsec4_in_reject(struct mbuf *m, struct inpcb *inp)
1736 {
1737         if (inp == NULL)
1738                 return ipsec4_in_reject_so(m, NULL);
1739         if (inp->inp_socket)
1740                 return ipsec4_in_reject_so(m, inp->inp_socket);
1741         else
1742                 panic("ipsec4_in_reject: invalid inpcb/socket");
1743 }
1744
1745 #ifdef INET6
1746 /*
1747  * Check AH/ESP integrity.
1748  * This function is called from tcp6_input(), udp6_input(),
1749  * and {ah,esp}6_input for tunnel mode
1750  */
1751 int
1752 ipsec6_in_reject_so(struct mbuf *m, struct socket *so)
1753 {
1754         struct secpolicy *sp = NULL;
1755         int error;
1756         int result;
1757
1758         /* sanity check */
1759         if (m == NULL)
1760                 return 0;       /* XXX should be panic ? */
1761
1762         /* get SP for this packet.
1763          * When we are called from ip_forward(), we call
1764          * ipsec6_getpolicybyaddr() with IP_FORWARDING flag.
1765          */
1766         lwkt_gettoken(&key_token);
1767         if (so == NULL)
1768                 sp = ipsec6_getpolicybyaddr(m, IPSEC_DIR_INBOUND, IP_FORWARDING, &error);
1769         else
1770                 sp = ipsec6_getpolicybysock(m, IPSEC_DIR_INBOUND, so, &error);
1771
1772         if (sp == NULL) {
1773                 lwkt_reltoken(&key_token);
1774                 return 0;       /* XXX should be panic ? */
1775         }
1776
1777         result = ipsec_in_reject(sp, m);
1778         KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
1779                 kprintf("DP ipsec6_in_reject_so call free SP:%p\n", sp));
1780         key_freesp(sp);
1781         lwkt_reltoken(&key_token);
1782
1783         return result;
1784 }
1785
1786 int
1787 ipsec6_in_reject(struct mbuf *m, struct in6pcb *in6p)
1788 {
1789         if (in6p == NULL)
1790                 return ipsec6_in_reject_so(m, NULL);
1791         if (in6p->in6p_socket)
1792                 return ipsec6_in_reject_so(m, in6p->in6p_socket);
1793         else
1794                 panic("ipsec6_in_reject: invalid in6p/socket");
1795 }
1796 #endif
1797
1798 /*
1799  * compute the byte size to be occupied by IPsec header.
1800  * in case it is tunneled, it includes the size of outer IP header.
1801  * NOTE: SP passed is free in this function.
1802  */
1803 static size_t
1804 ipsec_hdrsiz(struct secpolicy *sp)
1805 {
1806         struct ipsecrequest *isr;
1807         size_t siz, clen;
1808
1809         KEYDEBUG(KEYDEBUG_IPSEC_DATA,
1810                 kprintf("ipsec_hdrsiz: using SP\n");
1811                 kdebug_secpolicy(sp));
1812
1813         /* check policy */
1814         switch (sp->policy) {
1815         case IPSEC_POLICY_DISCARD:
1816         case IPSEC_POLICY_BYPASS:
1817         case IPSEC_POLICY_NONE:
1818                 return 0;
1819
1820         case IPSEC_POLICY_IPSEC:
1821                 break;
1822
1823         case IPSEC_POLICY_ENTRUST:
1824         default:
1825                 panic("ipsec_hdrsiz: Invalid policy found. %d", sp->policy);
1826         }
1827
1828         siz = 0;
1829
1830         for (isr = sp->req; isr != NULL; isr = isr->next) {
1831
1832                 clen = 0;
1833
1834                 switch (isr->saidx.proto) {
1835                 case IPPROTO_ESP:
1836 #ifdef IPSEC_ESP
1837                         clen = esp_hdrsiz(isr);
1838 #else
1839                         clen = 0;       /* XXX */
1840 #endif
1841                         break;
1842                 case IPPROTO_AH:
1843                         clen = ah_hdrsiz(isr);
1844                         break;
1845                 case IPPROTO_IPCOMP:
1846                         clen = sizeof(struct ipcomp);
1847                         break;
1848                 }
1849
1850                 if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
1851                         switch (((struct sockaddr *)&isr->saidx.dst)->sa_family) {
1852                         case AF_INET:
1853                                 clen += sizeof(struct ip);
1854                                 break;
1855 #ifdef INET6
1856                         case AF_INET6:
1857                                 clen += sizeof(struct ip6_hdr);
1858                                 break;
1859 #endif
1860                         default:
1861                                 ipseclog((LOG_ERR, "ipsec_hdrsiz: "
1862                                     "unknown AF %d in IPsec tunnel SA\n",
1863                                     ((struct sockaddr *)&isr->saidx.dst)->sa_family));
1864                                 break;
1865                         }
1866                 }
1867                 siz += clen;
1868         }
1869
1870         return siz;
1871 }
1872
1873 /* This function is called from ip_forward() and ipsec4_hdrsize_tcp(). */
1874 size_t
1875 ipsec4_hdrsiz(struct mbuf *m, u_int dir, struct inpcb *inp)
1876 {
1877         struct secpolicy *sp = NULL;
1878         int error;
1879         size_t size;
1880
1881         /* sanity check */
1882         if (m == NULL)
1883                 return 0;       /* XXX should be panic ? */
1884         if (inp != NULL && inp->inp_socket == NULL)
1885                 panic("ipsec4_hdrsize: why is socket NULL but there is PCB.");
1886
1887         /* get SP for this packet.
1888          * When we are called from ip_forward(), we call
1889          * ipsec4_getpolicybyaddr() with IP_FORWARDING flag.
1890          */
1891         lwkt_gettoken(&key_token);
1892         if (inp == NULL)
1893                 sp = ipsec4_getpolicybyaddr(m, dir, IP_FORWARDING, &error);
1894         else
1895                 sp = ipsec4_getpolicybysock(m, dir, inp->inp_socket, &error);
1896
1897         if (sp == NULL) {
1898                 lwkt_reltoken(&key_token);
1899                 return 0;       /* XXX should be panic ? */
1900         }
1901
1902         size = ipsec_hdrsiz(sp);
1903         KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
1904                 kprintf("DP ipsec4_hdrsiz call free SP:%p\n", sp));
1905         KEYDEBUG(KEYDEBUG_IPSEC_DATA,
1906                 kprintf("ipsec4_hdrsiz: size:%lu.\n", (unsigned long)size));
1907         key_freesp(sp);
1908         lwkt_reltoken(&key_token);
1909
1910         return size;
1911 }
1912
1913 #ifdef INET6
1914 /* This function is called from ipsec6_hdrsize_tcp(),
1915  * and maybe from ip6_forward.()
1916  */
1917 size_t
1918 ipsec6_hdrsiz(struct mbuf *m, u_int dir, struct in6pcb *in6p)
1919 {
1920         struct secpolicy *sp = NULL;
1921         int error;
1922         size_t size;
1923
1924         /* sanity check */
1925         if (m == NULL)
1926                 return 0;       /* XXX shoud be panic ? */
1927         if (in6p != NULL && in6p->in6p_socket == NULL)
1928                 panic("ipsec6_hdrsize: why is socket NULL but there is PCB.");
1929
1930         /* get SP for this packet */
1931         /* XXX Is it right to call with IP_FORWARDING. */
1932         lwkt_gettoken(&key_token);
1933         if (in6p == NULL)
1934                 sp = ipsec6_getpolicybyaddr(m, dir, IP_FORWARDING, &error);
1935         else
1936                 sp = ipsec6_getpolicybysock(m, dir, in6p->in6p_socket, &error);
1937
1938         if (sp == NULL) {
1939                 lwkt_reltoken(&key_token);
1940                 return 0;
1941         }
1942         size = ipsec_hdrsiz(sp);
1943         KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
1944                 kprintf("DP ipsec6_hdrsiz call free SP:%p\n", sp));
1945         KEYDEBUG(KEYDEBUG_IPSEC_DATA,
1946                 kprintf("ipsec6_hdrsiz: size:%lu.\n", (unsigned long)size));
1947         key_freesp(sp);
1948         lwkt_reltoken(&key_token);
1949
1950         return size;
1951 }
1952 #endif /* INET6 */
1953
1954 #ifdef INET
1955 /*
1956  * encapsulate for ipsec tunnel.
1957  * ip->ip_src must be fixed later on.
1958  */
1959 static int
1960 ipsec4_encapsulate(struct mbuf *m, struct secasvar *sav)
1961 {
1962         struct ip *oip;
1963         struct ip *ip;
1964         size_t hlen;
1965         size_t plen;
1966
1967         /* can't tunnel between different AFs */
1968         if (((struct sockaddr *)&sav->sah->saidx.src)->sa_family
1969                 != ((struct sockaddr *)&sav->sah->saidx.dst)->sa_family
1970          || ((struct sockaddr *)&sav->sah->saidx.src)->sa_family != AF_INET) {
1971                 m_freem(m);
1972                 return EINVAL;
1973         }
1974 #if 0
1975         /* XXX if the dst is myself, perform nothing. */
1976         if (key_ismyaddr((struct sockaddr *)&sav->sah->saidx.dst)) {
1977                 m_freem(m);
1978                 return EINVAL;
1979         }
1980 #endif
1981
1982         if (m->m_len < sizeof(*ip))
1983                 panic("ipsec4_encapsulate: assumption failed (first mbuf length)");
1984
1985         ip = mtod(m, struct ip *);
1986 #ifdef _IP_VHL
1987         hlen = _IP_VHL_HL(ip->ip_vhl) << 2;
1988 #else
1989         hlen = ip->ip_hl << 2;
1990 #endif
1991
1992         if (m->m_len != hlen)
1993                 panic("ipsec4_encapsulate: assumption failed (first mbuf length)");
1994
1995         /* generate header checksum */
1996         ip->ip_sum = 0;
1997 #ifdef _IP_VHL
1998         if (ip->ip_vhl == IP_VHL_BORING)
1999                 ip->ip_sum = in_cksum_hdr(ip);
2000         else
2001                 ip->ip_sum = in_cksum(m, hlen);
2002 #else
2003         ip->ip_sum = in_cksum(m, hlen);
2004 #endif
2005
2006         plen = m->m_pkthdr.len;
2007
2008         /*
2009          * grow the mbuf to accomodate the new IPv4 header.
2010          * NOTE: IPv4 options will never be copied.
2011          */
2012         if (M_LEADINGSPACE(m->m_next) < hlen) {
2013                 struct mbuf *n;
2014                 MGET(n, MB_DONTWAIT, MT_DATA);
2015                 if (!n) {
2016                         m_freem(m);
2017                         return ENOBUFS;
2018                 }
2019                 n->m_len = hlen;
2020                 n->m_next = m->m_next;
2021                 m->m_next = n;
2022                 m->m_pkthdr.len += hlen;
2023                 oip = mtod(n, struct ip *);
2024         } else {
2025                 m->m_next->m_len += hlen;
2026                 m->m_next->m_data -= hlen;
2027                 m->m_pkthdr.len += hlen;
2028                 oip = mtod(m->m_next, struct ip *);
2029         }
2030         ip = mtod(m, struct ip *);
2031         ovbcopy((caddr_t)ip, (caddr_t)oip, hlen);
2032         m->m_len = sizeof(struct ip);
2033         m->m_pkthdr.len -= (hlen - sizeof(struct ip));
2034
2035         /* construct new IPv4 header. see RFC 2401 5.1.2.1 */
2036         /* ECN consideration. */
2037         ip_ecn_ingress(ip4_ipsec_ecn, &ip->ip_tos, &oip->ip_tos);
2038 #ifdef _IP_VHL
2039         ip->ip_vhl = IP_MAKE_VHL(IPVERSION, sizeof(struct ip) >> 2);
2040 #else
2041         ip->ip_hl = sizeof(struct ip) >> 2;
2042 #endif
2043         ip->ip_off &= htons(~IP_OFFMASK);
2044         ip->ip_off &= htons(~IP_MF);
2045         switch (ip4_ipsec_dfbit) {
2046         case 0: /* clear DF bit */
2047                 ip->ip_off &= htons(~IP_DF);
2048                 break;
2049         case 1: /* set DF bit */
2050                 ip->ip_off |= htons(IP_DF);
2051                 break;
2052         default:        /* copy DF bit */
2053                 break;
2054         }
2055         ip->ip_p = IPPROTO_IPIP;
2056         if (plen + sizeof(struct ip) < IP_MAXPACKET)
2057                 ip->ip_len = htons(plen + sizeof(struct ip));
2058         else {
2059                 ipseclog((LOG_ERR, "IPv4 ipsec: size exceeds limit: "
2060                         "leave ip_len as is (invalid packet)\n"));
2061         }
2062 #ifdef RANDOM_IP_ID
2063         ip->ip_id = ip_randomid();
2064 #else
2065         ip->ip_id = htons(ip_id++);
2066 #endif
2067         bcopy(&((struct sockaddr_in *)&sav->sah->saidx.src)->sin_addr,
2068                 &ip->ip_src, sizeof(ip->ip_src));
2069         bcopy(&((struct sockaddr_in *)&sav->sah->saidx.dst)->sin_addr,
2070                 &ip->ip_dst, sizeof(ip->ip_dst));
2071         ip->ip_ttl = IPDEFTTL;
2072
2073         /* XXX Should ip_src be updated later ? */
2074
2075         return 0;
2076 }
2077 #endif /* INET */
2078
2079 #ifdef INET6
2080 static int
2081 ipsec6_encapsulate(struct mbuf *m, struct secasvar *sav)
2082 {
2083         struct ip6_hdr *oip6;
2084         struct ip6_hdr *ip6;
2085         size_t plen;
2086
2087         /* can't tunnel between different AFs */
2088         if (((struct sockaddr *)&sav->sah->saidx.src)->sa_family
2089                 != ((struct sockaddr *)&sav->sah->saidx.dst)->sa_family
2090          || ((struct sockaddr *)&sav->sah->saidx.src)->sa_family != AF_INET6) {
2091                 m_freem(m);
2092                 return EINVAL;
2093         }
2094 #if 0
2095         /* XXX if the dst is myself, perform nothing. */
2096         if (key_ismyaddr((struct sockaddr *)&sav->sah->saidx.dst)) {
2097                 m_freem(m);
2098                 return EINVAL;
2099         }
2100 #endif
2101
2102         plen = m->m_pkthdr.len;
2103
2104         /*
2105          * grow the mbuf to accomodate the new IPv6 header.
2106          */
2107         if (m->m_len != sizeof(struct ip6_hdr))
2108                 panic("ipsec6_encapsulate: assumption failed (first mbuf length)");
2109         if (M_LEADINGSPACE(m->m_next) < sizeof(struct ip6_hdr)) {
2110                 struct mbuf *n;
2111                 MGET(n, MB_DONTWAIT, MT_DATA);
2112                 if (!n) {
2113                         m_freem(m);
2114                         return ENOBUFS;
2115                 }
2116                 n->m_len = sizeof(struct ip6_hdr);
2117                 n->m_next = m->m_next;
2118                 m->m_next = n;
2119                 m->m_pkthdr.len += sizeof(struct ip6_hdr);
2120                 oip6 = mtod(n, struct ip6_hdr *);
2121         } else {
2122                 m->m_next->m_len += sizeof(struct ip6_hdr);
2123                 m->m_next->m_data -= sizeof(struct ip6_hdr);
2124                 m->m_pkthdr.len += sizeof(struct ip6_hdr);
2125                 oip6 = mtod(m->m_next, struct ip6_hdr *);
2126         }
2127         ip6 = mtod(m, struct ip6_hdr *);
2128         ovbcopy((caddr_t)ip6, (caddr_t)oip6, sizeof(struct ip6_hdr));
2129
2130         /* Fake link-local scope-class addresses */
2131         if (IN6_IS_SCOPE_LINKLOCAL(&oip6->ip6_src))
2132                 oip6->ip6_src.s6_addr16[1] = 0;
2133         if (IN6_IS_SCOPE_LINKLOCAL(&oip6->ip6_dst))
2134                 oip6->ip6_dst.s6_addr16[1] = 0;
2135
2136         /* construct new IPv6 header. see RFC 2401 5.1.2.2 */
2137         /* ECN consideration. */
2138         ip6_ecn_ingress(ip6_ipsec_ecn, &ip6->ip6_flow, &oip6->ip6_flow);
2139         if (plen < IPV6_MAXPACKET - sizeof(struct ip6_hdr))
2140                 ip6->ip6_plen = htons(plen);
2141         else {
2142                 /* ip6->ip6_plen will be updated in ip6_output() */
2143         }
2144         ip6->ip6_nxt = IPPROTO_IPV6;
2145         bcopy(&((struct sockaddr_in6 *)&sav->sah->saidx.src)->sin6_addr,
2146                 &ip6->ip6_src, sizeof(ip6->ip6_src));
2147         bcopy(&((struct sockaddr_in6 *)&sav->sah->saidx.dst)->sin6_addr,
2148                 &ip6->ip6_dst, sizeof(ip6->ip6_dst));
2149         ip6->ip6_hlim = IPV6_DEFHLIM;
2150
2151         /* XXX Should ip6_src be updated later ? */
2152
2153         return 0;
2154 }
2155 #endif /* INET6 */
2156
2157 /*
2158  * Check the variable replay window.
2159  * ipsec_chkreplay() performs replay check before ICV verification.
2160  * ipsec_updatereplay() updates replay bitmap.  This must be called after
2161  * ICV verification (it also performs replay check, which is usually done
2162  * beforehand).
2163  * 0 (zero) is returned if packet disallowed, 1 if packet permitted.
2164  *
2165  * based on RFC 2401.
2166  */
2167 int
2168 ipsec_chkreplay(u_int32_t seq, struct secasvar *sav)
2169 {
2170         const struct secreplay *replay;
2171         u_int32_t diff;
2172         int fr;
2173         u_int32_t wsizeb;       /* constant: bits of window size */
2174         int frlast;             /* constant: last frame */
2175
2176         /* sanity check */
2177         if (sav == NULL)
2178                 panic("ipsec_chkreplay: NULL pointer was passed.");
2179
2180         replay = sav->replay;
2181
2182         if (replay->wsize == 0)
2183                 return 1;       /* no need to check replay. */
2184
2185         /* constant */
2186         frlast = replay->wsize - 1;
2187         wsizeb = replay->wsize << 3;
2188
2189         /* sequence number of 0 is invalid */
2190         if (seq == 0)
2191                 return 0;
2192
2193         /* first time is always okay */
2194         if (replay->count == 0)
2195                 return 1;
2196
2197         if (seq > replay->lastseq) {
2198                 /* larger sequences are okay */
2199                 return 1;
2200         } else {
2201                 /* seq is equal or less than lastseq. */
2202                 diff = replay->lastseq - seq;
2203
2204                 /* over range to check, i.e. too old or wrapped */
2205                 if (diff >= wsizeb)
2206                         return 0;
2207
2208                 fr = frlast - diff / 8;
2209
2210                 /* this packet already seen ? */
2211                 if ((replay->bitmap)[fr] & (1 << (diff % 8)))
2212                         return 0;
2213
2214                 /* out of order but good */
2215                 return 1;
2216         }
2217 }
2218
2219 /*
2220  * check replay counter whether to update or not.
2221  * OUT: 0:      OK
2222  *      1:      NG
2223  */
2224 int
2225 ipsec_updatereplay(u_int32_t seq, struct secasvar *sav)
2226 {
2227         struct secreplay *replay;
2228         u_int32_t diff;
2229         int fr;
2230         u_int32_t wsizeb;       /* constant: bits of window size */
2231         int frlast;             /* constant: last frame */
2232
2233         /* sanity check */
2234         if (sav == NULL)
2235                 panic("ipsec_chkreplay: NULL pointer was passed.");
2236
2237         replay = sav->replay;
2238
2239         if (replay->wsize == 0)
2240                 goto ok;        /* no need to check replay. */
2241
2242         /* constant */
2243         frlast = replay->wsize - 1;
2244         wsizeb = replay->wsize << 3;
2245
2246         /* sequence number of 0 is invalid */
2247         if (seq == 0)
2248                 return 1;
2249
2250         /* first time */
2251         if (replay->count == 0) {
2252                 replay->lastseq = seq;
2253                 bzero(replay->bitmap, replay->wsize);
2254                 (replay->bitmap)[frlast] = 1;
2255                 goto ok;
2256         }
2257
2258         if (seq > replay->lastseq) {
2259                 /* seq is larger than lastseq. */
2260                 diff = seq - replay->lastseq;
2261
2262                 /* new larger sequence number */
2263                 if (diff < wsizeb) {
2264                         /* In window */
2265                         /* set bit for this packet */
2266                         vshiftl(replay->bitmap, diff, replay->wsize);
2267                         (replay->bitmap)[frlast] |= 1;
2268                 } else {
2269                         /* this packet has a "way larger" */
2270                         bzero(replay->bitmap, replay->wsize);
2271                         (replay->bitmap)[frlast] = 1;
2272                 }
2273                 replay->lastseq = seq;
2274
2275                 /* larger is good */
2276         } else {
2277                 /* seq is equal or less than lastseq. */
2278                 diff = replay->lastseq - seq;
2279
2280                 /* over range to check, i.e. too old or wrapped */
2281                 if (diff >= wsizeb)
2282                         return 1;
2283
2284                 fr = frlast - diff / 8;
2285
2286                 /* this packet already seen ? */
2287                 if ((replay->bitmap)[fr] & (1 << (diff % 8)))
2288                         return 1;
2289
2290                 /* mark as seen */
2291                 (replay->bitmap)[fr] |= (1 << (diff % 8));
2292
2293                 /* out of order but good */
2294         }
2295
2296 ok:
2297         if (replay->count == ~0) {
2298
2299                 /* set overflow flag */
2300                 replay->overflow++;
2301
2302                 /* don't increment, no more packets accepted */
2303                 if ((sav->flags & SADB_X_EXT_CYCSEQ) == 0)
2304                         return 1;
2305
2306                 ipseclog((LOG_WARNING, "replay counter made %d cycle. %s\n",
2307                     replay->overflow, ipsec_logsastr(sav)));
2308         }
2309
2310         replay->count++;
2311
2312         return 0;
2313 }
2314
2315 /*
2316  * shift variable length buffer to left.
2317  * IN:  bitmap: pointer to the buffer
2318  *      nbit:   the number of to shift.
2319  *      wsize:  buffer size (bytes).
2320  */
2321 static void
2322 vshiftl(unsigned char *bitmap, int nbit, int wsize)
2323 {
2324         int s, j, i;
2325         unsigned char over;
2326
2327         for (j = 0; j < nbit; j += 8) {
2328                 s = (nbit - j < 8) ? (nbit - j): 8;
2329                 bitmap[0] <<= s;
2330                 for (i = 1; i < wsize; i++) {
2331                         over = (bitmap[i] >> (8 - s));
2332                         bitmap[i] <<= s;
2333                         bitmap[i-1] |= over;
2334                 }
2335         }
2336
2337         return;
2338 }
2339
2340 const char *
2341 ipsec4_logpacketstr(struct ip *ip, u_int32_t spi)
2342 {
2343         static char buf[256];
2344         char *p;
2345         u_int8_t *s, *d;
2346
2347         s = (u_int8_t *)(&ip->ip_src);
2348         d = (u_int8_t *)(&ip->ip_dst);
2349
2350         p = buf;
2351         ksnprintf(buf, sizeof(buf), "packet(SPI=%u ", (u_int32_t)ntohl(spi));
2352         while (p && *p)
2353                 p++;
2354         ksnprintf(p, sizeof(buf) - (p - buf), "src=%u.%u.%u.%u",
2355                 s[0], s[1], s[2], s[3]);
2356         while (p && *p)
2357                 p++;
2358         ksnprintf(p, sizeof(buf) - (p - buf), " dst=%u.%u.%u.%u",
2359                 d[0], d[1], d[2], d[3]);
2360         while (p && *p)
2361                 p++;
2362         ksnprintf(p, sizeof(buf) - (p - buf), ")");
2363
2364         return buf;
2365 }
2366
2367 #ifdef INET6
2368 const char *
2369 ipsec6_logpacketstr(struct ip6_hdr *ip6, u_int32_t spi)
2370 {
2371         static char buf[256];
2372         char *p;
2373
2374         p = buf;
2375         ksnprintf(buf, sizeof(buf), "packet(SPI=%u ", (u_int32_t)ntohl(spi));
2376         while (p && *p)
2377                 p++;
2378         ksnprintf(p, sizeof(buf) - (p - buf), "src=%s",
2379                 ip6_sprintf(&ip6->ip6_src));
2380         while (p && *p)
2381                 p++;
2382         ksnprintf(p, sizeof(buf) - (p - buf), " dst=%s",
2383                 ip6_sprintf(&ip6->ip6_dst));
2384         while (p && *p)
2385                 p++;
2386         ksnprintf(p, sizeof(buf) - (p - buf), ")");
2387
2388         return buf;
2389 }
2390 #endif /* INET6 */
2391
2392 const char *
2393 ipsec_logsastr(struct secasvar *sav)
2394 {
2395         static char buf[256];
2396         char *p;
2397         struct secasindex *saidx = &sav->sah->saidx;
2398
2399         /* validity check */
2400         if (((struct sockaddr *)&sav->sah->saidx.src)->sa_family
2401                         != ((struct sockaddr *)&sav->sah->saidx.dst)->sa_family)
2402                 panic("ipsec_logsastr: family mismatched.");
2403
2404         p = buf;
2405         ksnprintf(buf, sizeof(buf), "SA(SPI=%u ", (u_int32_t)ntohl(sav->spi));
2406         while (p && *p)
2407                 p++;
2408         if (((struct sockaddr *)&saidx->src)->sa_family == AF_INET) {
2409                 u_int8_t *s, *d;
2410                 s = (u_int8_t *)&((struct sockaddr_in *)&saidx->src)->sin_addr;
2411                 d = (u_int8_t *)&((struct sockaddr_in *)&saidx->dst)->sin_addr;
2412                 ksnprintf(p, sizeof(buf) - (p - buf),
2413                         "src=%d.%d.%d.%d dst=%d.%d.%d.%d",
2414                         s[0], s[1], s[2], s[3], d[0], d[1], d[2], d[3]);
2415         }
2416 #ifdef INET6
2417         else if (((struct sockaddr *)&saidx->src)->sa_family == AF_INET6) {
2418                 ksnprintf(p, sizeof(buf) - (p - buf),
2419                         "src=%s",
2420                         ip6_sprintf(&((struct sockaddr_in6 *)&saidx->src)->sin6_addr));
2421                 while (p && *p)
2422                         p++;
2423                 ksnprintf(p, sizeof(buf) - (p - buf),
2424                         " dst=%s",
2425                         ip6_sprintf(&((struct sockaddr_in6 *)&saidx->dst)->sin6_addr));
2426         }
2427 #endif
2428         while (p && *p)
2429                 p++;
2430         ksnprintf(p, sizeof(buf) - (p - buf), ")");
2431
2432         return buf;
2433 }
2434
2435 void
2436 ipsec_dumpmbuf(struct mbuf *m)
2437 {
2438         int totlen;
2439         int i;
2440         u_char *p;
2441
2442         totlen = 0;
2443         kprintf("---\n");
2444         while (m) {
2445                 p = mtod(m, u_char *);
2446                 for (i = 0; i < m->m_len; i++) {
2447                         kprintf("%02x ", p[i]);
2448                         totlen++;
2449                         if (totlen % 16 == 0)
2450                                 kprintf("\n");
2451                 }
2452                 m = m->m_next;
2453         }
2454         if (totlen % 16 != 0)
2455                 kprintf("\n");
2456         kprintf("---\n");
2457 }
2458
2459 #ifdef INET
2460 /*
2461  * IPsec output logic for IPv4.
2462  */
2463 int
2464 ipsec4_output(struct ipsec_output_state *state, struct secpolicy *sp, int flags)
2465 {
2466         struct ip *ip = NULL;
2467         struct ipsecrequest *isr = NULL;
2468         struct secasindex saidx;
2469         int error;
2470         struct sockaddr_in *dst4;
2471         struct sockaddr_in *sin;
2472
2473         if (!state)
2474                 panic("state == NULL in ipsec4_output");
2475         if (!state->m)
2476                 panic("state->m == NULL in ipsec4_output");
2477         if (!state->ro)
2478                 panic("state->ro == NULL in ipsec4_output");
2479         if (!state->dst)
2480                 panic("state->dst == NULL in ipsec4_output");
2481
2482         KEYDEBUG(KEYDEBUG_IPSEC_DATA,
2483                 kprintf("ipsec4_output: applyed SP\n");
2484                 kdebug_secpolicy(sp));
2485
2486         for (isr = sp->req; isr != NULL; isr = isr->next) {
2487
2488 #if 0   /* give up to check restriction of transport mode */
2489         /* XXX but should be checked somewhere */
2490                 /*
2491                  * some of the IPsec operation must be performed only in
2492                  * originating case.
2493                  */
2494                 if (isr->saidx.mode == IPSEC_MODE_TRANSPORT
2495                  && (flags & IP_FORWARDING))
2496                         continue;
2497 #endif
2498
2499                 /* make SA index for search proper SA */
2500                 ip = mtod(state->m, struct ip *);
2501                 bcopy(&isr->saidx, &saidx, sizeof(saidx));
2502                 saidx.mode = isr->saidx.mode;
2503                 saidx.reqid = isr->saidx.reqid;
2504                 sin = (struct sockaddr_in *)&saidx.src;
2505                 if (sin->sin_len == 0) {
2506                         sin->sin_len = sizeof(*sin);
2507                         sin->sin_family = AF_INET;
2508                         sin->sin_port = IPSEC_PORT_ANY;
2509                         bcopy(&ip->ip_src, &sin->sin_addr,
2510                             sizeof(sin->sin_addr));
2511                 }
2512                 sin = (struct sockaddr_in *)&saidx.dst;
2513                 if (sin->sin_len == 0) {
2514                         sin->sin_len = sizeof(*sin);
2515                         sin->sin_family = AF_INET;
2516                         sin->sin_port = IPSEC_PORT_ANY;
2517                         bcopy(&ip->ip_dst, &sin->sin_addr,
2518                             sizeof(sin->sin_addr));
2519                 }
2520
2521                 if ((error = key_checkrequest(isr, &saidx)) != 0) {
2522                         /*
2523                          * IPsec processing is required, but no SA found.
2524                          * I assume that key_acquire() had been called
2525                          * to get/establish the SA. Here I discard
2526                          * this packet because it is responsibility for
2527                          * upper layer to retransmit the packet.
2528                          */
2529                         ipsecstat.out_nosa++;
2530                         goto bad;
2531                 }
2532
2533                 /* validity check */
2534                 if (isr->sav == NULL) {
2535                         switch (ipsec_get_reqlevel(isr)) {
2536                         case IPSEC_LEVEL_USE:
2537                                 continue;
2538                         case IPSEC_LEVEL_REQUIRE:
2539                                 /* must be not reached here. */
2540                                 panic("ipsec4_output: no SA found, but required.");
2541                         }
2542                 }
2543
2544                 /*
2545                  * If there is no valid SA, we give up to process any
2546                  * more.  In such a case, the SA's status is changed
2547                  * from DYING to DEAD after allocating.  If a packet
2548                  * send to the receiver by dead SA, the receiver can
2549                  * not decode a packet because SA has been dead.
2550                  */
2551                 if (isr->sav->state != SADB_SASTATE_MATURE
2552                  && isr->sav->state != SADB_SASTATE_DYING) {
2553                         ipsecstat.out_nosa++;
2554                         error = EINVAL;
2555                         goto bad;
2556                 }
2557
2558                 /*
2559                  * There may be the case that SA status will be changed when
2560                  * we are refering to one. So calling crit_enter().
2561                  */
2562                 crit_enter();
2563
2564                 if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
2565                         /*
2566                          * build IPsec tunnel.
2567                          */
2568                         /* XXX should be processed with other familiy */
2569                         if (((struct sockaddr *)&isr->sav->sah->saidx.src)->sa_family != AF_INET) {
2570                                 ipseclog((LOG_ERR, "ipsec4_output: "
2571                                     "family mismatched between inner and outer spi=%u\n",
2572                                     (u_int32_t)ntohl(isr->sav->spi)));
2573                                 crit_exit();
2574                                 error = EAFNOSUPPORT;
2575                                 goto bad;
2576                         }
2577
2578                         state->m = ipsec4_splithdr(state->m);
2579                         if (!state->m) {
2580                                 crit_exit();
2581                                 error = ENOMEM;
2582                                 goto bad;
2583                         }
2584                         error = ipsec4_encapsulate(state->m, isr->sav);
2585                         crit_exit();
2586                         if (error) {
2587                                 state->m = NULL;
2588                                 goto bad;
2589                         }
2590                         ip = mtod(state->m, struct ip *);
2591
2592                         state->ro = &isr->sav->sah->sa_route;
2593                         state->dst = (struct sockaddr *)&state->ro->ro_dst;
2594                         dst4 = (struct sockaddr_in *)state->dst;
2595                         if (state->ro->ro_rt
2596                          && ((state->ro->ro_rt->rt_flags & RTF_UP) == 0
2597                           || dst4->sin_addr.s_addr != ip->ip_dst.s_addr)) {
2598                                 RTFREE(state->ro->ro_rt);
2599                                 state->ro->ro_rt = NULL;
2600                         }
2601                         if (state->ro->ro_rt == 0) {
2602                                 dst4->sin_family = AF_INET;
2603                                 dst4->sin_len = sizeof(*dst4);
2604                                 dst4->sin_addr = ip->ip_dst;
2605                                 rtalloc(state->ro);
2606                         }
2607                         if (state->ro->ro_rt == 0) {
2608                                 ipstat.ips_noroute++;
2609                                 error = EHOSTUNREACH;
2610                                 goto bad;
2611                         }
2612
2613                         /* adjust state->dst if tunnel endpoint is offlink */
2614                         if (state->ro->ro_rt->rt_flags & RTF_GATEWAY) {
2615                                 state->dst = (struct sockaddr *)state->ro->ro_rt->rt_gateway;
2616                                 dst4 = (struct sockaddr_in *)state->dst;
2617                         }
2618                 } else
2619                         crit_exit();
2620
2621                 state->m = ipsec4_splithdr(state->m);
2622                 if (!state->m) {
2623                         error = ENOMEM;
2624                         goto bad;
2625                 }
2626                 switch (isr->saidx.proto) {
2627                 case IPPROTO_ESP:
2628 #ifdef IPSEC_ESP
2629                         if ((error = esp4_output(state->m, isr)) != 0) {
2630                                 state->m = NULL;
2631                                 goto bad;
2632                         }
2633                         break;
2634 #else
2635                         m_freem(state->m);
2636                         state->m = NULL;
2637                         error = EINVAL;
2638                         goto bad;
2639 #endif
2640                 case IPPROTO_AH:
2641                         if ((error = ah4_output(state->m, isr)) != 0) {
2642                                 state->m = NULL;
2643                                 goto bad;
2644                         }
2645                         break;
2646                 case IPPROTO_IPCOMP:
2647                         if ((error = ipcomp4_output(state->m, isr)) != 0) {
2648                                 state->m = NULL;
2649                                 goto bad;
2650                         }
2651                         break;
2652                 default:
2653                         ipseclog((LOG_ERR,
2654                             "ipsec4_output: unknown ipsec protocol %d\n",
2655                             isr->saidx.proto));
2656                         m_freem(state->m);
2657                         state->m = NULL;
2658                         error = EINVAL;
2659                         goto bad;
2660                 }
2661
2662                 if (state->m == 0) {
2663                         error = ENOMEM;
2664                         goto bad;
2665                 }
2666                 ip = mtod(state->m, struct ip *);
2667         }
2668
2669         return 0;
2670
2671 bad:
2672         m_freem(state->m);
2673         state->m = NULL;
2674         return error;
2675 }
2676 #endif
2677
2678 #ifdef INET6
2679 /*
2680  * IPsec output logic for IPv6, transport mode.
2681  */
2682 int
2683 ipsec6_output_trans(struct ipsec_output_state *state, u_char *nexthdrp,
2684                     struct mbuf *mprev, struct secpolicy *sp, int flags,
2685                     int *tun)
2686 {
2687         struct ip6_hdr *ip6;
2688         struct ipsecrequest *isr = NULL;
2689         struct secasindex saidx;
2690         int error = 0;
2691         int plen;
2692         struct sockaddr_in6 *sin6;
2693
2694         if (!state)
2695                 panic("state == NULL in ipsec6_output_trans");
2696         if (!state->m)
2697                 panic("state->m == NULL in ipsec6_output_trans");
2698         if (!nexthdrp)
2699                 panic("nexthdrp == NULL in ipsec6_output_trans");
2700         if (!mprev)
2701                 panic("mprev == NULL in ipsec6_output_trans");
2702         if (!sp)
2703                 panic("sp == NULL in ipsec6_output_trans");
2704         if (!tun)
2705                 panic("tun == NULL in ipsec6_output_trans");
2706
2707         KEYDEBUG(KEYDEBUG_IPSEC_DATA,
2708                 kprintf("ipsec6_output_trans: applyed SP\n");
2709                 kdebug_secpolicy(sp));
2710
2711         lwkt_gettoken(&key_token);
2712
2713         *tun = 0;
2714         for (isr = sp->req; isr; isr = isr->next) {
2715                 if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
2716                         /* the rest will be handled by ipsec6_output_tunnel() */
2717                         break;
2718                 }
2719
2720                 /* make SA index for search proper SA */
2721                 ip6 = mtod(state->m, struct ip6_hdr *);
2722                 bcopy(&isr->saidx, &saidx, sizeof(saidx));
2723                 saidx.mode = isr->saidx.mode;
2724                 saidx.reqid = isr->saidx.reqid;
2725                 sin6 = (struct sockaddr_in6 *)&saidx.src;
2726                 if (sin6->sin6_len == 0) {
2727                         sin6->sin6_len = sizeof(*sin6);
2728                         sin6->sin6_family = AF_INET6;
2729                         sin6->sin6_port = IPSEC_PORT_ANY;
2730                         bcopy(&ip6->ip6_src, &sin6->sin6_addr,
2731                             sizeof(ip6->ip6_src));
2732                         if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
2733                                 /* fix scope id for comparing SPD */
2734                                 sin6->sin6_addr.s6_addr16[1] = 0;
2735                                 sin6->sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
2736                         }
2737                 }
2738                 sin6 = (struct sockaddr_in6 *)&saidx.dst;
2739                 if (sin6->sin6_len == 0) {
2740                         sin6->sin6_len = sizeof(*sin6);
2741                         sin6->sin6_family = AF_INET6;
2742                         sin6->sin6_port = IPSEC_PORT_ANY;
2743                         bcopy(&ip6->ip6_dst, &sin6->sin6_addr,
2744                             sizeof(ip6->ip6_dst));
2745                         if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
2746                                 /* fix scope id for comparing SPD */
2747                                 sin6->sin6_addr.s6_addr16[1] = 0;
2748                                 sin6->sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
2749                         }
2750                 }
2751
2752                 if (key_checkrequest(isr, &saidx) == ENOENT) {
2753                         /*
2754                          * IPsec processing is required, but no SA found.
2755                          * I assume that key_acquire() had been called
2756                          * to get/establish the SA. Here I discard
2757                          * this packet because it is responsibility for
2758                          * upper layer to retransmit the packet.
2759                          */
2760                         ipsec6stat.out_nosa++;
2761                         error = ENOENT;
2762
2763                         /*
2764                          * Notify the fact that the packet is discarded
2765                          * to ourselves. I believe this is better than
2766                          * just silently discarding. (jinmei@kame.net)
2767                          * XXX: should we restrict the error to TCP packets?
2768                          * XXX: should we directly notify sockets via
2769                          *      kpfctlinputs?
2770                          */
2771                         icmp6_error(state->m, ICMP6_DST_UNREACH,
2772                                     ICMP6_DST_UNREACH_ADMIN, 0);
2773                         state->m = NULL; /* icmp6_error freed the mbuf */
2774                         goto bad;
2775                 }
2776
2777                 /* validity check */
2778                 if (isr->sav == NULL) {
2779                         switch (ipsec_get_reqlevel(isr)) {
2780                         case IPSEC_LEVEL_USE:
2781                                 continue;
2782                         case IPSEC_LEVEL_REQUIRE:
2783                                 /* must be not reached here. */
2784                                 panic("ipsec6_output_trans: no SA found, but required.");
2785                         }
2786                 }
2787
2788                 /*
2789                  * If there is no valid SA, we give up to process.
2790                  * see same place at ipsec4_output().
2791                  */
2792                 if (isr->sav->state != SADB_SASTATE_MATURE
2793                  && isr->sav->state != SADB_SASTATE_DYING) {
2794                         ipsec6stat.out_nosa++;
2795                         error = EINVAL;
2796                         goto bad;
2797                 }
2798
2799                 switch (isr->saidx.proto) {
2800                 case IPPROTO_ESP:
2801 #ifdef IPSEC_ESP
2802                         error = esp6_output(state->m, nexthdrp, mprev->m_next, isr);
2803 #else
2804                         m_freem(state->m);
2805                         error = EINVAL;
2806 #endif
2807                         break;
2808                 case IPPROTO_AH:
2809                         error = ah6_output(state->m, nexthdrp, mprev->m_next, isr);
2810                         break;
2811                 case IPPROTO_IPCOMP:
2812                         error = ipcomp6_output(state->m, nexthdrp, mprev->m_next, isr);
2813                         break;
2814                 default:
2815                         ipseclog((LOG_ERR, "ipsec6_output_trans: "
2816                             "unknown ipsec protocol %d\n", isr->saidx.proto));
2817                         m_freem(state->m);
2818                         ipsec6stat.out_inval++;
2819                         error = EINVAL;
2820                         break;
2821                 }
2822                 if (error) {
2823                         state->m = NULL;
2824                         goto bad;
2825                 }
2826                 plen = state->m->m_pkthdr.len - sizeof(struct ip6_hdr);
2827                 if (plen > IPV6_MAXPACKET) {
2828                         ipseclog((LOG_ERR, "ipsec6_output_trans: "
2829                             "IPsec with IPv6 jumbogram is not supported\n"));
2830                         ipsec6stat.out_inval++;
2831                         error = EINVAL; /* XXX */
2832                         goto bad;
2833                 }
2834                 ip6 = mtod(state->m, struct ip6_hdr *);
2835                 ip6->ip6_plen = htons(plen);
2836         }
2837
2838         /* if we have more to go, we need a tunnel mode processing */
2839         if (isr != NULL)
2840                 *tun = 1;
2841         lwkt_reltoken(&key_token);
2842         return 0;
2843
2844 bad:
2845         lwkt_reltoken(&key_token);
2846         m_freem(state->m);
2847         state->m = NULL;
2848         return error;
2849 }
2850
2851 /*
2852  * IPsec output logic for IPv6, tunnel mode.
2853  */
2854 int
2855 ipsec6_output_tunnel(struct ipsec_output_state *state, struct secpolicy *sp,
2856                      int flags)
2857 {
2858         struct ip6_hdr *ip6;
2859         struct ipsecrequest *isr = NULL;
2860         struct secasindex saidx;
2861         int error = 0;
2862         int plen;
2863         struct sockaddr_in6* dst6;
2864
2865         if (!state)
2866                 panic("state == NULL in ipsec6_output_tunnel");
2867         if (!state->m)
2868                 panic("state->m == NULL in ipsec6_output_tunnel");
2869         if (!sp)
2870                 panic("sp == NULL in ipsec6_output_tunnel");
2871
2872         KEYDEBUG(KEYDEBUG_IPSEC_DATA,
2873                 kprintf("ipsec6_output_tunnel: applyed SP\n");
2874                 kdebug_secpolicy(sp));
2875
2876         /*
2877          * transport mode ipsec (before the 1st tunnel mode) is already
2878          * processed by ipsec6_output_trans().
2879          */
2880         lwkt_gettoken(&key_token);
2881         for (isr = sp->req; isr; isr = isr->next) {
2882                 if (isr->saidx.mode == IPSEC_MODE_TUNNEL)
2883                         break;
2884         }
2885
2886         for (/* already initialized */; isr; isr = isr->next) {
2887                 if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
2888                         /* When tunnel mode, SA peers must be specified. */
2889                         bcopy(&isr->saidx, &saidx, sizeof(saidx));
2890                 } else {
2891                         /* make SA index to look for a proper SA */
2892                         struct sockaddr_in6 *sin6;
2893
2894                         bzero(&saidx, sizeof(saidx));
2895                         saidx.proto = isr->saidx.proto;
2896                         saidx.mode = isr->saidx.mode;
2897                         saidx.reqid = isr->saidx.reqid;
2898
2899                         ip6 = mtod(state->m, struct ip6_hdr *);
2900                         sin6 = (struct sockaddr_in6 *)&saidx.src;
2901                         if (sin6->sin6_len == 0) {
2902                                 sin6->sin6_len = sizeof(*sin6);
2903                                 sin6->sin6_family = AF_INET6;
2904                                 sin6->sin6_port = IPSEC_PORT_ANY;
2905                                 bcopy(&ip6->ip6_src, &sin6->sin6_addr,
2906                                     sizeof(ip6->ip6_src));
2907                                 if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
2908                                         /* fix scope id for comparing SPD */
2909                                         sin6->sin6_addr.s6_addr16[1] = 0;
2910                                         sin6->sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
2911                                 }
2912                         }
2913                         sin6 = (struct sockaddr_in6 *)&saidx.dst;
2914                         if (sin6->sin6_len == 0) {
2915                                 sin6->sin6_len = sizeof(*sin6);
2916                                 sin6->sin6_family = AF_INET6;
2917                                 sin6->sin6_port = IPSEC_PORT_ANY;
2918                                 bcopy(&ip6->ip6_dst, &sin6->sin6_addr,
2919                                     sizeof(ip6->ip6_dst));
2920                                 if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
2921                                         /* fix scope id for comparing SPD */
2922                                         sin6->sin6_addr.s6_addr16[1] = 0;
2923                                         sin6->sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
2924                                 }
2925                         }
2926                 }
2927
2928                 if (key_checkrequest(isr, &saidx) == ENOENT) {
2929                         /*
2930                          * IPsec processing is required, but no SA found.
2931                          * I assume that key_acquire() had been called
2932                          * to get/establish the SA. Here I discard
2933                          * this packet because it is responsibility for
2934                          * upper layer to retransmit the packet.
2935                          */
2936                         ipsec6stat.out_nosa++;
2937                         error = ENOENT;
2938                         goto bad;
2939                 }
2940
2941                 /* validity check */
2942                 if (isr->sav == NULL) {
2943                         switch (ipsec_get_reqlevel(isr)) {
2944                         case IPSEC_LEVEL_USE:
2945                                 continue;
2946                         case IPSEC_LEVEL_REQUIRE:
2947                                 /* must be not reached here. */
2948                                 panic("ipsec6_output_tunnel: no SA found, but required.");
2949                         }
2950                 }
2951
2952                 /*
2953                  * If there is no valid SA, we give up to process.
2954                  * see same place at ipsec4_output().
2955                  */
2956                 if (isr->sav->state != SADB_SASTATE_MATURE
2957                  && isr->sav->state != SADB_SASTATE_DYING) {
2958                         ipsec6stat.out_nosa++;
2959                         error = EINVAL;
2960                         goto bad;
2961                 }
2962
2963                 /*
2964                  * There may be the case that SA status will be changed when
2965                  * we are refering to one. So calling crit_enter().
2966                  */
2967                 crit_enter();
2968
2969                 if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
2970                         /*
2971                          * build IPsec tunnel.
2972                          */
2973                         /* XXX should be processed with other familiy */
2974                         if (((struct sockaddr *)&isr->sav->sah->saidx.src)->sa_family != AF_INET6) {
2975                                 ipseclog((LOG_ERR, "ipsec6_output_tunnel: "
2976                                     "family mismatched between inner and outer, spi=%u\n",
2977                                     (u_int32_t)ntohl(isr->sav->spi)));
2978                                 crit_exit();
2979                                 ipsec6stat.out_inval++;
2980                                 error = EAFNOSUPPORT;
2981                                 goto bad;
2982                         }
2983
2984                         state->m = ipsec6_splithdr(state->m);
2985                         if (!state->m) {
2986                                 crit_exit();
2987                                 ipsec6stat.out_nomem++;
2988                                 error = ENOMEM;
2989                                 goto bad;
2990                         }
2991                         error = ipsec6_encapsulate(state->m, isr->sav);
2992                         crit_exit();
2993                         if (error) {
2994                                 state->m = 0;
2995                                 goto bad;
2996                         }
2997                         ip6 = mtod(state->m, struct ip6_hdr *);
2998
2999                         state->ro = &isr->sav->sah->sa_route;
3000                         state->dst = (struct sockaddr *)&state->ro->ro_dst;
3001                         dst6 = (struct sockaddr_in6 *)state->dst;
3002                         if (state->ro->ro_rt
3003                          && ((state->ro->ro_rt->rt_flags & RTF_UP) == 0
3004                           || !IN6_ARE_ADDR_EQUAL(&dst6->sin6_addr, &ip6->ip6_dst))) {
3005                                 RTFREE(state->ro->ro_rt);
3006                                 state->ro->ro_rt = NULL;
3007                         }
3008                         if (state->ro->ro_rt == 0) {
3009                                 bzero(dst6, sizeof(*dst6));
3010                                 dst6->sin6_family = AF_INET6;
3011                                 dst6->sin6_len = sizeof(*dst6);
3012                                 dst6->sin6_addr = ip6->ip6_dst;
3013                                 rtalloc(state->ro);
3014                         }
3015                         if (state->ro->ro_rt == 0) {
3016                                 ip6stat.ip6s_noroute++;
3017                                 ipsec6stat.out_noroute++;
3018                                 error = EHOSTUNREACH;
3019                                 goto bad;
3020                         }
3021
3022                         /* adjust state->dst if tunnel endpoint is offlink */
3023                         if (state->ro->ro_rt->rt_flags & RTF_GATEWAY) {
3024                                 state->dst = (struct sockaddr *)state->ro->ro_rt->rt_gateway;
3025                                 dst6 = (struct sockaddr_in6 *)state->dst;
3026                         }
3027                 } else
3028                         crit_exit();
3029
3030                 state->m = ipsec6_splithdr(state->m);
3031                 if (!state->m) {
3032                         ipsec6stat.out_nomem++;
3033                         error = ENOMEM;
3034                         goto bad;
3035                 }
3036                 ip6 = mtod(state->m, struct ip6_hdr *);
3037                 switch (isr->saidx.proto) {
3038                 case IPPROTO_ESP:
3039 #ifdef IPSEC_ESP
3040                         error = esp6_output(state->m, &ip6->ip6_nxt, state->m->m_next, isr);
3041 #else
3042                         m_freem(state->m);
3043                         error = EINVAL;
3044 #endif
3045                         break;
3046                 case IPPROTO_AH:
3047                         error = ah6_output(state->m, &ip6->ip6_nxt, state->m->m_next, isr);
3048                         break;
3049                 case IPPROTO_IPCOMP:
3050                         /* XXX code should be here */
3051                         /* FALLTHROUGH */
3052                 default:
3053                         ipseclog((LOG_ERR, "ipsec6_output_tunnel: "
3054                             "unknown ipsec protocol %d\n", isr->saidx.proto));
3055                         m_freem(state->m);
3056                         ipsec6stat.out_inval++;
3057                         error = EINVAL;
3058                         break;
3059                 }
3060                 if (error) {
3061                         state->m = NULL;
3062                         goto bad;
3063                 }
3064                 plen = state->m->m_pkthdr.len - sizeof(struct ip6_hdr);
3065                 if (plen > IPV6_MAXPACKET) {
3066                         ipseclog((LOG_ERR, "ipsec6_output_tunnel: "
3067                             "IPsec with IPv6 jumbogram is not supported\n"));
3068                         ipsec6stat.out_inval++;
3069                         error = EINVAL; /* XXX */
3070                         goto bad;
3071                 }
3072                 ip6 = mtod(state->m, struct ip6_hdr *);
3073                 ip6->ip6_plen = htons(plen);
3074         }
3075         lwkt_reltoken(&key_token);
3076
3077         return 0;
3078
3079 bad:
3080         lwkt_reltoken(&key_token);
3081         m_freem(state->m);
3082         state->m = NULL;
3083         return error;
3084 }
3085 #endif /* INET6 */
3086
3087 #ifdef INET
3088 /*
3089  * Chop IP header and option off from the payload.
3090  */
3091 static struct mbuf *
3092 ipsec4_splithdr(struct mbuf *m)
3093 {
3094         struct mbuf *mh;
3095         struct ip *ip;
3096         int hlen;
3097
3098         if (m->m_len < sizeof(struct ip))
3099                 panic("ipsec4_splithdr: first mbuf too short");
3100         ip = mtod(m, struct ip *);
3101 #ifdef _IP_VHL
3102         hlen = _IP_VHL_HL(ip->ip_vhl) << 2;
3103 #else
3104         hlen = ip->ip_hl << 2;
3105 #endif
3106         if (m->m_len > hlen) {
3107                 MGETHDR(mh, MB_DONTWAIT, MT_HEADER);
3108                 if (!mh) {
3109                         m_freem(m);
3110                         return NULL;
3111                 }
3112                 M_MOVE_PKTHDR(mh, m);
3113                 MH_ALIGN(mh, hlen);
3114                 m->m_len -= hlen;
3115                 m->m_data += hlen;
3116                 mh->m_next = m;
3117                 m = mh;
3118                 m->m_len = hlen;
3119                 bcopy((caddr_t)ip, mtod(m, caddr_t), hlen);
3120         } else if (m->m_len < hlen) {
3121                 m = m_pullup(m, hlen);
3122                 if (!m)
3123                         return NULL;
3124         }
3125         return m;
3126 }
3127 #endif
3128
3129 #ifdef INET6
3130 static struct mbuf *
3131 ipsec6_splithdr(struct mbuf *m)
3132 {
3133         struct mbuf *mh;
3134         struct ip6_hdr *ip6;
3135         int hlen;
3136
3137         if (m->m_len < sizeof(struct ip6_hdr))
3138                 panic("ipsec6_splithdr: first mbuf too short");
3139         ip6 = mtod(m, struct ip6_hdr *);
3140         hlen = sizeof(struct ip6_hdr);
3141         if (m->m_len > hlen) {
3142                 MGETHDR(mh, MB_DONTWAIT, MT_HEADER);
3143                 if (!mh) {
3144                         m_freem(m);
3145                         return NULL;
3146                 }
3147                 M_MOVE_PKTHDR(mh, m);
3148                 MH_ALIGN(mh, hlen);
3149                 m->m_len -= hlen;
3150                 m->m_data += hlen;
3151                 mh->m_next = m;
3152                 m = mh;
3153                 m->m_len = hlen;
3154                 bcopy((caddr_t)ip6, mtod(m, caddr_t), hlen);
3155         } else if (m->m_len < hlen) {
3156                 m = m_pullup(m, hlen);
3157                 if (!m)
3158                         return NULL;
3159         }
3160         return m;
3161 }
3162 #endif
3163
3164 /* validate inbound IPsec tunnel packet. */
3165 int
3166 ipsec4_tunnel_validate(struct mbuf *m, /* no pullup permitted, m->m_len >= ip */
3167                        int off, u_int nxt0, struct secasvar *sav)
3168 {
3169         u_int8_t nxt = nxt0 & 0xff;
3170         struct sockaddr_in *sin;
3171         struct sockaddr_in osrc, odst, isrc, idst;
3172         int hlen;
3173         struct secpolicy *sp;
3174         struct ip *oip;
3175
3176 #ifdef DIAGNOSTIC
3177         if (m->m_len < sizeof(struct ip))
3178                 panic("too short mbuf on ipsec4_tunnel_validate");
3179 #endif
3180         if (nxt != IPPROTO_IPV4)
3181                 return 0;
3182         if (m->m_pkthdr.len < off + sizeof(struct ip))
3183                 return 0;
3184         /* do not decapsulate if the SA is for transport mode only */
3185         if (sav->sah->saidx.mode == IPSEC_MODE_TRANSPORT)
3186                 return 0;
3187
3188         oip = mtod(m, struct ip *);
3189 #ifdef _IP_VHL
3190         hlen = _IP_VHL_HL(oip->ip_vhl) << 2;
3191 #else
3192         hlen = oip->ip_hl << 2;
3193 #endif
3194         if (hlen != sizeof(struct ip))
3195                 return 0;
3196
3197         /* AF_INET6 should be supported, but at this moment we don't. */
3198         sin = (struct sockaddr_in *)&sav->sah->saidx.dst;
3199         if (sin->sin_family != AF_INET)
3200                 return 0;
3201         if (bcmp(&oip->ip_dst, &sin->sin_addr, sizeof(oip->ip_dst)) != 0)
3202                 return 0;
3203
3204         /* XXX slow */
3205         bzero(&osrc, sizeof(osrc));
3206         bzero(&odst, sizeof(odst));
3207         bzero(&isrc, sizeof(isrc));
3208         bzero(&idst, sizeof(idst));
3209         osrc.sin_family = odst.sin_family = isrc.sin_family = idst.sin_family =
3210             AF_INET;
3211         osrc.sin_len = odst.sin_len = isrc.sin_len = idst.sin_len =
3212             sizeof(struct sockaddr_in);
3213         osrc.sin_addr = oip->ip_src;
3214         odst.sin_addr = oip->ip_dst;
3215         m_copydata(m, off + offsetof(struct ip, ip_src), sizeof(isrc.sin_addr),
3216             (caddr_t)&isrc.sin_addr);
3217         m_copydata(m, off + offsetof(struct ip, ip_dst), sizeof(idst.sin_addr),
3218             (caddr_t)&idst.sin_addr);
3219
3220         /*
3221          * RFC2401 5.2.1 (b): (assume that we are using tunnel mode)
3222          * - if the inner destination is multicast address, there can be
3223          *   multiple permissible inner source address.  implementation
3224          *   may want to skip verification of inner source address against
3225          *   SPD selector.
3226          * - if the inner protocol is ICMP, the packet may be an error report
3227          *   from routers on the other side of the VPN cloud (R in the
3228          *   following diagram).  in this case, we cannot verify inner source
3229          *   address against SPD selector.
3230          *      me -- gw === gw -- R -- you
3231          *
3232          * we consider the first bullet to be users responsibility on SPD entry
3233          * configuration (if you need to encrypt multicast traffic, set
3234          * the source range of SPD selector to 0.0.0.0/0, or have explicit
3235          * address ranges for possible senders).
3236          * the second bullet is not taken care of (yet).
3237          *
3238          * therefore, we do not do anything special about inner source.
3239          */
3240
3241         lwkt_gettoken(&key_token);
3242         sp = key_gettunnel((struct sockaddr *)&osrc, (struct sockaddr *)&odst,
3243                            (struct sockaddr *)&isrc, (struct sockaddr *)&idst);
3244         if (sp) {
3245                 key_freesp(sp);
3246                 lwkt_reltoken(&key_token);
3247                 return 1;
3248         } else{
3249                 lwkt_reltoken(&key_token);
3250                 return 0;
3251         }
3252 }
3253
3254 #ifdef INET6
3255 /* validate inbound IPsec tunnel packet. */
3256 int
3257 ipsec6_tunnel_validate(struct mbuf *m, /* no pullup permitted, m->m_len >= ip */
3258                        int off, u_int nxt0, struct secasvar *sav)
3259 {
3260         u_int8_t nxt = nxt0 & 0xff;
3261         struct sockaddr_in6 *sin6;
3262         struct sockaddr_in6 osrc, odst, isrc, idst;
3263         struct secpolicy *sp;
3264         struct ip6_hdr *oip6;
3265
3266 #ifdef DIAGNOSTIC
3267         if (m->m_len < sizeof(struct ip6_hdr))
3268                 panic("too short mbuf on ipsec6_tunnel_validate");
3269 #endif
3270         if (nxt != IPPROTO_IPV6)
3271                 return 0;
3272         if (m->m_pkthdr.len < off + sizeof(struct ip6_hdr))
3273                 return 0;
3274         /* do not decapsulate if the SA is for transport mode only */
3275         if (sav->sah->saidx.mode == IPSEC_MODE_TRANSPORT)
3276                 return 0;
3277
3278         oip6 = mtod(m, struct ip6_hdr *);
3279         /* AF_INET should be supported, but at this moment we don't. */
3280         sin6 = (struct sockaddr_in6 *)&sav->sah->saidx.dst;
3281         if (sin6->sin6_family != AF_INET6)
3282                 return 0;
3283         if (!IN6_ARE_ADDR_EQUAL(&oip6->ip6_dst, &sin6->sin6_addr))
3284                 return 0;
3285
3286         /* XXX slow */
3287         bzero(&osrc, sizeof(osrc));
3288         bzero(&odst, sizeof(odst));
3289         bzero(&isrc, sizeof(isrc));
3290         bzero(&idst, sizeof(idst));
3291         osrc.sin6_family = odst.sin6_family = isrc.sin6_family =
3292             idst.sin6_family = AF_INET6;
3293         osrc.sin6_len = odst.sin6_len = isrc.sin6_len = idst.sin6_len =
3294             sizeof(struct sockaddr_in6);
3295         osrc.sin6_addr = oip6->ip6_src;
3296         odst.sin6_addr = oip6->ip6_dst;
3297         m_copydata(m, off + offsetof(struct ip6_hdr, ip6_src),
3298             sizeof(isrc.sin6_addr), (caddr_t)&isrc.sin6_addr);
3299         m_copydata(m, off + offsetof(struct ip6_hdr, ip6_dst),
3300             sizeof(idst.sin6_addr), (caddr_t)&idst.sin6_addr);
3301
3302         /*
3303          * regarding to inner source address validation, see a long comment
3304          * in ipsec4_tunnel_validate.
3305          */
3306
3307         lwkt_gettoken(&key_token);
3308         sp = key_gettunnel((struct sockaddr *)&osrc, (struct sockaddr *)&odst,
3309                            (struct sockaddr *)&isrc, (struct sockaddr *)&idst);
3310         /*
3311          * when there is no suitable inbound policy for the packet of the ipsec
3312          * tunnel mode, the kernel never decapsulate the tunneled packet
3313          * as the ipsec tunnel mode even when the system wide policy is "none".
3314          * then the kernel leaves the generic tunnel module to process this
3315          * packet.  if there is no rule of the generic tunnel, the packet
3316          * is rejected and the statistics will be counted up.
3317          */
3318         if (sp) {
3319                 key_freesp(sp);
3320                 lwkt_reltoken(&key_token);
3321                 return 1;
3322         } else {
3323                 lwkt_reltoken(&key_token);
3324                 return 0;
3325         }
3326 }
3327 #endif
3328
3329 /*
3330  * Make a mbuf chain for encryption.
3331  * If the original mbuf chain contains a mbuf with a cluster,
3332  * allocate a new cluster and copy the data to the new cluster.
3333  * XXX: this hack is inefficient, but is necessary to handle cases
3334  * of TCP retransmission...
3335  */
3336 struct mbuf *
3337 ipsec_copypkt(struct mbuf *m)
3338 {
3339         struct mbuf *n, **mpp, *mnew;
3340
3341         for (n = m, mpp = &m; n; n = n->m_next) {
3342                 if (n->m_flags & M_EXT) {
3343                         /*
3344                          * Make a copy only if there are more than one
3345                          * references to the cluster.
3346                          * XXX: is this approach effective?
3347                          */
3348                         if (m_sharecount(n) > 1) {
3349                                 int remain, copied;
3350                                 struct mbuf *mm;
3351
3352                                 if (n->m_flags & M_PKTHDR) {
3353                                         MGETHDR(mnew, MB_DONTWAIT, MT_HEADER);
3354                                         if (mnew == NULL)
3355                                                 goto fail;
3356                                         if (!m_dup_pkthdr(mnew, n, MB_DONTWAIT)) {
3357                                                 m_free(mnew);
3358                                                 goto fail;
3359                                         }
3360                                 }
3361                                 else {
3362                                         MGET(mnew, MB_DONTWAIT, MT_DATA);
3363                                         if (mnew == NULL)
3364                                                 goto fail;
3365                                 }
3366                                 mnew->m_len = 0;
3367                                 mm = mnew;
3368
3369                                 /*
3370                                  * Copy data. If we don't have enough space to
3371                                  * store the whole data, allocate a cluster
3372                                  * or additional mbufs.
3373                                  * XXX: we don't use m_copyback(), since the
3374                                  * function does not use clusters and thus is
3375                                  * inefficient.
3376                                  */
3377                                 remain = n->m_len;
3378                                 copied = 0;
3379                                 while (1) {
3380                                         int len;
3381                                         struct mbuf *mn;
3382
3383                                         if (remain <= (mm->m_flags & M_PKTHDR ? MHLEN : MLEN))
3384                                                 len = remain;
3385                                         else { /* allocate a cluster */
3386                                                 MCLGET(mm, MB_DONTWAIT);
3387                                                 if (!(mm->m_flags & M_EXT)) {
3388                                                         m_free(mm);
3389                                                         goto fail;
3390                                                 }
3391                                                 len = remain < MCLBYTES ?
3392                                                         remain : MCLBYTES;
3393                                         }
3394
3395                                         bcopy(n->m_data + copied, mm->m_data,
3396                                               len);
3397
3398                                         copied += len;
3399                                         remain -= len;
3400                                         mm->m_len = len;
3401
3402                                         if (remain <= 0) /* completed? */
3403                                                 break;
3404
3405                                         /* need another mbuf */
3406                                         MGETHDR(mn, MB_DONTWAIT, MT_HEADER);
3407                                         if (mn == NULL)
3408                                                 goto fail;
3409                                         mn->m_pkthdr.rcvif = NULL;
3410                                         mm->m_next = mn;
3411                                         mm = mn;
3412                                 }
3413
3414                                 /* adjust chain */
3415                                 mm->m_next = m_free(n);
3416                                 n = mm;
3417                                 *mpp = mnew;
3418                                 mpp = &n->m_next;
3419
3420                                 continue;
3421                         }
3422                 }
3423                 *mpp = n;
3424                 mpp = &n->m_next;
3425         }
3426
3427         return (m);
3428 fail:
3429         m_freem(m);
3430         return (NULL);
3431 }
3432
3433 void
3434 ipsec_delaux(struct mbuf *m)
3435 {
3436         struct m_tag *tag;
3437
3438         while ((tag = m_tag_find(m, PACKET_TAG_IPSEC_HISTORY, NULL)) != NULL)
3439                 m_tag_delete(m, tag);
3440 }
3441
3442 int
3443 ipsec_addhist(struct mbuf *m, int proto, u_int32_t spi)
3444 {
3445         struct m_tag *tag;
3446         struct ipsec_history *p;
3447
3448         tag = m_tag_get(PACKET_TAG_IPSEC_HISTORY,
3449                         sizeof (struct ipsec_history), MB_DONTWAIT);
3450         if (tag == NULL)
3451                 return ENOBUFS;
3452         p = (struct ipsec_history *)m_tag_data(tag);
3453         bzero(p, sizeof(*p));
3454         p->ih_proto = proto;
3455         p->ih_spi = spi;
3456         m_tag_prepend(m, tag);
3457         return 0;
3458 }
3459
3460 struct ipsec_history *
3461 ipsec_gethist(struct mbuf *m, int *lenp)
3462 {
3463         struct m_tag *tag;
3464
3465         tag = m_tag_find(m, PACKET_TAG_IPSEC_HISTORY, NULL);
3466         if (tag == NULL)
3467                 return NULL;
3468         /* XXX NB: noone uses this so fake it */
3469         if (lenp)
3470                 *lenp = sizeof (struct ipsec_history);
3471         return ((struct ipsec_history *)(tag+1));
3472 }