af_key.c

Linux Kernel 2.6.9 for OMAP1710

				break;
			if (aalg->available)
				*ap++ = aalg->desc;
		}
	}

	if (enc_len) {
		struct sadb_supported *sp;
		struct sadb_alg *ap;

		sp = (struct sadb_supported *) skb_put(skb, enc_len);
		ap = (struct sadb_alg *) (sp + 1);

		sp->sadb_supported_len = enc_len / sizeof(uint64_t);
		sp->sadb_supported_exttype = SADB_EXT_SUPPORTED_ENCRYPT;

		for (i = 0; ; i++) {
			struct xfrm_algo_desc *ealg = xfrm_ealg_get_byidx(i);
			if (!ealg)
				break;
			if (ealg->available)
				*ap++ = ealg->desc;
		}
	}

out_put_algs:
	return skb;
}

static int pfkey_register(struct sock *sk, struct sk_buff *skb, struct sadb_msg *hdr, void **ext_hdrs)
{
	struct pfkey_opt *pfk = pfkey_sk(sk);
	struct sk_buff *supp_skb;

	if (hdr->sadb_msg_satype > SADB_SATYPE_MAX)
		return -EINVAL;

	if (hdr->sadb_msg_satype != SADB_SATYPE_UNSPEC) {
		if (pfk->registered&(1<<hdr->sadb_msg_satype))
			return -EEXIST;
		pfk->registered |= (1<<hdr->sadb_msg_satype);
	}

	xfrm_probe_algs();
	
	supp_skb = compose_sadb_supported(hdr, GFP_KERNEL);
	if (!supp_skb) {
		if (hdr->sadb_msg_satype != SADB_SATYPE_UNSPEC)
			pfk->registered &= ~(1<<hdr->sadb_msg_satype);

		return -ENOBUFS;
	}

	pfkey_broadcast(supp_skb, GFP_KERNEL, BROADCAST_REGISTERED, sk);

	return 0;
}

static int pfkey_flush(struct sock *sk, struct sk_buff *skb, struct sadb_msg *hdr, void **ext_hdrs)
{
	unsigned proto;
	struct sk_buff *skb_out;
	struct sadb_msg *hdr_out;

	proto = pfkey_satype2proto(hdr->sadb_msg_satype);
	if (proto == 0)
		return -EINVAL;

	skb_out = alloc_skb(sizeof(struct sadb_msg) + 16, GFP_KERNEL);
	if (!skb_out)
		return -ENOBUFS;

	xfrm_state_flush(proto);

	hdr_out = (struct sadb_msg *) skb_put(skb_out, sizeof(struct sadb_msg));
	pfkey_hdr_dup(hdr_out, hdr);
	hdr_out->sadb_msg_errno = (uint8_t) 0;
	hdr_out->sadb_msg_len = (sizeof(struct sadb_msg) / sizeof(uint64_t));

	pfkey_broadcast(skb_out, GFP_KERNEL, BROADCAST_ALL, NULL);

	return 0;
}

struct pfkey_dump_data
{
	struct sk_buff *skb;
	struct sadb_msg *hdr;
	struct sock *sk;
};

static int dump_sa(struct xfrm_state *x, int count, void *ptr)
{
	struct pfkey_dump_data *data = ptr;
	struct sk_buff *out_skb;
	struct sadb_msg *out_hdr;

	out_skb = pfkey_xfrm_state2msg(x, 1, 3);
	if (IS_ERR(out_skb))
		return PTR_ERR(out_skb);

	out_hdr = (struct sadb_msg *) out_skb->data;
	out_hdr->sadb_msg_version = data->hdr->sadb_msg_version;
	out_hdr->sadb_msg_type = SADB_DUMP;
	out_hdr->sadb_msg_satype = pfkey_proto2satype(x->id.proto);
	out_hdr->sadb_msg_errno = 0;
	out_hdr->sadb_msg_reserved = 0;
	out_hdr->sadb_msg_seq = count;
	out_hdr->sadb_msg_pid = data->hdr->sadb_msg_pid;
	pfkey_broadcast(out_skb, GFP_ATOMIC, BROADCAST_ONE, data->sk);
	return 0;
}

static int pfkey_dump(struct sock *sk, struct sk_buff *skb, struct sadb_msg *hdr, void **ext_hdrs)
{
	u8 proto;
	struct pfkey_dump_data data = { .skb = skb, .hdr = hdr, .sk = sk };

	proto = pfkey_satype2proto(hdr->sadb_msg_satype);
	if (proto == 0)
		return -EINVAL;

	return xfrm_state_walk(proto, dump_sa, &data);
}

static int pfkey_promisc(struct sock *sk, struct sk_buff *skb, struct sadb_msg *hdr, void **ext_hdrs)
{
	struct pfkey_opt *pfk = pfkey_sk(sk);
	int satype = hdr->sadb_msg_satype;

	if (hdr->sadb_msg_len == (sizeof(*hdr) / sizeof(uint64_t))) {
		/* XXX we mangle packet... */
		hdr->sadb_msg_errno = 0;
		if (satype != 0 && satype != 1)
			return -EINVAL;
		pfk->promisc = satype;
	}
	pfkey_broadcast(skb_clone(skb, GFP_KERNEL), GFP_KERNEL, BROADCAST_ALL, NULL);
	return 0;
}

static int check_reqid(struct xfrm_policy *xp, int dir, int count, void *ptr)
{
	int i;
	u32 reqid = *(u32*)ptr;

	for (i=0; i<xp->xfrm_nr; i++) {
		if (xp->xfrm_vec[i].reqid == reqid)
			return -EEXIST;
	}
	return 0;
}

static u32 gen_reqid(void)
{
	u32 start;
	static u32 reqid = IPSEC_MANUAL_REQID_MAX;

	start = reqid;
	do {
		++reqid;
		if (reqid == 0)
			reqid = IPSEC_MANUAL_REQID_MAX+1;
		if (xfrm_policy_walk(check_reqid, (void*)&reqid) != -EEXIST)
			return reqid;
	} while (reqid != start);
	return 0;
}

static int
parse_ipsecrequest(struct xfrm_policy *xp, struct sadb_x_ipsecrequest *rq)
{
	struct xfrm_tmpl *t = xp->xfrm_vec + xp->xfrm_nr;
	struct sockaddr_in *sin;
#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
	struct sockaddr_in6 *sin6;
#endif

	if (xp->xfrm_nr >= XFRM_MAX_DEPTH)
		return -ELOOP;

	if (rq->sadb_x_ipsecrequest_mode == 0)
		return -EINVAL;

	t->id.proto = rq->sadb_x_ipsecrequest_proto; /* XXX check proto */
	t->mode = rq->sadb_x_ipsecrequest_mode-1;
	if (rq->sadb_x_ipsecrequest_level == IPSEC_LEVEL_USE)
		t->optional = 1;
	else if (rq->sadb_x_ipsecrequest_level == IPSEC_LEVEL_UNIQUE) {
		t->reqid = rq->sadb_x_ipsecrequest_reqid;
		if (t->reqid > IPSEC_MANUAL_REQID_MAX)
			t->reqid = 0;
		if (!t->reqid && !(t->reqid = gen_reqid()))
			return -ENOBUFS;
	}

	/* addresses present only in tunnel mode */
	if (t->mode) {
		switch (xp->family) {
		case AF_INET:
			sin = (void*)(rq+1);
			if (sin->sin_family != AF_INET)
				return -EINVAL;
			t->saddr.a4 = sin->sin_addr.s_addr;
			sin++;
			if (sin->sin_family != AF_INET)
				return -EINVAL;
			t->id.daddr.a4 = sin->sin_addr.s_addr;
			break;
#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
		case AF_INET6:
			sin6 = (void *)(rq+1);
			if (sin6->sin6_family != AF_INET6)
				return -EINVAL;
			memcpy(t->saddr.a6, &sin6->sin6_addr, sizeof(struct in6_addr));
			sin6++;
			if (sin6->sin6_family != AF_INET6)
				return -EINVAL;
			memcpy(t->id.daddr.a6, &sin6->sin6_addr, sizeof(struct in6_addr));
			break;
#endif
		default:
			return -EINVAL;
		}
	}
	/* No way to set this via kame pfkey */
	t->aalgos = t->ealgos = t->calgos = ~0;
	xp->xfrm_nr++;
	return 0;
}

static int
parse_ipsecrequests(struct xfrm_policy *xp, struct sadb_x_policy *pol)
{
	int err;
	int len = pol->sadb_x_policy_len*8 - sizeof(struct sadb_x_policy);
	struct sadb_x_ipsecrequest *rq = (void*)(pol+1);

	while (len >= sizeof(struct sadb_x_ipsecrequest)) {
		if ((err = parse_ipsecrequest(xp, rq)) < 0)
			return err;
		len -= rq->sadb_x_ipsecrequest_len;
		rq = (void*)((u8*)rq + rq->sadb_x_ipsecrequest_len);
	}
	return 0;
}

static int pfkey_xfrm_policy2msg_size(struct xfrm_policy *xp)
{
	int sockaddr_size = pfkey_sockaddr_size(xp->family);
	int socklen = (xp->family == AF_INET ?
		       sizeof(struct sockaddr_in) :
		       sizeof(struct sockaddr_in6));

	return sizeof(struct sadb_msg) +
		(sizeof(struct sadb_lifetime) * 3) +
		(sizeof(struct sadb_address) * 2) + 
		(sockaddr_size * 2) +
		sizeof(struct sadb_x_policy) +
		(xp->xfrm_nr * (sizeof(struct sadb_x_ipsecrequest) +
				(socklen * 2)));
}

static struct sk_buff * pfkey_xfrm_policy2msg_prep(struct xfrm_policy *xp)
{
	struct sk_buff *skb;
	int size;

	size = pfkey_xfrm_policy2msg_size(xp);

	skb =  alloc_skb(size + 16, GFP_ATOMIC);
	if (skb == NULL)
		return ERR_PTR(-ENOBUFS);

	return skb;
}

static void pfkey_xfrm_policy2msg(struct sk_buff *skb, struct xfrm_policy *xp, int dir)
{
	struct sadb_msg *hdr;
	struct sadb_address *addr;
	struct sadb_lifetime *lifetime;
	struct sadb_x_policy *pol;
	struct sockaddr_in   *sin;
#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
	struct sockaddr_in6  *sin6;
#endif
	int i;
	int size;
	int sockaddr_size = pfkey_sockaddr_size(xp->family);
	int socklen = (xp->family == AF_INET ?
		       sizeof(struct sockaddr_in) :
		       sizeof(struct sockaddr_in6));

	size = pfkey_xfrm_policy2msg_size(xp);

	/* call should fill header later */
	hdr = (struct sadb_msg *) skb_put(skb, sizeof(struct sadb_msg));
	memset(hdr, 0, size);	/* XXX do we need this ? */

	/* src address */
	addr = (struct sadb_address*) skb_put(skb, 
					      sizeof(struct sadb_address)+sockaddr_size);
	addr->sadb_address_len = 
		(sizeof(struct sadb_address)+sockaddr_size)/
			sizeof(uint64_t);
	addr->sadb_address_exttype = SADB_EXT_ADDRESS_SRC;
	addr->sadb_address_proto = pfkey_proto_from_xfrm(xp->selector.proto);
	addr->sadb_address_prefixlen = xp->selector.prefixlen_s;
	addr->sadb_address_reserved = 0;
	/* src address */
	if (xp->family == AF_INET) {
		sin = (struct sockaddr_in *) (addr + 1);
		sin->sin_family = AF_INET;
		sin->sin_addr.s_addr = xp->selector.saddr.a4;
		sin->sin_port = xp->selector.sport;
		memset(sin->sin_zero, 0, sizeof(sin->sin_zero));
	}
#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
	else if (xp->family == AF_INET6) {
		sin6 = (struct sockaddr_in6 *) (addr + 1);
		sin6->sin6_family = AF_INET6;
		sin6->sin6_port = xp->selector.sport;
		sin6->sin6_flowinfo = 0;
		memcpy(&sin6->sin6_addr, xp->selector.saddr.a6,
		       sizeof(struct in6_addr));
		sin6->sin6_scope_id = 0;
	}
#endif
	else
		BUG();

	/* dst address */
	addr = (struct sadb_address*) skb_put(skb, 
					      sizeof(struct sadb_address)+sockaddr_size);
	addr->sadb_address_len =
		(sizeof(struct sadb_address)+sockaddr_size)/
			sizeof(uint64_t);
	addr->sadb_address_exttype = SADB_EXT_ADDRESS_DST;
	addr->sadb_address_proto = pfkey_proto_from_xfrm(xp->selector.proto);
	addr->sadb_address_prefixlen = xp->selector.prefixlen_d; 
	addr->sadb_address_reserved = 0;
	if (xp->family == AF_INET) {
		sin = (struct sockaddr_in *) (addr + 1);
		sin->sin_family = AF_INET;
		sin->sin_addr.s_addr = xp->selector.daddr.a4;
		sin->sin_port = xp->selector.dport;
		memset(sin->sin_zero, 0, sizeof(sin->sin_zero));
	}
#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
	else if (xp->family == AF_INET6) {
		sin6 = (struct sockaddr_in6 *) (addr + 1);
		sin6->sin6_family = AF_INET6;
		sin6->sin6_port = xp->selector.dport;
		sin6->sin6_flowinfo = 0;
		memcpy(&sin6->sin6_addr, xp->selector.daddr.a6,
		       sizeof(struct in6_addr));
		sin6->sin6_scope_id = 0;
	}
#endif
	else
		BUG();

	/* hard time */
	lifetime = (struct sadb_lifetime *)  skb_put(skb, 
						     sizeof(struct sadb_lifetime));
	lifetime->sadb_lifetime_len =
		sizeof(struct sadb_lifetime)/sizeof(uint64_t);
	lifetime->sadb_lifetime_exttype = SADB_EXT_LIFETIME_HARD;
	lifetime->sadb_lifetime_allocations =  _X2KEY(xp->lft.hard_packet_limit);
	lifetime->sadb_lifetime_bytes = _X2KEY(xp->lft.hard_byte_limit);
	lifetime->sadb_lifetime_addtime = xp->lft.hard_add_expires_seconds;
	lifetime->sadb_lifetime_usetime = xp->lft.hard_use_expires_seconds;
	/* soft time */
	lifetime = (struct sadb_lifetime *)  skb_put(skb, 
						     sizeof(struct sadb_lifetime));
	lifetime->sadb_lifetime_len =
		sizeof(struct sadb_lifetime)/sizeof(uint64_t);
	lifetime->sadb_lifetime_exttype = SADB_EXT_LIFETIME_SOFT;
	lifetime->sadb_lifetime_allocations =  _X2KEY(xp->lft.soft_packet_limit);
	lifetime->sadb_lifetime_bytes = _X2KEY(xp->lft.soft_byte_limit);
	lifetime->sadb_lifetime_addtime = xp->lft.soft_add_expires_seconds;
	lifetime->sadb_lifetime_usetime = xp->lft.soft_use_expires_seconds;
	/* current time */
	lifetime = (struct sadb_lifetime *)  skb_put(skb, 
						     sizeof(struct sadb_lifetime));
	lifetime->sadb_lifetime_len =
		sizeof(struct sadb_lifetime)/sizeof(uint64_t);
	lifetime->sadb_lifetime_exttype = SADB_EXT_LIFETIME_CURRENT;
	lifetime->sadb_lifetime_allocations = xp->curlft.packets;
	lifetime->sadb_lifetime_bytes = xp->curlft.bytes;
	lifetime->sadb_lifetime_addtime = xp->curlft.add_time;
	lifetime->sadb_lifetime_usetime = xp->curlft.use_time;

	pol = (struct sadb_x_policy *)  skb_put(skb, sizeof(struct sadb_x_policy));
	pol->sadb_x_policy_len = sizeof(struct sadb_x_policy)/sizeof(uint64_t);
	pol->sadb_x_policy_exttype = SADB_X_EXT_POLICY;
	pol->sadb_x_policy_type = IPSEC_POLICY_DISCARD;
	if (xp->action == XFRM_POLICY_ALLOW) {
		if (xp->xfrm_nr)
			pol->sadb_x_policy_type = IPSEC_POLICY_IPSEC;
		else
			pol->sadb_x_policy_type = IPSEC_POLICY_NONE;
	}
	pol->sadb_x_policy_dir = dir+1;
	pol->sadb_x_policy_id = xp->index;
	pol->sadb_x_policy_priority = xp->priority;

	for (i=0; i<xp->xfrm_nr; i++) {
		struct sadb_x_ipsecrequest *rq;
		struct xfrm_tmpl *t = xp->xfrm_vec + i;
		int req_size;

		req_size = sizeof(struct sadb_x_ipsecrequest);
		if (t->mode)
			req_size += 2*socklen;
		else
			size -= 2*socklen;
		rq = (void*)skb_put(skb, req_size);
		pol->sadb_x_policy_len += req_size/8;
		memset(rq, 0, sizeof(*rq));
		rq->sadb_x_ipsecrequest_len = req_size;
		rq->sadb_x_ipsecrequest_proto = t->id.proto;
		rq->sadb_x_ipsecrequest_mode = t->mode+1;
		rq->sadb_x_ipsecrequest_level = IPSEC_LEVEL_REQUIRE;
		if (t->reqid)
			rq->sadb_x_ipsecrequest_level = IPSEC_LEVEL_UNIQUE;
		if (t->optional)
			rq->sadb_x_ipsecrequest_level = IPSEC_LEVEL_USE;
		rq->sadb_x_ipsecrequest_reqid = t->reqid;
		if (t->mode) {
			switch (xp->family) {
			case AF_INET:
				sin = (void*)(rq+1);
				sin->sin_family = AF_INET;
				sin->sin_addr.s_addr = t->saddr.a4;
				sin->sin_port = 0;
				memset(sin->sin_zero, 0, sizeof(sin->sin_zero));
				sin++;
				sin->sin_family = AF_INET;
				sin->sin_addr.s_addr = t->id.daddr.a4;
				sin->sin_port = 0;
				memset(sin->sin_zero, 0, sizeof(sin->sin_zero));
				break;
#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
			case AF_INET6:
				sin6 = (void*)(rq+1);
				sin6->sin6_family = AF_INET6;
				sin6->sin6_port = 0;
				sin6->sin6_flowinfo = 0;
				memcpy(&sin6->sin6_addr, t->saddr.a6,
				       sizeof(struct in6_addr));
				sin6->sin6_scope_id = 0;

				sin6++;
				sin6->sin6_family = AF_INET6;
				sin6->sin6_port = 0;
				sin6->sin6_flowinfo = 0;
				memcpy(&sin6->sin6_addr, t->id.daddr.a6,
				       sizeof(struct in6_addr));
				sin6->sin6_scope_id = 0;
				break;
#endif