Added reception of DHCP responses via PACKET socket

src/libcharon/plugins/dhcp/dhcp_socket.c

@@ -22,6 +22,8 @@
 #include <netinet/ip.h>
 #include <netinet/udp.h>
 #include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/filter.h>
 
 #include <utils/linked_list.h>
 #include <utils/identification.h>
@@ -53,9 +55,14 @@ struct private_dhcp_socket_t {
 	rng_t *rng;
 
 	/**
-	 * List of active transactions
+	 * List of transactions in DISCOVER
 	 */
-	linked_list_t *active;
+	linked_list_t *discover;
+
+	/**
+	 * List of transactions in REQUEST
+	 */
+	linked_list_t *request;
 
 	/**
 	 * List of successfully completed transactions
@@ -78,9 +85,14 @@ struct private_dhcp_socket_t {
 	int waiting;
 
 	/**
-	 * RAW socket
+	 * DHCP send socket
 	 */
-	int skt;
+	int send;
+
+	/**
+	 * DHCP receive socket
+	 */
+	int receive;
 
 	/**
 	 * DHCP server address, or broadcast
@@ -102,10 +114,18 @@ typedef enum {
 	DHCP_HOST_NAME = 12,
 	DHCP_MESSAGE_TYPE = 53,
 	DHCP_PARAM_REQ_LIST = 55,
+	DHCP_OPTEND = 255,
 } dhcp_option_type_t;
 
 typedef enum {
 	DHCP_DISCOVER = 1,
+	DHCP_OFFER = 2,
+	DHCP_REQUEST = 3,
+	DHCP_DECLINE = 4,
+	DHCP_ACK = 5,
+	DHCP_NAK = 6,
+	DHCP_RELEASE = 7,
+	DHCP_INFORM = 8,
 } dhcp_message_type_t;
 
 typedef enum {
@@ -140,25 +160,23 @@ typedef struct __attribute__((packed)) {
 } dhcp_t;
 
 /**
- * Send DHCP discover using a given transaction
+ * Prepare a DHCP message for a given transaction
  */
-static void discover(private_dhcp_socket_t *this,
+static int prepare_dhcp(private_dhcp_socket_t *this,
-					 dhcp_transaction_t *transaction)
+						dhcp_transaction_t *transaction, dhcp_t *dhcp)
 {
-	chunk_t id_data, broadcast = chunk_from_chars(0xFF,0xFF,0xFF,0xFF);
+	chunk_t chunk, broadcast = chunk_from_chars(0xFF,0xFF,0xFF,0xFF);
 	identification_t *identity;
 	dhcp_option_t *option;
-	dhcp_t dhcp;
 	int optlen = 0;
-	u_int hash;
 	host_t *src;
-	ssize_t len;
+	u_int hash;
 
-	memset(&dhcp, 0, sizeof(dhcp));
+	memset(dhcp, 0, sizeof(*dhcp));
-	dhcp.opcode = BOOTREQUEST;
+	dhcp->opcode = BOOTREQUEST;
-	dhcp.hw_type = ARPHRD_ETHER;
+	dhcp->hw_type = ARPHRD_ETHER;
-	dhcp.hw_addr_len = 6;
+	dhcp->hw_addr_len = 6;
-	dhcp.transaction_id = transaction->get_id(transaction);
+	dhcp->transaction_id = transaction->get_id(transaction);
 	if (chunk_equals(broadcast, this->dst->get_address(this->dst)))
 	{
 		/* TODO: send with 0.0.0.0 source address */
@@ -170,36 +188,53 @@ static void discover(private_dhcp_socket_t *this,
 									charon->kernel_interface, this->dst, NULL);
 		if (src)
 		{
-			memcpy(&dhcp.gateway_address, src->get_address(src).ptr,
+			memcpy(&dhcp->gateway_address, src->get_address(src).ptr,
-				   sizeof(dhcp.gateway_address));
+				   sizeof(dhcp->gateway_address));
 			src->destroy(src);
 		}
 	}
 
 	identity = transaction->get_identity(transaction);
-	id_data = identity->get_encoding(identity);
+	chunk = identity->get_encoding(identity);
-
 	/* magic bytes, a locally administered unicast MAC */
-	dhcp.client_hw_addr[0] = 0x7A;
+	dhcp->client_hw_addr[0] = 0x7A;
-	dhcp.client_hw_addr[1] = 0xA7;
+	dhcp->client_hw_addr[1] = 0xA7;
 	/* with ID specific postfix */
-	hash = htonl(chunk_hash(id_data));
+	hash = htonl(chunk_hash(chunk));
-	memcpy(&dhcp.client_hw_addr[2], &hash, 4);
+	memcpy(&dhcp->client_hw_addr[2], &hash, 4);
 
-	dhcp.magic_cookie = htonl(0x63825363);
+	dhcp->magic_cookie = htonl(0x63825363);
 
-	option = (dhcp_option_t*)&dhcp.options[optlen];
+	option = (dhcp_option_t*)&dhcp->options[optlen];
 	option->type = DHCP_MESSAGE_TYPE;
 	option->len = 1;
 	option->data[0] = DHCP_DISCOVER;
 	optlen += sizeof(dhcp_option_t) + option->len;
 
-	option = (dhcp_option_t*)&dhcp.options[optlen];
+	option = (dhcp_option_t*)&dhcp->options[optlen];
 	option->type = DHCP_HOST_NAME;
-	option->len = min(id_data.len, 64);
+	option->len = min(chunk.len, 64);
-	memcpy(option->data, id_data.ptr, option->len);
+	memcpy(option->data, chunk.ptr, option->len);
 	optlen += sizeof(dhcp_option_t) + option->len;
 
+	return optlen;
+}
+
+/**
+ * Send DHCP discover using a given transaction
+ */
+static bool discover(private_dhcp_socket_t *this,
+					 dhcp_transaction_t *transaction)
+{
+	dhcp_option_t *option;
+	dhcp_t dhcp;
+	ssize_t len;
+	int optlen;
+
+	optlen = prepare_dhcp(this, transaction, &dhcp);
+
+	DBG1(DBG_CFG, "sending DHCP DISCOVER to %H", this->dst);
+
 	option = (dhcp_option_t*)&dhcp.options[optlen];
 	option->type = DHCP_PARAM_REQ_LIST;
 	option->len = 2;
@@ -207,14 +242,25 @@ static void discover(private_dhcp_socket_t *this,
 	option->data[1] = DHCP_DNS_SERVER;
 	optlen += sizeof(dhcp_option_t) + option->len;
 
-	dhcp.options[optlen++] = 0xFF;
+	dhcp.options[optlen++] = DHCP_OPTEND;
 
 	len = offsetof(dhcp_t, magic_cookie) + ((optlen + 4) / 64 * 64 + 64);
-	if (sendto(this->skt, &dhcp, len, 0, this->dst->get_sockaddr(this->dst),
+	if (sendto(this->send, &dhcp, len, 0, this->dst->get_sockaddr(this->dst),
 			   *this->dst->get_sockaddr_len(this->dst)) != len)
 	{
 		DBG1(DBG_CFG, "sending DHCP DISCOVER failed: %s", strerror(errno));
+		return FALSE;
 	}
+	return TRUE;
+}
+
+/**
+ * Send DHCP request using a given transaction
+ */
+static bool request(private_dhcp_socket_t *this,
+					 dhcp_transaction_t *transaction)
+{
+	return FALSE;
 }
 
 METHOD(dhcp_socket_t, enroll, dhcp_transaction_t*,
@@ -222,15 +268,138 @@ METHOD(dhcp_socket_t, enroll, dhcp_transaction_t*,
 {
 	dhcp_transaction_t *transaction;
 	u_int32_t id;
+	int tries;
 
 	this->rng->get_bytes(this->rng, sizeof(id), (u_int8_t*)&id);
 	transaction = dhcp_transaction_create(id, identity);
-	discover(this, transaction);
-	transaction->destroy(transaction);
 
+	this->mutex->lock(this->mutex);
+	this->discover->insert_last(this->discover, transaction);
+	tries = 3;
+	while (tries && discover(this, transaction))
+	{
+		if (!this->condvar->timed_wait(this->condvar, this->mutex, 1000))
+		{
+			break;
+		}
+		tries--;
+	}
+	if (this->discover->remove(this->discover, transaction, NULL))
+	{	/* no OFFER received */
+		this->mutex->unlock(this->mutex);
+		transaction->destroy(transaction);
+		return NULL;
+	}
+
+	tries = 3;
+	while (tries && request(this, transaction))
+	{
+		if (!this->condvar->timed_wait(this->condvar, this->mutex, 1000))
+		{
+			break;
+		}
+		tries--;
+	}
+	if (this->request->remove(this->request, transaction, NULL))
+	{	/* no ACK received */
+		this->mutex->unlock(this->mutex);
+		transaction->destroy(transaction);
+		return NULL;
+	}
+	this->mutex->unlock(this->mutex);
+
+	transaction->destroy(transaction);
 	return NULL;
 }
 
+/**
+ * Handle a DHCP offer
+ */
+static void handle_offer(private_dhcp_socket_t *this, dhcp_t *dhcp, int optlen)
+{
+	dhcp_transaction_t *transaction;
+	enumerator_t *enumerator;
+	host_t *offer;
+
+	offer = host_create_from_chunk(AF_INET,
+								   chunk_from_thing(dhcp->your_address), 0);
+	if (!offer)
+	{
+		return;
+	}
+	DBG1(DBG_CFG, "received DHCP OFFER %H", offer);
+
+	this->mutex->lock(this->mutex);
+	enumerator = this->discover->create_enumerator(this->discover);
+	while (enumerator->enumerate(enumerator, &transaction))
+	{
+		if (transaction->get_id(transaction) == dhcp->transaction_id)
+		{
+			this->discover->remove_at(this->discover, enumerator);
+			this->request->insert_last(this->request, transaction);
+			transaction->set_address(transaction, offer->clone(offer));
+			break;
+		}
+	}
+	enumerator->destroy(enumerator);
+	this->mutex->unlock(this->mutex);
+	this->condvar->broadcast(this->condvar);
+	offer->destroy(offer);
+}
+
+/**
+ * Receive DHCP responses
+ */
+static job_requeue_t receive_dhcp(private_dhcp_socket_t *this)
+{
+	struct sockaddr_ll addr;
+	socklen_t addr_len = sizeof(addr);
+	struct __attribute__((packed)) {
+		struct iphdr ip;
+		struct udphdr udp;
+		dhcp_t dhcp;
+	} packet;
+	int oldstate, optlen, origoptlen, optsize, optpos = 0;
+	ssize_t len;
+	dhcp_option_t *option;
+
+	oldstate = thread_cancelability(TRUE);
+	len = recvfrom(this->receive, &packet, sizeof(packet), 0,
+					(struct sockaddr*)&addr, &addr_len);
+	thread_cancelability(oldstate);
+
+	if (len >= sizeof(struct iphdr) + sizeof(struct udphdr) +
+		offsetof(dhcp_t, options))
+	{
+		origoptlen = optlen = len - sizeof(struct iphdr) +
+					 sizeof(struct udphdr) + offsetof(dhcp_t, options);
+		while (optlen > sizeof(dhcp_option_t))
+		{
+			option = (dhcp_option_t*)&packet.dhcp.options[optpos];
+			optsize = sizeof(dhcp_option_t) + option->len;
+			if (option->type == DHCP_OPTEND || optlen < optsize)
+			{
+				break;
+			}
+			if (option->type == DHCP_MESSAGE_TYPE && option->len == 1)
+			{
+				switch (option->data[0])
+				{
+					case DHCP_OFFER:
+						handle_offer(this, &packet.dhcp, origoptlen);
+						break;
+					default:
+						break;
+				}
+				break;
+			}
+			optlen -= optsize;
+			optpos += optsize;
+		}
+	}
+	return JOB_REQUEUE_DIRECT;
+}
+
 METHOD(dhcp_socket_t, destroy, void,
 	private_dhcp_socket_t *this)
 {
@@ -242,14 +411,22 @@ METHOD(dhcp_socket_t, destroy, void,
 	{
 		this->condvar->signal(this->condvar);
 	}
-	if (this->skt > 0)
+	if (this->send > 0)
 	{
-		close(this->skt);
+		close(this->send);
+	}
+	if (this->receive > 0)
+	{
+		close(this->receive);
 	}
 	this->mutex->destroy(this->mutex);
 	this->condvar->destroy(this->condvar);
-	this->active->destroy(this->active);
+	this->discover->destroy_offset(this->discover,
-	this->completed->destroy(this->completed);
+								offsetof(dhcp_transaction_t, destroy));
+	this->request->destroy_offset(this->request,
+								offsetof(dhcp_transaction_t, destroy));
+	this->completed->destroy_offset(this->completed,
+								offsetof(dhcp_transaction_t, destroy));
 	DESTROY_IF(this->rng);
 	DESTROY_IF(this->dst);
 	free(this);
@@ -263,6 +440,36 @@ dhcp_socket_t *dhcp_socket_create()
 	private_dhcp_socket_t *this;
 	struct sockaddr_in src;
 	int on = 1;
+	struct sock_filter dhcp_filter_code[] = {
+		BPF_STMT(BPF_LD+BPF_B+BPF_ABS,
+				 offsetof(struct iphdr, protocol)),
+		BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, IPPROTO_UDP, 0, 14),
+		BPF_STMT(BPF_LD+BPF_H+BPF_ABS, sizeof(struct iphdr) +
+				 offsetof(struct udphdr, source)),
+		BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, DHCP_SERVER_PORT, 0, 12),
+		BPF_STMT(BPF_LD+BPF_H+BPF_ABS, sizeof(struct iphdr) +
+				 offsetof(struct udphdr, dest)),
+		BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, DHCP_CLIENT_PORT, 0, 10),
+		BPF_STMT(BPF_LD+BPF_B+BPF_ABS, sizeof(struct iphdr) +
+				 sizeof(struct udphdr) + offsetof(dhcp_t, opcode)),
+		BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, BOOTREPLY, 0, 8),
+		BPF_STMT(BPF_LD+BPF_B+BPF_ABS, sizeof(struct iphdr) +
+				 sizeof(struct udphdr) + offsetof(dhcp_t, hw_type)),
+		BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, ARPHRD_ETHER, 0, 6),
+		BPF_STMT(BPF_LD+BPF_B+BPF_ABS, sizeof(struct iphdr) +
+				 sizeof(struct udphdr) + offsetof(dhcp_t, hw_addr_len)),
+		BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, 6, 0, 4),
+		BPF_STMT(BPF_LD+BPF_W+BPF_ABS, sizeof(struct iphdr) +
+				 sizeof(struct udphdr) + offsetof(dhcp_t, magic_cookie)),
+		BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, 0x63825363, 0, 2),
+		BPF_STMT(BPF_LD+BPF_W+BPF_LEN, 0),
+		BPF_STMT(BPF_RET+BPF_A, 0),
+		BPF_STMT(BPF_RET+BPF_K, 0),
+	};
+	struct sock_fprog dhcp_filter = {
+		sizeof(dhcp_filter_code) / sizeof(struct sock_filter),
+		dhcp_filter_code,
+	};
 
 	INIT(this,
 		.public = {
@@ -272,7 +479,8 @@ dhcp_socket_t *dhcp_socket_create()
 		.rng = lib->crypto->create_rng(lib->crypto, RNG_WEAK),
 		.mutex = mutex_create(MUTEX_TYPE_DEFAULT),
 		.condvar = condvar_create(CONDVAR_TYPE_DEFAULT),
-		.active = linked_list_create(),
+		.discover = linked_list_create(),
+		.request = linked_list_create(),
 		.completed = linked_list_create(),
 	);
 
@@ -282,7 +490,6 @@ dhcp_socket_t *dhcp_socket_create()
 		destroy(this);
 		return NULL;
 	}
-
 	this->dst = host_create_from_string(lib->settings->get_str(lib->settings,
 							"charon.plugins.dhcp.server", "255.255.255.255"),
 							DHCP_SERVER_PORT);
@@ -293,37 +500,55 @@ dhcp_socket_t *dhcp_socket_create()
 		return NULL;
 	}
 
-	this->skt = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	this->send = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-	if (this->skt == -1)
+	if (this->send == -1)
 	{
 		DBG1(DBG_CFG, "unable to create DHCP send socket: %s", strerror(errno));
 		destroy(this);
 		return NULL;
 	}
-
+	if (setsockopt(this->send, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) == -1)
-	if (setsockopt(this->skt, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) == -1)
 	{
 		DBG1(DBG_CFG, "unable to reuse DHCP socket address: %s", strerror(errno));
 		destroy(this);
 		return NULL;
 	}
-	if (setsockopt(this->skt, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) == -1)
+	if (setsockopt(this->send, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) == -1)
 	{
 		DBG1(DBG_CFG, "unable to broadcast on DHCP socket: %s", strerror(errno));
 		destroy(this);
 		return NULL;
 	}
-
 	src.sin_family = AF_INET;
 	src.sin_port = htons(DHCP_CLIENT_PORT);
 	src.sin_addr.s_addr = INADDR_ANY;
-	if (bind(this->skt, (struct sockaddr*)&src, sizeof(src)) == -1)
+	if (bind(this->send, (struct sockaddr*)&src, sizeof(src)) == -1)
 	{
 		DBG1(DBG_CFG, "unable to bind DHCP send socket: %s", strerror(errno));
 		destroy(this);
 		return NULL;
 	}
 
+	this->receive = socket(AF_PACKET, SOCK_DGRAM, htons(ETH_P_IP));
+	if (this->receive == -1)
+	{
+		DBG1(DBG_NET, "opening DHCP receive socket failed: %s", strerror(errno));
+		destroy(this);
+		return NULL;
+	}
+	if (setsockopt(this->receive, SOL_SOCKET, SO_ATTACH_FILTER,
+				   &dhcp_filter, sizeof(dhcp_filter)) < 0)
+	{
+		DBG1(DBG_CFG, "installing DHCP socket filter failed: %s",
+			 strerror(errno));
+		destroy(this);
+		return NULL;
+	}
+
+	this->job = callback_job_create((callback_job_cb_t)receive_dhcp,
+									this, NULL, NULL);
+	charon->processor->queue_job(charon->processor, (job_t*)this->job);
+
 	return &this->public;
 }