[undi] Allow underlying PXE stack to construct link-layer header

Some PXE stacks (observed with a QLogic 8242) will always try to
prepend a link-layer header, even if the caller uses P_UNKNOWN to
indicate that the link-layer header has already been filled in.  This
results in an invalid packet being transmitted.

Work around these faulty PXE stacks where possible by stripping the
existing link-layer header and allowing the PXE stack to (re)construct
the link-layer header itself.

Originally-fixed-by: Buck Huppmann <buckh@pobox.com>
Signed-off-by: Michael Brown <mcb30@ipxe.org>

src/arch/i386/drivers/net/undinet.c

@@ -20,6 +20,7 @@ FILE_LICENCE ( GPL2_OR_LATER );
 
 #include <string.h>
 #include <unistd.h>
+#include <byteswap.h>
 #include <pxe.h>
 #include <realmode.h>
 #include <pic8259.h>
@@ -166,6 +167,10 @@ static int undinet_isr_triggered ( void ) {
 static struct s_PXENV_UNDI_TBD __data16 ( undinet_tbd );
 #define undinet_tbd __use_data16 ( undinet_tbd )
 
+/** UNDI transmit destination address */
+static uint8_t __data16_array ( undinet_destaddr, [ETH_ALEN] );
+#define undinet_destaddr __use_data16 ( undinet_destaddr )
+
 /**
  * Transmit packet
  *
@@ -175,8 +180,14 @@ static struct s_PXENV_UNDI_TBD __data16 ( undinet_tbd );
  */
 static int undinet_transmit ( struct net_device *netdev,
 			      struct io_buffer *iobuf ) {
+	struct undi_nic *undinic = netdev->priv;
 	struct s_PXENV_UNDI_TRANSMIT undi_transmit;
-	size_t len = iob_len ( iobuf );
+	const void *ll_dest;
+	const void *ll_source;
+	uint16_t net_proto;
+	unsigned int flags;
+	uint8_t protocol;
+	size_t len;
 	int rc;
 
 	/* Technically, we ought to make sure that the previous
@@ -189,15 +200,49 @@ static int undinet_transmit ( struct net_device *netdev,
 	 * transmit the next packet.
 	 */
 
+	/* Some PXE stacks are unable to cope with P_UNKNOWN, and will
+	 * always try to prepend a link-layer header.  Work around
+	 * these stacks by stripping the existing link-layer header
+	 * and allowing the PXE stack to (re)construct the link-layer
+	 * header itself.
+	 */
+	if ( ( rc = eth_pull ( netdev, iobuf, &ll_dest, &ll_source,
+			       &net_proto, &flags ) ) != 0 ) {
+		DBGC ( undinic, "UNDINIC %p could not strip Ethernet header: "
+		       "%s\n", undinic, strerror ( rc ) );
+		return rc;
+	}
+	memcpy ( undinet_destaddr, ll_dest, sizeof ( undinet_destaddr ) );
+	switch ( net_proto ) {
+	case htons ( ETH_P_IP ) :
+		protocol = P_IP;
+		break;
+	case htons ( ETH_P_ARP ) :
+		protocol = P_ARP;
+		break;
+	case htons ( ETH_P_RARP ) :
+		protocol = P_RARP;
+		break;
+	default:
+		/* Unknown protocol; restore the original link-layer header */
+		iob_push ( iobuf, sizeof ( struct ethhdr ) );
+		protocol = P_UNKNOWN;
+		break;
+	}
+
 	/* Copy packet to UNDI I/O buffer */
+	len = iob_len ( iobuf );
 	if ( len > sizeof ( basemem_packet ) )
 		len = sizeof ( basemem_packet );
 	memcpy ( &basemem_packet, iobuf->data, len );
 
 	/* Create PXENV_UNDI_TRANSMIT data structure */
 	memset ( &undi_transmit, 0, sizeof ( undi_transmit ) );
+	undi_transmit.Protocol = protocol;
+	undi_transmit.XmitFlag = ( ( flags & LL_BROADCAST ) ?
+				   XMT_BROADCAST : XMT_DESTADDR );
 	undi_transmit.DestAddr.segment = rm_ds;
-	undi_transmit.DestAddr.offset = __from_data16 ( &undinet_tbd );
+	undi_transmit.DestAddr.offset = __from_data16 ( &undinet_destaddr );
 	undi_transmit.TBD.segment = rm_ds;
 	undi_transmit.TBD.offset = __from_data16 ( &undinet_tbd );
 

src/include/ipxe/ethernet.h

@@ -10,6 +10,8 @@
 FILE_LICENCE ( GPL2_OR_LATER );
 
 #include <stdint.h>
+#include <ipxe/netdevice.h>
+#include <ipxe/iobuf.h>
 
 /**
  * Check if Ethernet address is all zeroes
@@ -77,6 +79,12 @@ static inline int is_valid_ether_addr ( const void *addr ) {
 		 ( ! is_zero_ether_addr ( addr ) ) );
 }
 
+extern int eth_push ( struct net_device *netdev, struct io_buffer *iobuf,
+		      const void *ll_dest, const void *ll_source,
+		      uint16_t net_proto );
+extern int eth_pull ( struct net_device *netdev, struct io_buffer *iobuf,
+		      const void **ll_dest, const void **ll_source,
+		      uint16_t *net_proto, unsigned int *flags );
 extern void eth_init_addr ( const void *hw_addr, void *ll_addr );
 extern const char * eth_ntoa ( const void *ll_addr );
 extern int eth_mc_hash ( unsigned int af, const void *net_addr,

src/net/ethernet.c

@@ -50,9 +50,9 @@ static uint8_t eth_broadcast[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
  * @v net_proto		Network-layer protocol, in network-byte order
  * @ret rc		Return status code
  */
-static int eth_push ( struct net_device *netdev __unused,
+int eth_push ( struct net_device *netdev __unused, struct io_buffer *iobuf,
-		      struct io_buffer *iobuf, const void *ll_dest,
+	       const void *ll_dest, const void *ll_source,
-		      const void *ll_source, uint16_t net_proto ) {
+	       uint16_t net_proto ) {
 	struct ethhdr *ethhdr = iob_push ( iobuf, sizeof ( *ethhdr ) );
 
 	/* Build Ethernet header */
@@ -74,10 +74,9 @@ static int eth_push ( struct net_device *netdev __unused,
  * @ret flags		Packet flags
  * @ret rc		Return status code
  */
-static int eth_pull ( struct net_device *netdev __unused, 
+int eth_pull ( struct net_device *netdev __unused, struct io_buffer *iobuf,
-		      struct io_buffer *iobuf, const void **ll_dest,
+	       const void **ll_dest, const void **ll_source,
-		      const void **ll_source, uint16_t *net_proto,
+	       uint16_t *net_proto, unsigned int *flags ) {
-		      unsigned int *flags ) {
 	struct ethhdr *ethhdr = iobuf->data;
 
 	/* Sanity check */