[efi] Include VLAN in SNP device path if applicable

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

diff --git a/src/interface/efi/efi_snp.c b/src/interface/efi/efi_snp.c
index 4508967e628940872fa45701fc625b09d2d79f5b..a6e94c00b5c4408d43af7cbfb6011f3569fdc92b 100644 (file)
--- a/src/interface/efi/efi_snp.c
+++ b/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;