pfkey_v2_parser.c

This a good VPN source

			kfree(akp);
		}
		break;
# endif /* CONFIG_KLIPS_AUTH_HMAC_SHA1 */
		default:
			KLIPS_PRINT(debug_pfkey,
				    "klips_debug:pfkey_ipsec_sa_init: "
				    "authalg=%d support not available in the kernel",
				    ipsp->ips_authalg);
			SENDERR(EINVAL);
		}
	break;
#endif /* CONFIG_KLIPS_AH */
#ifdef CONFIG_KLIPS_ESP
	case IPPROTO_ESP: {
#if defined (CONFIG_KLIPS_AUTH_HMAC_MD5) || defined (CONFIG_KLIPS_AUTH_HMAC_SHA1)
		unsigned char *akp;
		unsigned int aks;
#endif
#if defined (CONFIG_KLIPS_ENC_3DES)
		unsigned char *ekp;
		unsigned int eks;
#endif

		ipsp->ips_iv_size = 0;
#ifdef CONFIG_KLIPS_ALG
		if ((ixt_e=ipsp->ips_alg_enc)) {
			ipsp->ips_iv_size = ixt_e->ixt_ivlen/8;
		} else	
#endif /* CONFIG_KLIPS_ALG */
		switch(ipsp->ips_encalg) {
# ifdef CONFIG_KLIPS_ENC_3DES
		case ESP_3DES:
# endif /* CONFIG_KLIPS_ENC_3DES */
# if defined(CONFIG_KLIPS_ENC_3DES)
			KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
			            "klips_debug:pfkey_ipsec_sa_init: "
			            "allocating %u bytes for iv.\n",
			            EMT_ESPDES_IV_SZ);
			if((ipsp->ips_iv = (caddr_t)
			    kmalloc((ipsp->ips_iv_size = EMT_ESPDES_IV_SZ), GFP_ATOMIC)) == NULL) {
				SENDERR(ENOMEM);
			}
			prng_bytes(&ipsec_prng, (char *)ipsp->ips_iv, EMT_ESPDES_IV_SZ);
			ipsp->ips_iv_bits = ipsp->ips_iv_size * 8;
			ipsp->ips_iv_size = EMT_ESPDES_IV_SZ;
			break;
# endif /* defined(CONFIG_KLIPS_ENC_3DES) */
		case ESP_NONE:
			break;
		default:
			KLIPS_PRINT(debug_pfkey,
				    "klips_debug:pfkey_ipsec_sa_init: "
				    "encalg=%d support not available in the kernel",
				    ipsp->ips_encalg);
			SENDERR(EINVAL);
		}

		/* Create IV */
		if (ipsp->ips_iv_size) {
			if((ipsp->ips_iv = (caddr_t)
			    kmalloc(ipsp->ips_iv_size, GFP_ATOMIC)) == NULL) {
				SENDERR(ENOMEM);
			}
			prng_bytes(&ipsec_prng, (char *)ipsp->ips_iv, ipsp->ips_iv_size);
			ipsp->ips_iv_bits = ipsp->ips_iv_size * 8;
		}
		
#ifdef CONFIG_KLIPS_ALG
		if (ixt_e) {
			if ((error=ipsec_alg_enc_key_create(ipsp)) < 0)
				SENDERR(-error);
		} else
#endif /* CONFIG_KLIPS_ALG */
		switch(ipsp->ips_encalg) {
# ifdef CONFIG_KLIPS_ENC_3DES
		case ESP_3DES:
			if(ipsp->ips_key_bits_e != (EMT_ESP3DES_KEY_SZ * 8)) {
				KLIPS_PRINT(debug_pfkey,
					    "klips_debug:pfkey_ipsec_sa_init: "
					    "incorrect encryption key size: %d bits -- must be %d bits\n"/*octets (bytes)\n"*/,
					    ipsp->ips_key_bits_e, EMT_ESP3DES_KEY_SZ * 8);
				SENDERR(EINVAL);
			}
			
			/* save encryption key pointer */
			ekp = ipsp->ips_key_e;
			eks = ipsp->ips_key_e_size;
			
			KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
			            "klips_debug:pfkey_ipsec_sa_init: "
			            "allocating %lu bytes for 3des.\n",
			            (unsigned long) (3 * sizeof(struct des_eks)));
			if((ipsp->ips_key_e = (caddr_t)
			    kmalloc(3 * sizeof(struct des_eks), GFP_ATOMIC)) == NULL) {
				ipsp->ips_key_e = ekp;
				SENDERR(ENOMEM);
			}
			ipsp->ips_key_e_size = 3 * sizeof(struct des_eks);

			for(i = 0; i < 3; i++) {
#if KLIPS_DIVULGE_CYPHER_KEY
				KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
					    "klips_debug:pfkey_ipsec_sa_init: "
					    "3des key %d/3 is 0x%08x%08x\n",
					    i + 1,
					    ntohl(*((__u32 *)ekp + i * 2)),
					    ntohl(*((__u32 *)ekp + i * 2 + 1)));
#  endif
#if KLIPS_FIXES_DES_PARITY				
				/* force parity */
				des_set_odd_parity((des_cblock *)(ekp + EMT_ESPDES_KEY_SZ * i));
#endif
				error = des_set_key((des_cblock *)(ekp + EMT_ESPDES_KEY_SZ * i),
						    ((struct des_eks *)(ipsp->ips_key_e))[i].ks);
				if (error == -1)
					printk("klips_debug:pfkey_ipsec_sa_init: "
					       "parity error in des key %d/3\n",
					       i + 1);
				else if (error == -2)
					printk("klips_debug:pfkey_ipsec_sa_init: "
					       "illegal weak des key %d/3\n", i + 1);
				if (error) {
					memset(ekp, 0, eks);
					kfree(ekp);
					SENDERR(EINVAL);
				}
			}

			/* paranoid */
			memset(ekp, 0, eks);
			kfree(ekp);
			break;
# endif /* CONFIG_KLIPS_ENC_3DES */
                case ESP_NONE:
			break;
		default:
			KLIPS_PRINT(debug_pfkey,
				    "klips_debug:pfkey_ipsec_sa_init: "
				    "encalg=%d support not available in the kernel",
				    ipsp->ips_encalg);
			SENDERR(EINVAL);
		}

#ifdef CONFIG_KLIPS_ALG
		if ((ixt_a=ipsp->ips_alg_auth)) {
			if ((error=ipsec_alg_auth_key_create(ipsp)) < 0)
				SENDERR(-error);
		} else	
#endif /* CONFIG_KLIPS_ALG */
		
		switch(ipsp->ips_authalg) {
# ifdef CONFIG_KLIPS_AUTH_HMAC_MD5
		case AH_MD5: {
			MD5_CTX *ictx;
			MD5_CTX *octx;

			if(ipsp->ips_key_bits_a != (AHMD596_KLEN * 8)) {
				KLIPS_PRINT(debug_pfkey,
					    "klips_debug:pfkey_ipsec_sa_init: "
					    "incorrect authorisation key size: %d bits -- must be %d bits\n"/*octets (bytes)\n"*/,
					    ipsp->ips_key_bits_a,
					    AHMD596_KLEN * 8);
				SENDERR(EINVAL);
			}
			
#  if KLIPS_DIVULGE_HMAC_KEY
			KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
				    "klips_debug:pfkey_ipsec_sa_init: "
				    "hmac md5-96 key is 0x%08x %08x %08x %08x\n",
				    ntohl(*(((__u32 *)(ipsp->ips_key_a))+0)),
				    ntohl(*(((__u32 *)(ipsp->ips_key_a))+1)),
				    ntohl(*(((__u32 *)(ipsp->ips_key_a))+2)),
				    ntohl(*(((__u32 *)(ipsp->ips_key_a))+3)));
#  endif /* KLIPS_DIVULGE_HMAC_KEY */
			ipsp->ips_auth_bits = AHMD596_ALEN * 8;
			
			/* save the pointer to the key material */
			akp = ipsp->ips_key_a;
			aks = ipsp->ips_key_a_size;
			
			KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
			            "klips_debug:pfkey_ipsec_sa_init: "
			            "allocating %lu bytes for md5_ctx.\n",
			            (unsigned long) sizeof(struct md5_ctx));
			if((ipsp->ips_key_a = (caddr_t)
			    kmalloc(sizeof(struct md5_ctx), GFP_ATOMIC)) == NULL) {
				ipsp->ips_key_a = akp;
				SENDERR(ENOMEM);
			}
			ipsp->ips_key_a_size = sizeof(struct md5_ctx);

			for (i = 0; i < DIVUP(ipsp->ips_key_bits_a, 8); i++) {
				kb[i] = akp[i] ^ HMAC_IPAD;
			}
			for (; i < AHMD596_BLKLEN; i++) {
				kb[i] = HMAC_IPAD;
			}

			ictx = &(((struct md5_ctx*)(ipsp->ips_key_a))->ictx);
			osMD5Init(ictx);
			osMD5Update(ictx, kb, AHMD596_BLKLEN);

			for (i = 0; i < AHMD596_BLKLEN; i++) {
				kb[i] ^= (HMAC_IPAD ^ HMAC_OPAD);
			}

			octx = &(((struct md5_ctx*)(ipsp->ips_key_a))->octx);
			osMD5Init(octx);
			osMD5Update(octx, kb, AHMD596_BLKLEN);
			
#  if KLIPS_DIVULGE_HMAC_KEY
			KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
				    "klips_debug:pfkey_ipsec_sa_init: "
				    "MD5 ictx=0x%08x %08x %08x %08x octx=0x%08x %08x %08x %08x\n",
				    ((__u32*)ictx)[0],
				    ((__u32*)ictx)[1],
				    ((__u32*)ictx)[2],
				    ((__u32*)ictx)[3],
				    ((__u32*)octx)[0],
				    ((__u32*)octx)[1],
				    ((__u32*)octx)[2],
				    ((__u32*)octx)[3] );
#  endif /* KLIPS_DIVULGE_HMAC_KEY */
			/* paranoid */
			memset(akp, 0, aks);
			kfree(akp);
			break;
		}
# endif /* CONFIG_KLIPS_AUTH_HMAC_MD5 */
# ifdef CONFIG_KLIPS_AUTH_HMAC_SHA1
		case AH_SHA: {
			SHA1_CTX *ictx;
			SHA1_CTX *octx;

			if(ipsp->ips_key_bits_a != (AHSHA196_KLEN * 8)) {
				KLIPS_PRINT(debug_pfkey,
					    "klips_debug:pfkey_ipsec_sa_init: "
					    "incorrect authorisation key size: %d bits -- must be %d bits\n"/*octets (bytes)\n"*/,
					    ipsp->ips_key_bits_a,
					    AHSHA196_KLEN * 8);
				SENDERR(EINVAL);
			}
			
#  if KLIPS_DIVULGE_HMAC_KEY
			KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
				    "klips_debug:pfkey_ipsec_sa_init: "
				    "hmac sha1-96 key is 0x%08x %08x %08x %08x\n",
				    ntohl(*(((__u32 *)ipsp->ips_key_a)+0)),
				    ntohl(*(((__u32 *)ipsp->ips_key_a)+1)),
				    ntohl(*(((__u32 *)ipsp->ips_key_a)+2)),
				    ntohl(*(((__u32 *)ipsp->ips_key_a)+3)));
#  endif /* KLIPS_DIVULGE_HMAC_KEY */
			ipsp->ips_auth_bits = AHSHA196_ALEN * 8;
			
			/* save the pointer to the key material */
			akp = ipsp->ips_key_a;
			aks = ipsp->ips_key_a_size;

			KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
			            "klips_debug:pfkey_ipsec_sa_init: "
			            "allocating %lu bytes for sha1_ctx.\n",
			            (unsigned long) sizeof(struct sha1_ctx));
			if((ipsp->ips_key_a = (caddr_t)
			    kmalloc(sizeof(struct sha1_ctx), GFP_ATOMIC)) == NULL) {
				ipsp->ips_key_a = akp;
				SENDERR(ENOMEM);
			}
			ipsp->ips_key_a_size = sizeof(struct sha1_ctx);

			for (i = 0; i < DIVUP(ipsp->ips_key_bits_a, 8); i++) {
				kb[i] = akp[i] ^ HMAC_IPAD;
			}
			for (; i < AHMD596_BLKLEN; i++) {
				kb[i] = HMAC_IPAD;
			}

			ictx = &(((struct sha1_ctx*)(ipsp->ips_key_a))->ictx);
			SHA1Init(ictx);
			SHA1Update(ictx, kb, AHSHA196_BLKLEN);

			for (i = 0; i < AHSHA196_BLKLEN; i++) {
				kb[i] ^= (HMAC_IPAD ^ HMAC_OPAD);
			}

			octx = &((struct sha1_ctx*)(ipsp->ips_key_a))->octx;
			SHA1Init(octx);
			SHA1Update(octx, kb, AHSHA196_BLKLEN);
			
#  if KLIPS_DIVULGE_HMAC_KEY
			KLIPS_PRINT(debug_pfkey && sysctl_ipsec_debug_verbose,
				    "klips_debug:pfkey_ipsec_sa_init: "
				    "SHA1 ictx=0x%08x %08x %08x %08x octx=0x%08x %08x %08x %08x\n",
				    ((__u32*)ictx)[0],
				    ((__u32*)ictx)[1],
				    ((__u32*)ictx)[2],
				    ((__u32*)ictx)[3],
				    ((__u32*)octx)[0],
				    ((__u32*)octx)[1],
				    ((__u32*)octx)[2],
				    ((__u32*)octx)[3] );
#  endif /* KLIPS_DIVULGE_HMAC_KEY */
			memset(akp, 0, aks);
			kfree(akp);
			break;
		}
# endif /* CONFIG_KLIPS_AUTH_HMAC_SHA1 */
		case AH_NONE:
			break;
		default:
			KLIPS_PRINT(debug_pfkey,
				    "klips_debug:pfkey_ipsec_sa_init: "
				    "authalg=%d support not available in the kernel.\n",
				    ipsp->ips_authalg);
			SENDERR(EINVAL);
		}
	}
			break;
#endif /* !CONFIG_KLIPS_ESP */
#ifdef CONFIG_KLIPS_IPCOMP
	case IPPROTO_COMP:
		ipsp->ips_comp_adapt_tries = 0;
		ipsp->ips_comp_adapt_skip = 0;
		ipsp->ips_comp_ratio_cbytes = 0;
		ipsp->ips_comp_ratio_dbytes = 0;
		break;
#endif /* CONFIG_KLIPS_IPCOMP */
	default:
		printk(KERN_ERR "KLIPS sa initialization: "
		       "proto=%d unknown.\n",
		       ipsp->ips_said.proto);
		SENDERR(EINVAL);
	}
	
 errlab:
	return(error);
}


int
pfkey_safe_build(int error, struct sadb_ext *extensions[SADB_MAX+1])
{
	KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_safe_build: "
		    "error=%d\n",
		    error);
	if (!error) {
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_safe_build:"
			    "success.\n");
		return 1;
	} else {
		KLIPS_PRINT(debug_pfkey, "klips_debug:pfkey_safe_build:"
			    "caught error %d\n",
			    error);
		pfkey_extensions_free(extensions);
		return 0;
	}
}


DEBUG_NO_STATIC int
pfkey_getspi_parse(struct sock *sk, struct sadb_ext **extensions, struct pfkey_extracted_data* extr)
{
	int error = 0;
	ipsec_spi_t minspi = htonl(256), maxspi = htonl(-1L);
	int found_avail = 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;

	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_getspi_parse: .\n");

	pfkey_extensions_init(extensions_reply);

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

	if(extensions[SADB_EXT_SPIRANGE]) {
		minspi = ((struct sadb_spirange *)extensions[SADB_EXT_SPIRANGE])->sadb_spirange_min;
		maxspi = ((struct sadb_spirange *)extensions[SADB_EXT_SPIRANGE])->sadb_spirange_max;
	}

	if(maxspi == minspi) {
		extr->ips->ips_said.spi = maxspi;
		ipsq = ipsec_sa_getbyid(&(extr->ips->ips_said));
		if(ipsq != NULL) {
			sa_len = satot(&extr->ips->ips_said, 0, sa, sizeof(sa));
			ipsec_sa_put(ipsq);
			KLIPS_PRINT(debug_pfkey,
				    "klips_debug:pfkey_getspi_parse: "