diff --git a/src/main/java/net/onrc/onos/apps/segmentrouting/ArpHandler.java b/src/main/java/net/onrc/onos/apps/segmentrouting/ArpHandler.java
index 9f0fa86..fc6c02e 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/ArpHandler.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/ArpHandler.java
@@ -8,10 +8,6 @@
 
 package net.onrc.onos.apps.segmentrouting;
 
-import java.net.Inet4Address;
-import java.net.InetAddress;
-import java.net.UnknownHostException;
-
 import net.floodlightcontroller.core.IFloodlightProviderService;
 import net.floodlightcontroller.core.module.FloodlightModuleContext;
 import net.floodlightcontroller.util.MACAddress;
@@ -149,54 +145,7 @@
         }
     }
 
-    /**
-     * The function checks if given IP matches to the given subnet mask
-     *
-     * @param addr - subnet address to match
-     * @param addr1 - IP address to check
-     * @return true if the IP address matches to the subnet, otherwise false
-     */
 
-    public static boolean netMatch(String addr, String addr1){ //addr is subnet address and addr1 is ip address. Function will return true, if addr1 is within addr(subnet)
-
-        String[] parts = addr.split("/");
-        String ip = parts[0];
-        int prefix;
-
-        if (parts.length < 2) {
-            prefix = 0;
-        } else {
-            prefix = Integer.parseInt(parts[1]);
-        }
-
-        Inet4Address a =null;
-        Inet4Address a1 =null;
-        try {
-            a = (Inet4Address) InetAddress.getByName(ip);
-            a1 = (Inet4Address) InetAddress.getByName(addr1);
-        } catch (UnknownHostException e){}
-
-        byte[] b = a.getAddress();
-        int ipInt = ((b[0] & 0xFF) << 24) |
-                         ((b[1] & 0xFF) << 16) |
-                         ((b[2] & 0xFF) << 8)  |
-                         ((b[3] & 0xFF) << 0);
-
-        byte[] b1 = a1.getAddress();
-        int ipInt1 = ((b1[0] & 0xFF) << 24) |
-                         ((b1[1] & 0xFF) << 16) |
-                         ((b1[2] & 0xFF) << 8)  |
-                         ((b1[3] & 0xFF) << 0);
-
-        int mask = ~((1 << (32 - prefix)) - 1);
-
-        if ((ipInt & mask) == (ipInt1 & mask)) {
-            return true;
-        }
-        else {
-            return false;
-        }
-}
 
 
 
diff --git a/src/main/java/net/onrc/onos/apps/segmentrouting/IcmpHandler.java b/src/main/java/net/onrc/onos/apps/segmentrouting/IcmpHandler.java
index 676e68a..8e58eac 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/IcmpHandler.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/IcmpHandler.java
@@ -19,6 +19,8 @@
 import net.onrc.onos.core.topology.Switch;
 import net.onrc.onos.core.util.SwitchPort;
 
+import org.json.JSONArray;
+import org.json.JSONException;
 import org.projectfloodlight.openflow.protocol.OFFactory;
 import org.projectfloodlight.openflow.protocol.OFMatchV3;
 import org.projectfloodlight.openflow.protocol.OFMessage;
@@ -29,6 +31,7 @@
 import org.projectfloodlight.openflow.protocol.oxm.OFOxmEthDst;
 import org.projectfloodlight.openflow.protocol.oxm.OFOxmEthSrc;
 import org.projectfloodlight.openflow.protocol.oxm.OFOxmEthType;
+import org.projectfloodlight.openflow.protocol.oxm.OFOxmInPort;
 import org.projectfloodlight.openflow.protocol.oxm.OFOxmIpv4DstMasked;
 import org.projectfloodlight.openflow.protocol.oxm.OFOxmMplsLabel;
 import org.projectfloodlight.openflow.protocol.oxm.OFOxmVlanVid;
@@ -54,7 +57,7 @@
             .getLogger(IcmpHandler.class);
 
     private IFlowPusherService flowPusher;
-    private boolean added;
+    private boolean controllerPortAllowed = false;
 
     private static final int TABLE_VLAN = 0;
     private static final int TABLE_TMAC = 1;
@@ -69,6 +72,9 @@
     private static final short SLASH_8_PRIORITY = (short) 0xf000;
     private static final short MIN_PRIORITY = 0x0;
 
+    private static final int ICMP_TYPE_ECHO = 0x08;
+    private static final int ICMP_TYPE_REPLY = 0x00;
+
 
     public IcmpHandler(FloodlightModuleContext context, SegmentRoutingManager manager) {
 
@@ -79,7 +85,6 @@
         this.mutableTopology = topologyService.getTopology();
 
         this.srManager = manager;
-        this.added = false;
 
         packetService.registerPacketListener(this);
 
@@ -101,9 +106,8 @@
                     String switchIpAddressStr = switchIpAddressSlash.substring(0, switchIpAddressSlash.indexOf('/'));
                     IPv4Address switchIpAddress = IPv4Address.of(switchIpAddressStr);
 
-                    if (((ICMP)ipv4.getPayload()).getIcmpType() == 0x08 &&
+                    if (((ICMP)ipv4.getPayload()).getIcmpType() == ICMP_TYPE_ECHO &&
                             destinationAddress == switchIpAddress.getInt()) {
-                        //addControlPortInVlanTable(sw);
                         sendICMPResponse(sw, inPort, payload);
                         return;
                     }
@@ -150,7 +154,7 @@
 
         ICMP icmpReply = (ICMP)icmpRequestIpv4.getPayload().clone();
         icmpReply.setIcmpCode((byte)0x00);
-        icmpReply.setIcmpType((byte) 0x00);
+        icmpReply.setIcmpType((byte) ICMP_TYPE_REPLY);
         icmpReply.setChecksum((short)0);
 
         icmpReplyIpv4.setPayload(icmpReply);
@@ -160,22 +164,23 @@
         icmpReplyEth.setDestinationMACAddress(icmpRequest.getSourceMACAddress());
         icmpReplyEth.setSourceMACAddress(icmpRequest.getDestinationMACAddress());
 
-        sendPacketOut(sw, icmpReplyEth, new SwitchPort(sw.getDpid(), inPort.getPortNumber()));
+        sendPacketOut(sw, icmpReplyEth, new SwitchPort(sw.getDpid(), inPort.getPortNumber()), false);
 
         log.debug("Send an ICMP response {}", icmpReplyIpv4.toString());
 
     }
 
-
-
     /**
-     * Send PACKET_OUT message with some actions
+     * Send PACKET_OUT message with actions
+     * If switches support OFPP_TABLE action, it sends out packet to TABLE port
+     * Otherwise, it sends the packet to the port the packet came from
+     * (in this case, MPLS label is added if the packet needs go through transit switches)
      *
      * @param sw  Switch the packet came from
      * @param packet Ethernet packet to send
      * @param switchPort port to send the packet
      */
-    private void sendPacketOut(Switch sw, Ethernet packet, SwitchPort switchPort) {
+    private void sendPacketOut(Switch sw, Ethernet packet, SwitchPort switchPort, boolean supportOfppTable) {
 
         boolean sameSubnet = false;
         IOFSwitch ofSwitch = floodlightProvider.getMasterSwitch(sw.getDpid().value());
@@ -183,31 +188,50 @@
 
         List<OFAction> actions = new ArrayList<>();
 
-        // Check if the destination is the host attached to the switch
-        int destinationAddress = ((IPv4)packet.getPayload()).getDestinationAddress();
-        for (net.onrc.onos.core.topology.Host host: mutableTopology.getHosts(switchPort)) {
-            IPv4Address hostIpAddress = IPv4Address.of(host.getIpAddress());
-            if (hostIpAddress != null && hostIpAddress.getInt() == destinationAddress) {
-                sameSubnet = true;
-                break;
+        // If OFPP_TABLE action is not supported in the switch, MPLS label needs to be set
+        // if the packet needs to be delivered crossing switches
+        if (!supportOfppTable) {
+            // Check if the destination is the host attached to the switch
+            int destinationAddress = ((IPv4)packet.getPayload()).getDestinationAddress();
+            for (net.onrc.onos.core.topology.Host host: mutableTopology.getHosts(switchPort)) {
+                IPv4Address hostIpAddress = IPv4Address.of(host.getIpAddress());
+                if (hostIpAddress != null && hostIpAddress.getInt() == destinationAddress) {
+                    sameSubnet = true;
+                    break;
+                }
             }
-        }
 
-        // If the destination host is not attached in the switch, add MPLS label
-        if (!sameSubnet) {
-            OFAction pushlabel = factory.actions().pushMpls(EthType.MPLS_UNICAST);
-            OFOxmMplsLabel l = factory.oxms()
-                    .mplsLabel(U32.of(103));
-            OFAction setlabelid = factory.actions().buildSetField()
-                    .setField(l).build();
-            OFAction copyTtlOut = factory.actions().copyTtlOut();
-            actions.add(pushlabel);
-            actions.add(setlabelid);
-            actions.add(copyTtlOut);
-        }
+            // If the destination host is not attached in the switch, add MPLS label
+            if (!sameSubnet) {
 
-        OFAction outport = factory.actions().output(OFPort.of((int) switchPort.getPortNumber().value()), Short.MAX_VALUE);
-        actions.add(outport);
+                IPv4Address targetAddress = IPv4Address.of(((IPv4)packet.getPayload()).getDestinationAddress());
+                int mplsLabel = getMplsLabelFromConfig(targetAddress);
+                if (mplsLabel > 0) {
+                    OFAction pushlabel = factory.actions().pushMpls(EthType.MPLS_UNICAST);
+                    OFOxmMplsLabel l = factory.oxms()
+                            .mplsLabel(U32.of(mplsLabel));
+                    OFAction setlabelid = factory.actions().buildSetField()
+                            .setField(l).build();
+                    OFAction copyTtlOut = factory.actions().copyTtlOut();
+                    actions.add(pushlabel);
+                    actions.add(setlabelid);
+                    actions.add(copyTtlOut);
+                }
+            }
+
+            OFAction outport = factory.actions().output(OFPort.of(switchPort.getPortNumber().shortValue()), Short.MAX_VALUE);
+            actions.add(outport);
+        }
+        // If OFPP_TABLE action is supported, first set a rule to allow packet from CONTROLLER port.
+        // Then, send the packet to the table port
+        else {
+            if (!controllerPortAllowed) {
+                addControlPortInVlanTable(sw);
+                controllerPortAllowed = true;
+            }
+            OFAction outport = factory.actions().output(OFPort.TABLE, Short.MAX_VALUE);
+            actions.add(outport);
+        }
 
         OFPacketOut po = factory.buildPacketOut()
                 .setData(packet.serialize())
@@ -218,6 +242,38 @@
     }
 
     /**
+     * Get MPLS label for the target address from the network config file
+     *
+     * @param targetAddress - IP address of the target host
+     * @return MPLS label of the switch to send packets to the target address
+     */
+    private int getMplsLabelFromConfig(IPv4Address targetAddress) {
+
+        int mplsLabel = -1;
+
+        for (Switch sw: mutableTopology.getSwitches()) {
+
+            String subnets = sw.getStringAttribute("subnets");
+            try {
+                JSONArray arry = new JSONArray(subnets);
+                for (int i = 0; i < arry.length(); i++) {
+                    String subnetIp = (String) arry.getJSONObject(i).get("subnetIp");
+                    if (srManager.netMatch(subnetIp, targetAddress.toString())) {
+                        String mplsLabelStr = sw.getStringAttribute("nodeSid");
+                        if (mplsLabelStr != null)
+                            mplsLabel = Integer.parseInt(mplsLabelStr);
+                    }
+                }
+            } catch (JSONException e) {
+                // TODO Auto-generated catch block
+                e.printStackTrace();
+            }
+        }
+
+        return mplsLabel;
+    }
+
+    /**
      * Add routing rules to forward packets to known hosts
      *
      * @param sw Switch
@@ -319,19 +375,16 @@
      */
     private void addControlPortInVlanTable(Switch sw) {
 
-        if (added)
-            return;
-        else
-            added = true;
-
         IOFSwitch ofSwitch = floodlightProvider.getMasterSwitch(sw.getDpid().value());
         OFFactory factory = ofSwitch.getFactory();
 
-        //OFOxmInPort oxp = factory.oxms().inPort(p.getPortNo());
+        OFOxmInPort oxp = factory.oxms().inPort(OFPort.CONTROLLER);
         OFOxmVlanVid oxv = factory.oxms()
                 .vlanVid(OFVlanVidMatch.UNTAGGED);
         OFOxmList oxmList = OFOxmList.of(oxv);
 
+        /* Cqpd switch does not seems to support CONTROLLER port as in_port match rule */
+        //OFOxmList oxmList = OFOxmList.of(oxp, oxv);
 
         OFMatchV3 match = factory.buildMatchV3()
                 .setOxmList(oxmList)
@@ -340,7 +393,6 @@
         OFInstruction gotoTbl = factory.instructions().buildGotoTable()
                 .setTableId(TableId.of(TABLE_TMAC)).build();
         List<OFInstruction> instructions = new ArrayList<OFInstruction>();
-        // instructions.add(appAction);
         instructions.add(gotoTbl);
         OFMessage flowEntry = factory.buildFlowAdd()
                 .setTableId(TableId.of(TABLE_VLAN))
@@ -355,7 +407,7 @@
                 .build();
 
         flowPusher.add(sw.getDpid(), flowEntry);;
-        //log.debug("Adding {} vlan-rules in sw {}", msglist.size(), getStringId());
+        log.debug("Adding a new vlan-rules in sw {}", sw.getDpid());
 
     }
 
diff --git a/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java b/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
index 257d143..d710b10 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
@@ -1,5 +1,8 @@
 package net.onrc.onos.apps.segmentrouting;
 
+import java.net.Inet4Address;
+import java.net.InetAddress;
+import java.net.UnknownHostException;
 import java.util.ArrayList;
 import java.util.Collection;
 import java.util.Iterator;
@@ -12,16 +15,16 @@
 import net.floodlightcontroller.core.module.IFloodlightModule;
 import net.floodlightcontroller.core.module.IFloodlightService;
 import net.onrc.onos.api.packet.IPacketService;
-import net.onrc.onos.core.datastore.serializers.Topology;
 import net.onrc.onos.core.flowprogrammer.IFlowPusherService;
+import net.onrc.onos.core.intent.Path;
 import net.onrc.onos.core.main.config.IConfigInfoService;
 import net.onrc.onos.core.packet.ARP;
-import net.onrc.onos.core.topology.ITopologyService;
 import net.onrc.onos.core.topology.ITopologyListener;
+import net.onrc.onos.core.topology.ITopologyService;
+import net.onrc.onos.core.topology.LinkData;
 import net.onrc.onos.core.topology.MutableTopology;
-import net.onrc.onos.core.topology.TopologyEvents;
 import net.onrc.onos.core.topology.Switch;
-import net.onrc.onos.core.intent.Path;
+import net.onrc.onos.core.topology.TopologyEvents;
 
 import org.projectfloodlight.openflow.types.IPv4Address;
 import org.projectfloodlight.openflow.util.HexString;
@@ -170,8 +173,77 @@
                     log.debug("ECMPShortestPathGraph:Paths from switch {} to switch {} is {}",
                             HexString.toHexString(sw.getDpid().value()),
                             HexString.toHexString(dstSw.getDpid().value()), paths);
+                    setSegmentRoutingRule(sw, paths);
                 }
             }
     	}
     }
+
+    /**
+     * Set segment routing rule to switches in the ECMP shortest path to the switch
+     *
+     * @param sw source switch
+     * @param paths  ECMP path
+     */
+    private void setSegmentRoutingRule(Switch sw, ArrayList<Path> paths) {
+
+        log.debug("Set routing infor for {} to .. ", sw.getDpid());
+        for (Path path: paths) {
+
+            for (Object obj : path.toArray()) {
+                LinkData link = (LinkData)obj;
+                log.debug("  ---- Set a rule in {} [Forward to {}] " , link.getSrc(), link.getDst().getDpid());
+            }
+        }
+
+    }
+
+    /**
+     * The function checks if given IP matches to the given subnet mask
+     *
+     * @param addr - subnet address to match
+     * @param addr1 - IP address to check
+     * @return true if the IP address matches to the subnet, otherwise false
+     */
+
+    public boolean netMatch(String addr, String addr1){ //addr is subnet address and addr1 is ip address. Function will return true, if addr1 is within addr(subnet)
+
+        String[] parts = addr.split("/");
+        String ip = parts[0];
+        int prefix;
+
+        if (parts.length < 2) {
+            prefix = 0;
+        } else {
+            prefix = Integer.parseInt(parts[1]);
+        }
+
+        Inet4Address a =null;
+        Inet4Address a1 =null;
+        try {
+            a = (Inet4Address) InetAddress.getByName(ip);
+            a1 = (Inet4Address) InetAddress.getByName(addr1);
+        } catch (UnknownHostException e){}
+
+        byte[] b = a.getAddress();
+        int ipInt = ((b[0] & 0xFF) << 24) |
+                         ((b[1] & 0xFF) << 16) |
+                         ((b[2] & 0xFF) << 8)  |
+                         ((b[3] & 0xFF) << 0);
+
+        byte[] b1 = a1.getAddress();
+        int ipInt1 = ((b1[0] & 0xFF) << 24) |
+                         ((b1[1] & 0xFF) << 16) |
+                         ((b1[2] & 0xFF) << 8)  |
+                         ((b1[3] & 0xFF) << 0);
+
+        int mask = ~((1 << (32 - prefix)) - 1);
+
+        if ((ipInt & mask) == (ipInt1 & mask)) {
+            return true;
+        }
+        else {
+            return false;
+        }
+    }
 }