[efi] Include VLAN in SNP device path if applicable

Signed-off-by: Michael Brown <mcb30@ipxe.org>

src/interface/efi/efi_snp.c

@@ -25,6 +25,7 @@ FILE_LICENCE ( GPL2_OR_LATER );
 #include <assert.h>
 #include <byteswap.h>
 #include <ipxe/netdevice.h>
+#include <ipxe/vlan.h>
 #include <ipxe/iobuf.h>
 #include <ipxe/in.h>
 #include <ipxe/version.h>
@@ -1545,8 +1546,10 @@ static int efi_snp_probe ( struct net_device *netdev ) {
 	struct efi_snp_device *snpdev;
 	EFI_DEVICE_PATH_PROTOCOL *path_end;
 	MAC_ADDR_DEVICE_PATH *macpath;
+	VLAN_DEVICE_PATH *vlanpath;
 	size_t path_prefix_len = 0;
 	unsigned int ifcnt;
+	unsigned int tag;
 	void *interface;
 	EFI_STATUS efirc;
 	int rc;
@@ -1634,7 +1637,7 @@ static int efi_snp_probe ( struct net_device *netdev ) {
 	/* Allocate the new device path */
 	path_prefix_len = efi_devpath_len ( efidev->path );
 	snpdev->path = zalloc ( path_prefix_len + sizeof ( *macpath ) +
-				sizeof ( *path_end ) );
+				sizeof ( *vlanpath ) + sizeof ( *path_end ) );
 	if ( ! snpdev->path ) {
 		rc = -ENOMEM;
 		goto err_alloc_device_path;
@@ -1643,7 +1646,6 @@ static int efi_snp_probe ( struct net_device *netdev ) {
 	/* Populate the device path */
 	memcpy ( snpdev->path, efidev->path, path_prefix_len );
 	macpath = ( ( ( void * ) snpdev->path ) + path_prefix_len );
-	path_end = ( ( void * ) ( macpath + 1 ) );
 	memset ( macpath, 0, sizeof ( *macpath ) );
 	macpath->Header.Type = MESSAGING_DEVICE_PATH;
 	macpath->Header.SubType = MSG_MAC_ADDR_DP;
@@ -1651,6 +1653,17 @@ static int efi_snp_probe ( struct net_device *netdev ) {
 	memcpy ( &macpath->MacAddress, netdev->ll_addr,
 		 sizeof ( macpath->MacAddress ) );
 	macpath->IfType = ntohs ( netdev->ll_protocol->ll_proto );
+	if ( ( tag = vlan_tag ( netdev ) ) ) {
+		vlanpath = ( ( ( void * ) macpath ) + sizeof ( *macpath ) );
+		memset ( vlanpath, 0, sizeof ( *vlanpath ) );
+		vlanpath->Header.Type = MESSAGING_DEVICE_PATH;
+		vlanpath->Header.SubType = MSG_VLAN_DP;
+		vlanpath->Header.Length[0] = sizeof ( *vlanpath );
+		vlanpath->VlanId = tag;
+		path_end = ( ( ( void * ) vlanpath ) + sizeof ( *vlanpath ) );
+	} else {
+		path_end = ( ( ( void * ) macpath ) + sizeof ( *macpath ) );
+	}
 	memset ( path_end, 0, sizeof ( *path_end ) );
 	path_end->Type = END_DEVICE_PATH_TYPE;
 	path_end->SubType = END_ENTIRE_DEVICE_PATH_SUBTYPE;