pfkey_v2_parser.c

This a good VPN source

				    "EMT_GETSPI found an old ipsec_sa for SA: %s, delete it first.\n",
				    sa_len ? sa : " (error)");
			SENDERR(EEXIST);
		} else {
			found_avail = 1;
		}
	} else {
		int i = 0;
		__u32 rand_val;
		__u32 spi_diff;
		while( ( i < (spi_diff = (ntohl(maxspi) - ntohl(minspi)))) && !found_avail ) {
			prng_bytes(&ipsec_prng, (char *) &(rand_val),
					 ( (spi_diff < (2^8))  ? 1 :
					   ( (spi_diff < (2^16)) ? 2 :
					     ( (spi_diff < (2^24)) ? 3 :
					   4 ) ) ) );
			extr->ips->ips_said.spi = htonl(ntohl(minspi) +
					      (rand_val %
					      (spi_diff + 1)));
			i++;
			ipsq = ipsec_sa_getbyid(&(extr->ips->ips_said));
			if(ipsq == NULL) {
				found_avail = 1;
			} else {
				ipsec_sa_put(ipsq);
			}
		}
	}

	sa_len = satot(&extr->ips->ips_said, 0, sa, sizeof(sa));

	if (!found_avail) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_getspi_parse: "
			    "found an old ipsec_sa for SA: %s, delete it first.\n",
			    sa_len ? sa : " (error)");
		SENDERR(EEXIST);
	}

	if(ip_chk_addr((unsigned long)extr->ips->ips_said.dst.u.v4.sin_addr.s_addr) == IS_MYADDR) {
		extr->ips->ips_flags |= EMT_INBOUND;
	}
	
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_getspi_parse: "
		    "existing ipsec_sa not found (this is good) for SA: %s, %s-bound, allocating.\n",
		    sa_len ? sa : " (error)",
		    extr->ips->ips_flags & EMT_INBOUND ? "in" : "out");
	
	/* XXX extr->ips->ips_rcvif = &(enc_softc[em->em_if].enc_if);*/
	extr->ips->ips_rcvif = NULL;
	extr->ips->ips_life.ipl_addtime.ipl_count = jiffies/HZ;

	extr->ips->ips_state = SADB_SASTATE_LARVAL;

	if(!extr->ips->ips_life.ipl_allocations.ipl_count) {
		extr->ips->ips_life.ipl_allocations.ipl_count += 1;
	}

	if(!(pfkey_safe_build(error = pfkey_msg_hdr_build(&extensions_reply[0],
							  SADB_GETSPI,
							  satype,
							  0,
							  ((struct sadb_msg*)extensions[SADB_EXT_RESERVED])->sadb_msg_seq,
							  ((struct sadb_msg*)extensions[SADB_EXT_RESERVED])->sadb_msg_pid),
			      extensions_reply)
	     && pfkey_safe_build(error = pfkey_sa_ref_build(&extensions_reply[SADB_EXT_SA],
							SADB_EXT_SA,
							extr->ips->ips_said.spi,
							0,
							SADB_SASTATE_LARVAL,
							0,
							0,
							0,
							extr->ips->ips_ref),
				 extensions_reply)
	     && pfkey_safe_build(error = pfkey_address_build(&extensions_reply[SADB_EXT_ADDRESS_SRC],
						     SADB_EXT_ADDRESS_SRC,
						     0, /*extr->ips->ips_said.proto,*/
						     0,
						     extr->ips->ips_addr_s),
				 extensions_reply)
	     && pfkey_safe_build(error = pfkey_address_build(&extensions_reply[SADB_EXT_ADDRESS_DST],
						     SADB_EXT_ADDRESS_DST,
						     0, /*extr->ips->ips_said.proto,*/
						     0,
						     extr->ips->ips_addr_d),
				 extensions_reply) )) {
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_getspi_parse: "
			    "failed to build the getspi reply message extensions\n");
		goto errlab;
	}
	
	if((error = pfkey_msg_build(&pfkey_reply, extensions_reply, EXT_BITS_OUT))) {
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_getspi_parse: "
			    "failed to build the getspi reply message\n");
		SENDERR(-error);
	}
	for(pfkey_socketsp = pfkey_open_sockets;
	    pfkey_socketsp;
	    pfkey_socketsp = pfkey_socketsp->next) {
		if((error = pfkey_upmsg(pfkey_socketsp->socketp, pfkey_reply))) {
			KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_getspi_parse: "
				    "sending up getspi reply message for satype=%d(%s) to socket=0p%p failed with error=%d.\n",
				    satype,
				    satype2name(satype),
				    pfkey_socketsp->socketp,
				    error);
			SENDERR(-error);
		}
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_getspi_parse: "
			    "sending up getspi reply message for satype=%d(%s) to socket=0p%p succeeded.\n",
			    satype,
			    satype2name(satype),
			    pfkey_socketsp->socketp);
	}
	
	if((error = ipsec_sa_add(extr->ips))) {
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_getspi_parse: "
			    "failed to add the larval SA=%s with error=%d.\n",
			    sa_len ? sa : " (error)",
			    error);
		SENDERR(-error);
	}
	extr->ips = NULL;
	
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_getspi_parse: "
		    "successful for SA: %s\n",
		    sa_len ? sa : " (error)");
	
 errlab:
	if (pfkey_reply) {
		pfkey_msg_free(&pfkey_reply);
	}
	pfkey_extensions_free(extensions_reply);
	return error;
}

DEBUG_NO_STATIC int
pfkey_update_parse(struct sock *sk, struct sadb_ext **extensions, struct pfkey_extracted_data* extr)
{
	int error = 0;
	struct ipsec_sa* ipsq;
	char sa[SATOT_BUF];
	size_t sa_len;
	struct sadb_ext *extensions_reply[SADB_EXT_MAX+1];
	struct sadb_msg *pfkey_reply = NULL;
	struct socket_list *pfkey_socketsp;
	uint8_t satype = ((struct sadb_msg*)extensions[SADB_EXT_RESERVED])->sadb_msg_satype;
#ifdef CONFIG_KLIPS_NAT_TRAVERSAL
	struct ipsec_sa *nat_t_ips_saved = NULL;
#endif
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_update_parse: .\n");

	pfkey_extensions_init(extensions_reply);

	if(((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_state != SADB_SASTATE_MATURE) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_update_parse: "
			    "error, sa_state=%d must be MATURE=%d\n",
			    ((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_state,
			    SADB_SASTATE_MATURE);
		SENDERR(EINVAL);
	}

	if(extr == NULL || extr->ips == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_update_parse: "
			    "error, extr or extr->ips pointer NULL\n");
		SENDERR(EINVAL);
	}

	sa_len = satot(&extr->ips->ips_said, 0, sa, sizeof(sa));

	spin_lock_bh(&tdb_lock);

	ipsq = ipsec_sa_getbyid(&(extr->ips->ips_said));
	if (ipsq == NULL) {
		spin_unlock_bh(&tdb_lock);
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_update_parse: "
			    "reserved ipsec_sa for SA: %s not found.  Call SADB_GETSPI first or call SADB_ADD instead.\n",
			    sa_len ? sa : " (error)");
		SENDERR(ENOENT);
	}

	if(ip_chk_addr((unsigned long)extr->ips->ips_said.dst.u.v4.sin_addr.s_addr) == IS_MYADDR) {
		extr->ips->ips_flags |= EMT_INBOUND;
	}

	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_update_parse: "
		    "existing ipsec_sa found (this is good) for SA: %s, %s-bound, updating.\n",
		    sa_len ? sa : " (error)",
		    extr->ips->ips_flags & EMT_INBOUND ? "in" : "out");

#ifdef CONFIG_KLIPS_NAT_TRAVERSAL
	if (extr->ips->ips_natt_sport || extr->ips->ips_natt_dport) {
		KLIPS_PRINT(debug_pfkey,
			"klips_debug:pfkey_update_parse: only updating NAT-T ports "
			"(%u:%u -> %u:%u)\n", 
			ipsq->ips_natt_sport, ipsq->ips_natt_dport,
			extr->ips->ips_natt_sport, extr->ips->ips_natt_dport);

		if (extr->ips->ips_natt_sport) {
			ipsq->ips_natt_sport = extr->ips->ips_natt_sport;
			if (ipsq->ips_addr_s->sa_family == AF_INET) {
				((struct sockaddr_in *)(ipsq->ips_addr_s))->sin_port = htons(extr->ips->ips_natt_sport);
			}
		}

		if (extr->ips->ips_natt_dport) {
			ipsq->ips_natt_dport = extr->ips->ips_natt_dport;
			if (ipsq->ips_addr_d->sa_family == AF_INET) {
				((struct sockaddr_in *)(ipsq->ips_addr_d))->sin_port = htons(extr->ips->ips_natt_dport);
			}
		}

		nat_t_ips_saved = extr->ips;
		extr->ips = ipsq;
	}
	else {
#endif
	
	/* XXX extr->ips->ips_rcvif = &(enc_softc[em->em_if].enc_if);*/
	extr->ips->ips_rcvif = NULL;
	if ((error = pfkey_ipsec_sa_init(extr->ips, extensions))) {
		ipsec_sa_put(ipsq);
		spin_unlock_bh(&tdb_lock);
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_update_parse: "
			    "not successful for SA: %s, deleting.\n",
			    sa_len ? sa : " (error)");
		SENDERR(-error);
	}

	extr->ips->ips_life.ipl_addtime.ipl_count = ipsq->ips_life.ipl_addtime.ipl_count;
	ipsec_sa_put(ipsq);
	if((error = ipsec_sa_delchain(ipsq))) {
		spin_unlock_bh(&tdb_lock);
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_update_parse: "
			    "error=%d, trouble deleting intermediate ipsec_sa for SA=%s.\n",
			    error,
			    sa_len ? sa : " (error)");
		SENDERR(-error);
	}
#ifdef CONFIG_KLIPS_NAT_TRAVERSAL
	}
#endif

	spin_unlock_bh(&tdb_lock);
	
	if(!(pfkey_safe_build(error = pfkey_msg_hdr_build(&extensions_reply[0],
							  SADB_UPDATE,
							  satype,
							  0,
							  ((struct sadb_msg*)extensions[SADB_EXT_RESERVED])->sadb_msg_seq,
							  ((struct sadb_msg*)extensions[SADB_EXT_RESERVED])->sadb_msg_pid),
			      extensions_reply)
	     && pfkey_safe_build(error = pfkey_sa_ref_build(&extensions_reply[SADB_EXT_SA],
							SADB_EXT_SA,
							extr->ips->ips_said.spi,
							extr->ips->ips_replaywin,
							extr->ips->ips_state,
							extr->ips->ips_authalg,
							extr->ips->ips_encalg,
							extr->ips->ips_flags,
							extr->ips->ips_ref),
				 extensions_reply)
	     /* The 3 lifetime extentions should only be sent if non-zero. */
	     && (extensions[SADB_EXT_LIFETIME_HARD]
		 ? pfkey_safe_build(error = pfkey_lifetime_build(&extensions_reply[SADB_EXT_LIFETIME_HARD],
								 SADB_EXT_LIFETIME_HARD,
								 extr->ips->ips_life.ipl_allocations.ipl_hard,
								 extr->ips->ips_life.ipl_bytes.ipl_hard,
								 extr->ips->ips_life.ipl_addtime.ipl_hard,
								 extr->ips->ips_life.ipl_usetime.ipl_hard,
								 extr->ips->ips_life.ipl_packets.ipl_hard),
				    extensions_reply) : 1)
	     && (extensions[SADB_EXT_LIFETIME_SOFT]
		 ? pfkey_safe_build(error = pfkey_lifetime_build(&extensions_reply[SADB_EXT_LIFETIME_SOFT],
								 SADB_EXT_LIFETIME_SOFT,
								 extr->ips->ips_life.ipl_allocations.ipl_count,
								 extr->ips->ips_life.ipl_bytes.ipl_count,
								 extr->ips->ips_life.ipl_addtime.ipl_count,
								 extr->ips->ips_life.ipl_usetime.ipl_count,
								 extr->ips->ips_life.ipl_packets.ipl_count),
				    extensions_reply) : 1)
	     && (extr->ips->ips_life.ipl_allocations.ipl_count
		 || extr->ips->ips_life.ipl_bytes.ipl_count
		 || extr->ips->ips_life.ipl_addtime.ipl_count
		 || extr->ips->ips_life.ipl_usetime.ipl_count
		 || extr->ips->ips_life.ipl_packets.ipl_count

		 ? pfkey_safe_build(error = pfkey_lifetime_build(&extensions_reply[SADB_EXT_LIFETIME_CURRENT],
								 SADB_EXT_LIFETIME_CURRENT,
								 extr->ips->ips_life.ipl_allocations.ipl_count,
								 extr->ips->ips_life.ipl_bytes.ipl_count,
								 extr->ips->ips_life.ipl_addtime.ipl_count,
								 extr->ips->ips_life.ipl_usetime.ipl_count,
								 extr->ips->ips_life.ipl_packets.ipl_count),
				    extensions_reply) : 1)
	     && pfkey_safe_build(error = pfkey_address_build(&extensions_reply[SADB_EXT_ADDRESS_SRC],
							     SADB_EXT_ADDRESS_SRC,
							     0, /*extr->ips->ips_said.proto,*/
							     0,
							     extr->ips->ips_addr_s),
				 extensions_reply)
	     && pfkey_safe_build(error = pfkey_address_build(&extensions_reply[SADB_EXT_ADDRESS_DST],
							     SADB_EXT_ADDRESS_DST,
							     0, /*extr->ips->ips_said.proto,*/
							     0,
							     extr->ips->ips_addr_d),
				 extensions_reply)
	     && (extr->ips->ips_ident_s.data
                 ? pfkey_safe_build(error = pfkey_ident_build(&extensions_reply[SADB_EXT_IDENTITY_SRC],
                                                              SADB_EXT_IDENTITY_SRC,
							      extr->ips->ips_ident_s.type,
							      extr->ips->ips_ident_s.id,
                                                              extr->ips->ips_ident_s.len,
							      extr->ips->ips_ident_s.data),
                                    extensions_reply) : 1)
	     && (extr->ips->ips_ident_d.data
                 ? pfkey_safe_build(error = pfkey_ident_build(&extensions_reply[SADB_EXT_IDENTITY_DST],
                                                              SADB_EXT_IDENTITY_DST,
							      extr->ips->ips_ident_d.type,
							      extr->ips->ips_ident_d.id,
                                                              extr->ips->ips_ident_d.len,
							      extr->ips->ips_ident_d.data),
                                    extensions_reply) : 1)
#if 0
	     /* FIXME: This won't work yet because I have not finished
		it. */
	     && (extr->ips->ips_sens_
		 ? pfkey_safe_build(error = pfkey_sens_build(&extensions_reply[SADB_EXT_SENSITIVITY],
							     extr->ips->ips_sens_dpd,
							     extr->ips->ips_sens_sens_level,
							     extr->ips->ips_sens_sens_len,
							     extr->ips->ips_sens_sens_bitmap,
							     extr->ips->ips_sens_integ_level,
							     extr->ips->ips_sens_integ_len,
							     extr->ips->ips_sens_integ_bitmap),
				    extensions_reply) : 1)
#endif
		)) {
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_update_parse: "
			    "failed to build the update reply message extensions\n");
		SENDERR(-error);
	}
		
	if((error = pfkey_msg_build(&pfkey_reply, extensions_reply, EXT_BITS_OUT))) {
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_update_parse: "
			    "failed to build the update reply message\n");
		SENDERR(-error);
	}
	for(pfkey_socketsp = pfkey_open_sockets;
	    pfkey_socketsp;
	    pfkey_socketsp = pfkey_socketsp->next) {
		if((error = pfkey_upmsg(pfkey_socketsp->socketp, pfkey_reply))) {
			KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_update_parse: "
				    "sending up update reply message for satype=%d(%s) to socket=0p%p failed with error=%d.\n",
				    satype,
				    satype2name(satype),
				    pfkey_socketsp->socketp,
				    error);
			SENDERR(-error);
		}
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_update_parse: "
			    "sending up update reply message for satype=%d(%s) to socket=0p%p succeeded.\n",
			    satype,
			    satype2name(satype),
			    pfkey_socketsp->socketp);
	}

#ifdef CONFIG_KLIPS_NAT_TRAVERSAL
	if (nat_t_ips_saved) {
		/**
		 * As we _really_ update existing SA, we keep tdbq and need to delete
		 * parsed ips (nat_t_ips_saved, was extr->ips).
		 *
		 * goto errlab with extr->ips = nat_t_ips_saved will free it.
		 */

		extr->ips = nat_t_ips_saved;

		error = 0;
		KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_update_parse (NAT-T ports): "
		    "successful for SA: %s\n",
		    sa_len ? sa : " (error)");

		goto errlab;
	}
#endif