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