Add sketch code to reassemble a DHCP packet from our internal "everything

is a DHCP option" data structures.

We need this code in order to be able to return a DHCP packet to a PXE NBP
which reflects options from our multiple sources (e.g. NVS and DHCP
server).  This is expensive, but necessary.  Having paid this cost, we may
as well try to use the same code to generate our DHCP request packets,
since the process is similar.

4 files changed, 243 insertions, 8 deletions

diff --git a/src/include/errno.h b/src/include/errno.h
index 8c9f515e..d59880bc 100644
--- a/src/include/errno.h
+++ b/src/include/errno.h
@@ -151,6 +151,7 @@
 #define ENOENT		0xe8	/**< No such file or directory */
 #define ENOEXEC		0xe9	/**< Exec format error */
 #define ENOMSG		ENODATA	/**< No message of the desired type */
+#define ENOSPC		ENOMEM	/**< No space left on device */
 #define ENOSR		0xea	/**< No stream resources */
 #define ENOSTR		0xeb	/**< Not a stream */
 #define ENOSYS		0xec	/**< Function not implemented */
diff --git a/src/include/gpxe/dhcp.h b/src/include/gpxe/dhcp.h
index 9442f857..e35b7eab 100644
--- a/src/include/gpxe/dhcp.h
+++ b/src/include/gpxe/dhcp.h
@@ -118,6 +118,12 @@ struct dhcp_packet {
  */
 #define DHCP_PAD 0
 
+/** Minimum normal DHCP option */
+#define DHCP_MIN_OPTION 1
+
+/** Vendor encapsulated options */
+#define DHCP_VENDOR_ENCAP 43
+
 /** Option overloading
  *
  * The value of this option is the bitwise-OR of zero or more
@@ -131,6 +137,17 @@ struct dhcp_packet {
 /** The "sname" field is overloaded to contain extra DHCP options */
 #define DHCP_OPTION_OVERLOAD_SNAME 2
 
+/** DHCP message type */
+#define DHCP_MESSAGE_TYPE 53
+#define DHCPDISCOVER 1
+#define DHCPOFFER 2
+#define DHCPREQUEST 3
+#define DHCPDECLINE 4
+#define DHCPACK 5
+#define DHCPNAK 6
+#define DHCPRELEASE 7
+#define DHCPINFORM 8
+
 /** TFTP server name
  *
  * This option replaces the fixed "sname" field, when that field is
@@ -178,6 +195,9 @@ struct dhcp_packet {
  */
 #define DHCP_EB_SIADDR DHCP_ENCAP_OPT ( DHCP_EB_ENCAP, 3 )
 
+/** Maximum normal DHCP option */
+#define DHCP_MAX_OPTION 254
+
 /** End of options
  *
  * This tag does not have a length field; it is always only a single
@@ -260,6 +280,8 @@ find_dhcp_option ( struct dhcp_option_block *options, unsigned int tag );
 extern struct dhcp_option * find_global_dhcp_option ( unsigned int tag );
 extern void register_dhcp_options ( struct dhcp_option_block *options );
 extern void unregister_dhcp_options ( struct dhcp_option_block *options );
+extern void init_dhcp_options ( struct dhcp_option_block *options,
+				void *data, size_t max_len );
 extern struct dhcp_option_block * alloc_dhcp_options ( size_t max_len );
 extern void free_dhcp_options ( struct dhcp_option_block *options );
 extern struct dhcp_option *
diff --git a/src/net/dhcpopts.c b/src/net/dhcpopts.c
index 6b06b9a2..70196346 100644
--- a/src/net/dhcpopts.c
+++ b/src/net/dhcpopts.c
@@ -228,6 +228,27 @@ void unregister_dhcp_options ( struct dhcp_option_block *options ) {
 }
 
 /**
+ * Initialise empty block of DHCP options
+ *
+ * @v options		Uninitialised DHCP option block
+ * @v data		Memory for DHCP option data
+ * @v max_len		Length of memory for DHCP option data
+ *
+ * Populates the DHCP option data with a single @c DHCP_END option and
+ * fills in the fields of the @c dhcp_option_block structure.
+ */
+void init_dhcp_options ( struct dhcp_option_block *options,
+			 void *data, size_t max_len ) {
+	struct dhcp_option *option;
+
+	options->data = data;
+	options->max_len = max_len;
+	option = options->data;
+	option->tag = DHCP_END;
+	options->len = 1;
+}
+
+/**
  * Allocate space for a block of DHCP options
  *
  * @v max_len		Maximum length of option block
@@ -238,17 +259,12 @@ void unregister_dhcp_options ( struct dhcp_option_block *options ) {
  */
 struct dhcp_option_block * alloc_dhcp_options ( size_t max_len ) {
 	struct dhcp_option_block *options;
-	struct dhcp_option *option;
 
 	options = malloc ( sizeof ( *options ) + max_len );
 	if ( options ) {
-		options->data = ( ( void * ) options + sizeof ( *options ) );
-		options->max_len = max_len;
-		if ( max_len ) {
-			option = options->data;
-			option->tag = DHCP_END;
-			options->len = 1;
-		}
+		init_dhcp_options ( options, 
+				    ( (void *) options + sizeof ( *options ) ),
+				    max_len );
 	}
 	return options;
 }
diff --git a/src/net/udp/dhcp.c b/src/net/udp/dhcp.c
index 89c3eb90..1523e404 100644
--- a/src/net/udp/dhcp.c
+++ b/src/net/udp/dhcp.c
@@ -17,6 +17,7 @@
  */
 
 #include <string.h>
+#include <errno.h>
 #include <assert.h>
 #include <byteswap.h>
 #include <gpxe/netdevice.h>
@@ -28,6 +29,197 @@
  *
  */
 
+struct dhcp_session {
+	struct net_device *netdev;
+	uint32_t xid;
+};
+
+/** DHCP operation types
+ *
+ * This table maps from DHCP message types (i.e. values of the @c
+ * DHCP_MESSAGE_TYPE option) to values of the "op" field within a DHCP
+ * packet.
+ */
+static const uint8_t dhcp_op[] = {
+	[DHCPDISCOVER]	= BOOTP_REQUEST,
+	[DHCPOFFER]	= BOOTP_REPLY,
+	[DHCPREQUEST]	= BOOTP_REQUEST,
+	[DHCPDECLINE]	= BOOTP_REQUEST,
+	[DHCPACK]	= BOOTP_REPLY,
+	[DHCPNAK]	= BOOTP_REPLY,
+	[DHCPRELEASE]	= BOOTP_REQUEST,
+	[DHCPINFORM]	= BOOTP_REQUEST,
+};
+
+/** DHCP packet option block fill order
+ *
+ * This is the order in which option blocks are filled when
+ * reassembling a DHCP packet.  We fill the smallest field ("sname")
+ * first, to maximise the chances of being able to fit large options
+ * within fields which are large enough to contain them.
+ */
+enum dhcp_packet_option_block_fill_order {
+	OPTS_SNAME = 0,
+	OPTS_FILE,
+	OPTS_MAIN,
+	NUM_OPT_BLOCKS
+};
+
+/** DHCP option blocks within a DHCP packet
+ *
+ * A DHCP packet contains three fields which can be used to contain
+ * options: the actual "options" field plus the "file" and "sname"
+ * fields (which can be overloaded to contain options).
+ */
+struct dhcp_packet_option_blocks {
+	struct dhcp_option_block options[NUM_OPT_BLOCKS];
+};
+
+/**
+ * Set option within DHCP packet
+ *
+ * @v optblocks		DHCP packet option blocks
+ * @v tag		DHCP option tag
+ * @v data		New value for DHCP option
+ * @v len		Length of value, in bytes
+ * @ret option		DHCP option, or NULL
+ *
+ * Sets the option within the first available options block within the
+ * DHCP packet.  Option blocks are tried in the order specified by @c
+ * dhcp_option_block_fill_order.
+ */
+static struct dhcp_option *
+set_dhcp_packet_option ( struct dhcp_packet_option_blocks *optblocks,
+			 unsigned int tag, const void *data, size_t len ) {
+	struct dhcp_option_block *options;
+	struct dhcp_option *option;
+
+	for ( options = optblocks->options ;
+	      options < &optblocks->options[NUM_OPT_BLOCKS] ; options++ ) {
+		option = set_dhcp_option ( options, tag, data, len );
+		if ( option )
+			return option;
+	}
+	return NULL;
+}
+
+/**
+ * Copy options to DHCP packet
+ *
+ * @v optblocks		DHCP packet option blocks
+ * @v encapsulator	Encapsulating option, or zero
+ * @ret rc		Return status code
+ * 
+ * Copies options from DHCP options blocks into a DHCP packet.  Most
+ * options are copied verbatim.  Recognised encapsulated options
+ * fields are handled as such.  Selected options (e.g. @c
+ * DHCP_OPTION_OVERLOAD) are always ignored, since these special cases
+ * are handled by other code.
+ */
+static int
+copy_dhcp_options_to_packet ( struct dhcp_packet_option_blocks *optblocks,
+			      unsigned int encapsulator ) {
+	unsigned int subtag;
+	unsigned int tag;
+	struct dhcp_option *option;
+	struct dhcp_option *copied;
+	int rc;
+
+	for ( subtag = DHCP_MIN_OPTION; subtag <= DHCP_MAX_OPTION; subtag++ ) {
+		tag = DHCP_ENCAP_OPT ( encapsulator, subtag );
+		switch ( tag ) {
+		case DHCP_OPTION_OVERLOAD:
+			/* Hard-coded in packets we reassemble; skip
+			 * this option
+			 */
+			break;
+		case DHCP_EB_ENCAP:
+		case DHCP_VENDOR_ENCAP:
+			/* Process encapsulated options field */
+			if ( ( rc = copy_dhcp_options_to_packet ( optblocks,
+								  tag ) ) != 0)
+				return rc;
+			break;
+		default:
+			/* Copy option to reassembled packet */
+			option = find_global_dhcp_option ( tag );
+			if ( ! option )
+				break;
+			copied = set_dhcp_packet_option ( optblocks, tag,
+							  &option->data,
+							  option->len );
+			if ( ! copied )
+				return -ENOSPC;
+			break;
+		};
+	}
+
+	return 0;
+}
+
+/**
+ * Assemble a DHCP packet
+ *
+ * @v dhcp		DHCP session
+ * @v data		Packet to be filled in
+ * @v max_len		Length of packet buffer
+ * @ret len		Length of assembled packet
+ *
+ * Reconstruct a DHCP packet from a DHCP options list.
+ */
+size_t dhcp_assemble ( struct dhcp_session *dhcp, void *data,
+		       size_t max_len ) {
+	struct dhcp_packet *dhcppkt = data;
+	struct dhcp_option *option;
+	struct dhcp_packet_option_blocks optblocks;
+	unsigned int dhcp_message_type;
+	static const uint8_t overloading = ( DHCP_OPTION_OVERLOAD_FILE |
+					     DHCP_OPTION_OVERLOAD_SNAME );
+
+	/* Fill in constant fields */
+	memset ( dhcppkt, 0, max_len );
+	dhcppkt->xid = dhcp->xid;
+	dhcppkt->magic = htonl ( DHCP_MAGIC_COOKIE );
+
+	/* Derive "op" field from DHCP_MESSAGE_TYPE option value */
+	dhcp_message_type = find_global_dhcp_num_option ( DHCP_MESSAGE_TYPE );
+	dhcppkt->op = dhcp_op[dhcp_message_type];
+
+	/* Fill in NIC details */
+	dhcppkt->htype = dhcp->netdev->ll_protocol->ll_proto;
+	dhcppkt->hlen = dhcp->netdev->ll_protocol->ll_addr_len;
+	memcpy ( dhcppkt->chaddr, dhcp->netdev->ll_addr, dhcppkt->hlen );
+
+	/* Fill in IP addresses if present */
+	option = find_global_dhcp_option ( DHCP_EB_YIADDR );
+	if ( option ) {
+		memcpy ( &dhcppkt->yiaddr, &option->data,
+			 sizeof ( dhcppkt->yiaddr ) );
+	}
+	option = find_global_dhcp_option ( DHCP_EB_SIADDR );
+	if ( option ) {
+		memcpy ( &dhcppkt->siaddr, &option->data,
+			 sizeof ( dhcppkt->siaddr ) );
+	}
+
+	/* Initialise option blocks */
+	init_dhcp_options ( &optblocks.options[OPTS_MAIN], dhcppkt->options,
+			    ( max_len -
+			      offsetof ( typeof ( *dhcppkt ), options ) ) );
+	init_dhcp_options ( &optblocks.options[OPTS_FILE], dhcppkt->file,
+			    sizeof ( dhcppkt->file ) );
+	init_dhcp_options ( &optblocks.options[OPTS_SNAME], dhcppkt->sname,
+			    sizeof ( dhcppkt->sname ) );
+	set_dhcp_option ( &optblocks.options[OPTS_MAIN], DHCP_OPTION_OVERLOAD,
+			  &overloading, sizeof ( overloading ) );
+
+	/* Populate option blocks */
+	copy_dhcp_options_to_packet ( &optblocks, 0 );
+
+	return ( offsetof ( typeof ( *dhcppkt ), options )
+		 + optblocks.options[OPTS_MAIN].len );
+}
+
 /**
  * Calculate used length of a field containing DHCP options
  *
@@ -107,6 +299,10 @@ struct dhcp_option_block * dhcp_parse ( const void *data, size_t len ) {
 	size_t options_len;
 	unsigned int overloading;
 
+	/* Sanity check */
+	if ( len < sizeof ( *dhcppkt ) )
+		return NULL;
+
 	/* Calculate size of resulting concatenated option block:
 	 *
 	 *   The "options" field : length of the field minus the DHCP_END tag.