CORD-906 Support trunk and native VLAN

- Include trunk L2IG in L2F
- Populate bridging rules for trunk vlan
- Extend populateVlanMacFilters to generate filtering obj for trunk port
- Extend host handler to check vlan mismatch between host and interface
  (Temporarily disabled for now. Check TODOs in the code for detail.)
- Extend getForwardingObjectiveBuilder in RoutingRulePopulator to support tagged host

Change-Id: Id168a02015f58b0957ba43ad7c52798029d895bc
diff --git a/src/main/java/org/onosproject/segmentrouting/HostHandler.java b/src/main/java/org/onosproject/segmentrouting/HostHandler.java
index 45dd4e9..12d6d78 100644
--- a/src/main/java/org/onosproject/segmentrouting/HostHandler.java
+++ b/src/main/java/org/onosproject/segmentrouting/HostHandler.java
@@ -108,7 +108,7 @@
                 // Populate IP table entry
                 if (srManager.deviceConfiguration.inSameSubnet(location, ip)) {
                     srManager.routingRulePopulator.populateRoute(
-                            deviceId, ip.toIpPrefix(), mac, port);
+                            deviceId, ip.toIpPrefix(), mac, vlanId, port);
                 }
             });
         }
@@ -145,7 +145,7 @@
             ips.forEach(ip -> {
                 if (srManager.deviceConfiguration.inSameSubnet(location, ip)) {
                     srManager.routingRulePopulator.revokeRoute(
-                            deviceId, ip.toIpPrefix(), mac, port);
+                            deviceId, ip.toIpPrefix(), mac, vlanId, port);
                 }
             });
         }
@@ -183,7 +183,7 @@
             prevIps.forEach(ip -> {
                 if (srManager.deviceConfiguration.inSameSubnet(prevLocation, ip)) {
                     srManager.routingRulePopulator.revokeRoute(
-                            prevDeviceId, ip.toIpPrefix(), mac, prevPort);
+                            prevDeviceId, ip.toIpPrefix(), mac, vlanId, prevPort);
                 }
             });
         }
@@ -206,7 +206,7 @@
             newIps.forEach(ip -> {
                 if (srManager.deviceConfiguration.inSameSubnet(newLocation, ip)) {
                     srManager.routingRulePopulator.populateRoute(
-                            newDeviceId, ip.toIpPrefix(), mac, newPort);
+                            newDeviceId, ip.toIpPrefix(), mac, vlanId, newPort);
                 }
             });
         }
@@ -231,7 +231,7 @@
                 if (srManager.deviceConfiguration.inSameSubnet(prevLocation, ip)) {
                     log.info("revoking previous IP rule:{}", ip);
                     srManager.routingRulePopulator.revokeRoute(
-                            prevDeviceId, ip.toIpPrefix(), mac, prevPort);
+                            prevDeviceId, ip.toIpPrefix(), mac, vlanId, prevPort);
                 }
             });
         }
@@ -242,7 +242,7 @@
                 if (srManager.deviceConfiguration.inSameSubnet(newLocation, ip)) {
                     log.info("populating new IP rule:{}", ip);
                     srManager.routingRulePopulator.populateRoute(
-                            newDeviceId, ip.toIpPrefix(), mac, newPort);
+                            newDeviceId, ip.toIpPrefix(), mac, vlanId, newPort);
                 }
             });
         }
@@ -256,43 +256,66 @@
      *
      * @param deviceId Device that host attaches to
      * @param mac MAC address of the host
-     * @param vlanId VLAN ID of the host
+     * @param hostVlanId VLAN ID of the host
      * @param outport Port that host attaches to
      * @return Forwarding objective builder
      */
     private ForwardingObjective.Builder bridgingFwdObjBuilder(
-            DeviceId deviceId, MacAddress mac, VlanId vlanId,
+            DeviceId deviceId, MacAddress mac, VlanId hostVlanId,
             PortNumber outport) {
-        VlanId untaggedVlan = srManager.getUntaggedVlanId(new ConnectPoint(deviceId, outport));
-        VlanId outvlan = (untaggedVlan != null) ? untaggedVlan : INTERNAL_VLAN;
+        ConnectPoint connectPoint = new ConnectPoint(deviceId, outport);
+        VlanId untaggedVlan = srManager.getUntaggedVlanId(connectPoint);
+        Set<VlanId> taggedVlans = srManager.getTaggedVlanId(connectPoint);
+        VlanId nativeVlan = srManager.getNativeVlanId(connectPoint);
 
-        // match rule
+        // Create host selector
         TrafficSelector.Builder sbuilder = DefaultTrafficSelector.builder();
         sbuilder.matchEthDst(mac);
-        /*
-         * Note: for untagged packets, match on the assigned VLAN.
-         *       for tagged packets, match on its incoming VLAN.
-         */
-        if (vlanId.equals(VlanId.NONE)) {
-            sbuilder.matchVlanId(outvlan);
-        } else {
-            sbuilder.matchVlanId(vlanId);
-        }
 
+        // Create host treatment
         TrafficTreatment.Builder tbuilder = DefaultTrafficTreatment.builder();
-        tbuilder.immediate().popVlan();
         tbuilder.immediate().setOutput(outport);
 
-        // for switch pipelines that need it, provide outgoing vlan as metadata
-        TrafficSelector meta = DefaultTrafficSelector.builder()
-                .matchVlanId(outvlan).build();
+        // Create host meta
+        TrafficSelector.Builder mbuilder = DefaultTrafficSelector.builder();
+
+        // Adjust the selector, treatment and meta according to VLAN configuration
+        if (taggedVlans.contains(hostVlanId)) {
+            sbuilder.matchVlanId(hostVlanId);
+            mbuilder.matchVlanId(hostVlanId);
+        } else if (hostVlanId.equals(VlanId.NONE)) {
+            if (untaggedVlan != null) {
+                sbuilder.matchVlanId(untaggedVlan);
+                mbuilder.matchVlanId(untaggedVlan);
+                tbuilder.immediate().popVlan();
+            } else if (nativeVlan != null) {
+                sbuilder.matchVlanId(nativeVlan);
+                mbuilder.matchVlanId(nativeVlan);
+                tbuilder.immediate().popVlan();
+            } else {
+                // TODO: This check is turned off for now since vRouter still assumes that
+                // hosts are internally tagged with INTERNAL_VLAN.
+                // We should turn this back on when we move forward to the bridging CPR approach.
+                //
+                //log.warn("Untagged host {}/{} is not allowed on {} without untagged or native vlan",
+                //        mac, hostVlanId, connectPoint);
+                //return null;
+                sbuilder.matchVlanId(INTERNAL_VLAN);
+                mbuilder.matchVlanId(INTERNAL_VLAN);
+                tbuilder.immediate().popVlan();
+            }
+        } else {
+            log.warn("Tagged host {}/{} is not allowed on {} without VLAN listed in tagged vlan",
+                    mac, hostVlanId, connectPoint);
+            return null;
+        }
 
         // All forwarding is via Groups. Drivers can re-purpose to flow-actions if needed.
         int portNextObjId = srManager.getPortNextObjectiveId(deviceId, outport,
                 tbuilder.build(),
-                meta);
+                mbuilder.build());
         if (portNextObjId == -1) {
-            // warning log will come from getPortNextObjective method
+            // Warning log will come from getPortNextObjective method
             return null;
         }
 
diff --git a/src/main/java/org/onosproject/segmentrouting/RouteHandler.java b/src/main/java/org/onosproject/segmentrouting/RouteHandler.java
index 6838af5..6fe9127 100644
--- a/src/main/java/org/onosproject/segmentrouting/RouteHandler.java
+++ b/src/main/java/org/onosproject/segmentrouting/RouteHandler.java
@@ -19,6 +19,7 @@
 import com.google.common.collect.ImmutableSet;
 import org.onlab.packet.IpPrefix;
 import org.onlab.packet.MacAddress;
+import org.onlab.packet.VlanId;
 import org.onosproject.incubator.net.routing.ResolvedRoute;
 import org.onosproject.incubator.net.routing.RouteEvent;
 import org.onosproject.net.ConnectPoint;
@@ -57,12 +58,15 @@
     private void processRouteAddedInternal(ResolvedRoute route) {
         IpPrefix prefix = route.prefix();
         MacAddress nextHopMac = route.nextHopMac();
+        // TODO ResolvedRoute does not contain VLAN information.
+        //      Therefore we only support untagged nexthop for now.
+        VlanId nextHopVlan = VlanId.NONE;
         ConnectPoint location = route.location();
 
         srManager.deviceConfiguration.addSubnet(location, prefix);
         srManager.defaultRoutingHandler.populateSubnet(location, ImmutableSet.of(prefix));
         srManager.routingRulePopulator.populateRoute(location.deviceId(), prefix,
-                nextHopMac, location.port());
+                nextHopMac, nextHopVlan, location.port());
     }
 
     protected void processRouteUpdated(RouteEvent event) {
@@ -79,11 +83,14 @@
     private void processRouteRemovedInternal(ResolvedRoute route) {
         IpPrefix prefix = route.prefix();
         MacAddress nextHopMac = route.nextHopMac();
+        // TODO ResolvedRoute does not contain VLAN information.
+        //      Therefore we only support untagged nexthop for now.
+        VlanId nextHopVlan = VlanId.NONE;
         ConnectPoint location = route.location();
 
         srManager.deviceConfiguration.removeSubnet(location, prefix);
         srManager.defaultRoutingHandler.revokeSubnet(ImmutableSet.of(prefix));
         srManager.routingRulePopulator.revokeRoute(
-                location.deviceId(), prefix, nextHopMac, location.port());
+                location.deviceId(), prefix, nextHopMac, nextHopVlan, location.port());
     }
 }
diff --git a/src/main/java/org/onosproject/segmentrouting/RoutingRulePopulator.java b/src/main/java/org/onosproject/segmentrouting/RoutingRulePopulator.java
index dae8a71..06a9dbc 100644
--- a/src/main/java/org/onosproject/segmentrouting/RoutingRulePopulator.java
+++ b/src/main/java/org/onosproject/segmentrouting/RoutingRulePopulator.java
@@ -25,7 +25,6 @@
 import org.onlab.packet.MacAddress;
 import org.onlab.packet.MplsLabel;
 import org.onlab.packet.VlanId;
-import org.onosproject.incubator.net.intf.Interface;
 import org.onosproject.net.ConnectPoint;
 import org.onosproject.net.flowobjective.DefaultObjectiveContext;
 import org.onosproject.net.flowobjective.Objective;
@@ -61,7 +60,6 @@
 import java.util.Optional;
 import java.util.Set;
 import java.util.concurrent.atomic.AtomicLong;
-import java.util.stream.Collectors;
 
 import static com.google.common.base.Preconditions.checkNotNull;
 import static org.onlab.packet.Ethernet.TYPE_ARP;
@@ -115,16 +113,16 @@
      * @param deviceId device ID of the device that next hop attaches to
      * @param prefix IP prefix of the route
      * @param hostMac MAC address of the next hop
+     * @param hostVlanId Vlan ID of the nexthop
      * @param outPort port where the next hop attaches to
      */
     public void populateRoute(DeviceId deviceId, IpPrefix prefix,
-                                      MacAddress hostMac, PortNumber outPort) {
+                              MacAddress hostMac, VlanId hostVlanId, PortNumber outPort) {
         log.debug("Populate IP table entry for route {} at {}:{}",
                 prefix, deviceId, outPort);
         ForwardingObjective.Builder fwdBuilder;
         try {
-            fwdBuilder = routingFwdObjBuilder(
-                    deviceId, prefix, hostMac, outPort);
+            fwdBuilder = routingFwdObjBuilder(deviceId, prefix, hostMac, hostVlanId, outPort);
         } catch (DeviceConfigNotFoundException e) {
             log.warn(e.getMessage() + " Aborting populateIpRuleForHost.");
             return;
@@ -148,16 +146,16 @@
      * @param deviceId device ID of the device that next hop attaches to
      * @param prefix IP prefix of the route
      * @param hostMac MAC address of the next hop
+     * @param hostVlanId Vlan ID of the nexthop
      * @param outPort port that next hop attaches to
      */
     public void revokeRoute(DeviceId deviceId, IpPrefix prefix,
-            MacAddress hostMac, PortNumber outPort) {
+            MacAddress hostMac, VlanId hostVlanId, PortNumber outPort) {
         log.debug("Revoke IP table entry for route {} at {}:{}",
                 prefix, deviceId, outPort);
         ForwardingObjective.Builder fwdBuilder;
         try {
-            fwdBuilder = routingFwdObjBuilder(
-                    deviceId, prefix, hostMac, outPort);
+            fwdBuilder = routingFwdObjBuilder(deviceId, prefix, hostMac, hostVlanId, outPort);
         } catch (DeviceConfigNotFoundException e) {
             log.warn(e.getMessage() + " Aborting revokeIpRuleForHost.");
             return;
@@ -183,42 +181,69 @@
      * @param deviceId device ID
      * @param prefix prefix that need to be routed
      * @param hostMac MAC address of the nexthop
+     * @param hostVlanId Vlan ID of the nexthop
      * @param outPort port where the nexthop attaches to
      * @return forwarding objective builder
      * @throws DeviceConfigNotFoundException if given device is not configured
      */
     private ForwardingObjective.Builder routingFwdObjBuilder(
             DeviceId deviceId, IpPrefix prefix,
-            MacAddress hostMac, PortNumber outPort)
+            MacAddress hostMac, VlanId hostVlanId, PortNumber outPort)
             throws DeviceConfigNotFoundException {
         MacAddress deviceMac;
         deviceMac = config.getDeviceMac(deviceId);
 
-        TrafficSelector.Builder sbuilder = buildIpSelectorFromIpPrefix(prefix);
-        TrafficSelector selector = sbuilder.build();
+        ConnectPoint connectPoint = new ConnectPoint(deviceId, outPort);
+        VlanId untaggedVlan = srManager.getUntaggedVlanId(connectPoint);
+        Set<VlanId> taggedVlans = srManager.getTaggedVlanId(connectPoint);
+        VlanId nativeVlan = srManager.getNativeVlanId(connectPoint);
 
+        // Create route selector
+        TrafficSelector.Builder sbuilder = buildIpSelectorFromIpPrefix(prefix);
+
+        // Create route treatment
         TrafficTreatment.Builder tbuilder = DefaultTrafficTreatment.builder();
         tbuilder.deferred()
                 .setEthDst(hostMac)
                 .setEthSrc(deviceMac)
                 .setOutput(outPort);
-        TrafficTreatment treatment = tbuilder.build();
 
-        // All forwarding is via Groups. Drivers can re-purpose to flow-actions if needed.
-        // for switch pipelines that need it, provide outgoing vlan as metadata
-        VlanId untaggedVlan = srManager.getUntaggedVlanId(new ConnectPoint(deviceId, outPort));
-        VlanId outvlan = (untaggedVlan != null) ? untaggedVlan : INTERNAL_VLAN;
+        // Create route meta
+        TrafficSelector.Builder mbuilder = DefaultTrafficSelector.builder();
 
-        TrafficSelector meta = DefaultTrafficSelector.builder()
-                                    .matchVlanId(outvlan).build();
-        int portNextObjId = srManager.getPortNextObjectiveId(deviceId, outPort,
-                                                             treatment, meta);
-        if (portNextObjId == -1) {
-            // warning log will come from getPortNextObjective method
+        // Adjust the meta according to VLAN configuration
+        if (taggedVlans.contains(hostVlanId)) {
+            tbuilder.setVlanId(hostVlanId);
+        } else if (hostVlanId.equals(VlanId.NONE)) {
+            if (untaggedVlan != null) {
+                mbuilder.matchVlanId(untaggedVlan);
+            } else if (nativeVlan != null) {
+                mbuilder.matchVlanId(nativeVlan);
+            } else {
+                // TODO: This check is turned off for now since vRouter still assumes that
+                // hosts are internally tagged with INTERNAL_VLAN.
+                // We should turn this back on when we move forward to the bridging CPR approach.
+                //
+                //log.warn("Untagged nexthop {}/{} is not allowed on {} without untagged or native vlan",
+                //        hostMac, hostVlanId, connectPoint);
+                //return null;
+                mbuilder.matchVlanId(INTERNAL_VLAN);
+            }
+        } else {
+            log.warn("Tagged nexthop {}/{} is not allowed on {} without VLAN listed in tagged vlan",
+                    hostMac, hostVlanId, connectPoint);
             return null;
         }
+
+        int portNextObjId = srManager.getPortNextObjectiveId(deviceId, outPort,
+                                                             tbuilder.build(), mbuilder.build());
+        if (portNextObjId == -1) {
+            // Warning log will come from getPortNextObjective method
+            return null;
+        }
+
         return DefaultForwardingObjective.builder()
-                .withSelector(selector)
+                .withSelector(sbuilder.build())
                 .nextStep(portNextObjId)
                 .fromApp(srManager.appId).makePermanent()
                 .withPriority(getPriorityFromPrefix(prefix))
@@ -638,7 +663,40 @@
      * @return true if no errors occurred during the build of the filtering objective
      */
     public boolean populateSinglePortFilters(DeviceId deviceId, PortNumber portnum) {
-        FilteringObjective.Builder fob = buildFilteringObjective(deviceId, portnum);
+        ConnectPoint connectPoint = new ConnectPoint(deviceId, portnum);
+        VlanId untaggedVlan = srManager.getUntaggedVlanId(connectPoint);
+        Set<VlanId> taggedVlans = srManager.getTaggedVlanId(connectPoint);
+        VlanId nativeVlan = srManager.getNativeVlanId(connectPoint);
+
+        if (taggedVlans.size() != 0) {
+            // Filter for tagged vlans
+            if (!srManager.getTaggedVlanId(connectPoint).stream().allMatch(taggedVlanId ->
+                populateSinglePortFiltersInternal(deviceId, portnum, false, taggedVlanId))) {
+                return false;
+            }
+            if (nativeVlan != null) {
+                // Filter for native vlan
+                if (!populateSinglePortFiltersInternal(deviceId, portnum, true, nativeVlan)) {
+                    return false;
+                }
+            }
+        } else if (untaggedVlan != null) {
+            // Filter for untagged vlan
+            if (!populateSinglePortFiltersInternal(deviceId, portnum, true, untaggedVlan)) {
+                return false;
+            }
+        } else {
+            // Unconfigure port, use INTERNAL_VLAN
+            if (!populateSinglePortFiltersInternal(deviceId, portnum, true, INTERNAL_VLAN)) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private boolean populateSinglePortFiltersInternal(DeviceId deviceId, PortNumber portnum,
+                                                      boolean pushVlan, VlanId vlanId) {
+        FilteringObjective.Builder fob = buildFilteringObjective(deviceId, portnum, pushVlan, vlanId);
         if (fob == null) {
             // error encountered during build
             return false;
@@ -659,12 +717,43 @@
      *
      * @param deviceId device identifier
      * @param portnum port identifier
+     * @return true if no errors occurred during the destruction of the filtering objective
      */
-    public void revokeSinglePortFilters(DeviceId deviceId, PortNumber portnum) {
-        FilteringObjective.Builder fob = buildFilteringObjective(deviceId, portnum);
+    public boolean revokeSinglePortFilters(DeviceId deviceId, PortNumber portnum) {
+        ConnectPoint connectPoint = new ConnectPoint(deviceId, portnum);
+        VlanId untaggedVlan = srManager.getUntaggedVlanId(connectPoint);
+        Set<VlanId> taggedVlans = srManager.getTaggedVlanId(connectPoint);
+        VlanId nativeVlan = srManager.getNativeVlanId(connectPoint);
+
+        if (taggedVlans.size() != 0) {
+            // Filter for tagged vlans
+            if (!srManager.getTaggedVlanId(connectPoint).stream().allMatch(taggedVlanId ->
+                    revokeSinglePortFiltersInternal(deviceId, portnum, false, taggedVlanId))) {
+                return false;
+            }
+            // Filter for native vlan
+            if (nativeVlan == null ||
+                    !revokeSinglePortFiltersInternal(deviceId, portnum, true, nativeVlan)) {
+                return false;
+            }
+        // Filter for untagged vlan
+        } else if (untaggedVlan == null ||
+                !revokeSinglePortFiltersInternal(deviceId, portnum, true, untaggedVlan)) {
+            return false;
+        // Unconfigure port, use INTERNAL_VLAN
+        } else if (!revokeSinglePortFiltersInternal(deviceId, portnum, true, INTERNAL_VLAN)) {
+            return false;
+        }
+
+        return true;
+    }
+
+    private boolean revokeSinglePortFiltersInternal(DeviceId deviceId, PortNumber portnum,
+                                                    boolean pushVlan, VlanId vlanId) {
+        FilteringObjective.Builder fob = buildFilteringObjective(deviceId, portnum, pushVlan, vlanId);
         if (fob == null) {
             // error encountered during build
-            return;
+            return false;
         }
         log.info("Removing filtering objectives for dev/port:{}/{}", deviceId, portnum);
         ObjectiveContext context = new DefaultObjectiveContext(
@@ -673,11 +762,11 @@
             log.warn("Failed to remove filter for {}/{}: {}",
                      deviceId, portnum, error));
         srManager.flowObjectiveService.filter(deviceId, fob.remove(context));
-        return;
+        return true;
     }
 
-    private FilteringObjective.Builder buildFilteringObjective(DeviceId deviceId,
-                                                               PortNumber portnum) {
+    private FilteringObjective.Builder buildFilteringObjective(DeviceId deviceId, PortNumber portnum,
+                                                               boolean pushVlan, VlanId vlanId) {
         MacAddress deviceMac;
         try {
             deviceMac = config.getDeviceMac(deviceId);
@@ -688,17 +777,20 @@
         // suppressed ports still have filtering rules pushed by SR using default vlan
         ConnectPoint connectPoint = new ConnectPoint(deviceId, portnum);
 
-        VlanId untaggedVlan = srManager.getUntaggedVlanId(connectPoint);
-        VlanId assignedVlan = (untaggedVlan != null) ? untaggedVlan : INTERNAL_VLAN;
-
         FilteringObjective.Builder fob = DefaultFilteringObjective.builder();
         fob.withKey(Criteria.matchInPort(portnum))
             .addCondition(Criteria.matchEthDst(deviceMac))
-            .addCondition(Criteria.matchVlanId(VlanId.NONE))
             .withPriority(SegmentRoutingService.DEFAULT_PRIORITY);
-        TrafficTreatment tt = DefaultTrafficTreatment.builder()
-                .pushVlan().setVlanId(assignedVlan).build();
-        fob.withMeta(tt);
+
+        if (pushVlan) {
+            fob.addCondition(Criteria.matchVlanId(VlanId.NONE));
+            TrafficTreatment tt = DefaultTrafficTreatment.builder()
+                    .pushVlan().setVlanId(vlanId).build();
+            fob.withMeta(tt);
+        } else {
+            fob.addCondition(Criteria.matchVlanId(vlanId));
+        }
+
         fob.permit().fromApp(srManager.appId);
         return fob;
     }
@@ -852,15 +944,7 @@
      * @param deviceId switch ID to set the rules
      */
     public void populateSubnetBroadcastRule(DeviceId deviceId) {
-        Set<Interface> interfaces = srManager.interfaceService.getInterfaces();
-        Set<VlanId> vlans =
-                interfaces.stream()
-                        .filter(intf -> intf.connectPoint().deviceId().equals(deviceId) &&
-                                !intf.vlanUntagged().equals(VlanId.NONE))
-                        .map(Interface::vlanUntagged)
-                        .collect(Collectors.toSet());
-
-        vlans.forEach(vlanId -> {
+        srManager.getVlanPortMap(deviceId).asMap().forEach((vlanId, ports) -> {
             int nextId = srManager.getVlanNextObjectiveId(deviceId, vlanId);
 
             if (nextId < 0) {
diff --git a/src/main/java/org/onosproject/segmentrouting/SegmentRoutingManager.java b/src/main/java/org/onosproject/segmentrouting/SegmentRoutingManager.java
index 0c30cef..3c801b7 100644
--- a/src/main/java/org/onosproject/segmentrouting/SegmentRoutingManager.java
+++ b/src/main/java/org/onosproject/segmentrouting/SegmentRoutingManager.java
@@ -15,7 +15,9 @@
  */
 package org.onosproject.segmentrouting;
 
+import com.google.common.collect.HashMultimap;
 import com.google.common.collect.Maps;
+import com.google.common.collect.Multimap;
 import org.apache.felix.scr.annotations.Activate;
 import org.apache.felix.scr.annotations.Component;
 import org.apache.felix.scr.annotations.Deactivate;
@@ -110,6 +112,7 @@
 import java.util.concurrent.ScheduledExecutorService;
 import java.util.concurrent.ScheduledFuture;
 import java.util.concurrent.TimeUnit;
+import java.util.stream.Collectors;
 
 import static com.google.common.base.Preconditions.checkState;
 import static org.onlab.packet.Ethernet.TYPE_ARP;
@@ -519,8 +522,12 @@
         return tunnelHandler.getTunnel(tunnelId);
     }
 
+    // TODO Consider moving these to InterfaceService
     /**
      * Returns untagged VLAN configured on given connect point.
+     * <p>
+     * Only returns the first match if there are multiple untagged VLAN configured
+     * on the connect point.
      *
      * @param connectPoint connect point
      * @return untagged VLAN or null if not configured
@@ -533,6 +540,64 @@
     }
 
     /**
+     * Returns tagged VLAN configured on given connect point.
+     * <p>
+     * Returns all matches if there are multiple tagged VLAN configured
+     * on the connect point.
+     *
+     * @param connectPoint connect point
+     * @return tagged VLAN or empty set if not configured
+     */
+    public Set<VlanId> getTaggedVlanId(ConnectPoint connectPoint) {
+        Set<Interface> interfaces = interfaceService.getInterfacesByPort(connectPoint);
+        return interfaces.stream()
+                .map(Interface::vlanTagged)
+                .flatMap(vlanIds -> vlanIds.stream())
+                .collect(Collectors.toSet());
+    }
+
+    /**
+     * Returns native VLAN configured on given connect point.
+     * <p>
+     * Only returns the first match if there are multiple native VLAN configured
+     * on the connect point.
+     *
+     * @param connectPoint connect point
+     * @return native VLAN or null if not configured
+     */
+    public VlanId getNativeVlanId(ConnectPoint connectPoint) {
+        Set<Interface> interfaces = interfaceService.getInterfacesByPort(connectPoint);
+        return interfaces.stream()
+                .filter(intf -> !intf.vlanNative().equals(VlanId.NONE))
+                .map(Interface::vlanNative)
+                .findFirst()
+                .orElse(null);
+    }
+
+    /**
+     * Returns vlan port map of given device.
+     *
+     * @param deviceId device id
+     * @return vlan-port multimap
+     */
+    public Multimap<VlanId, PortNumber> getVlanPortMap(DeviceId deviceId) {
+        HashMultimap<VlanId, PortNumber> vlanPortMap = HashMultimap.create();
+
+        interfaceService.getInterfaces().stream()
+                .filter(intf -> intf.connectPoint().deviceId().equals(deviceId))
+                .forEach(intf -> {
+                    vlanPortMap.put(intf.vlanUntagged(), intf.connectPoint().port());
+                    intf.vlanTagged().forEach(vlanTagged -> {
+                        vlanPortMap.put(vlanTagged, intf.connectPoint().port());
+                    });
+                    vlanPortMap.put(intf.vlanNative(), intf.connectPoint().port());
+                });
+        vlanPortMap.removeAll(VlanId.NONE);
+
+        return vlanPortMap;
+    }
+
+    /**
      * Returns the next objective ID for the given NeighborSet.
      * If the nextObjective does not exist, a new one is created and
      * its id is returned.
diff --git a/src/main/java/org/onosproject/segmentrouting/grouphandler/DefaultGroupHandler.java b/src/main/java/org/onosproject/segmentrouting/grouphandler/DefaultGroupHandler.java
index 2b1a8c2..9e7fd04 100644
--- a/src/main/java/org/onosproject/segmentrouting/grouphandler/DefaultGroupHandler.java
+++ b/src/main/java/org/onosproject/segmentrouting/grouphandler/DefaultGroupHandler.java
@@ -49,6 +49,7 @@
 
 import java.net.URI;
 import java.util.ArrayList;
+import java.util.Collection;
 import java.util.Collections;
 import java.util.HashSet;
 import java.util.List;
@@ -737,20 +738,8 @@
      */
     public void createGroupsFromVlanConfig() {
         Set<Interface> interfaces = srManager.interfaceService.getInterfaces();
-        Set<VlanId> vlans =
-        interfaces.stream()
-                .filter(intf -> intf.connectPoint().deviceId().equals(deviceId) &&
-                        !intf.vlanUntagged().equals(VlanId.NONE))
-                .map(Interface::vlanUntagged)
-                .collect(Collectors.toSet());
 
-        vlans.forEach(vlanId -> {
-            Set<PortNumber> ports = interfaces.stream()
-                    .filter(intf -> intf.connectPoint().deviceId().equals(deviceId) &&
-                            intf.vlanUntagged().equals(vlanId))
-                    .map(Interface::connectPoint)
-                    .map(ConnectPoint::port)
-                    .collect(Collectors.toSet());
+        srManager.getVlanPortMap(deviceId).asMap().forEach((vlanId, ports) -> {
             createBcastGroupFromVlan(vlanId, ports);
         });
     }
@@ -761,7 +750,7 @@
      * @param vlanId vlan id
      * @param ports list of ports in the subnet
      */
-    public void createBcastGroupFromVlan(VlanId vlanId, Set<PortNumber> ports) {
+    public void createBcastGroupFromVlan(VlanId vlanId, Collection<PortNumber> ports) {
         VlanNextObjectiveStoreKey key = new VlanNextObjectiveStoreKey(deviceId, vlanId);
 
         if (vlanNextObjStore.containsKey(key)) {
@@ -782,7 +771,9 @@
 
         ports.forEach(port -> {
             TrafficTreatment.Builder tBuilder = DefaultTrafficTreatment.builder();
-            tBuilder.popVlan();
+            if (toPopVlan(port, vlanId)) {
+                tBuilder.popVlan();
+            }
             tBuilder.setOutput(port);
             nextObjBuilder.addTreatment(tBuilder.build());
         });
@@ -796,6 +787,18 @@
     }
 
     /**
+     * Determine if we should pop given vlan before sending packets to the given port.
+     *
+     * @param portNumber port number
+     * @param vlanId vlan id
+     * @return true if the vlan id is not contained in any vlanTagged config
+     */
+    private boolean toPopVlan(PortNumber portNumber, VlanId vlanId) {
+        return srManager.interfaceService.getInterfacesByPort(new ConnectPoint(deviceId, portNumber))
+                .stream().noneMatch(intf -> intf.vlanTagged().contains(vlanId));
+    }
+
+    /**
      * Create simple next objective for a single port. The treatments can include
      * all outgoing actions that need to happen on the packet.
      *